一种姿态决策动态规划方法[发明专利]

(12)发明专利申请

(10)申请公布号 CN 108638058 A(43)申请公布日 2018.10.12

(21)申请号 2018103263.9(22)申请日 2018.04.23

(71)申请人 华南理工大学

地址 511458 广东省广州市南沙区环市大

道南路25号华工大广州产研院(72)发明人 张智军　孔令东　

(74)专利代理机构 广州市华学知识产权代理有

限公司 44245

代理人 李斌(51)Int.Cl.

B25J 9/16(2006.01)

权利要求书5页 说明书10页 附图22页

(54)发明名称

一种姿态决策动态规划方法(57)摘要

本发明公开了一种姿态决策动态规划方法，包括下述步骤：首先建立模型：列写该类人机器人左臂和右臂的前向运动学方程；然后进行模型解析：分别列写类人机器人左臂和右臂的逆运动学方程，依据逆运动学方程，分别列写类人机器人左臂和右臂的二次规划问题表达式；然后将左臂和右臂的二次规划指标和约束合并，得到类人机器人的标准二次规划问题表达式；再基于该标准二次规划问题表达式，设计姿态决策动态方程以及建立姿态决策动态规划方法；最后设计求解器以对姿态决策动态规划方法进行解算。本发明的姿态决策动态规划方法相对于传统方法，能够更好地完成对类人机器人双臂的姿态规划和控制，并且具有较高的控制精度。

CN 108638058 ACN 108638058 A

权　利　要　求　书

1/5页

1.一种姿态决策动态规划方法，应用于类人机器人的双臂，左臂和右臂分别包含7个自由度，其特征在于，包括下述步骤：

1)依据类人机器人双臂的运动学模型，分别列写该类人机器人左臂和右臂的前向运动学方程；

2)依据步骤1)中的前向运动学方程，分别列写该类人机器人左臂和右臂的逆运动学方程表达式；

3)依据步骤2)中的逆运动学方程，基于二次规划问题，分别列写该类人机器人左臂和右臂的二次规划问题表达式；

4)依据步骤3)中的二次规划表达式，分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并，列写该类人机器人的标准二次规划问题表达式，并根据所述的标准二次规划问题表达式，设定该类人机器人运动规划的三种判决方法；

5)依据步骤4)中的标准二次规划问题表达式，根据该类人机器人的实际姿态控制需要，设计姿态决策动态方程；

6)结合步骤5)中的姿态决策动态方程，重构步骤4)中标准二次规划问题的约束指标，建立该类人机器人的姿态决策动态规划方法；

7)将步骤6)中的姿态决策动态规划方法代入求解器中进行解算，即可完成对该类人机器人双臂的姿态决策控制与运动规划。

2.根据权利要求1所述的姿态决策动态规划方法，其特征在于，步骤1)中，类人机器人双臂的运动学模型，如下：根据类人机器人双臂的前向运动学问题，给出左、右臂关节角向量θθ则左、右臂末L和R，端位置向量rL和rR通过如下的前向运动学方程得到：

其中，和均为光滑的非线性方程。

3.根据权利要求2所述的姿态决策动态规划方法，其特征在于，步骤2)中，依据前向运动学方程的式(2)和式(3)，并根据伪逆方法，在角速度层分别写出如下的左臂、右臂的逆运动学方程：

其中，为左臂的关节角速度；为右臂的关节角速度；为左为左臂的

为右为单位矩

臂末端执行器的速度；雅克比矩阵臂的雅克比矩阵

的伪逆，

为右臂末端执行器的速度；

的伪逆，

2

CN 108638058 A

权　利　要　求　书

为左臂的优化指标向量；

为右臂的优化指标向量。

2/5页

阵；

4.根据权利要求3所述的姿态决策动态规划方法，其特征在于，步骤3)中，基于式(4)和式(5)，所述类人机器人左臂和右臂的二次规划问题表达式，具体如下：

对于左臂：

对于右臂：

其中，和

为左臂的二次项系数矩阵；为右臂的二次项系数矩阵；

为左、右臂约束指标线性项的从属系数；式(7)和式(11)分别表示左、右臂末端执

和角速度

之间的线性关系；

