[發明專利]一種冗余機械臂零空間避障規劃方法

在審

• 申請號: 202210333465.3
• 申請日: 2022-03-30
• 公開（公告）號: CN114800491A
• 公開（公告）日: 2022-07-29
• 發明（設計）人: 李通通;楊濤;楊澤林;杜寶森;趙劍;張浩;王燕波
• 申請（專利權）人: 北京精密機電控制設備研究所
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 李晶堯

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軸互相垂直。