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

权利要求书

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

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

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

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

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

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

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

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

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

根据类人机器人双臂的前向运动学问题，给出左、右臂关节角向量θ_L和θ_R，则左、右臂末端位置向量r_L和r_R通过如下的前向运动学方程得到：

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

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

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

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

对于左臂：

对于右臂：

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