和

为大小为m×m且对称

行器的速度

正定的反馈矩阵；表示左臂的位置误差反馈；

表示右臂的位置误差反馈；式(8)和式(12)分别表示左、右臂的关

节角度极限约束；式(9)和式(13)分别表示左、右臂的关节角速度极限约束。5.根据权利要求4所述的姿态决策动态规划方法，其特征在于，步骤4)中，分别将左臂二次规划指标式(6)、约束条件式(7)-式(9)与右臂的二次规划指标式(10)、约束条件合并式(11)-式(13)合并；

其中，左臂、右臂的约束指标式(6)和式(10)被合并为：

左臂、右臂的前向运动约束式(7)和式(11)被合并为：

左臂、右臂的关节角度极限约束式(8)和式(12)被合并为：

3

CN 108638058 A

权　利　要　求　书

3/5页

左臂、右臂的关节角速度极限约束式(9)和式(13)被合并为：

至此，结合式(14)-式(17)，得到如下类人机器人的标准二次规划问题表达式：

其中，为左、右臂的关节角向量

b为左、右臂的约束指标线性项的从属系数

为左、右臂的关节角度下极限约束

和和

组成的向量；组成的向量；和和和组成的向组成的组成和

和

量；向量；的向量；组成的向量；

组成的向量；向量；

表示矩阵的转置；

矩阵

矩阵

为左、右臂的关节角度上极限约束

为左、右臂的关节角速度向量

为左、右臂的关节角速度下极限约束

为左、右臂的关节角速度上极限约束Υ为左、右臂的末端位置向量

为左、右臂的末端速度向量

和

和

组成的

组成的向量；符号T

矩阵分别定义为：

4

CN 108638058 A

权　利　要　求　书

4/5页

6.根据权利要求5所述的姿态决策动态规划方法，其特征在于，步骤4)中，根据类人机器人的标准二次规划问题表达式的式(18)-式(21)，设定类人机器人的三种运动规划判决方法，具体如下：

①小能量判决方法：当矩阵

为惯性矩阵，且线性项从属系数

b＝0时，类人机器人的标准二次规划问题表达式的式(18)-式(21)构成最小能量判决方法；

②重复运动判决方法：当矩阵被设定为单位矩阵，λ为弹性常系数，且线性项从属系

T

数b＝[λ(θθ λ(θθ类人机器人的标准二次规划问题表达式的式(18)-左-左(0))右-右(0))]时，式(21)构成重复运动判决方法；

③最小速度范数判决方法：当被设定为单位矩阵，且线性项从属系数b＝0时，类人机器人的标准二次规划问题表达式的式(18)-式(21)构成重复运动判决方法。

7.根据权利要求5所述的姿态决策动态规划方法，其特征在于，步骤5)中，根据人类机器人的实际姿态控制需要，设计出如下的姿态决策动态方程：

其中，为预期角度值；cturning为用于

协制变化趋势的时间调整参数，且0＜cturning＜1；TSP＝Td/N，Td为任务执行时间，N≥1为影响调整值与初始值近似关系的参数；所述姿态决策动态方程式(25)将第i个初始关节角度值

规划至目标关节角度值

8.根据权利要求7所述的姿态决策动态规划方法，其特征在于，步骤6)中，结合姿态决策动态方程式(25)，重构人类机器人的标准二次规划问题的角度约束指标式(20)，建立该类人机器人的姿态决策动态规划方法，其中，重构的角度约束指标具有如下形式：

将新的关节角度约束式(26)转换成如下的角速度约束：

其中，参数v＞0被用于调整角速度的可行域；至此可知，类人机器人的标准二次规划问题表达式的式(18)-式(21)中的角度约束指标式(20)和角速度约束指标式(21)被重构成为如下形式：

令约束下界和约束下界分别满足如下关系：

5

CN 108638058 A

权　利　要　求　书

5/5页

则类人机器人的姿态决策动态规划方法被设计成为如下形式:

9.根据权利要求8所述的姿态决策动态规划方法，其特征在于，步骤7)中，在步骤7)中，将步骤6)中的姿态决策动态规划方法式(31)-式(33)代入求解器中进行解算，完成对该类人机器人双臂的姿态决策控制与运动规划；其中，姿态决策动态规划方法式(31)-式(33)代入被设计为如下的线性投影方程求解器进行解算：

