冗余度机械臂位姿控制方法、系统及机器人技术方案

本发明专利技术公开了一种冗余度机械臂位姿控制方法、系统及机器人，方法包括如下步骤：步骤一：规划冗余机械臂执行器末端位姿；步骤二：提出一种位姿控制的约束优化方案；步骤三：将步骤二的约束优化方案转化为二次规划QP问题；步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；步骤五：驱动机械臂运动到达指定位姿。本发明专利技术针对冗余度机械臂提出一种新的位姿控制的约束优化方案和一种新的ZNN求解器。所设计的ZNN求解器是一种全新的神经网络算法，其可处理冗余度机械臂的位姿控制问题，同时实现关节运动的优化控制。本发明专利技术相比其他方法而言，无需训练和迭代计算，求解精度高，还可通过选择合适的激活函数，保障本方法良好的抗噪性能。法良好的抗噪性能。法良好的抗噪性能。

【技术实现步骤摘要】
冗余度机械臂位姿控制方法、系统及机器人


[0001]本专利技术涉及位姿控制的
，更具体的，涉及一种冗余度机械臂位姿控制方法、系统及机器人。

技术介绍

[0002]冗余度机械臂具有关节自由度数目大于任务空间所需自由度的特性，即：完成某一特定任务时，机械臂具有冗余的自由度。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节构型的问题。冗余度机械臂的位姿控制应用范围广泛，包括医疗领域中手术机器人的遥操作，工业领域中使用机械臂进行抓取、焊接和喷漆，航天领域中通过扩展机器人手臂进行样品的采集等。由于具备冗余的自由度，一方面，冗余度机械臂的逆运动学问题具有无穷多解，给冗余度机械臂的位姿控制带来了挑战；另一方面，冗余的自由度可保证机械臂末端在完成所要求任务的同时，还能根据不同的任务要求和工作环境选择不同的优化指标。因此，冗余度机械臂的位姿控制问题通常可以进一步表征为求解优化问题。
[0003]实际上，机器人的每个关节都拥有自身极限，在任务执行过程中如果超出机器人的关节极限，将可能损坏机器人。因此，一般为了处理冗余机器人的关节约束，可将其逆运动学问题表示成约束优化问题，如二次规划QP问题。求解约束优化问题的求解器有动态神经网络 (Dynamic Neural Network,DNN)求解器、基于梯度的神经网络(Gradient
‑
based NeuralNetwork,GNN)求解器、零化神经网络(Zeroing Neural Network,ZNN)求解器和数值算法等。ZNN求解器广泛应用时变优化问题的求解，具有求解精度高、无需训练和迭代运算等优点。然而，目前尚没有适用于位姿控制的ZNN求解器和优化控制方案。

技术实现思路

