[发明专利]基于新型数值求解器的冗余度机械臂重复运动规划方法

权利要求书

1.基于新型数值求解器的冗余度机械臂重复运动规划方法,其特征在于包括如下步骤：

1)以重复性能指标为最小性能指标，将冗余度机械臂重复运动要求设计为二次规划问题；

2)将步骤1)的二次规划问题转化为分段线性投影方程；

3)将步骤2)的分段线性投影方程，用新型数值求解器求解；

4)将步骤3)的求解结果传递给下位机控制机械臂运动。

2.根据权利要求1所述的基于新型数值求解器的冗余度机械臂重复运动规划方法，其特征在于步骤1)所述冗余度机械臂重复运动要求设计为二次规划问题，具体是：在速度层上设计重复运动性能指标，其中θ为冗余度机械臂的关节角度向量，表示关节角速度，上标T表示向量的转置，c＝λ(θ(t)-θ(0))，θ(t)和θ(0)分别为机械手臂运动过程中的关节角度和初始状态的关节角度，λ＞0为机械臂对关节位移θ(t)-θ(0)的响应系数；同时在重复运动中要考虑机械臂末端运动轨迹雅克比等式约束以及机械臂的关节物理角度极限和角速度极限：θ^-≤θ≤θ⁺，其中J(θ)表示机械臂的雅克比矩阵，表示机械臂末端轨迹方程对时间的导数，θ⁺和θ^-分别表示对关节角度的上下约束，和分别对关节角速度的上下约束；将角度极限和角速度极限统一为速度层上的双端约束为其中和分别表示对速度向量的上下约束向量，对这两个约束向量中的第i个分量即机械臂的第i个关节有系数v＞0是用来调节关节角速度的可行域；下标i表示冗余的机械臂的第i个关节对应的前述已经定义的相应参数，系数v＞0是用来调节关节角速度的可行域。

3.根据权利要求1所述的基于新型数值求解器的冗余度机械臂重复运动规划方法，其特征在于步骤2)所述分段线性投影方程为：P_Ω(u-(Mu+q))-u＝0，其中P_Ω(·)是从R^n+m到集合的映射，u为待求解的原对偶平衡矢量，M∈R^(n+m)×(n+m)和q∈R^n+m为扩展矩阵，m是末端执行器的笛卡尔空间维数；n是机器人手臂的关节数；u⁺和u^-分别表示u的上下取值范围，在数学形式上定义为：

其中u的前n项x即为所求的机械臂关节的角速度，后m项y为使u的维数为n+m的量；l＝[1,…,1]^T∈R^m是一个全是1的矩阵，w＞＞0在数学上代替+∞；m是末端执行器的笛卡尔空间维数；n是机器人手臂的关节数；M和q为扩展系数矩阵，定义为：

M=W-JTJ0∈R(n+m)×(n+m),q=c-b∈Rn+m,]]>

其中J为机械臂的雅克比矩阵，J^T表示其转置，W＝I为单位矩阵，表示机械臂末端轨迹方程对时间的导数。

4.根据权利要求3所述的基于新型数值求解器的冗余度机械臂重复运动规划方法，其特征在于步骤3)所述新型数值求解器为：u^k+1＝u^k-ρ(u^k)Qe(u^k)求解，其中误差函数定义为e(u^k)＝u^k-P_Ω(u^k-(Mu^k+q))，

Q=I,ρ(uk)=||e(uk)||22e(uk)T(I+M)e(uk);]]>

上标k表示迭代方程的迭代次数，I表示单位矩阵，表示向量二范数的平方。

首先给定冗余度机械臂的初始位置、预设精度以及最大迭代次数，当e(u^k)小于预设精度或者k达到最大迭代次数时，计算结束，得到分段线性投影方程的解，以及冗余度机械臂重复运动速度层二次规划的最优解。