CN108638057A - 一种类人机器人双臂运动规划方法 - Google Patents

Abstract

本发明公开了一种类人机器人双臂运动规划方法，包括下述步骤：依据类人机器人左右双臂的运动学模型，分别列写该类人机器人左臂和右臂的前向运动学方程，在此基础上，分别列写类人机器人左臂和右臂的逆运动学方程；依据逆运动学方程，分别列写类人机器人左臂和右臂的二次规划问题表达式；然后将左臂和右臂的二次规划指标和约束合并，得到类人机器人的标准二次规划问题表达式；依据标准二次规划问题表达式，设定该类人机器人的三种运动规划判决方法；结合所述的三种运动规划判决方法，将所述的标准二次规划问题表达式进行解算，输出最优解。本发明方法能够很好地完成对类人机器人左、右手臂的控制，具有思路清晰、逻辑性强、简明有效等特点。

Description

一种类人机器人双臂运动规划方法

技术领域

本发明涉及机器人运动学规划领域，特别涉及一种类人机器人双臂运动规划方法。

背景技术

类人机器人可以帮助人们更好地适应点对点的协作环境，因此在各种应用中越来越受到欢迎。类人机器人的双臂不仅可以用于完成各种各样的操作任务，而且作为完成肢体语言的重要组成部分，可以使得类人机器人更加真实。与此同时，与传统机器人的单臂***相比，类人机器人的双臂***可以通过同时执行任务而大大提高效率。除此之外，类人机器人还可以根据实际需要，选择使用单臂或双臂完成各种任务。例如，它们可以通过共享负载来协作处理笨重的对象。早期的多臂***可以追溯到戈茨在1940年代用于放射性货物装卸的遥控机械手***。从1950年底到1970年初，由于深海和深空探测的发展，对双臂遥操作机器人的研究逐渐深入。

近年来，由于类人机器人的迅速发展，双臂应用再次吸引了研究人员和工程师的兴趣。类人机器人要求其双臂能够在家庭环境中自主或半自主地进行各种日常工作任务，与此同时，它们还要求能够通过展示情感肢体语言来提高机器人的社交能力。为了更好地使类人机器人的双臂完成各种复杂的日常任务，对于双臂运动规划问题的研究便具有了实际意义。逆运动学问题是双臂运动规划的基本问题之一，即给定双臂的末端执行器轨迹，如何在每个时刻计算关节变量的问题。大多数人形机器人的双臂在实际应用中具有三个以上的自由度，被称作冗余度机械臂。这种冗余度提高了机器人实现末端执行器任务时的灵活性和通用性，但与此同时，也不可避免的提高了计算的难度。在这样一种情况下，因为有很大数量的机械关节需要用于完成特定的末端执行器任务，所以求解得到用于类人机器人双臂运动规划的解析解是非常重要和具有挑战性的。传统的用于求解冗余度机械臂的方法是基于伪逆的方法，其中，一种基于伪逆法的闭环逆运动学方法被提出，并被用于移动平台上双臂的运动规划上。但是在应用这样一种基于伪逆法的运动规划方法时，必须考虑并计算矩阵的逆，而且很难考虑其中的不等式问题，大大增加了解析和计算的复杂性。因此，建立在上述伪逆方法的基础上，一种基于二次规划问题的方法于近期被提出，并得到了深入的研究。但现有的二次规划优化方法多集中在对单个机械臂的运动规划上，只有少数方法考虑了对类人机器人双臂运动规划的研究。面对这样一种研究现状，一种用于类人机器人双臂运动规划的方法在本发明专利中被提出，并应用于类人机器人的仿真及实物实验上。本发明为基于机器人学的动态规划方法，依据该方法可以很好地完成对类人机器人左、右手臂的控制，具有思路清晰、逻辑性强、简明有效等特点。

发明内容