ΦΩ(u-(Γu+q))-u＝0   (34)

其中，ΦΩ(·)：

为投影运算符；

ι表示

维度；表示单位1ι的系数参数；

为解算式(34)，定义如下的误差方程：ε(t)＝u-ΦΩ(u-(Γu+q))   (35)为使误差方程(35)趋于零，也即求得最优解，利用如下的迭代算法：设初始对偶决策变量为到如下的迭代方程：

迭代次数k＝0,1,2,…，如果

那么得

其中，||·||2为欧几里得范数；ε(uk)＝uk-ΦΩ(uk-(Γuk+q))；σ(uk)＝(ΓT+I)ε(uk)。对于由式(36)迭代得到的序列{uk}，k＝0,1,2,…，对于全体u*∈Ω*，其满足如下关系：

也即，序列{uk}收敛到解向量u*，且其前2n项组成了姿态决策动态规划方法(31)-(33)的最优解

其中，前n项为类人机器人左臂关节的最优解，后n项为类人机器人右

臂关节的最优解；至此，将所解算得到的最优解输出，即可完成对该类人机器人双臂的姿态决策与运动规划。

6

CN 108638058 A

说　明　书

一种姿态决策动态规划方法

1/10页

技术领域

[0001]本发明涉及机器人运动学和控制规划领域，特别涉及一种用于控制类人机器人双臂的姿态决策动态规划方法。

背景技术

[0002]类人机器人是现代的人形机器人，不仅外观像人，有人的模样，还能像人一样进行各种各样的活动。类人机器人之所以能像人一样活动、有人的行为，是因为运用传感器等组件搭建的机器人系统的中枢，能够像人脑一样控制和指挥机器人的行为。这样，类人机器人能运动、会思考、甚至自己去“想”。研制出外观和功能与人一样的类人机器人是现代科技发展的结果。全新组装的类人机器人全身布满了感应器，让它可以根据感应到的声音和动作做出适当反应。

[0003]类人机器人可以帮助人们更好地适应点对点的协作环境，因此在各种应用中越来越受到欢迎。类人机器人的双臂不仅可以用于完成各种各样的操作任务，而且作为完成肢体语言的重要组成部分，可以使得类人机器人更加真实。与此同时，与传统机器人的单臂系统相比，类人机器人的双臂系统可以通过同时执行任务而大大提高效率。除此之外，类人机器人还可以根据实际需要，选择使用单臂或双臂完成各种任务。例如，它们可以通过共享负载来协作处理笨重的对象。早期的多臂系统可以追溯到戈茨在1940年代用于放射性货物装卸的遥控机械手系统。从1950年底到1970年初，由于深海和深空探测的发展，对双臂遥操作机器人的研究逐渐深入。[0004]近年来，由于类人机器人的迅速发展，双臂应用再次吸引了研究人员和工程师的兴趣。类人机器人要求其双臂能够在家庭环境中自主或半自主地进行各种日常工作任务，与此同时，它们还要求能够通过展示情感肢体语言来提高机器人的社交能力。为了更好地使类人机器人的双臂完成各种复杂的日常任务，对于双臂运动规划问题的研究便具有了实际意义。逆运动学问题是双臂运动规划的基本问题之一，即给定双臂的末端执行器轨迹，如何在每个时刻计算关节变量的问题。大多数人形机器人的双臂在实际应用中具有三个以上的自由度，被称作冗余度机械臂。这种冗余度提高了机器人实现末端执行器任务时的灵活性和通用性，但与此同时，也不可避免的提高了计算的难度。在这样一种情况下，因为有很大数量的机械关节需要用于完成特定的末端执行器任务，所以求解得到用于类人机器人双臂运动规划的解析解是非常重要和具有挑战性的。传统的用于求解冗余度机械臂的方法是基于伪逆的方法，其中，一种基于伪逆法的闭环逆运动学方法被提出，并被用于移动平台上双臂的运动规划上。但是在应用这样一种基于伪逆法的运动规划方法时，必须考虑并计算矩阵的逆，而且很难考虑其中的不等式问题，大大增加了解析和计算的复杂性。因此，建立在上述伪逆方法的基础上，一种基于二次规划问题的方法于近期被提出，并得到了深入的研究。但现有的二次规划优化方法多集中在对单个机械臂的运动规划上，只有少数方法考虑了对类人机器人双臂运动规划的研究。面对这样一种研究现状，一种用于类人机器人双臂运动规划的姿态决策动态规划方法在本发明专利中被提出，并应用于类人机器人的仿真

