本发明涉及机器人运动学控制方法技术领域，具体涉及一种六自由度手腕偏置型串联机械臂的运动控制方法。
背景技术：
机器人逆运动学求解是进行机器人控制的前提和基础。末端的三个关节不相交于一点或三轴不平行的机器人通常称为手腕偏置型机械臂，该类型机械臂具有结构强度高、运动灵活的特点，但是很难通过常规的解析法计算该类型机械臂的逆运动学解，从而数值类方法被广泛使用来解决上述问题。数值法本身存在诸多局限性，例如对于高维非线性问题收敛速度慢甚至有时无法收敛、初始值选择不当会严重降低收敛速度和计算精度、无法同时获得多重解以至于在轨迹规划中有些约束条件很难添加、必须避开奇异位姿否则无法收敛等等。这些局限性直接导致该类型机器人控制过程繁琐、计算量大、控制精度不高、很难实现在线实时控制等问题。
技术实现要素：
针对现有技术存在的问题，本发明提供一种六自由度手腕偏置型串联机械臂的运动控制方法，根据数值法和解析混合的分层次运动学逆解算法来获得机械臂的运动学逆解，从运动学上对机械臂进行控制，具有计算简单、控制精度高、实施成本低等特点。
一种六自由度手腕偏置型串联机械臂的运动控制方法，包括以下步骤：
步骤1：根据d-h参数法建立六自由度手腕偏置型串联机械臂关节i的连杆坐标系{i}，其中，i＝1，2，3，4，5，6；
步骤2：根据机械臂的d-h参数采用齐次坐标变换矩阵建立机械臂的正运动学方程；
步骤3：采用启发式迭代方法获取精确的机械臂关节1的角度初始值，具体步骤如下：
步骤3.1：定义机械臂末端执行器的给定位姿为t、末端执行器为ee、腕关节为w、肘关节为e、肩关节为s、末端执行器ee与腕关节w之间的杆长距离为dt、肘关节e与肩关节s之间的杆长距离为d3、腕关节w与肘关节e之间的杆长距离为d5，其中，w包括关节5和关节6，e包括关节3和关节4，s包括关节1和关节2；
步骤3.2：取符合机械臂关节1限位的最大值；
步骤3.3：令机械臂末端执行器ee与末端执行器的给定位姿t重合，确定机械臂末端执行器ee的位姿ee’；
所述位姿为关节所在位置和姿态；
步骤3.4：令腕关节w沿机械臂末端执行器的位姿ee’的固定方向移动机械臂末端执行器ee与腕关节w之间的杆长距离dt，确定机械臂腕关节w的位姿w’；
步骤3.5：过机械臂肘关节e的中心在末端执行器的位姿ee’与腕关节w的位姿w’所在直线上作垂线，并根据腕关节w的关节限位在此垂线所在的平面内确定肘关节e的可行域；
步骤3.6：将机械臂肘关节e移动到可行域内得到肘关节e在可行域内的位姿ec；
步骤3.7：令肘关节e沿腕关节w的位姿w’与肘关节e在可行域内的位姿ec所在直线方向移动腕关节w与肘关节e之间的杆长距离d5，确定肘关节e的位姿e’；
步骤3.8：过机械臂肩关节s的中心在腕关节w的位姿w’与肘关节e的位姿e’所在直线上作垂线，并根据肘关节e的关节限位在此垂线所在的平面内确定肩关节s的可行域；
步骤3.9：将机械肩关节s移动到可行域内得到肩关节s在可行域内的位姿sc；
步骤3.10：令肩关节s沿肘关节e的位姿e’与肩关节s在可行域内的位姿sc所在直线方向移动肘关节e与肩关节s之间的杆长距离d3，确定肩关节s的位姿s’；
步骤3.9：令肩关节s的位姿s’与原肩关节s的位姿重合，保持其余关节角度不变，利用启发式迭代方法分别得到机械臂各关节的逆运动学解的位姿；
步骤3.10：取符合机械臂关节1限位的最小值，重复步骤3.3-步骤3.9，得到机械臂各关节的另一组逆运动学解的位姿；
步骤3.11：定义垂直于关节1轴线的平面为φ，平面φ内竖直向下的向量为v0，上臂的方向向量为v1，v1在平面φ内的投影向量为v2；
步骤3.12：分别根据两组机械臂各关节的逆解位姿下平面φ内竖直向下的向量v0和v1在平面φ内的投影向量v2，计算机械臂关节1的两个角度初始值θ1和θ1’；
步骤4：根据机械臂关节1的两个角度初始值θ1和θ1’分别计算关节2到关节6的逆运动学解的角度值；
步骤5：根据机械臂的正运动学方程计算机械臂末端执行器ee的位姿ee’与末端执行器的给定位姿t的误差r；
步骤6：判断误差r是否小于误差阈值，若是，则继续步骤7，若否，则将步骤3中得到的关节1的角度初始值增加或减去一个微小的α角度，返回步骤4；
步骤7：在步骤6得到的多重解中筛选出符合关节限位和关节位移最小原则的一组逆运动学解的角度值；所述关节位移最小为机械臂各个关节的位移和最小；
步骤8：将步骤7得到的逆运动学解的角度值输入到已知机器人的运动控制模块，完成对六自由度手腕偏置型串联机械臂的运动控制。
进一步地，所述步骤2及步骤5中机械臂的正运动学方程如下：
其中，
nz＝-cθ6[cθ5(cθ2sθ4-cθ3cθ4sθ2)+sθ2sθ3sθ5]-sθ6(cθ2cθ4+cθ3sθ2sθ4)；
oz＝sθ6[cθ5(cθ2sθ4-cθ3cθ4sθ2)+sθ2sθ3sθ5]-cθ6(cθ2cθ4+cθ3sθ2sθ4)；
ax＝sθ5[cθ4(sθ1sθ3-cθ1cθ2cθ3)-cθ1sθ2sθ4]-cθ5(cθ3sθ1+cθ1cθ2sθ3)；
ay＝-sθ5[cθ4(cθ1sθ3+sθ1cθ2cθ3)+sθ1sθ2sθ4]+cθ5(cθ3cθ1-sθ1cθ2sθ3)；
az＝sθ5(cθ2sθ4-cθ3cθ4sθ2)-sθ2sθ3cθ5；
px＝d5[sθ4(sθ1sθ3-cθ1cθ2cθ3)+cθ1cθ4sθ2]+d3cθ1sθ2；
py＝-d5[sθ4(cθ1sθ3+sθ1cθ2cθ3)-sθ1cθ4sθ2]+d3sθ1sθ2；
pz＝-d5(cθ2cθ4+cθ3sθ4sθ2)-d3cθ2；
其中，θi为机械臂关节i的角度初始值，cθi为cosθi，sθi为sinθi。
进一步地，所述步骤4中计算关节2到关节6的逆运动学解的角度值的公式如下：
其中：
φ＝atan2(pz，pxcθ1+pysθ1)；
θ3＝atan2(pxsθ1-pycθ1，-pzsθ2-pxcθ1cθ2-pycθ2sθ1)；
其中：
χ＝(pxcθ1sθ2+pysθ1sθ2-pzcθ2-d3)/d5；
θ5＝atan2(-ζ，η)；
其中：
ζ＝ax(cθ1sθ2sθ4-cθ4sθ1sθ3+cθ1cθ2cθ3cθ4)-az(cθ2sθ1-cθ3cθ4sθ2)；
η＝ay(cθ1cθ3-cθ2sθ1sθ3)-ax(cθ3sθ1+cθ1cθ2sθ3)-azsθ2sθ3；
θ6＝atan2(λ，τ)；
其中：
其中，θi为机械臂关节i的角度初始值，cθi为cosθi，sθi为sinθi，nx、ny和nz分别为机械臂末端坐标系中x6轴与基坐标系的x0轴、y0轴和z0轴的夹角余弦值，ox、oy和oz分别为机械臂末端坐标系中y6轴与基坐标系的x0轴、y0轴和z0轴的夹角余弦值，ax、ay和az分别为机械臂末端坐标系中z6轴与基坐标系的x0轴、y0轴和z0轴的夹角余弦值，px、py和pz分别为机械臂末端执行器原点o6在基坐标系的x0、y0、z0轴上的坐标位置。
进一步地，所述步骤5和步骤6中对应的误差r和微小的α角度均根据实际情况具体设定。
本发明的有益效果：
本发明提出一种六自由度手腕偏置型串联机械臂的运动控制方法，无需考虑基于雅克比矩阵的奇异解问题，简化了控制过程，首先根据数值方法提供精度较高的初始估计值，提高数值方法的收敛速度和计算精度，然后将逆运动学的多变量迭代求解问题简化为一维变量迭代问题，有效降低数值迭代方法的计算量，在满足精度的前提下能够保证机器人的实时性控制，再把解析法融合到数值法当中，同时获得给定末端位姿的多重运动学逆解，其次利用正运动学校验末端位置误差和姿态误差，确定满足精度的多重运动学逆解，降低多重运动学逆解的误差，提升机械臂控制精度，最后采用关节限制和关节位移最小约束筛选多重运动学逆解，得到可行、合理的机械臂控制轨迹，具有计算简单、控制精度高、实施成本低等特点。
附图说明
图1为本发明实施例中六自由度手腕偏置型串联机械臂的运动控制方法的流程图；
图2为本发明实施例中建立的机械臂d-h坐标系示意图；
图3为本发明实施例中启发式迭代方法的单次迭代过程示意图；
其中，(a)为机械臂各关节初始位姿和末端目标位姿的示意图；(b)为启发式正向迭代过程中由目标位姿t和机构约束确定末端位姿ee和腕关节w的位姿示意图；(c)为启发式正向迭代过程中确定肘关节e相对于腕关节w的偏转角度示意图；(d)为启发式正向迭代过程中确定肘关节e的位姿示意图；(e)为采用启发式正向迭代得到机械臂各关节的位姿示意图；(f)为启发式反向迭代过程中确定肩关节s和肘关节e的位姿示意图；(g)为采用启发式反向迭代得到机械臂各关节的位姿示意图；(h)为采用完整的启发式迭代方法得到的机械臂各关节的位姿示意图；
图4为本发明实施例中机械臂关节1分别以最大值和最小值作为输入值由启发式迭代方法得到的不同结果示意图；
其中，(a)为机械臂关节1以最大值作为输入值时机械臂各关节初始位姿示意图；(b)为机械臂关节1以最大值作为输入值由启发式迭代方法得到的机械臂各关节位姿示意图；(c)为机械臂关节1以最小值作为输入值时机械臂各关节初始位姿示意图；(d)为机械臂关节1以最小值作为输入值由启发式迭代方法得到的机械臂各关节位姿示意图；
图5为本发明实施例中机械臂关节1的角度初始值计算方法示意图；
图6为本发明实施例中机械臂正方形轨迹运动规划。
具体实施方式
为了使本发明的目的、技术方案及优势更加清晰，下面结合附图和具体实施例对本发明做进一步详细说明。此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
一种六自由度手腕偏置型串联机械臂的运动控制方法，流程如图1所示，包括以下步骤：
步骤1：根据d-h参数法建立六自由度手腕偏置型串联机械臂关节i的连杆坐标系{i}，其中，i＝1，2，3，4，5，6。
本实施例中，所建立的机械臂d-h坐标系示意图如图2所示，坐标系{i}与坐标系{i-1}之间的变换由四个参数构成，如下表所示：
其中：
ai-1为从zi-1到zi沿xi-1测量的距离；
αi-1为从zi-1到zi绕xi-1旋转的角度；
di为从xi-1到xi沿zi-1测量的距离；
θi为从xi-1到xi绕zi-1旋转的角度；
步骤2：根据机械臂的d-h参数采用齐次坐标变换矩阵建立机械臂的正运动学方程。
所述机械臂的正运动学方程如公式(1)所示：
其中，
nz＝-cθ6[cθ5(cθ2sθ4-cθ3cθ4sθ2)+sθ2sθ3sθ5]-sθ6(cθ2cθ4+cθ3sθ2sθ4)；
oz＝sθ6[cθ5(cθ2sθ4-cθ3cθ4sθ2)+sθ2sθ3sθ5]-cθ6(cθ2cθ4+cθ3sθ2sθ4)；
ax＝sθ5[cθ4(sθ1sθ3-cθ1cθ2cθ3)-cθ1sθ2sθ4]-cθ5(cθ3sθ1+cθ1cθ2sθ3)；
ay＝-sθ5[cθ4(cθ1sθ3+sθ1cθ2cθ3)+sθ1sθ2sθ4]+cθ5(cθ3cθ1-sθ1cθ2sθ3)；
az＝sθ5(cθ2sθ4-cθ3cθ4sθ2)-sθ2sθ3cθ5；
px＝d5[sθ4(sθ1sθ3-cθ1cθ2cθ3)+cθ1cθ4sθ2]+d3cθ1sθ2；
py＝-d5[sθ4(cθ1sθ3+sθ1cθ2cθ3)-sθ1cθ4sθ2]+d3sθ1sθ2；
pz＝-d5(cθ2cθ4+cθ3sθ4sθ2)-d3cθ2；
其中，θi为机械臂关节i的角度初始值，cθi为cosθi，sθi为sinθi。
步骤3：采用启发式迭代方法获取精确的机械臂关节1的角度初始值，迭代过程示意图如图3所示，具体步骤如下：
步骤3.1：定义机械臂末端执行器的给定位姿为t、末端执行器为ee、腕关节为w、肘关节为e、肩关节为s、末端执行器ee与腕关节w之间的杆长距离为dt、肘关节e与肩关节s之间的杆长距离为d3、腕关节w与肘关节e之间的杆长距离为d5，其中，w包括关节5和关节6，e包括关节3和关节4，s包括关节1和关节2，如图3(a)所示。
步骤3.2：取符合机械臂关节1限位的最大值。
步骤3.3：令机械臂末端执行器ee与末端执行器的给定位姿t重合，确定机械臂末端执行器ee的位姿ee’。
所述位姿为关节所在位置和姿态。
步骤3.4：令腕关节w沿机械臂末端执行器的位姿ee’的固定方向移动机械臂末端执行器ee与腕关节w之间的杆长距离dt，确定机械臂腕关节w的位姿w’，如图3(b)所示。
步骤3.5：过机械臂肘关节e的中心在末端执行器的位姿ee’与腕关节w的位姿w’所在直线上作垂线，并根据腕关节w的关节限位在此垂线所在的平面内确定肘关节e的可行域；
步骤3.6：将机械臂肘关节e移动到可行域内得到肘关节e在可行域内的位姿ec，如图3(c)所示。
步骤3.7：令肘关节e沿腕关节w的位姿w’与肘关节e在可行域内的位姿ec所在直线方向移动腕关节w与肘关节e之间的杆长距离d5，确定肘关节e的位姿e’，如图3(d)所示。
步骤3.8：过机械臂肩关节s的中心在腕关节w的位姿w’与肘关节e的位姿e’所在直线上作垂线，并根据肘关节e的关节限位在此垂线所在的平面内确定肩关节s的可行域，如图3(d)所示；
步骤3.9：将机械肩关节s移动到可行域内得到肩关节s在可行域内的位姿sc。
步骤3.10：令肩关节s沿肘关节e的位姿e’与肩关节s在可行域内的位姿sc所在直线方向移动肘关节e与肩关节s之间的杆长距离d3，确定肩关节s的位姿s’，如图3(e)所示。
步骤3.9：令肩关节s的位姿s’与原肩关节s的位姿base重合为s”，保持其余关节角度不变，利用启发式迭代方法分别得到机械臂各关节的逆运动学解的位姿e”、w”和ee”，如图3(f)-(h)所示。
步骤3.10：取符合机械臂关节1限位的最小值，重复步骤3.3-步骤3.9，得到机械臂各关节的另一组逆运动学解的位姿。
本实施例中，取符合机械臂关节1限位的最大值和最小值如图4(a)和(c)所示，所得到的机械臂各关节逆运动学解的位姿如图4(b)和(d)所示。
步骤3.11：定义垂直于关节1轴线的平面为φ，平面φ内竖直向下的向量为v0，上臂的方向向量为v1，v1在平面φ内的投影向量为v2，如图5所示。
步骤3.12：分别根据两组机械臂各关节的逆解位姿下平面φ内竖直向下的向量v0和v1在平面φ内的投影向量v2，计算机械臂关节1的两个角度初始值θ1和θ1’。
步骤4：根据机械臂关节1的两个角度初始值θ1和θ1’分别计算关节2到关节6的逆运动学解的角度值。
所述计算关节2到关节6的逆运动学解的角度值的公式如公式(2)-公式(6)所示：
其中：
φ＝atan2(pz，pxcθ1+pysθ1)；
θ3＝atan2(pxsθ1-pycθ1，-pzsθ2-pxcθ1cθ2-pycθ2sθ1)(3)
其中：
χ＝(pxcθ1sθ2+pysθ1sθ2-pzcθ2-d3)/d5；
θ5＝atan2(-ζ，η)(5)
其中：
ζ＝ax(cθ1sθ2sθ4-cθ4sθ1sθ3+cθ1cθ2cθ3cθ4)-az(cθ2sθ1-cθ3cθ4sθ2)；
η＝ay(cθ1cθ3-cθ2sθ1sθ3)-ax(cθ3sθ1+cθ1cθ2sθ3)-azsθ2sθ3；
θ6＝atan2(λ，τ)(6)
其中：
其中，θi为机械臂关节i的角度初始值，cθi为cosθi，sθi为sinθi。
步骤5：根据机械臂的正运动学方程计算机械臂末端执行器ee的位姿ee’与末端执行器的给定位姿t的误差r。
所述误差r根据实际情况具体设定。
本实施例中，设定r＝0.1°。
本实施例中，将所得到的机械臂各关节逆运动学解的位姿代入到公式(1)中进行验证。
步骤6：判断误差r是否小于误差阈值，若是，则继续步骤7，若否，则将步骤3中得到的关节1的角度初始值增加或减去一个微小的α角度，返回步骤4。
所述微小的α角度均根据实际情况具体设定。
本实施例中，设定α＝0.001°。
步骤7：在步骤6得到的多重解中筛选出符合关节限位和关节位移最小原则的一组逆运动学解的角度值；所述关节位移最小为机械臂各个关节的位移和最小。
步骤8：将步骤7得到的逆运动学解的角度值输入到已知机器人的运动控制模块，完成对六自由度手腕偏置型串联机械臂的运动控制。
对本发明提供的控制方法进行实验验证，实验环境如下：
cpu：intelcorei7-8700k3.70ghz；内存：16gbram；实验平台：matlab2016b。
本实施例中，根据本发明提出的控制方法进行正方形轨迹规划，规划100个插值点，实验结果如图6所示，其中，末端执行器位置误差为10-15m，姿态误差为0.1°，规划100个插值点共耗时1.768s，平均每个插值点耗时17.7ms。
由实验结果可知，本发明提出的六自由度手腕偏置型串联机械臂的运动控制方法能够满足机器人高精度实时性控制要求，具有很高的应用价值和可推广性。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解；其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；因而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。