[发明专利]一种冗余度机械臂力矩层重复运动控制方法

权利要求书

1.一种冗余度机械臂力矩层重复运动控制方法，其特征在于：设计力矩层的重复运动性能指标；基于所需要优化的重复运动性能指标，建立力矩层重复运动控制方案，该方案受约束于力矩的雅可比矩阵等式、关节角度极限、关节速度极限和关节力矩极限；将力矩层重复运动控制方案转化为一个标准的二次规划问题，并通过数值算法求解器对其进行求解；下位机控制器根据求解结果驱动机械臂动作使其完成给定的末端任务；

所述力矩层的重复运动性能指标为：

其中||·||₂表示向量的二范数，α0和β0为调节因子；θ表示机械臂关节角度，θ(0)表示机械臂关节角度的初始状态，表示机械臂关节速度，τ表示机械臂关节力矩，H^-1表示机械臂惯性矩阵H的逆矩阵，c表示离心力变量，g表示重力变量；

力矩层重复运动控制方案设计为：

最小化

受约束于

θ^-≤θ≤θ⁺，

τ^-≤τ≤τ⁺，

其中，等式约束对应于机械臂在力矩层的运动轨迹，κ₁0和κ₂0为误差反馈系数，J_H表示机械臂的惯性雅可比矩阵且J_H＝JH^-1，J表示机械臂的雅可比矩阵，表示J的时间导数；e表示机械臂末端执行器的定位误差且e＝f(θ)-r_d，f表示一个非线性映射函数，r_d表示机械臂末端执行器期望的运动轨迹；ε表示机械臂末端执行器的速度误差且表示r_d的时间导数；另外，表示的时间导数，θ^±、和τ^±分别表示关节角度极限、关节速度极限和关节力矩极限；

将双端约束θ^-≤θ≤θ⁺和转化为如下基于关节力矩τ的双端约束：

η^-+H^-1(c+g)≤H^-1τ≤η⁺+H^-1(c+g)，

其中η^±表示联立关节角度θ和关节速度后的上下极限，并且，k_p0和k_v0为关节极限转换参数，为关节极限转换裕量。

2.如权利要求1所述的一种冗余度机械臂力矩层重复运动控制方法，其特征在于：将力矩层重复运动控制方案转化为一个标准的二次规划问题，

最小化x^TQx/2+p^Tx，约束条件为Cx＝d，Ax≤b，x^-≤x≤x⁺，

其中，x＝τ，Q＝(H^-1)²，C＝J_H，A＝[-(H^-1)^T(H^-1)^T]^T，b＝[-(η^-+H^-1(c+g))^T(η⁺+H^-1(c+g))^T]^T，x^±表示x的上下限，上标^T表示矩阵或向量的转置。

3.如权利要求1所述的一种冗余度机械臂力矩层重复运动控制方法，其特征在于，所述通过数值算法求解器对二次规划问题进行求解，具体为：将所述二次规划问题进一步变换为分段线性投影方程，从而构造相应的数值算法求解器进行求解。