[发明专利]一种冗余度机械臂末端执行器姿态保持的解决方法

摘要

本发明公开了一种冗余度机械臂末端执行器姿态保持的解决方法，包括如下步骤：S1、给机械臂建立D‑H模型获得正运动学模型，并对其求导，转化为速度层，并建立目标轨迹等式约束指标；S2、测量机械臂任务平面的法向量在机械臂基坐标系中的数值；S3、通过建立的D‑H模型以及法向量建立两种末端执行器姿态保持等式约束指标；S4、将建立的目标轨迹等式约束指标以及两种末端执行器姿态保持等式约束指标，写为统一形式的二次规划问题；S5、将二次规划问题转化为线性变分不等式；S6、使用原对偶神经网络求解器求解线性变分不等式；S7、将原对偶神经网络求解出来的机械臂关节角控制量输出到机械臂以实现控制冗余度机械臂。

主权项

1.一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的解决方法包括下列步骤：S1、给机械臂建立D‑H模型，并对其求导，转化为速度层，并建立目标轨迹等式约束指标，所述的目标轨迹等式约束指标为：其中J_E为末端执行器的雅克比矩阵，为机械臂关节角的角速度，为速度层的目标轨迹；S2、测量机械臂任务平面的法向量在机械臂基坐标系中的数值；S3、将步骤S1中建立的D‑H模型以及步骤S2中的法向量的数值建立两种末端执行器姿态保持等式约束指标，选择其中一种约束指标使用；所述的步骤S3中的两种末端执行器姿态保持等式约束指标包括：三元素姿态保持指标和两元素姿态保持指标，其中，所述的三元素姿态保持指标的建立过程具体如下：为保持末端执行器始终垂直于任务平面，应保证在任何时间t中，任务平面的法向量在末端执行器坐标系中的数值保持不变，即：式中，向量和向量代表在t和0时刻的向量而向量是在基坐标系中的任务平面的法向量在末端执行器坐标系中的投影，其中，可由以下公式得到：式中，为从末端执行器坐标系转化到机械臂基坐标系的旋转变化矩阵，为了便于写成规范的二次规划形式，两端对时间求导，得：其中也就是如下等式：其中J_ω表示向量的雅克比矩阵；其中，所述的两元素姿态保持指标的建立过程具体如下：在三元素姿态保持指标中，由旋转矩阵的性质因此当固定中的任意两个元素，中的另一个元素随之固定，所以，只固定中的任意两个元素即可达到姿态保持效果，两元素姿态保持指标通过定义为中的任意两个元素，可以得到如下简化形式：其中，J_υ为向量的雅克比矩阵，表示向量[0 0]^T；S4、将步骤S1建立的目标轨迹等式约束指标，以及步骤S3建立的两种末端执行器姿态保持等式约束指标，写为统一形式的二次规划问题；S5、将步骤S4中的二次规划问题转化为线性变分不等式；S6、使用原对偶神经网络求解器求解步骤S5中的线性变分不等式；S7、将步骤S6中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂。