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

权利要求书

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中原对偶神经网络求解器求解出来的机械臂关节角控制变量输出到机械臂以实现控制冗余度机械臂。

2.根据权利要求1所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S4中写为统一形式的二次规划问题具体为：

取最小二范数的二次规划指标，即：

在这里，代表欧拉二范数，即

其中，针对所述的三元素姿态保持指标，将其写为标准的QP形式，即：

其中，针对所述的两元素姿态保持指标，将其写为标准的QP形式，即：

3.根据权利要求2所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S5中将二次规划问题转化为线性变分不等式具体为：

为了解决二次规划问题，设定原对偶矢量u^*，满足以下条件：

其中是原对偶决定变量矢量，其中，矢量g代表等式的原对偶决定变量矢量中等式限制的部分，Ω为原对偶决定变量矢量的取值范围，其为一凸集，其中的各符号定义如下：

Ω＝{u|u^-≤u≤u⁺}

u⁺＝+∞,and u^-＝-∞

W＝E,and C＝0

上式中E为单位矩阵，其它变量与前文的定义一致；

线性变分不等式可以转化为以下分段线性投影方程：

P_Ω(u-(Mu+q))-u＝0

上式中函数P_Ω为一分段线性投影算子，将自变量投影到Ω，

其中，

另外矩阵M，矢量u与前文定义一致。

4.根据权利要求3所述的一种冗余度机械臂末端执行器姿态保持的解决方法，其特征在于，所述的步骤S6中使用原对偶神经网络求解器求解线性变分不等式具体为：

采用基于线性变分不等式的原对偶神经网络：

其中，参数β被设置为正数，并且其值要设置得尽可能的大，这样将增加基于线性变分不等式的原对偶神经网络的收敛率，按照该公式，原对偶神经网络将实现全局收敛，其原对偶决定变量矢量的导数将收敛到期望的值，将原对偶决定变量矢量的导数做积分处理后即可得到冗余度机械臂关节控制向量。