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

摘要

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

主权项

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)、将第七关节空间位置坐标P7导入位置精度逆解算法，可以求出前段关节逆解Pos1To5＝[d1 θ2 θ3 θ4 θ5]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解Pos1To5；其中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轴方向向量z6和目标姿态Z轴方向向量zEND得到法向量Vpublic1，方向符合右手准则；(1.5)、通过第六关节Y轴方向向量y6与法向量Vpublic1的夹角θjud＝arccos(z6*zEND/(|z6|*|zEND|))来判定θ6的值：θ6是八轴多功能机械臂中第六关节的转动变量；(1.6)、通过前段关节逆解Pos_1To7＝[d₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]得到第七关节的姿态位置矩阵其中第七关节X轴方向向量x₇和目标姿态X轴方向向量x_END，得到法向量V_public2，方向符合右手准则，通过V_public2与z_END的方向关系判定θ₈的值：其中x7为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵，P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8是八轴多功能机械臂中第八关节的转动变量；(2)、位置精度逆解算法其过程如下：(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：由d1划分三种情况：d1∈[0，2705)，d1∈[2705，4455)，d1＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P7，按照d1递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；(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*θ10‑0.008523502454628*θ9+0.106922940971428*θ9‑0.696566524761767*θ7+2.447743902396733*θ6‑4.160347429809979*θ5+1.503776922304815*θ4+3.856518561349923*θ3‑3.237876202808929*θ2+1.010407153490720*θ1+1.623553179189534)*(1.0e+03)安全域极坐标方程：lsafe(θ)＝0.62*l(θ)；(3.2)、根据关节变量求得各关节的空间位置坐标：在P_i与P_i+1之间等间距取100个待检测点，k∈[0，101]；(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T1，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T2，确定T1与T2的角度θT1T2，极点Ppolar相对环状空间中心轴坐标系的位置坐标(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平面的夹角即极角导入安全域极坐标方程：在的情况下，待检测点通过碰撞检测。