7

CN 108638058 A

说　明　书

2/10页

及实物实验上。

发明内容

[0005]本发明的目的在于克服现有技术中的缺点与不足，提供一种用于控制类人机器人双臂的姿态决策动态规划方法，能够很好地完成对类人机器人双臂的姿态规划与控制，并且具有很高的控制精度。[0006]为实现以上目的，本发明采取如下技术方案：[0007]一种姿态决策动态规划方法，应用于类人机器人的双臂，左臂和右臂分别包含7个自由度，包括下述步骤：

[0008]1)依据类人机器人双臂的运动学模型，分别列写该类人机器人左臂和右臂的前向运动学方程；

[0009]2)依据步骤1)中的前向运动学方程，分别列写该类人机器人左臂和右臂的逆运动学方程表达式；

[0010]3)依据步骤2)中的逆运动学方程，基于二次规划问题，分别列写该类人机器人左臂和右臂的二次规划问题表达式；

[0011]4)依据步骤3)中的二次规划表达式，分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并，列写该类人机器人的标准二次规划问题表达式，并根据所述的标准二次规划问题表达式，设定该类人机器人运动规划的三种判决方法；[0012]5)依据步骤4)中的标准二次规划问题表达式，根据该类人机器人的实际姿态控制需要，设计姿态决策动态方程；

[0013]6)结合步骤5)中的姿态决策动态方程，重构步骤4)中标准二次规划问题的约束指标，建立该类人机器人的姿态决策动态规划方法；

[0014]7)将步骤6)中的姿态决策动态规划方法代入求解器中进行解算，即可完成对该类人机器人双臂的姿态决策控制与运动规划。

[0015]本发明相对于现有技术具有如下的优点和效果：[0016](1)本发明为基于机器人学和控制学的动态规划方法，依据该方法可以很好地完成对类人机器人左臂、右臂的控制，使该类人机器人运用其双臂完成各种姿态规划。[0017](2)本发明相比于传统控制方法，本发明的姿态决策动态规划控制方法具有思路清晰、控制精准、简明有效等特点。附图说明

[0018]图1为本实施例的姿态决策动态规划方法的流程图；[0019]图2为本实施例的类人机器人左臂、右臂的关节框架及各自由度分布示意图；附图标记：1～7为类人机器人的右臂关节；8～14为类人机器人的左臂关节；15为类人机器人的头部，16为类人机器人的腰部；

[0020]图3(a)和图3(b)分别为本实施例的姿态决策动态方程参数值cturning和N(TSP)的特征曲线图；

[0021]图4(a)-图4(n)为在应用传统方法的情况下，本实施例的类人机器人双臂执行任务时所得到的14个关节角角度的仿真结果图；

8

CN 108638058 A[0022]

说　明　书

3/10页

图5(a)-图5(n)为在应用传统方法的情况下，本实施例的类人机器人双臂执行任

务时所得到的14个关节角角速度的仿真结果图；

[0023]图6(a)-图6(n)为在应用本实施例所述的姿态决策动态规划方法的情况下，类人机器人双臂执行任务时所得到的14个关节角角度的仿真结果图；

[0024]图7(a)-图7(n)为在应用本实施例所述的姿态决策动态规划方法的情况下，类人机器人双臂执行任务时所得到的14个关节角角速度的仿真结果图；

[0025]图8(a)和图8(b)分别为在应用本实施例所述的姿态决策动态规划方法的情况下，类人机器人左臂、右臂执行任务时的位置误差仿真结果图。

具体实施方式

[0026]为了使本发明的目的、技术方案以及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不限于本发明。[0027]实施例

