[發明專利]基于改進A*算法的工業機械臂無碰撞路徑規劃方法

有效

• 申請號: 201810045890.6
• 申請日: 2018-01-17
• 公開（公告）號: CN108356819B
• 公開（公告）日: 2020-08-14
• 發明（設計）人: 陶唐飛;鄭翔;賀華;徐光華;姚旿冬
• 申請（專利權）人: 西安交通大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 西安智大知識產權代理事務所 61215
• 代理人: 賀建斌

1.基于改進A*算法的工業機械臂無碰撞路徑規劃方法，其特征在于，包括以下步驟：

步驟1、對六自由度機械臂進行參數化建模，并建立機械臂的正逆運動學模型：

采用D-H法描述機械臂的位姿，建立機械臂的連桿坐標系，確定機械臂的D-H參數；

根據D-H參數，連桿i和連桿i-1相對位置的齊次變換矩陣^i-1A_i為：

式中C_i＝cosθ_i，S_i＝sinθ_i，其中i表示機械臂的連桿個數，i＝1,2,3,4,5,6；

將各關節的齊次變換矩陣相乘，得到了機械臂末端執行器的齊次變換矩陣為：

其中，p_x,p_y,p_z表示平移變換在x，y，z三個方向的分量，n＝[n_x n_y n_z]，o＝[o_x o_y o_z]，a＝[a_x a_y a_z]，分別為繞x，y，z三個方向旋轉矢量；

步驟2、建立機械臂及環境的OBB包圍盒：利用soildwork建模軟件建立機械臂及環境模型，并將機械臂及環境模型另存為STL格式文件，讀取STL格式文件，從而得到模型中各個物體的三角形網格模型，采用OBB包圍盒算法建立機械臂及環境障礙物的OBB包圍盒；OBB包圍盒方法流程為：

2.1)通過物體的三角形網格模型中的三角面片信息計算每個三角形平面的法向矢量，并在每個三角面片上建立一個坐標系，原點為三角形任意頂點，三角形平面為x-y平面，z軸方向為三角形平面的法向矢量；

2.2)遍歷每一個三角面片的平面法向矢量，依次繞每一個法向矢量旋轉三角面片的坐標系，尋找物體的三角形網格模型投影到三角面片的x-y平面面積最小的方向，記錄下在旋轉后的坐標系下的包圍盒的體積；

2.3)將最小體積對應的坐標系的方向作為包圍盒的三個方向向量，將物體的三角形網格模型中所有頂點投影到三個方向向量中，求出每個方向的最大值和最小值，得到OBB包圍盒的八個頂點；

步驟3、設置A*算法的估價函數和搜索步長，采用基于分離軸的碰撞檢測算法在搜索的每一步中判斷機械臂是否與環境發生碰撞：

機械臂在關節空間中某個節點的構型矢量為q_i，其中包括六個旋轉關節的角度變幅，即q_i＝(q_i1,q_i2,q_i3,q_i4,q_i5,q_i6)，距離估價函數為所有關節轉動的角度之和，在估價函數中加入一項，即當前節點到目標節點的各關節位移變化量之和，估價函數如下：

在機械臂的作業場景中，障礙物區域設置0.1到0.2度之間較小的搜索步長；而在非障礙物區域設置1到1.5度之間較大的搜索步長；

采用基于分離軸相交測試的方法在搜索過程中進行碰撞檢測，對于包圍盒子A與B的碰撞檢測算法流程為：

3.1)計算兩個包圍盒子潛在的15條分離軸，對每一條分離軸進行相交測試；

3.2)對每一條分離軸，將八個頂點做投影，得到兩個包圍盒投影的最大值和最小值分別為maxA、minA、maxB和minB,若maxA<mimB或maxB<minA，則在此分離軸下，兩個包圍盒分離，否則相交；

3.3)若對所有的分離軸均滿足maxA<mimB或maxB<minA，則兩個包圍盒分離，否則兩個包圍盒相交；

步驟4、根據關節轉動角度之和最小且不發生碰撞的原則，在機械臂的六維關節空間中進行搜索，得到無碰撞的路徑；

設置兩個存儲矩陣，分別為OPEN表和CLOSED表，其中OPEN存放所有搜索節點，CLOSED中存放每一步搜索到的最佳節點，并將它們均初始化為空，機械臂起始位姿的構型矢量為q_ori，目標位姿的構型矢量為q_fin，機械臂當前位姿的構型矢量為q_cur，當前位姿的相鄰位姿構型矢量為q_{nei_i}；

將機械臂的初始位姿的構型矢量、它的父親位姿的構型矢量，即它本身和與之對應的g、h、f合并為一個向量放入OPEN表的第一行，即：

OPEN(1,:)＝(q_ori,q_ori,g(q_ori),h(q_ori),f(q_ori))

OPEN(1,:)是一個1×15的矢量，存放到OPEN表的第一行，并將這個向量放入CLOSED表的第一行；

重復進行以下步驟，直到q_cur＝q_fin，完成搜索任務，若OPEN表變為空，則跳出循環，搜索失敗；

4.1)OPEN表中找到f(q_i)值最小的那一行向量，將該行向量作為當前搜索節點，并將該節點向量放入CLOSED表中，同時從OPEN表中刪除該節點向量；

4.2)判斷當前搜索節點是否為目標節點，如果是，那么搜索成功；否則繼續下一步；

4.3)對于機械臂而言，每一個關節有三種變化方式，分別是保持不變、增加步長，減少步長，這樣在一個六維空間中，當前搜索節點就有3⁶-1＝728個鄰近的搜索節點，設第i個鄰近節點為q_{nei_i}；

4.4)對于每一個鄰近的搜索節點執行如下操作：

a)對鄰近的搜索節點進行碰撞檢測，如果發生了碰撞，則忽略該節點；

b)判斷該鄰近的搜索節點是否超出了該關節的最大角度或者最小角度，如果超限，則轉至下一個鄰近節點；

c)若鄰近節點既沒有發生碰撞也沒有超限，則往下執行；

d)判斷該節點矢量是否在OPEN表中，如果在OPEN表中，則通過節點索引函數查找該節點在OPEN表中的位置，然后判斷當前q_{nei_i}節點中的g_{nei_i}與OPEN表中已經存在的g_{old_i}的大小關系，如果g_nei-i＜g_{old_i}，則就將當前的節點、當前節點的父親節點以及新的g、h、f更新至OPEN表中原來舊節點的位置；

e)若該節點矢量不在OPEN表中，則直接把該節點矢量、當前節點矢量、該節點矢量的g、h、f值放入到OPEN表新的一行中，同時將當前節點、當前節點的父親節點、當前節點的g、h、f值放入到CLOSED表新的一行中；

當搜索成功后，從CLOSED表中的最后一行開始，通過節點索引函數依次查找當前節點的父親節點，一直到起始節點，就完成了機械臂在關節空間的構型搜索任務。