[发明专利]一种类人机器人双臂运动规划方法

权利要求书

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

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

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

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

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

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

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

其中，步骤1)中，所述类人机器人双臂的运动学模型为：

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

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

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

其中，步骤3)所述类人机器人左臂和右臂的二次规划问题表达式，具体如下：

对于左臂：

对于右臂：

其中，为左臂的二次项系数矩阵；为右臂的二次项系数矩阵；和为左、右臂约束指标线性项的从属系数；式(7)和式(11)分别表示左、右臂末端执行器的速度和角速度之间的线性关系；和为大小为m×m且对称正定的反馈矩阵；表示左臂的位置误差反馈；表示右臂的位置误差反馈；式(8)和式(12)分别表示左、右臂的关节角度极限约束；式(9)和式(13)分别表示左、右臂的关节角速度极限约束。

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

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

3.根据权利要求1所述的类人机器人双臂运动规划方法，其特征在于，步骤4)中，分别将左臂二次规划指标式(6)、约束条件式(7)-式(9)与右臂的二次规划指标式(10)、约束条件合并式(11)-式(13)合并；

其中，左、右臂的二次规划指标式(6)和式(10)被合并为：

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

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

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

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

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

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