[0028]如图1所示，一种姿态决策动态规划方法，包括下述步骤：[0029]1)依据类人机器人双臂的运动学模型，分别列写该类人机器人左臂和右臂的前向运动学方程；

[0030]2)依据步骤1)中的前向运动学方程，分别列写该类人机器人左臂和右臂的逆运动学方程表达式；

[0031]3)依据步骤2)中的逆运动学方程，基于二次规划问题，分别列写该类人机器人左臂和右臂的二次规划问题表达式；

[0032]4)依据步骤3)中的二次规划表达式，分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并，列写该类人机器人的标准二次规划问题表达式，并根据所述的标准二次规划问题表达式，设定该类人机器人运动规划的三种判决方法；[0033]5)依据步骤4)中的标准二次规划问题表达式，根据该类人机器人的实际姿态控制需要，设计姿态决策动态方程；

[0034]6)结合步骤5)中的姿态决策动态方程，重构步骤4)中标准二次规划问题的约束指标，建立该类人机器人的姿态决策动态规划方法；

[0035]7)将步骤6)中的姿态决策动态规划方法代入求解器中进行解算，即可完成对该类人机器人双臂的姿态决策控制与运动规划。

[0036]图2所示为本实施例的类人机器人的左、右臂关节框架示意图；该类人机器人的双臂共包含14个关节即14个自由度，1～7为类人机器人的右臂关节，其中关节3位于右肩部，关节5位于右肘部，关节6位于右前臂，关节7位于右腕部；8～14为类人机器人的左臂关节，其中，关节10位于左肩部，关节12位于左肘部，关节13位于左前臂，关节14位于左腕部；15为类人机器人的头部，16为类人机器人的腰部；

[0037]图3所示为本实施例的类人机器人关节角的物理极限参数；该物理极限参数分别包含类人机器人左、右臂各个关节的物理极限上限及物理极限下限；[0038]图4所示为本实施例的类人机器人关节的D-H参数；该D-H参数共包含有该类人机器人左、右臂14个关节的四个D-H模型参数；

9

CN 108638058 A[0039]

说　明　书

4/10页

一、模型建立

[0040]根据如图1的设计流程图的相关步骤，在此针对类人机器人双臂运动规划方法进行详细的算法说明，首先，对于该类人机器人的双臂，根据传统的运动学方法，可以分别列写出其左臂和右臂的前向运动学方程表达式，其中，该类人机器人的双臂共包含14个自由度，左、右臂分别包含7个，该类人机器人左、右臂关节框架示意图如图2所示；类人机器人关节角的物理极限参数如表1所示；类人机器人关节的D-H参数如表2所示；[0041]表1

[0042]

[0043][0044]

表2

ααi-1(rad)i-1(m)π/20.040π/20.000-π/20.000π/20.000π/20.000-π/20.000π/20.000π/2-0.040π/20.000π/20.000-π/20.000-π/20.000π/20.000-π/20.000

类人机器人双臂的运动学模型：

di(m)0.2820-0.13700.0000-0.15500.15500.13050.13050.28200.13700.00000.15500.15500.13050.1305

θi(rad)θ1θ2θ3θ4θ5θ6θ7θ8θ9θ10θ11θ12θ13θ14

关节i

12345671011121314

[0045][0046][0047]

其中，r为机器人手臂末端位置向量，θ为关节角向量，

10

为光滑的非线性方

CN 108638058 A

说　明　书

5/10页

程；

[0048]

根据类人机器人双臂的前向运动学问题，给出左、右臂关节角向量θθ则左、右L和R，

臂末端位置向量rL和rR通过如下的前向运动学方程得到：

[0049][0050][0051][0052]

其中，和均为光滑的非线性方程。

二、模型解析

[0053]依据前向运动学方程的式(2)和式(3)，并根据伪逆方法，在角速度层分别写出如下的左臂、右臂的逆运动学方程：

[0054][0055][0056]

其中，为左臂的关节角速度；为右臂的关节角速度；

为左臂末端执行器的速度；

为左臂的雅克比矩阵为右臂的雅克比矩阵矩阵；

[0057]

为右臂末端执行器的速度；

的伪逆，的伪逆，

为右臂的优化指标向量。

为单位

为左臂的优化指标向量；

