[發(fā)明專利]一種基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法

在審

• 申請?zhí)枺?/td>: 202210798681.5
• 申請日: 2022-07-08
• 公開（公告）號: CN115061475A
• 公開（公告）日: 2022-09-16
• 發(fā)明（設(shè)計(jì)）人: 齊飛;裴海珊;朱靖;張恒;邵笑校
• 申請（專利權(quán)）人: 常州大學(xué)
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 常州市英諾創(chuàng)信專利代理事務(wù)所(普通合伙) 32258
• 代理人: 張秋月

1.一種基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，包括以下步驟：

步驟一、對機(jī)器人的參數(shù)進(jìn)行初始化設(shè)定，包括：關(guān)節(jié)段數(shù)、關(guān)節(jié)長度、彎曲單元彎曲角、旋轉(zhuǎn)角、起始點(diǎn)和目標(biāo)點(diǎn)；

步驟二、將機(jī)器人的起始點(diǎn)與目標(biāo)點(diǎn)之間的理想路徑曲線均勻離散，并控制機(jī)器人從初始零位開始步進(jìn)；

步驟三、根據(jù)理想路徑曲線上的各離散點(diǎn)以及離散點(diǎn)的主法線、副法線和切線構(gòu)建機(jī)器人實(shí)際步進(jìn)點(diǎn)在基坐標(biāo)系的位姿矩陣；

步驟四、基于距離函數(shù)求解機(jī)器人末端彎曲單元的關(guān)節(jié)參數(shù)；

步驟五、基于逆向遞推法求解機(jī)器人其他彎曲單元的關(guān)節(jié)參數(shù)；

步驟六、根據(jù)步驟三-五求解的機(jī)器人各彎曲單元的關(guān)節(jié)參數(shù)，控制機(jī)器人前進(jìn)一步長；

步驟七、判斷機(jī)器人是否達(dá)到目標(biāo)位置，未到達(dá)目標(biāo)點(diǎn)時(shí)，重新求解再前進(jìn)一步長時(shí)各彎曲單元的兩個(gè)關(guān)節(jié)變量值，若到達(dá)目標(biāo)點(diǎn)，結(jié)束機(jī)器人的路徑規(guī)劃。

2.根據(jù)權(quán)利要求1所述的基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，步驟三具體包括：

設(shè)理想路徑曲線上距離機(jī)器人最近的離散點(diǎn)設(shè)為M，s為理想路徑的起點(diǎn)到M點(diǎn)的弧長，則曲線在點(diǎn)M處的切向量為：

其中，t為切向量，曲線上點(diǎn)M處的徑矢為

如果則M點(diǎn)處的切向量和位置向量唯一確定一個(gè)密切平面，密切平面方程為：

其中，為副法線方程；副法線標(biāo)準(zhǔn)方程表示為：

在已知切向量和副法線向量的情況下，M點(diǎn)處主法線通過右手定則求解為：

其中，為主法線向量；

機(jī)器人沿著理想路徑曲線前進(jìn)弧長s₀時(shí)，機(jī)器人實(shí)際步進(jìn)點(diǎn)相對于基坐標(biāo)系的齊次變換矩陣為：

其中，M_m為M點(diǎn)在基坐標(biāo)系下的位置坐標(biāo)；

以每個(gè)點(diǎn)的切線、主法線、副法線為點(diǎn)的坐標(biāo)系的x、y、z軸，求解出點(diǎn)相對基坐標(biāo)系的齊次變換矩陣，進(jìn)而獲得機(jī)器人在所有理想路徑點(diǎn)的位姿矩陣。

3.根據(jù)權(quán)利要求1所述的基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，步驟四具體包括：利用距離函數(shù)、末端彎曲單元的齊次變換矩陣和機(jī)器人實(shí)際步進(jìn)點(diǎn)位置坐標(biāo)計(jì)算得到彎曲角和旋轉(zhuǎn)角。

4.根據(jù)權(quán)利要求3所述的基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，距離函數(shù)的公式為：

d＝min{(x_s-x_l)²+(y_s-y_l)²+(z_s-z_l)²} (7)

式中，(x_s,y_s,z_s)為機(jī)器人實(shí)際步進(jìn)點(diǎn)位置，通過正運(yùn)動(dòng)學(xué)模型獲得，(x_l,y_l,z_l)為路徑上理想路徑點(diǎn)的位置。

5.根據(jù)權(quán)利要求3所述的基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，末端彎曲單元的齊次變換矩陣的公式為：

其中，為第i段彎曲單元相對第1段的初始坐標(biāo)系的齊次變換矩陣。

6.根據(jù)權(quán)利要求3所述的基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，彎曲角和旋轉(zhuǎn)角的公式為：

其中，(x_i,y_i,z_i)為機(jī)器人實(shí)際步進(jìn)點(diǎn)位置坐標(biāo)。

7.根據(jù)權(quán)利要求1所述的基于導(dǎo)航路徑約束的柔性機(jī)器人路徑規(guī)劃方法，其特征在于，步驟五具體包括：

將機(jī)器人未介入彎曲單元與推送單元等效為機(jī)器人的平移關(guān)節(jié)，未介入彎曲單元的關(guān)節(jié)參數(shù)始終為0；采用逆向遞推法，將機(jī)器人實(shí)際步進(jìn)點(diǎn)矩陣乘以第n段彎曲單元相對前n-1段彎曲單元的齊次變換矩陣的逆矩陣獲得n-1段其他彎曲單元變換矩陣及實(shí)際步進(jìn)點(diǎn)位置。