[发明专利]一种冗余度机械臂容错型运动规划方法

权利要求书

1.一种冗余度机械臂容错型运动规划方法，其特征在于包括如下步骤：

1）确定发生故障出现锁死的关节，建立基于二次规划的容错型冗余度解析方案，所述解析方案设计的最小性能指标为速度向量、重复运动或动能，受约束于速度的雅可比矩阵等式、关节速度极限和关节角极限，该关节速度极限是随关节角度变化的；

2）将步骤1）的基于二次规划的容错型冗余度解析方案运用数值方法进行求解；

3）将步骤2）的求解结果传递给下位机控制器，驱动机械臂完成给定的末端任务。

2.根据权利要求1所述的冗余度机械臂容错型运动规划方法，其特征在于：所述步骤1）中基于二次规划的容错型冗余度解析方案设计为：最小化                                                ，受约束于、、、，其中为欲优化的运动性能指标，表示关节速度向量，和为合适维数的矩阵和向量，上标表示矩阵和向量的转置，等式约束对应机械臂末端运动轨迹，表示机械臂的雅可比矩阵，表示关节角向量，表示机械臂末端执行器速度向量，、分别表示关节角范围和关节速度范围，表示关节角上下限，表示关节速度上下限，表示机械臂关节的状态故障矩阵，用于在解析方案中确定故障关节。

3.根据权利要求2所述的冗余度机械臂容错型运动规划方法，其特征在于：所述状态故障矩阵，根据发生故障出现锁死的关节编号，调整状态故障矩阵中的相应元素为1，而中该行其余元素均为0。

4.根据权利要求2所述的冗余度机械臂容错型运动规划方法，其特征在于：所述步骤1）的基于二次规划的容错型冗余度解析方案中，所述容错型运动规划方法转化为一个标准二次规划：最小化，受约束于，，其中，，，，，正的常数用来调节和保证关节速度的足够大的可行域。

5.根据权利要求4所述的冗余度机械臂容错型运动规划方法，其特征在于：所述标准二次规划等价于一个线性投影方程，其中为空间到集合的分段线性投影算子，为笛卡尔空间的维数，为关节空间的维数，表示原对偶变量，表示原对偶变量下极限，表示原对偶变量上极限，原对偶变量及其上下限定义如下：

，，；

其中是对应于等式约束的对偶决策向量，是元素都为1的相应维数向量；是足够大的常数，用于数值上替代无穷大，而增广矩阵定义如下：

；

接着，线性投影方程用数值方法求解。

6.根据权利要求5所述的线性投影方程，其特征在于：设计其计算误差为：当误差为零时，其对应的值便为分段线性方程的解， 其前个元素组成二次规划的解，给定初始值，通过如下的迭代来得到使得误差达到预设的精度：，其中，和分别定义为，，通过算法不断迭代，得到分段线性投影方程的解，从而得到冗余度机械臂容错型运动规划的最优解。