[发明专利]冗余度机械臂位姿控制的方法、系统及机器人

权利要求书

1.一种冗余度机械臂位姿控制方法，其特征在于，包括下述步骤：

步骤一：规划冗余度机械臂执行器末端位姿，即冗余度机械臂执行器末端预期到达的位置P_d以及姿态q_d；

步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束其中J_p为冗余度机械臂的位置雅可比矩阵，以及关节极限θ^-≤θ≤θ⁺和关节速度极限的双端约束，由此建立其相应的位姿控制方案；

步骤三：将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为x^TWx/2+h^Tx，等式约束为Cx＝d以及不等式约束为Ex≤f；

步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；首先基于maxQ函数L＝min³{0,Ex-f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器，其中α为调节ZNN求解器收敛速度的正实数；

步骤五：驱动机械臂关节到达指定位姿；首先，通过对上述步骤四所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。

2.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂由末端执行器和7个驱动关节θ₁…θ₇组成，所述冗余度机械臂速度层运动学方程为其中v∈R³，ω∈R³分别表示末端执行器的线速度和角速度，表示关节速度，J表示雅可比矩阵。

3.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂加速度层运动学方程为其中a∈R³，分别表示末端执行器的线加速度和角加速度，表示关节速度，表示关节加速度，J表示雅可比矩阵，是雅可比矩阵对时间的一阶导数。

4.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂末端执行器的姿态误差为其中q_se和q_ve分别代表四元数q_e的实部和虚部，表示用四元数所表征的机械臂末端执行器的期望姿态，表示用四元数所表征的机械臂末端执行器的实际姿态，代表q的共轭四元数，表示四元数乘法。

5.根据权利要求4所述冗余度机械臂位姿控制方法，其特征在于，所述位姿控制方案的姿态误差取四元数q_e的虚部q_ve，也即姿态误差e＝q_ve＝Bq，其中

6.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂的速度层运动学方程中，设计优化性能指标并考虑各个关节角度及其速度的上下限约束，建立冗余度机械臂的位姿控制方案；

经过等价整理，数学上表征为具有一般形式的二次规划问题，其中性能指标为x^TWx/2+h^Tx，等式约束为Cx＝d以及不等式约束为Ex≤f；进一步基于maxQ函数和KKT条件，将二次规划问题等价转化成非线性方程组g(t,x)＝0；所述maxQ函数是处处可微的。