[发明专利]一种冗余度机械臂位姿控制的单94LVI迭代算法

权利要求书

1.一种冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，包括如下步骤：

S1：设计二次型形式的姿态控制指标，将末端执行器任务描述成等式约束，将机械臂物理极限描述成不等式约束，生成二次型位姿控制解析方案；

S2：将二次型位姿控制解析方案转化为标准的二次规划；

S3：运用单94LVI迭代算法求解；

S4：将计算结果传给下位机控制器驱动机械臂运动；

所述步骤S1中的二次型位姿控制解析方案为：

对于自由度为n，工作空间为m的冗余度机械臂，设计其性能指标为最小化受约束于θ^-≤θ≤θ⁺和其中A＝J₁(θ)，θ表示机械臂的关节角度向量，表示机械臂的关节角速度向量，J₁(θ)表示机械臂末端执行器位置雅克比矩阵，J₂(θ)表示机械臂末端执行器姿态雅克比矩阵，o_a表示末端执行器实际姿态向量，o_d表示末端执行器期望姿态向量，表示末端执行器期望姿态速度向量，r_a表示末端执行器实际位置向量，r_d表示末端执行器期望位置向量，表示末端执行器期望位置速度向量，θ^±表示关节角度上下限，表示关节角速度上下限，上标^T表示矩阵或向量的转置，λ和γ分别是用来控制末端执行器姿态和位置收敛率的设计参数。

2.根据权利要求1所述的冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，所述步骤S2中的的二次型位姿控制解析方案转化为标准的二次规划，即性能指标为最小化x^TQx/2+p^Tx，受约束于Ax＝b和x^-≤x≤x⁺，其中正的常数η用来调节关节角速度的可行域。

3.根据权利要求2所述的冗余度机械臂位姿控制的单94LVI迭代算法，其特征在于，所述步骤S3的标准二次规划用如下单94LVI迭代算法求解：

e(z_k)＝z_k-P_Ω(z_k-(M_kz_k+c_k))，

z_k+1＝z_k-ρ(z_k)d(z_k)，

θ_k+1＝θ_k+τx_k+1

其中，(·)_k代表在t_k＝kτ时刻矩阵或者向量的值，τ为采样间隔，z＝[x；y]，y∈R^m为对应于等式约束Ax＝b的对偶决策向量，M＝[Q,-A^T；A,0]，P_Ω(z)的第i个元素定义为：

(·)_i代表向量的第i个元素，1_v是长度为m的全1向量，正的常数应该被设置得足够大来代表+∞。