发明内容
本发明的目的在于克服现有技术中的缺点与不足,提供一种类人机器人双臂运动规划方法,将类人机器人的双臂运动转化为二次规划问题形式,求解出类人机器人双臂关节运动的最优解,能够有效实现对类人机器人左、右手臂的精确控制。
为实现以上目的,本发明采取如下技术方案:
一种类人机器人双臂运动规划方法,其特征在于,包括下述步骤:
1)依据类人机器人双臂的运动学模型,分别列写该类人机器人左臂和右臂的前向运动学方程;
2)依据步骤1)得出的前向运动学方程,并根据伪逆方法,分别列写类人机器人左臂和右臂的逆运动学方程;
3)依据步骤2)得出的逆运动学方程,分别列写类人机器人左臂和右臂的二次规划问题表达式;
4)依据步骤3)得出的左臂和右臂的二次规划问题表达式,分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并,得到类人机器人的标准二次规划问题表达式;
5)依据步骤4)得出的类人机器人的标准二次规划问题表达式,设定该类人机器人的三种运动规划判决方法;
6)结合步骤5)得出的三种运动规划判决方法,利用求解器将类人机器人的标准二次规划问题表达式进行解算,得出满足于不同运动规划判决方法条件下的解,即可完成对该类人机器人双臂的运动规划。
本发明相对于现有技术具有如下的优点和效果:
(1)本发明为基于机器人学的动态规划方法,依据该方法可以很好地完成对类人机器人左、右手臂的控制,具有思路清晰、逻辑性强、简明有效等特点。
(2)本发明方法能够很好地完成对类人机器人左、右手臂的控制,与传统方法只能实现离线控制不同,在运用本发明所述方法进行类人机器人手臂运动学解算时,可实现实时在线控制,具有思路清晰、逻辑性强、简明有效等特点。
实施例
如图1所示,一种类人机器人双臂运动规划方法,包括下述步骤:
1)依据类人机器人双臂的运动学模型,分别列写该类人机器人左臂和右臂的前向运动学方程;
2)依据步骤1)得出的前向运动学方程,同时根据伪逆方法,分别列写类人机器人左臂和右臂的逆运动学方程;
3)依据步骤2)得出的逆运动学方程,分别列写类人机器人左臂和右臂的二次规划问题表达式;
4)依据步骤3)得出的左臂和右臂的二次规划问题表达式,分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并,得到类人机器人的标准二次规划问题表达式;
5)依据步骤4)得出的类人机器人的标准二次规划问题表达式,设定该类人机器人的三种运动规划判决方法;
6)结合步骤5)得出的三种运动规划判决方法,利用求解器将类人机器人的标准二次规划问题表达式进行解算,得出满足于不同运动规划判决方法条件下的解,即可完成对该类人机器人双臂的运动规划。
图2所示为本实施例的类人机器人的左、右臂关节框架示意图;该类人机器人的双臂共包含14个关节即14个自由度,1~7为类人机器人的右臂关节,其中关节3位于右肩部,关节5位于右肘部,关节6位于右前臂,关节7位于右腕部;8~14为类人机器人的左臂关节,其中,关节10位于左肩部,关节12 位于左肘部,关节13位于左前臂,关节14位于左腕部;15为类人机器人的头部,16为类人机器人的腰部;
表1为本实施例的类人机器人关节角的物理极限参数;该物理极限参数分别包含类人机器人左、右臂各个关节的物理极限上限及物理极限下限。
表2为本实施例的类人机器人关节的D-H参数;该D-H参数共包含有该类人机器人左、右臂14个关节的四个D-H模型参数。
图3所示为本实施例的类人机器人应用双臂执行任务时所得到的各个关节角角度的仿真结果;其中,图3(a)-图3(n)分别展示了该类人机器人执行任务时左、右臂14个关节的关节角度结果;
图4所示为本实施例的类人机器人应用双臂执行任务时所得到的各个关节角角速度的仿真结果;其中,图4(a)-图4(n)分别展示了该类人机器人执行任务时左、右臂14个关节的关节角速度结果。
表1类人机器人关节角的物理极限参数
表2类人机器人关节的D-H参数
关节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 |
根据如图1的设计流程图的相关步骤,在此针对类人机器人双臂运动规划方法进行详细的算法说明,首先,对于该类人机器人的双臂,根据传统的运动学方法,可以分别列写出其左臂和右臂的前向运动学方程表达式,其中,该类人机器人的双臂共包含14个自由度,左、右臂分别包含7个,该类人机器人左、右臂关节框架示意图如图2所示;类人机器人关节角的物理极限参数如表1所示;类人机器人关节的D-H参数如表2所示;
类人机器人双臂的运动学模型:
其中,r为机器人手臂末端位置向量,θ为关节角向量,为光滑的非线性方程;
根据该类人机器人双臂的前向运动学问题即分别给出左、右关节角向量θ左和θ右,则该双臂的末端位置向量r左和r右可以通过如下的前向运动学方程得到:
其中,r左、r右分别表示类人机器人左臂和右臂的末端位置向量;θ左、θ右分别表示类人机器人左臂和右臂的关节角向量,和均为光滑的非线性方程。
依据前向运动学方程的式(2)和式(3),并根据伪逆方法,在角速度层分别写出如下的左臂、右臂的逆运动学方程:
其中,为左臂的关节角速度;为右臂的关节角速度;为左臂末端执行器的速度;为右臂末端执行器的速度;为左臂的雅克比矩阵的伪逆, 为右臂的雅克比矩阵的伪逆, 为单位矩阵;为左臂的优化指标向量;为右臂的优化指标向量。
基于式(4)和式(5),可以将左、右臂逆运动方程学设计成如下的二次规划问题形式:
对于左臂:
minimize
subject to
对于右臂:
minimize
subject to
其中,为左臂的二次项系数矩阵;为右臂的二次项系数矩阵;和为左、右臂约束指标线性项的从属系数;式(7)和式(11)分别表示左、右臂末端执行器的速度和角速度之间的线性关系;和为大小为m×m且对称正定的反馈矩阵;表示左臂的位置误差反馈;表示右臂的位置误差反馈;式(8)和式(12)分别表示左、右臂的关节角度极限约束;式(9)和式(13)分别表示左、右臂的关节角速度极限约束。
然后分别将左臂二次规划指标式(6)、约束条件式(7)-式(9)与右臂的二次规划指标式(10)、约束条件合并式(11)-式(13)合并;
其中,左、右臂的约束指标式(6)和式(10)被合并为:
minimize
左、右臂的前向运动约束式(7)和式(11)被合并为:
左、右臂的关节角度极限约束式(8)和式(12)被合并为:
左、右臂的关节角速度极限约束式(9)和式(13)被合并为:
至此,结合式(14)-式(17),得到如下类人机器人的标准二次规划问题表达式:
minimize
subject to
其中,为左、右臂的关节角向量和组成的向量;b为左、右臂的约束指标线性项的从属系数和组成的向量;为左、右臂的关节角度下极限约束和组成的向量;为左、右臂的关节角度上极限约束和组成的向量;为左、右臂的关节角速度向量和组成的向量;为左、右臂的关节角速度下极限约束和组成的向量;为左、右臂的关节角速度上极限约束和组成的向量;γ为左、右臂的末端位置向量和组成的向量;为左、右臂的末端速度向量印组成的向量;符号T表示矩阵的转置;
矩阵矩阵矩阵分别定义为:
根据类人机器人的标准二次规划问题表达式的式(18)-式(21),设定类人机器人的三种运动规划判决方法:
①最小能量判决方法:当矩阵为惯性矩阵,且线性项从属系数b=0时,类人机器人的标准二次规划问题表达式(18)-式(21)构成最小能量判决方法;
②重复运动判决方法:当矩阵被设定为单位矩阵,λ为弹性常系数,且线性项从属系数b=[λ(θ左-θ左(0)) λ(θ右-θ右(0))]T时,类人机器人的标准二次规划问题表达式(18)-式(21)构成重复运动判决方法;
③最小速度范数判决方法:当被设定为单位矩阵,且线性项从属系数 b=0时,类人机器人的标准二次规划问题表达式(18)-式(21)构成重复运动判决方法。
结合三种运动规划判别方法,利用求解器将类人机器人的标准二次规划问题表达式进行解算,可以求解得到二次规划问题式(18)-式(21)的最优解,得到满足不同运动规划判决方法条件下的解,即可完成该类人机器人双臂的运动规划,在本实施例中,采用如下的线性投影方程(求解器)进行解算:
ΦΩ(u-(Γu+q))-u=0 (25)
其中,ΦΩ(·)表示为投影运算符; ι表示维度;表示单位1ι的系数参数;
为解算式(25),定义如下的误差方程:
ε(t)=u-ΦΩ(u-(Γu+q)) (26) 为使误差方程式(26)趋于零,也即求得最优解,利用如下的迭代算法:
设初始对偶决策变量为迭代次数k=0,1,2,...,如果Ω*表示集合u*为解向量,那么得到如下的迭代方程:
其中,||·||2为欧几里得范数;ε(uk)=uk-ΦΩ(uk-(Γuk+q));σ(uk)=(ΓT +I)ε(uk);I为单位矩阵;
对于由式(27)迭代得到的序列{uk},k=0,1,2,...,对于全体u*∈Ω*,且u*满足如下关系:
也即,序列{uk}收敛到解向量u*,其前2n项组成了标准二次规划问题表达式式(18)-式(21)的最优解其中,前n项为类人机器人左臂关节的最优解,后n项为类人机器人右臂关节的最优解;至此,将所解算得到的最优解输出,即完成了对该类人机器人双臂的运动规划。
在本实施例中,为展示本发明的类人机器人双臂运动规划方法的实际应用过程,利用一个仿真实例进行说明,该仿真实例是在类人机器人应用双臂进行物品操控的基础之上进行。其中,类人机器人左、右臂的初始关节角向量分别被设定为:
除此之外,该仿真实例的执行时间被设定为T=18s;双臂角速度的上下限分别被设定为
本仿真实例所得到的14个关节角角度的仿真结果如图3(a)-图3(n) 所示,14个关节角角速度的仿真结果如图4(a)-图4(n)所示,从仿真结果图中可以看出,各个关节的关节角角度和关节角角速度均取得了较优的结果,在运用本发明所述方法进行类人机器人手臂运动学解算时,得到的解算结果保证了以较高的精度完成对类人机器人手臂的运动规划。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。因此,本发明专利的保护范围应以权利要求所述为准。