[发明专利]一种八轴多功能机械臂的逆解工程及其碰撞检测算法

权利要求书

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

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

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

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

表1 D-H坐标系过程中d₁-d₈参数

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

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

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

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

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

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

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

(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_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平面的夹角即极角导入安全域极坐标方程：

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