CN108481324A - 一种八轴多功能机械臂的逆解工程及其碰撞检测算法 - Google Patents

Abstract

本发明公开了一种八轴多功能机械臂的逆解工程及其碰撞检测算法，包括位置精度逆解算法、方位角逆解算法、碰撞检测算法三个部分。方位角逆解算法根据目标姿态Z轴方向向量及空间位置坐标求解第七关节空间位置坐标，以及验证位置精度逆解算法得到的前段关节逆解，并求解出满足末端目标姿态的后段关节逆解。位置精度逆解算法通过关节之间的约束，简化逆解求解过程。碰撞检测算法通过简化多功能机械臂的特殊工作空间，提高逆解工程的运算效率，该算法嵌入到位置精度逆解算法，验证前段六个关节的空间合理性，以及全关节逆解的空间合理性。本发明降低了八轴机械臂逆解工程的运算量，提高了运算效率，提高了逆解空间合理性的验证效率。

Description

一种八轴多功能机械臂的逆解工程及其碰撞检测算法

技术领域

本发明涉及机械臂运动算法领域，具体是一种八轴多功能机械臂的逆解工程及其碰撞检测算法。

背景技术

在中国热核聚变反应堆工程的常规维护中，八轴多功能机械臂扮演着重要的角色，承担着装置维护的重大责任。为了争取到更多的聚变反应实验时间，需要尽可能快的以及尽可能好的完成日常维护作业。多功能机械臂的末端搭载着末端执行器，是反应堆维护工作的直接操作者。为了保证维护过程中的精度控制，需要多功能机械臂保证一个精确地末端定位精度，为末端执行器提供一个精确定位的支撑平台。

八轴机械臂的运动学逆解是一个计算量大且复杂的过程，需要根据实际运动情况进行简化及解的筛选。通过简化和筛选才能尽快的得到符合空间要求及机械臂构型的运动学逆解。

发明内容

本发明的目的是提供一种逆解工程及其碰撞检测算法，尤其是涉及一种八轴多功能机械臂的逆解工程及其碰撞检测算法。

为了达到上述目的，本发明所采用的技术方案为：

一种八轴多功能机械臂的逆解工程及其碰撞检测算法，其特征在于：包括方位角逆解算法、位置精度逆解算法、碰撞检测算法，其中：

(1)、方位角逆解算法其过程如下：

(1.1)、目标姿态位置矩阵T_END由姿态矩阵R_END和位置矢量P_END组成，其中R_IND＝[x_END y_END z_END]，则第七关节空间位置坐标P_V＝P_END-(d_V+d_g)*z_END；

其中，px、py、pz、指该点在空间中的三维坐标；d7、d8是建立D-H坐标系过程中产生的对应的参数，根据机器人关节坐标系的建立得出，d1-d8参数表如表1所示：

表1 D-H坐标系过程中d1-d8参数

(1.2)、将第七关节空间位置坐标P₇导入位置精度逆解算法，可以求出前段关节逆解Pos_1To5＝[d₁ θ₂ θ₃ θ₄ θ₅]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解Pos_1To5；其中d1建立D-H坐标系过程中产生的对应的参数；θ2-θ5表示的是八轴多功能机械臂中第二-第五关节的转动变量，这些编号对应的是关节坐标系的下标，转动方向为绕着各个坐标系的Z轴转动；

(1.3)、通过关节逆解Pos_1To5＝[Pos_1To5 0]得到第六关节的姿态位置矩阵根据D-H法建立的关节坐标系，满足P₆＝P₇，θ₇＝arccos(z₆*z_END/(|z₆|*|z_END|))，其中z₆为第六关节Z轴方向向量，R6指的是第六关节的坐标转换后的方向矩阵、P6指的则是转换后的位置向量，P7指的是经过前7个关节坐标系转换之后的坐标中心点的位置向量，Zend指的是机器人末端点目标姿态的Z轴向量，θ7是八轴多功能机械臂中第七关节的转动变量；