[0004]本专利技术为克服上述所述的冗余度机械臂的位姿控制的方法的缺陷，提供一种冗余度机械臂位姿控制方法、系统及机器人，本专利技术可以处理冗余机械臂的位姿控制问题，同时利用冗余的自由度实现关节速度层的优化控制。
[0005]为了达到上述优点，本专利技术采用以下技术方案：
[0006]本专利技术一方面提供了一种冗余度机械臂位姿控制方法，包括下述步骤：
[0007]步骤一：规划冗余度机械臂执行器末端位姿，即冗余度机械臂执行器末端预期到达的位置P
d
以及姿态q
d
；
[0008]步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束以及关节极限θ
‑
≤θ≤θ
+
和关节速度极限的双端约束，由此建立其相应的位姿控制方案；
[0009]步骤三：将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度
的极限处理；最终得到的QP方案中，性能指标为x
T
Wx/2+h
T
x，等式约束为Cx＝d以及不等式约束为Ex≤f；
[0010]步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；首先基于maxQ函数L＝min3{0,Ex
‑
f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；
[0011]步骤五：驱动机械臂关节到达指定位姿；首先，通过对上述步骤四所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。
[0012]作为优选的技术方案，所述冗余度机械臂由末端执行器和7个驱动关节θ1…
θ7组成，所述冗余度机械臂速度层运动学方程为其中v∈R3，ω∈R3分别表示末端执行器的线速度和角速度，表示关节速度，J表示雅可比矩阵。
[0013]作为优选的技术方案，所述冗余度机械臂加速度层运动学方程为其中a∈R3，分别表示末端执行器的线加速度和角加速度，表示关节速度，表示关节加速度，J表示雅可比矩阵，是雅可比矩阵对时间的一阶导数。
[0014]作为优选的技术方案，所述冗余度机械臂末端执行器的姿态误差为其中q
se
和q
ve
分别代表四元数q
e
的实部和虚部，表示用四元数所表征的机械臂末端执行器的期望姿态，表示用四元数所表征的机械臂末端执行器的实际姿态，代表q的共轭四元数，表示四元数乘法。
[0015]作为优选的技术方案，所述位姿控制方案的姿态误差取四元数q
e
的虚部q
ve
，也即姿态误差e＝q
ve
＝Bq，其中
[0016]作为优选的技术方案，所述冗余度机械臂的速度层运动学方程中，设计优化性能指标并考虑各个关节角度及其速度的上下限约束，建立冗余度机械臂的位姿控制方案；
[0017]经过等价整理，数学上表征为具有一般形式的二次规划问题，其中性能指标为 x
T
Wx/2+h
T
x，等式约束为Cx＝d以及不等式约束为Ex≤f；进一步基于maxQ函数和KKT 条件，将二次规划问题等价转化成非线性方程组g(t,x)＝0；所述maxQ函数是处处可微的。
[0018]作为优选的技术方案，通过采用maxQ函数L＝min3{0,Ex
‑
f}并构建误差函数 e(t):＝g(t,y)，并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的 ZNN求解器其中有：
[0019][0020][0021][0022]其中D1＝diag{μ},D2＝diag{max{0,Cx
‑
d}}均为对角矩阵。
[0023]本专利技术另一方面提供了一种冗余度机械臂位姿控制系统，包括位姿规划模块、方案构建模块、转化模块、求解模块和驱动模块；
[0024]所述位姿规划模块，用于规划冗余度机械臂执行器末端的位姿，即冗余度机械臂执行器末端预期到达的位置P
d
以及姿态q
d
；
[0025]所述方案构建模块，用于根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑方案冗余度机械臂的关节速度和执行器末端的线速度 v之间的等式约束以及关节极限θ
‑
≤θ≤θ
+
和关节速度极限的双端约束，从而达到对末端执行器的位姿控制；
[0026]所述转化模块，用于将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为x...

【技术保护点】

【技术特征摘要】
1.一种冗余度机械臂位姿控制方法，其特征在于，包括下述步骤：步骤一：规划冗余度机械臂执行器末端位姿，即冗余度机械臂执行器末端预期到达的位置P
d
以及姿态q
d
；步骤二：根据具体的冗余度机械臂建立其相应的位姿控制方案；通过使用单位四元数定义姿态误差e，并考虑冗余度机械臂的关节速度和执行器末端的线速度v之间的等式约束以及关节极限θ
‑
≤θ≤θ
+
和关节速度极限的双端约束，由此建立其相应的位姿控制方案；步骤三：将上述位姿控制方案转化为QP问题；通过将用于考虑关节位置极限和关节速度极限的双端约束，转化成一个不等式约束，以在速度层实现对关节位置和关节速度的极限处理；最终得到的QP方案中，性能指标为x
T
Wx/2+h
T
x，等式约束为Cx＝d以及不等式约束为Ex≤f；步骤四：基于maxQ函数，设计新的零化神经网络ZNN模型作为QP问题求解器；首先基于maxQ函数L＝min3{0,Ex
‑
f}对不等式约束条件进行处理；然后，通过为QP问题定义相应的拉格朗日函数，进行求导得到KKT条件；最后，构建误差函数e(t):＝g(t,y)并使用演变法则和激活函数Φ(g(t,y))推导出一种全新的ZNN求解器；步骤五：驱动机械臂关节到达指定位姿；首先，通过对上述步骤四所得到的冗余度机械臂各个关节速度将其对时间进行积分求解得冗余度机械臂各个关节角度θ；最后，将冗余度机械臂各个关节速度或关节角度θ发送给下位机，驱动冗余度机械臂运动到达指定位姿。2.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂由末端执行器和7个驱动关节θ1…
θ7组成，所述冗余度机械臂速度层运动学方程为其中v∈R3，ω∈R3分别表示末端执行器的线速度和角速度，表示关节速度，J表示雅可比矩阵。3.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂加速度层运动学方程为其中a∈R3，分别表示末端执行器的线加速度和角加速度，表示关节速度，表示关节加速度，J表示雅可比矩阵，是雅可比矩阵对时间的一阶导数。4.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂末端执行器的姿态误差为其中q
se
和q
ve
分别代表四元数q
e
的实部和虚部，表示用四元数所表征的机械臂末端执行器的期望姿态，表示用四元数所表征的机械臂末端执行器的实际姿态，代表q的共轭四元数，表示四元数乘法。5.根据权利要求4所述冗余度机械臂位姿控制方法，其特征在于，所述位姿控制方案的姿态误差取四元数q
e
的虚部q
ve
，也即姿态误差e＝q
ve
＝Bq，其中
6.根据权利要求1所述冗余度机械臂位姿控制方法，其特征在于，所述冗余度机械臂的速度层运动学方程中，设计优化性能指标并考虑各个关节角度及其速度的上下限约束，建立冗余度机械臂的位姿控制方案；经过等价...

【专利技术属性】
技术研发人员：黎卫兵，伍海媚，潘永平，黄凯，
申请(专利权)人：中山大学，
类型：发明
国别省市：