实施例
如图1所示,一种姿态决策动态规划方法,包括下述步骤:
1)依据类人机器人双臂的运动学模型,分别列写该类人机器人左臂和右臂的前向运动学方程;
2)依据步骤1)中的前向运动学方程,分别列写该类人机器人左臂和右臂的逆运动学方程表达式;
3)依据步骤2)中的逆运动学方程,基于二次规划问题,分别列写该类人机器人左臂和右臂的二次规划问题表达式;
4)依据步骤3)中的二次规划表达式,分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并,列写该类人机器人的标准二次规划问题表达式,并根据所述的标准二次规划问题表达式,设定该类人机器人运动规划的三种判决方法;
5)依据步骤4)中的标准二次规划问题表达式,根据该类人机器人的实际姿态控制需要,设计姿态决策动态方程;
6)结合步骤5)中的姿态决策动态方程,重构步骤4)中标准二次规划问题的约束指标,建立该类人机器人的姿态决策动态规划方法;
7)将步骤6)中的姿态决策动态规划方法代入求解器中进行解算,即可完成对该类人机器人双臂的姿态决策控制与运动规划。
图2所示为本实施例的类人机器人的左、右臂关节框架示意图;该类人机器人的双臂共包含14个关节即14个自由度,1~7为类人机器人的右臂关节,其中关节3位于右肩部,关节5位于右肘部,关节6位于右前臂,关节7位于右腕部;8~14为类人机器人的左臂关节,其中,关节10位于左肩部,关节12位于左肘部,关节13位于左前臂,关节14位于左腕部;15为类人机器人的头部,16为类人机器人的腰部;
图3所示为本实施例的类人机器人关节角的物理极限参数;该物理极限参数分别包含类人机器人左、右臂各个关节的物理极限上限及物理极限下限;
图4所示为本实施例的类人机器人关节的D-H参数;该D-H参数共包含有该类人机器人左、右臂14个关节的四个D-H模型参数;
一、模型建立
根据如图1的设计流程图的相关步骤,在此针对类人机器人双臂运动规划方法进行详细的算法说明,首先,对于该类人机器人的双臂,根据传统的运动学方法,可以分别列写出其左臂和右臂的前向运动学方程表达式,其中,该类人机器人的双臂共包含14个自由度,左、右臂分别包含7个,该类人机器人左、右臂关节框架示意图如图2所示;类人机器人关节角的物理极限参数如表1所示;类人机器人关节的D-H参数如表2所示;
表1
表2
关节i |
αi-1(rad) |
αi-1(m) |
di(m) |
θi(rad) |
1 |
π/2 |
0.040 |
0.2820 |
θ1 |
2 |
π/2 |
0.000 |
-0.1370 |
θ2 |
3 |
-π/2 |
0.000 |
0.0000 |
θ3 |
4 |
π/2 |
0.000 |
-0.1550 |
θ4 |
5 |
π/2 |
0.000 |
0.1550 |
θ5 |
6 |
-π/2 |
0.000 |
0.1305 |
θ6 |
7 |
π/2 |
0.000 |
0.1305 |
θ7 |
8 |
π/2 |
-0.040 |
0.2820 |
θ8 |
9 |
π/2 |
0.000 |
0.1370 |
θ9 |
10 |
π/2 |
0.000 |
0.0000 |
θ10 |
11 |
-π/2 |
0.000 |
0.1550 |
θ11 |
12 |
-π/2 |
0.000 |
0.1550 |
θ12 |
13 |
π/2 |
0.000 |
0.1305 |
θ13 |
14 |
-π/2 |
0.000 |
0.1305 |
θ14 |
类人机器人双臂的运动学模型:
其中,r为机器人手臂末端位置向量,θ为关节角向量,为光滑的非线性方程;
根据类人机器人双臂的前向运动学问题,给出左、右臂关节角向量θL和θR,则左、右臂末端位置向量rL和rR通过如下的前向运动学方程得到:
其中,和均为光滑的非线性方程。
二、模型解析
依据前向运动学方程的式(2)和式(3),并根据伪逆方法,在角速度层分别写出如下的左臂、右臂的逆运动学方程:
其中,为左臂的关节角速度;为右臂的关节角速度;为左臂末端执行器的速度;为右臂末端执行器的速度;为左臂的雅克比矩阵的伪逆, 为右臂的雅克比矩阵的伪逆, 为单位矩阵;为左臂的优化指标向量;为右臂的优化指标向量。
基于式(4)和式(5),所述类人机器人左臂和右臂的二次规划问题表达式,具体如下:
对于左臂:
对于右臂:
其中,为左臂的二次项系数矩阵;为右臂的二次项系数矩阵;和为左、右臂约束指标线性项的从属系数;式(7)和式(11)分别表示左、右臂末端执行器的速度和角速度之间的线性关系;和为大小为m×m且对称正定的反馈矩阵;表示左臂的位置误差反馈;表示右臂的位置误差反馈;式(8)和式(12)分别表示左、右臂的关节角度极限约束;式(9)和式(13)分别表示左、右臂的关节角速度极限约束。
然后分别将左臂二次规划指标式(6)、约束条件式(7)-式(9)与右臂的二次规划指标式(10)、约束条件合并式(11)-式(13)合并;
其中,左臂、右臂的约束指标式(6)和式(10)被合并为:
左臂、右臂的前向运动约束式(7)和式(11)被合并为:
左臂、右臂的关节角度极限约束式(8)和式(12)被合并为:
左臂、右臂的关节角速度极限约束式(9)和式(13)被合并为:
至此,结合式(14)-式(17),得到如下类人机器人的标准二次规划问题表达式:
其中, 为左、右臂的关节角向量和组成的向量;b为左、右臂的约束指标线性项的从属系数和组成的向量; 为左、右臂的关节角度下极限约束和组成的向量; 为左、右臂的关节角度上极限约束和组成的向量; 为左、右臂的关节角速度向量和组成的向量; 为左、右臂的关节角速度下极限约束和组成的向量; 为左、右臂的关节角速度上极限约束和组成的向量;Υ为左、右臂的末端位置向量和组成的向量; 为左、右臂的末端速度向量和组成的向量;符号T表示矩阵的转置;
矩阵矩阵矩阵分别定义为:
根据类人机器人的标准二次规划问题表达式的式(18)-式(21),设定类人机器人的三种运动规划判决方法,具体如下:
①小能量判决方法:当矩阵 为惯性矩阵,且线性项从属系数b=0时,类人机器人的标准二次规划问题表达式的式(18)-式(21)构成最小能量判决方法;
②重复运动判决方法:当矩阵被设定为单位矩阵,λ为弹性常系数,且线性项从属系数b=[λ(θ左-θ左(0))λ(θ右-θ右(0))]T时,类人机器人的标准二次规划问题表达式的式(18)-式(21)构成重复运动判决方法;
③最小速度范数判决方法:当被设定为单位矩阵,且线性项从属系数b=0时,类人机器人的标准二次规划问题表达式的式(18)-式(21)构成重复运动判决方法。
三、姿态决策动态规划方法设计
在本实施例中,为使类人机器人双臂能够生成预期的姿态,必须依据时间动态调整双臂的某些关节。众所周知,关节极限会影响和约束关节结构,这启发我们通过设计新的关节极限方案以对关节进行更好地控制。对于二次规划方法,一个关节的物理极限被描述为不等式约束的两个界。因此,我们期望找到一个适当的函数,通过调整边界的界限,随着时间的推移得到预期的值。此外,变化的过程应该是渐进和平稳的。基于上述要求,本实施例提出一种建立在速度层上的关节约束求解方法,即冗余度求解方法。
首先根据人类机器人的实际姿态控制需要,设计出如下的姿态决策动态方程:
其中,为预期角度值;cturning为用于协调控制变化趋势的时间调整参数,且0<cturning<1;TSP=Td/N,Td为任务执行时间,N≥1为影响调整值与初始值近似关系的参数;
如图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为例进行说明。
然后结合姿态决策动态方程式(25),重构人类机器人的标准二次规划问题的角度约束指标式(20),建立该类人机器人的姿态决策动态规划方法,其中,重构的约束指标具有如下形式:
考虑到该冗余度求解方法建立在速度层上,新的关节角度约束式(26)将被如下约束代替:
其中,参数v>0被用于调整角速度的可行域;至此可知,类人机器人的标准二次规划问题表达式的式(18)-式(21)中的角度约束指标式(20)和角速度约束指标式(21)被重构成为如下形式:
令约束下界和约束下界分别满足如下关系:
则类人机器人的姿态决策动态规划方法被设计成为如下形式:
四、姿态决策动态规划方法解析
将姿态决策动态规划方法式(31)-式(33)代入求解器中进行解算,完成对该类人机器人双臂的姿态决策控制与运动规划;其中,姿态决策动态规划方法式(31)-式(33)代入被设计为如下的线性投影方程求解器进行解算:
ΦΩ(u-(Γu+q))-u=0 (34)
其中,为投影运算符;
l表示维度;表示单位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项为类人机器人右臂关节的最优解;至此,将所解算得到的最优解输出,即可完成对该类人机器人双臂的姿态决策与运动规划。
在本实施例中,为展示本发明所述方法的实际应用过程,利用一个仿真实例对所述问题进行说明。本仿真实例在类人机器人应用双臂进行物品操控的基础之上进行。其中,类人机器人左、右臂的初始关节角度分别被设定为:
除此之外,该仿真实例的执行时间被设定为T=18s;双臂角速度的上下限分别被设定为和
在应用传统方法的情况下,本仿真实例的类人机器人双臂执行任务时所得到的14个关节角度的仿真结果如图4(a)-图4(n)所示。由仿真结果可知,在类人机器人双臂执行任务时,关节角度和均超出了它们的上下限极限。除此之外,由图4(f),图4(g),图4(m),图4(n)可知,由传统方法解算得到的角度值超出了它们的预期值。在应用传统方法的情况下,本仿真实例的类人机器人双臂执行任务时所得到的14个关节角角速度的仿真结果如图5(a)-图5(n)所示。由仿真结果可知,在类人机器人双臂执行任务时,关节角速度和均超出了它们的上下限极限。综上所述,应用传统方法并不能很好地完成对类人机器人双臂的姿态规划与控制。
在应用本发明所述的姿态决策动态规划方法的情况下,类人机器人双臂执行任务时所得到的14个关节角角度的仿真结果如图6(a)-图6(n)所示。由仿真结果可知,在类人机器人双臂执行任务时,关节角度和达到了它们的上下限极限,关节角度和总能维持在上下限极限之内。在应用本发明实例所述的姿态决策动态规划方法的情况下,类人机器人双臂执行任务时所得到的14个关节角角速度的仿真结果如图7(a)-图7(n)所示。由仿真结果可知,在类人机器人双臂执行任务时,全部关节角速度均很好地保持在它们的上下限极限之内。综上所述,应用本发明实例所述的姿态决策动态规划方法可以很好地完成对类人机器人双臂的姿态规划与控制。
在应用本发明所述的姿态决策动态规划方法的情况下,类人机器人左臂、右臂执行任务时的位置误差仿真结果如图8(a)和图8(b)所示。由仿真结果可知,在类人机器人双臂执行任务时,左臂和右臂在X轴、Y轴、Z轴上的位置误差均维持在10-5m的范围内,也即达到了很高的控制精度。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。因此,本发明专利的保护范围应以权利要求所述为准。