(1.4)、由第六关节Z轴方向向量z₆和目标姿态Z轴方向向量z_END得到法向量V_publlc1，方向符合右手准则；

(1.5)、通过第六关节Y轴方向向量y₆与法向量V_publlc1的夹角θ_jud＝arccos(Z₆*Z_END/(|Z₆|*|Z_END|))来判定θ₆的值：

θ6是八轴多功能机械臂中第六关节的转动变量；

(1.6)、通过前段关节逆解Pos_1To7＝[d_i θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]得到第七关节的姿态位置矩阵其中第七关节X轴方向向量x₇和目标姿态X轴方向向量x_END，得到法向量V_publlc2，方向符合右手准则，通过V_publlc2与z_END的方向关系判定θ₈的值：

其中x₇为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵，P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8是八轴多功能机械臂中第八关节的转动变量；

(2)、位置精度逆解算法其过程如下：

(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：

由d₁划分三种情况：d₁∈[0，2705)，d₁∈[2705，4455)，d₁＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；

(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P₇，按照d₁递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；

(2.3)、运行全臂约束逆解算法，输入关节变量Pos_1To6＝[d₁ π/2-π/2 θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，通过方程联立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(2.4)、运行半臂约束逆解算法，输入关节变量Pos_1To5＝[d₁ π/2 θ₃ θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ₃∈[-π，0]，再通过方程联立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(2.5)、运行平动约束逆解算法，输入关节变量Pos_1To5＝[4455 θ₂ θ₃ θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ₅∈[-π，0]，由p_z(θ₄,θ₅)得到θ₄，再通过方程联立求解得到d₁，θ₂，θ₃，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(3)、碰撞检测算法其过程如下：

(3.1)、简化机械臂环状工作空间，空间切面折线以极坐标方程曲线代替，极点位于空间切面折线的中心位置，该位置由待检测点决定，环状壁极坐标方程：

l(θ)＝(0.000277500372481*θ¹⁰-0.008523502454628*θ⁹

+0.106922940971428*θ⁹-0.696566524761767

*θ⁷+2.447743902396733*θ⁶

-4.160347429809979*θ⁵+1.503776922304815

*θ⁴+3.856518561349923*θ³

-3.237876202808929*θ²+1.010407153490720

*θ¹+1.623553179189534)*(1.0e+03)

安全域极坐标方程：l_safe(θ)＝0.62*l(θ)；

(3.2)、根据关节变量求得各关节的空间位置坐标：

在P_i与P_i+1之间等间距取100个待检测点，

(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T₁，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T₂，确定T₁与T₂的角度θ_T1T2，极点P_polεr相对环状空间中心轴坐标系的位置坐标(center_x、center_y、center_z)：

center_x＝5738.4^*cos(theta_center_axis)；

center_y＝5738.4^*sin(theta_center_axis)；

center_z＝0

待检测点相对极点P_polar的空间向量得到极坐标半径和与xoy平面的夹角即极角导入安全域极坐标方程：

在的情况下，待检测点通过碰撞检测。

本发明通过八轴多功能机械臂的逆解工程及碰撞检测算法通过方位角逆解算法降低了八轴机械臂逆解工程的运算量，位置精度逆解算法通过拆解复杂情况提高了运算效率，碰撞检测算法通过特殊空间曲线拟合提高了逆解空间合理性的验证效率。

附图说明

图1为本发明的八轴多功能机械臂的关节坐标系轴侧方向示意图；

图2为本发明的机械臂工作示意图的轴侧方向示意图；

图3为本发明的机械臂工作示意图的左侧方向示意图；

图4为本发明的机械臂工作示意图的俯视方向示意图；

图5为本发明的避障算法的截面折线的拟合曲线示意图；

图6为本发明的避障算法的安全曲线示意图；

图7为本发明的目标工作装置环境剖视图；

图8为本发明的目标工作装置截面坐标系示意图；

图9为本发明的八轴多功能机械臂的工作示意图；

图10为本发明的八轴多功能机械臂的轴侧方向示意图；

图11为本发明的算法程序流程图；

图中标号：1、切面拟合曲线极坐标原点；2、机械臂基座标原点；3、目标工作装置切面采集点；4、目标工作装置切面简化折线；5、目标工作装置；6、机械臂基座；7、八轴机械臂；8、末端执行器。

具体实施方式

如图11所示，一种八轴多功能机械臂的逆解工程及其碰撞检测算法，包括方位角逆解算法、位置精度逆解算法、碰撞检测算法，如图1、图2、图3、图4所示，本发明首先根据D-H法建立八轴机械臂的关节坐标系。图8表示了极坐标系与机器人极坐标系的相对位置关系。

如图7、图9、图10，表示了机器人及该机器人应用的装置，八轴机械臂7安装在机械臂基座6上，末端执行器8安装在八轴机械臂7的末端，机械臂基座6实现与大装置的连接，其中：

(1)、方位角逆解算法其过程如下：

(1.1)、目标姿态位置矩阵T_END由姿态矩阵R_END和位置矢量P_END组成，其中R_IND＝[x_END y_END z_END，则第七关节空间位置坐标P_V＝P_END-(d_V+d_g)*z_END；

其中，px、py、pz、指该点在空间中的三维坐标；d7、d8是建立D-H坐标系过程中产生的对应的参数，根据机器人关节坐标系的建立得出，d1-d8参数表如表1所示：

表1 D-H坐标系过程中d1-d8参数

(1.2)、将第七关节空间位置坐标P₇导入位置精度逆解算法，可以求出前段关节逆解Pos_1To5＝[d₁ θ₂ θ₃ θ₄ θ₅]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解Pos_1To5；其中d1建立D-H坐标系过程中产生的对应的参数；θ2-θ5表示的是八轴多功能机械臂中第二-第五关节的转动变量，这些编号对应的是关节坐标系的下标，转动方向为绕着各个坐标系的Z轴转动；

(1.3)、通过关节逆解Pos_1To5＝[Pos_1To5 0]得到第六关节的姿态位置矩阵根据D-H法建立的关节坐标系，满足P₆＝P₇，θ₇＝arccos(z₆*z_END/(|z₆|*|z_END|))，其中z₆为第六关节Z轴方向向量，z_END为目标姿态Z轴方向向量；

其中z₆为第六关节Z轴方向向量，R6指的是第六关节的坐标转换后的方向矩阵、P6指的则是转换后的位置向量，P7指的是经过前7个关节坐标系转换之后的坐标中心点的位置向量，Zend指的是机器人末端点目标姿态的Z轴向量，θ7是八轴多功能机械臂中第七关节的转动变量；

(1.4)、由第六关节Z轴方向向量z₆和目标姿态Z轴方向向量z_END得到法向量V_public1，方向符合右手准则；

(1.5)、通过第六关节Y轴方向向量y₆与法向量V_public1的夹角θ_jud＝arccos(z₆*z_END/(|z₆|*|z_END|))来判定θ₆的值：

θ6是八轴多功能机械臂中第六关节的转动变量；

(1.6)、通过前段关节逆解Pos_1To7＝[d₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]得到第七关节的姿态位置矩阵其中第七关节X轴方向向量x₇和目标姿态X轴方向向量x_END，得到法向量V_public2，方向符合右手准则，通过V_public2与z_END的方向关系判定θ₈的值：

其中x₇为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵，P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8是八轴多功能机械臂中第八关节的转动变量；

(2)、位置精度逆解算法其过程如下：

(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：

由d₁划分三种情况：d₁∈[0，2705)，d₁∈[2705，4455)，d₁＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；

(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P₇，按照d₁递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；

(2.3)、运行全臂约束逆解算法，输入关节变量Pos_1To6＝[d₁ π/2 -π/2 θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，通过方程联立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(2.4)、运行半臂约束逆解算法，输入关节变量Pos_1To5＝[d₁ π/2 θ₃ θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ₃∈[-π，0]，再通过方程联立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(2.5)、运行平动约束逆解算法，输入关节变量Pos_1To5＝[4455 θ₂ θ₃ θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ₅∈[-π，0]，由p_z(θ₄，θ₅)得到θ₄，再通过方程联立求解得到d₁，θ₂，θ₃，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(3)、碰撞检测算法其过程如下：

(3.1)、a)如图6的out-curve所示，为简化后的工作空间截面，图5的out-curve所示，为简化后的工作空间截面的折线图(极角-极坐标半径)，对折线图进行曲线拟合，得到图5的fitting-curve，对应图6的fitting-curve。简化机械臂环状工作空间，空间切面折线以极坐标方程曲线(图6的fitting-curve)代替，然后进行极坐标半径的缩放，缩放比例为0.62，如图6的inner-curve，极点位于空间切面折线的中心位置，该位置由待检测点决定，环状壁(图6的fitting-curve)极坐标方程：l(θ)＝(0.000277500372481*θ¹⁰-0.008523502454628*θ⁹

+0.106922940971428*θ⁹-0.696566524761767

*θ⁷+2.447743902396733*θ⁶

-4.160347429809979*θ⁵+1.503776922304815

*θ⁴+3.856518561349923*θ³

-3.237876202808929*θ²+1.010407153490720

*θ¹+1.623553179189534)*(1.0e+03)

安全域极坐标方程：l_safe(θ)＝0.62*l(θ)；

(3.2)、根据关节变量求得各关节的空间位置坐标：

在P_i与P_i+1之间等间距取100个待检测点，

(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T₁，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T₂，确定T₁与T₂的角度θ_T1T2，极点P_polεr相对环状空间中心轴坐标系的位置坐标(center_x、center_y、center_z)：

center_x＝5738.4^*cos(theta_center_axis)；；

center_y＝5738.4^*sin(theta_center_axis)；；

center_z＝0

待检测点相对极点P_polar的空间向量得到极坐标半径和与xoy平面的夹角即极角导入安全域极坐标方程：

在的情况下，待检测点通过碰撞检测。

如图9所示为八轴多功能机械臂的工作示意图，搭载前端执行器8为目标工作装置开展维护作业。

本发明通过八轴多功能机械臂的逆解工程及碰撞检测算法通过方位角逆解算法降低了八轴机械臂逆解工程的运算量，位置精度逆解算法通过拆解复杂情况提高了运算效率，碰撞检测算法通过特殊空间曲线拟合提高了逆解空间合理性的验证效率。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。

Claims (1)

1.一种八轴多功能机械臂的逆解工程及其碰撞检测算法，其特征在于：包括方位角逆解算法、位置精度逆解算法、碰撞检测算法，其中：

(1)、方位角逆解算法其过程如下：

(1.1)、目标姿态位置矩阵T_END由姿态矩阵R_END和位置矢量P_END组成，其中R_END＝[x_END y_END z_END]，则第七关节空间位置坐标P₇＝P_END-(d₇+d₈)*z_END；

其中，px、py、pz、指该点在空间中的三维坐标；d7、d8是建立D-H坐标系过程中产生的对应的参数，根据机器人关节坐标系的建立得出；

(1.2)、将第七关节空间位置坐标P₇导入位置精度逆解算法，可以求出前段关节逆解Pos_1To5＝[d₁ θ₂ θ₃ θ₄ θ₅]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解Pos_1To5；其中d1建立D-H坐标系过程中产生的对应的参数；θ2-θ5表示的是八轴多功能机械臂中第二-第五关节的转动变量，这些编号对应的是关节坐标系的下标，转动方向为绕着各个坐标系的Z轴转动；

(1.3)、通过关节逆解Pos_1To5＝[Pos_1To5 0]得到第六关节的姿态位置矩阵根据D-H法建立的关节坐标系，满足P₆＝P₇，θ₇＝arccos(z₆*z_END/(|z₆|*|z_END|))，其中z₆为第六关节Z轴方向向量，R6指的是第六关节的坐标转换后的方向矩阵、P6指的则是转换后的位置向量，P7指的是经过前7个关节坐标系转换之后的坐标中心点的位置向量，Zend指的是机器人末端点目标姿态的Z轴向量，θ7是八轴多功能机械臂中第七关节的转动变量；

(1.4)、由第六关节Z轴方向向量z₆和目标姿态Z轴方向向量z_END得到法向量V_public1，方向符合右手准则；

(1.5)、通过第六关节Y轴方向向量y₆与法向量V_public1的夹角θ_jud＝arccos(z₆*z_END/(|z₆|*|z_END|))来判定θ₆的值：

θ6是八轴多功能机械臂中第六关节的转动变量；

(1.6)、通过前段关节逆解Pos_1To7＝[d₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]得到第七关节的姿态位置矩阵其中第七关节X轴方向向量x₇和目标姿态X轴方向向量x_END，得到法向量V_public2，方向符合右手准则，通过V_public2与z_END的方向关系判定θ₈的值：

其中x₇为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵，P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8是八轴多功能机械臂中第八关节的转动变量；

(2)、位置精度逆解算法其过程如下：

(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：

由d₁划分三种情况：d₁∈[0，2705)，d₁∈[2705，4455)，d₁＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；

(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P₇，按照d₁递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；

(2.3)、运行全臂约束逆解算法，输入关节变量Pos_1To6＝[d₁ π/2 -π/2 θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，通过方程联立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(2.4)、运行半臂约束逆解算法，输入关节变量Pos_1To5＝[d₁ π/2 θ₃ θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ₃∈[-π，０］，再通过方程联立求解得到d₁，θ₄，θ₅，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(2.5)、运行平动约束逆解算法，输入关节变量Pos_1To5＝[4455 θ₂ θ₃ θ₄ θ₅ 0]得到第六关节的姿态位置矩阵其中且P₆＝P₇，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ₅∈[-π，0]，由p_z(θ₄，θ₅)得到θ₄，再通过方程联立求解得到d₁，θ₂，θ₃，然后判定d₁是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；

(3)、碰撞检测算法其过程如下：

(3.1)、简化机械臂环状工作空间，空间切面折线以极坐标方程曲线代替，极点位于空间切面折线的中心位置，该位置由待检测点决定，环状壁极坐标方程：

l(θ)＝(0.000277500372481*θ¹⁰-0.008523502454628*θ⁹

+0.106922940971428*θ⁹-0.696566524761767

*θ⁷+2.447743902396733*θ⁶

-4.160347429809979*θ⁵+1.503776922304815

*θ⁴+3.856518561349923*θ³

-3.237876202808929*θ²+1.010407153490720

*θ¹+1.623553179189534)*(1.0e+03)

安全域极坐标方程：l_safe(θ)＝0.62*l(θ)；

(3.2)、根据关节变量求得各关节的空间位置坐标：

在P_i与P_i+1之间等间距取100个待检测点，k∈[0，101]；

(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T₁，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T₂，确定T₁与T₂的角度θ_T1T2，极点P_polar相对环状空间中心轴坐标系的位置坐标(center_x、center_y、center_z)：

center_x＝5738.4*cos(theta_center_axis)；

center_y＝5738.4*sin(theta_center_axis)；

center_z＝0

待检测点相对极点P_polar的空间向量得到极坐标半径和与xoy平面的夹角即极角导入安全域极坐标方程：

在的情况下，待检测点通过碰撞检测。