基于式(4)和式(5)，所述类人机器人左臂和右臂的二次规划问题表达式，具体如对于左臂：

下：

[0058][0059]

[0060]

[0061][0062][0063][00]

对于右臂：

[0065]

[0066]

11

CN 108638058 A[0067][0068]

说　明　书

6/10页

其中，和

为左臂的二次项系数矩阵；为右臂的二次项系数矩

阵；为左、右臂约束指标线性项的从属系数；式(7)和式(11)分别表示左、右臂末端

和角速度

之间的线性关系；

和

为大小为m×m且

执行器的速度

对称正定的反馈矩阵；表示左臂的位置误差反馈；

表示右臂的位置误差反馈；式(8)和式(12)分别表示左、右臂的关节角度极限约束；式(9)和

式(13)分别表示左、右臂的关节角速度极限约束。[0069]然后分别将左臂二次规划指标式(6)、约束条件式(7)-式(9)与右臂的二次规划指标式(10)、约束条件合并式(11)-式(13)合并；[0070]其中，左臂、右臂的约束指标式(6)和式(10)被合并为：

[0071]

[0072][0073]

左臂、右臂的前向运动约束式(7)和式(11)被合并为：

[0074][0075][0076]

左臂、右臂的关节角度极限约束式(8)和式(12)被合并为：

左臂、右臂的关节角速度极限约束式(9)和式(13)被合并为：

[0077][0078][0079]

至此，结合式(14)-式(17)，得到如下类人机器人的标准二次规划问题表达式：

[0080]

[0081][0082]

12

CN 108638058 A[0083]

说　明　书

为左、右臂的关节角向量

b为左、右臂的约束指标线性项的从属系数

为左、右臂的关节角度下极限约束

和和

和和

7/10页

其中，组成的向组成的向量；

组成的向

组成和

组和

量；

量；的向量；成的向量；

组成的向量；和

组成的向量；

为左、右臂的关节角度上极限约束

为左、右臂的关节角速度向量

为左、右臂的关节角速度下极限约束

为左、右臂的关节角速度上极限约束Υ为左、右臂的末端位置向量

为左、右臂的末端速度向量

和

和

组成的向量；

量；符号T表示矩阵的转置；

[0084][0085]

组成的向

矩阵矩阵矩阵分别定义为：

[0086]

[0087]

根据类人机器人的标准二次规划问题表达式的式(18)-式(21)，设定类人机器人

的三种运动规划判决方法，具体如下：

[00]

[0088]

①小能量判决方法：当矩阵为惯性矩阵，且线性项从属

系数b＝0时，类人机器人的标准二次规划问题表达式的式(18)-式(21)构成最小能量判决方法；

[0090]

②重复运动判决方法：当矩阵被设定为单位矩阵，λ为弹性常系数，且线性项从

T

属系数b＝[λ(θθλ(θθ类人机器人的标准二次规划问题表达式的式左-左(0))右-右(0))]时，(18)-式(21)构成重复运动判决方法；[0091]③最小速度范数判决方法：当被设定为单位矩阵，且线性项从属系数b＝0时，类人机器人的标准二次规划问题表达式的式(18)-式(21)构成重复运动判决方法。

13

CN 108638058 A[0092]

说　明　书

8/10页

三、姿态决策动态规划方法设计

[0093]在本实施例中，为使类人机器人双臂能够生成预期的姿态，必须依据时间动态调整双臂的某些关节。众所周知，关节极限会影响和约束关节结构，这启发我们通过设计新的关节极限方案以对关节进行更好地控制。对于二次规划方法，一个关节的物理极限被描述为不等式约束的两个界。因此，我们期望找到一个适当的函数，通过调整边界的界限，随着时间的推移得到预期的值。此外，变化的过程应该是渐进和平稳的。基于上述要求，本实施例提出一种建立在速度层上的关节约束求解方法，即冗余度求解方法。[0094]首先根据人类机器人的实际姿态控制需要，设计出如下的姿态决策动态方程：

[0095][0096]

其中，为预期角度值；cturning

为用于协制变化趋势的时间调整参数，且0＜cturning＜1；TSP＝Td/N，Td为任务执行时间，N≥1为影响调整值与初始值近似关系的参数；

