[发明专利]一种冗余机械臂零空间避障规划方法

权利要求书

1.一种冗余机械臂零空间避障规划方法，其特征在于：包括：

制作7个关节和基座组成的机械臂，7个关节分别为第一关节、第二关节、……、第七关节；第一关节与基座连接；第二关节、……、第七关节与第一关节依次串联；设定第一关节、第二关节、……、第七关节的旋转角度为θ₁、θ₂、……、θ₇；其中，第一关节、第二关节、第三关节为机械臂的肩部；第四关节为机械臂的肘部；第五关节、第六关节、第七关节为机械臂的腕部；

设定第一关节、第二关节、第三关节的轴线相交于S点；第五关节、第六关节、第七关节的轴线相交于W点；第四关节位于原点E′；SE′W所在平面为臂型面；当机械臂末端位姿给定后，臂型面绕SW旋转；

设定第一关节轴线方向的矢量J_1z与SW组成参考平面SEW，设定臂型面SE′W与参考平面SEW的夹角为臂角

建立臂型面坐标系AX′Y′Z′；建立机械臂基坐标系BXYZ，建立第一障碍物C和第二障碍物D；

将第一障碍物C和第二障碍物D向臂型面坐标系的AX′Y′平面投影；臂型面SE′W与X′轴正方形的夹角记为臂形角此时，机械臂基坐标系BXYZ下的障碍物随着机械臂的运动在臂型面坐标系AX′Y′Z′中运动；此时寻找臂形角的目标则是臂型面SE′W的在AX′Y′平面投影线AE′与所有障碍物之间的最小距离最大；

建立机械臂DH建模图；第一关节、第二关节、……、第七关节在DH建模中自动生成各关节的对应坐标系；获得第七关节相对于机械臂基坐标系的位姿矩阵⁰T₇；

设定⁰x₇为第七关节的位置的表达式、⁰R₇为第七关节的姿态的表达式；计算在机械臂基坐标系BXYZ下S点指向W点的向量⁰x_sw的表达式；

令w＝⁰x_sw，定义v为机械臂基坐标系BXYZ中Z轴的方向向量，即v＝[0 0 1]′；计算矢量J_1z与SW组成的参考平面与w的垂直向量k；对向量k进行单位化处理得到臂型面坐标系AX′Y′Z′的X′轴的单位向量k_n；则期望臂形角即为AE′与臂型面坐标系AX′Y′Z′的X′轴的夹角；

将障碍物向臂型面坐标系AX′Y′Z′的X′Y′面投影后，求解最优臂形角转化为求解AE′所在直线y＝mx的斜率m，使障碍物C、D的圆心与直线y＝mx的距离减去半径均最远；

设第一障碍物C的坐标为(x_C,y_C)，半径为R_C；第二障碍物D的坐标为(x_D,y_D)，半径为R_D；AE′所在直线y＝mx，存在两种情况：

S1、当(x_D,y_D)的连线穿过臂型面坐标系AX′Y′Z′的原点A，则AE′所在直线y＝mx的斜率而AE′方向存在2种，即远离坐标原点或指向坐标原点；计算S1情况下的期望臂形角，并选取最优臂形角；

S2、当(x_C,y_C)、(x_D,y_D)的连线不穿过臂型面坐标系AX′Y′Z′的原点A，求解方程得到两个斜率m₁和m₂；即向量AE′的方向存在4种情况，斜率为m₁时的两个方向和斜率为m₂的两个方向；计算S2情况下的期望臂形角，并选取最优臂形角；

完成避障规划。

2.根据权利要求1所述的一种冗余机械臂零空间避障规划方法，其特征在于：臂型面坐标系AX′Y′Z′的建立方法为：

臂型面坐标系X′Y′Z′的坐标原点A位于E点向SW连线的垂点；Z′轴与SW连线同向；X′轴平行于参考平面，且指向第四关节；Y′由右手定则确定；

机械臂基坐标系BXYZ的建立方法为：

机械臂基坐标系BXYZ的坐标原点位于基座上；Z轴竖直向上；X轴、Y轴位于水平面，且X轴、Y轴、Z轴互相垂直。