本发明的目的在于克服现有技术中的缺点与不足，提供一种类人机器人双臂运动规划方法，将类人机器人的双臂运动转化为二次规划问题形式，求解出类人机器人双臂关节运动的最优解，能够有效实现对类人机器人左、右手臂的精确控制。

为实现以上目的，本发明采取如下技术方案：

一种类人机器人双臂运动规划方法，其特征在于，包括下述步骤：

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

2)依据步骤1)得出的前向运动学方程，并根据伪逆方法，分别列写类人机器人左臂和右臂的逆运动学方程；

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

4)依据步骤3)得出的左臂和右臂的二次规划问题表达式，分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并，得到类人机器人的标准二次规划问题表达式；

5)依据步骤4)得出的类人机器人的标准二次规划问题表达式，设定该类人机器人的三种运动规划判决方法；

6)结合步骤5)得出的三种运动规划判决方法，利用求解器将类人机器人的标准二次规划问题表达式进行解算，得出满足于不同运动规划判决方法条件下的解，即可完成对该类人机器人双臂的运动规划。

本发明相对于现有技术具有如下的优点和效果：

(1)本发明为基于机器人学的动态规划方法，依据该方法可以很好地完成对类人机器人左、右手臂的控制，具有思路清晰、逻辑性强、简明有效等特点。

(2)本发明方法能够很好地完成对类人机器人左、右手臂的控制，与传统方法只能实现离线控制不同，在运用本发明所述方法进行类人机器人手臂运动学解算时，可实现实时在线控制，具有思路清晰、逻辑性强、简明有效等特点。

附图说明

图1为本实施例的类人机器人双臂运动规划方法的流程图；

图2为本实施例的类人机器人的左、右臂关节框架示意图；附图标记：1～7为类人机器人的右臂关节；8～14为类人机器人的左臂关节；15为类人机器人的头部，16为类人机器人的腰部；

图3为本实施例的类人机器人应用双臂执行任务时所得到的各个关节角角度的仿真结果；其中，图3(a)-图3(n)分别展示了该类人机器人执行任务时左、右臂14个关节的关节角度结果；

图4为本发明实例的类人机器人应用双臂执行任务时所得到的各个关节角角速度的仿真结果；其中，图4(a)-图4(n)分别展示了该类人机器人执行任务时左、右臂14个关节的关节角速度结果。

具体实施方式

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

实施例

如图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^*为解向量，那么得到如下的迭代方程：

其中，||·||₂为欧几里得范数；ε(u^k)＝u^k-Φ_Ω(u^k-(Γu^k+q))；σ(u^k)＝(Γ^T +I)ε(u^k)；I为单位矩阵；

对于由式(27)迭代得到的序列{u^k}，k＝0，1，2，...，对于全体u^*∈Ω^*，且u^*满足如下关系：

也即，序列{u^k}收敛到解向量u^*，其前2n项组成了标准二次规划问题表达式式(18)-式(21)的最优解其中，前n项为类人机器人左臂关节的最优解，后n项为类人机器人右臂关节的最优解；至此，将所解算得到的最优解输出，即完成了对该类人机器人双臂的运动规划。

在本实施例中，为展示本发明的类人机器人双臂运动规划方法的实际应用过程，利用一个仿真实例进行说明，该仿真实例是在类人机器人应用双臂进行物品操控的基础之上进行。其中，类人机器人左、右臂的初始关节角向量分别被设定为：

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

本仿真实例所得到的14个关节角角度的仿真结果如图3(a)-图3(n) 所示，14个关节角角速度的仿真结果如图4(a)-图4(n)所示，从仿真结果图中可以看出，各个关节的关节角角度和关节角角速度均取得了较优的结果，在运用本发明所述方法进行类人机器人手臂运动学解算时，得到的解算结果保证了以较高的精度完成对类人机器人手臂的运动规划。

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

Claims (7)

1.一种类人机器人双臂运动规划方法，其特征在于，包括下述步骤：

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