[0097]如图3(a)和图3(b)所示为本实施例的姿态决策动态方程参数值cturning和N(TSP)的特征曲线图。图3(a)表示应用姿态决策动态方程(25)能够使得类人机器人双臂达到期望姿态的同时兼顾渐进性与平滑性。该姿态决策动态方程(25)能够渐进且平滑地将第i个初始关节角度值

规划至目标关节角度值

不失一般性的，以i＝3为例进行说明。若设

则在应用姿态决策动态方程(24)后，第三个关节的上限极限和下

限极限能够在一定时间周期内逐渐渐进至5。通过分析可知，对于姿态决策动态方程(25)，应用不同的参数值cturning将取得不同的渐进效果。图3(a)分别以参数值cturning＝1和cturning＝0.1为例进行说明。图3(b)表示应用姿态决策动态方程(25)时，不同的参数值N或TSP会影响关节角调整值与初始值之间的近似关系。图3(b)分别以参数值N＝1和N＝10为例进行说明。

[0098]然后结合姿态决策动态方程式(25)，重构人类机器人的标准二次规划问题的角度约束指标式(20)，建立该类人机器人的姿态决策动态规划方法，其中，重构的约束指标具有如下形式：

[0099][0100]

考虑到该冗余度求解方法建立在速度层上，新的关节角度约束式(26)将被如下约

束代替：

[0101][0102]

其中，参数v＞0被用于调整角速度的可行域；至此可知，类人机器人的标准二次

规划问题表达式的式(18)-式(21)中的角度约束指标式(20)和角速度约束指标式(21)被重

构成为如下形式：

[0103]

[0104]令约束下界和约束下界

14

分别满足如下关系：

CN 108638058 A[0105][0106][0107][0108]

说　明　书

9/10页

则类人机器人的姿态决策动态规划方法被设计成为如下形式:

[0109]

[0110][0111]

四、姿态决策动态规划方法解析

[0112]将姿态决策动态规划方法式(31)-式(33)代入求解器中进行解算，完成对该类人机器人双臂的姿态决策控制与运动规划；其中，姿态决策动态规划方法式(31)-式(33)代入被设计为如下的线性投影方程求解器进行解算：[0113]ΦΩ(u-(Γu+q))-u＝0     (34)

[0114][0115]

其中，为投影运算符；

[0116]

l表示

维度；表示单位1ι的系数参数；

[0117][0118][0119][0120]

为解算式(34)，定义如下的误差方程：

ε(t)＝u-ΦΩ(u-(Γu+q))    (35)为使误差方程(35)趋于零，也即求得最优解，利用如下的迭代算法：设初始对偶决策变量为

迭代次数k＝0,1,2,…，如果

那

么得到如下的迭代方程：

[0121][0122]

其中，||·||2为欧几里得范数；ε(uk)＝uk-ΦΩ(uk-(Γuk+q))；σ(uk)＝(ΓT+I)ε对于由式(36)迭代得到的序列{uk}，k＝0,1,2,…，对于全体u*∈Ω*，其满足如下

(uk)。

[0123]

关系：

[0124][0125]

也即，序列{uk}收敛到解向量u*，且其前2n项组成了姿态决策动态规划方法(31)-

15

CN 108638058 A

说　明　书

10/10页

(33)的最优解其中，前n项为类人机器人左臂关节的最优解，后n项为类人机

器人右臂关节的最优解；至此，将所解算得到的最优解输出，即可完成对该类人机器人双臂

的姿态决策与运动规划。[0126]在本实施例中，为展示本发明所述方法的实际应用过程，利用一个仿真实例对所述问题进行说明。本仿真实例在类人机器人应用双臂进行物品操控的基础之上进行。其中，类人机器人左、右臂的初始关节角度分别被设定为：

[0127][0128][0129]

除此之外，该仿真实例的执行时间被设定为T＝18s；双臂角速度的上下限分别被

和

在应用传统方法的情况下，本仿真实例的类人机器人双臂执行任务时所得到的14

的仿真结果如图4(a)-图4(n)所示。由仿真结果可知，在类人机器人双

和