2)依据步骤1)得出的前向运动学方程，并根据伪逆方法，分别列写类人机器人左臂和右臂的逆运动学方程；

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

4)依据步骤3)得出的左臂和右臂的二次规划问题表达式，分别将左臂二次规划指标、约束条件与右臂的二次规划指标、约束条件合并，得到类人机器人的标准二次规划问题表达式；

5)依据步骤4)得出的类人机器人的标准二次规划问题表达式，设定该类人机器人的三种运动规划判决方法；

6)结合步骤5)得出的三种运动规划判决方法，利用求解器将类人机器人的标准二次规划问题表达式进行解算，得出满足于不同运动规划判决方法条件下的解，即可完成对该类人机器人双臂的运动规划。

2.根据权利要求1所述的类人机器人双臂运动规划方法，其特征在于，步骤1)中，类人机器人双臂的运动学模型为：

其中，r为机器人手臂末端位置向量，θ为关节角向量；为光滑的非线性方程；

列写类人机器人左臂和右臂的前向运动学方程，具体如下：

其中，r_左、r_右分别表示类人机器人左臂和右臂的末端位置向量；θ_左、θ_右分别表示类人机器人左臂和右臂的关节角向量，和均为光滑的非线性方程。

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

其中，为左臂的关节角速度；为右臂的关节角速度；为左臂末端执行器的速度；为右臂末端执行器的速度；为左臂的雅克比矩阵的伪逆， 为右臂的雅克比矩阵的伪逆， 为单位矩阵；为左臂的优化指标向量；为右臂的优化指标向量。

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)被合并为：

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

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

其中， 为左、右臂的关节角向量和组成的向量；b为左、右臂的约束指标线性项的从属系数和组成的向量； 为左、右臂的关节角度下极限约束和组成的向量； 为左、右臂的关节角度上极限约束和组成的向量； 为左、右臂的关节角速度向量和组成的向量； 为左、右臂的关节角速度下极限约束和组成的向量； 为左、右臂的关节角速度上极限约束和组成的向量；Υ为左、右臂的末端位置向量和组成的向量； 为左、右臂的末端速度向量和组成的向量；符号T表示矩阵的转置；

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

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

①最小能量判决方法：当矩阵 为惯性矩阵，且线性项从属系数b＝0时，类人机器人的标准二次规划问题表达式(18)-式(21)构成最小能量判决方法；

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

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

7.根据权利要求6所述的类人机器人双臂运动规划方法，其特征在于，步骤6)中，结合三种运动规划判别方法，利用求解器将类人机器人的标准二次规划问题表达式进行解算，得出满足三种运动规划判别方法条件下的解；

所述求解器设计为如下的线性投影方程：

Φ_Ω(u-(Γu+q))-u＝0 (25)

其中，Φ_Ω(·)表示为投影运算符； ι表示维度；表示单位1_ι的系数参数；

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

ε(t)＝u-Φ_Ω(u-(Γu+q)) (26)

为使误差方程式(26)趋于零，也即求得最优解，利用如下的迭代算法：

设初始对偶决策变量为迭代次数k＝0,1,2,…，如果Ω^*表示集合u^*表示解向量，那么得到如下的迭代方程：

其中，||·||₂为欧几里得范数；ε(u^k)＝u^k-Φ_Ω(u^k-(Γu^k+q))；σ(u^k)＝(Γ^T+I)ε(u^k)；I为单位矩阵；

对于由式(27)迭代得到的序列{u^k}，k＝0,1,2,…，对于全体u^*∈Ω^*，且u^*满足如下关系：

也即，序列{u^k}收敛到解向量u^*，其前2n项组成了标准二次规划问题表达式式(18)-式(21)的最优解其中，前n项为类人机器人左臂关节的最优解，后n项为类人机器人右臂关节的最优解；至此，将所解算得到的最优解输出，即完成了对该类人机器人双臂的运动规划。