均超出了它们的上下限极限。除此之外，由图4(f),图

设定为

[0130]

个关节角度

臂执行任务时，关节角度

4(g),图4(m),图4(n)可知，由传统方法解算得到的角度值超出了它们的预期值。在应用传统方法的情况下，本仿真实例的类人机器人双臂执行任务时所得到的14个关节角角速度

的仿真结果如图5(a)-图5(n)所示。由仿真结果可知，在类人机器人双臂执行任务

时，关节角速度

和

均超出了它们的上下限极限。综上所述，应用传统方法并不能

很好地完成对类人机器人双臂的姿态规划与控制。

[0131]在应用本发明所述的姿态决策动态规划方法的情况下，类人机器人双臂执行任务时所得到的14个关节角角度

的仿真结果如图6(a)-图6(n)所示。由仿真结果可知，

和

达到了它们的上下限极限，关节

在类人机器人双臂执行任务时，关节角度角度

和

总能维持在上下限极限之内。在应用本发明实例所述的姿态

决策动态规划方法的情况下，类人机器人双臂执行任务时所得到的14个关节角角速度

的仿真结果如图7(a)-图7(n)所示。由仿真结果可知，在类人机器人双臂执行任务

时，全部关节角速度均很好地保持在它们的上下限极限之内。综上所述，应用本发明实例所述的姿态决策动态规划方法可以很好地完成对类人机器人双臂的姿态规划与控制。[0132]在应用本发明所述的姿态决策动态规划方法的情况下，类人机器人左臂、右臂执行任务时的位置误差仿真结果如图8(a)和图8(b)所示。由仿真结果可知，在类人机器人双臂执行任务时，左臂和右臂在X轴、Y轴、Z轴上的位置误差均维持在10-5m的范围内，也即达到了很高的控制精度。

[0133]以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以权利要求所述为准。

16

CN 108638058 A

说　明　书　附　图

1/22页

图1

17

CN 108638058 A

说　明　书　附　图

图2

图3(a)

18

2/22页

CN 108638058 A

说　明　书　附　图

3/22页

图3(b)

图4(a)

图4(b)

19

CN 108638058 A

说　明　书　附　图

4/22页

图4(c)

图4(d)

图4(e)

20

CN 108638058 A

说　明　书　附　图

5/22页

图4(f)

图4(g)

图4(h)

21

CN 108638058 A

说　明　书　附　图

6/22页

图4(i)

图4(j)

图4(k)

22

CN 108638058 A

说　明　书　附　图

7/22页

图4(l)

图4(m)

图4(n)

23

CN 108638058 A

说　明　书　附　图

8/22页

图5(a)

图5(b)

图5(c)

24

CN 108638058 A

说　明　书　附　图

9/22页

图5(d)

图5(e)

图5(f)

25

CN 108638058 A

说　明　书　附　图

10/22页

图5(g)

图5(h)

图5(i)

26

CN 108638058 A

说　明　书　附　图

11/22页

图5(j)

图5(k)

图5(l)

27

CN 108638058 A

说　明　书　附　图

12/22页

图5(m)

图5(n)

图6(a)

28

CN 108638058 A

说　明　书　附　图

13/22页

图6(b)

图6(c)

图6(d)

29

CN 108638058 A

说　明　书　附　图

14/22页

图6(e)

图6(f)

图6(g)

30

CN 108638058 A

说　明　书　附　图

15/22页

图6(h)

图6(i)

图6(j)

31

CN 108638058 A

说　明　书　附　图

16/22页

图6(k)

图6(l)

图6(m)

32

CN 108638058 A

说　明　书　附　图

17/22页

图6(n)

图7(a)

图7(b)

33

CN 108638058 A

说　明　书　附　图

18/22页

图7(c)

图7(d)

图7(e)

34

CN 108638058 A

说　明　书　附　图

19/22页

图7(f)

图7(g)

图7(h)

35

CN 108638058 A

说　明　书　附　图

20/22页

图7(i)

图7(j)

图7(k)

36

CN 108638058 A

说　明　书　附　图

21/22页

图7(l)

图7(m)

图7(n)

37

CN 108638058 A

说　明　书　附　图

22/22页

图8(a)

图8(b)

38