本發(fā)明涉及一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解方法，屬于機器人。

背景技術(shù)：

1、串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解屬于機械臂與機器人領(lǐng)域。串聯(lián)復(fù)合關(guān)節(jié)是機械臂工程領(lǐng)域中的一種技術(shù)，它涉及到將多個關(guān)節(jié)或運動單元以串聯(lián)的方式組合在一起，從而實現(xiàn)更為復(fù)雜的運動和更高的靈活性。逆運動學(xué)求解是已知機械臂末端的位置和姿態(tài)，求解各個關(guān)節(jié)角度的過程。串聯(lián)復(fù)合關(guān)節(jié)的精確逆運動學(xué)求解對于實現(xiàn)快速穩(wěn)定控制和路徑規(guī)劃至關(guān)重要。然而，串聯(lián)復(fù)合關(guān)節(jié)具備多個自由度，各參數(shù)之間的轉(zhuǎn)換關(guān)系非線性且較為復(fù)雜，傳統(tǒng)的數(shù)值法和解析法等逆運動學(xué)求解方法，無法直接應(yīng)用于該類結(jié)構(gòu)的逆運動學(xué)求解。因此，針對串聯(lián)復(fù)合關(guān)節(jié)，亟需提出一種通用型的運動學(xué)求解方法。

2、針對機械臂的逆運動學(xué)求解，解析法可以得到封閉形式的解，計算速度快，但不適用于所有類型的機械臂。傳統(tǒng)數(shù)值法通過迭代逼近求解，適用范圍廣，計算效率存在優(yōu)化空間，針對特殊結(jié)構(gòu)，也需要對傳統(tǒng)方法進行相應(yīng)的修改與改進。專利zl?20191054836.6提出了一種機械臂逆運動學(xué)求解方法，通過迭代求解逆運動學(xué)問題。專利zl?201910278057.0針對六自由度機械臂提出了數(shù)值唯一解法，克服了對雅克比矩陣必須滿秩的要求。上述專利僅適用于自由度小于等于六自由度的機械臂，無法對包括復(fù)合關(guān)節(jié)的機械臂進行逆運動學(xué)求解。專利zl?202310437038.4針對一種手術(shù)執(zhí)行器，提出一種將位姿分段法與代數(shù)法相結(jié)合的逆運動學(xué)求解方法，該類型的結(jié)構(gòu)中也包括連續(xù)型等角度彎曲的串聯(lián)復(fù)合關(guān)節(jié)，但該復(fù)合關(guān)節(jié)中僅包括了偏轉(zhuǎn)這一種類型的自由度，未同時考慮俯仰與偏轉(zhuǎn)交替串聯(lián)的復(fù)合關(guān)節(jié)類型。針對串聯(lián)復(fù)合關(guān)節(jié)，尤其是俯仰與偏轉(zhuǎn)交替串聯(lián)的復(fù)合關(guān)節(jié)類型，目前還未構(gòu)建出通用型的運動學(xué)求解方法。

3、綜上，目前雖然存在許多與運動學(xué)相關(guān)的研究，但未存在針對串聯(lián)復(fù)合關(guān)節(jié)的通用型求解方法。因為該串聯(lián)復(fù)合關(guān)節(jié)存在多個自由度，無法寫成矩陣形式，進而無法通過逆矩陣進行逆運動學(xué)求解。

技術(shù)實現(xiàn)思路

1、本發(fā)明解決的技術(shù)問題是：克服現(xiàn)有技術(shù)的不足，提供了一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解方法，構(gòu)建串聯(lián)復(fù)合關(guān)節(jié)的整體轉(zhuǎn)換矩陣，篩選出可表征串聯(lián)復(fù)合關(guān)節(jié)運動學(xué)特性的表征參數(shù)，通過構(gòu)建迭代順序、設(shè)置參數(shù)初始值、求解復(fù)合關(guān)節(jié)轉(zhuǎn)換矩陣數(shù)值，并且針對復(fù)合關(guān)節(jié)參數(shù)提出調(diào)解方法，進而循環(huán)迭代求解或迭代調(diào)解求解出所有自由度數(shù)值，解決了復(fù)合關(guān)節(jié)矩陣維度多、復(fù)雜、無法用于逆運動學(xué)求解的問題。

2、本發(fā)明的技術(shù)解決方案是：一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解方法，包括：

3、構(gòu)建機械臂每個單關(guān)節(jié)與世界坐標(biāo)系之間的轉(zhuǎn)換矩陣；

4、根據(jù)各關(guān)節(jié)對應(yīng)的轉(zhuǎn)換矩陣構(gòu)建機械臂串聯(lián)復(fù)合關(guān)節(jié)整體的轉(zhuǎn)換矩陣，并作為機械臂串聯(lián)復(fù)合關(guān)節(jié)的特征矩陣；

5、根據(jù)機械臂串聯(lián)復(fù)合關(guān)節(jié)特征矩陣中各參數(shù)的映射關(guān)系，篩選串聯(lián)復(fù)合關(guān)節(jié)的表征參數(shù)，根據(jù)所述表征參數(shù)確定串聯(lián)復(fù)合關(guān)節(jié)的運動學(xué)特性；

6、根據(jù)機械臂串聯(lián)復(fù)合關(guān)節(jié)的特征矩陣和運動學(xué)特性構(gòu)建位置向量和姿態(tài)向量與各自由度的映射關(guān)系；

7、構(gòu)建迭代順序與求解方式，對所述位置向量和姿態(tài)向量與各自由度的映射關(guān)系進行迭代求解。

8、進一步地，所述機械臂串聯(lián)復(fù)合關(guān)節(jié)整體的轉(zhuǎn)換矩陣為其中，n為機械臂串聯(lián)復(fù)合關(guān)節(jié)中偏航自由度個數(shù)以及俯仰自由度個數(shù)，為坐標(biāo)系{o5-i}相對于坐標(biāo)系{o5-(i-1)}的齊次變換矩陣，i＝1，2，...，n。

9、進一步地，所述機械臂串聯(lián)復(fù)合關(guān)節(jié)的特征矩陣為其中，q4為串聯(lián)復(fù)合關(guān)節(jié)中的偏航自由度，q5為串聯(lián)復(fù)合關(guān)節(jié)中的俯仰自由度，ajk為轉(zhuǎn)換矩陣中第j行第k列的參數(shù)，j為行數(shù)變量，范圍1～4，k為列數(shù)變量，范圍1～4，為坐標(biāo)系{o5-n}相對于坐標(biāo)系{o3}的齊次變換矩陣。

10、進一步地，所述特征矩陣中第四行參數(shù)為定值，a41＝0、a42＝0、a43＝0、a44＝1。

11、進一步地，所述位置向量和姿態(tài)向量與各自由度的映射關(guān)系為

12、其中，為坐標(biāo)系{o3}相對于坐標(biāo)系{o1}的齊次變換矩陣，為坐標(biāo)系{o3}相對于坐標(biāo)系{o5-n}的齊次變換矩陣，q1為整體平移自由度，q2為偏航自由度，q3為俯仰自由度，q4為復(fù)合關(guān)節(jié)中的偏航自由度，q5為復(fù)合關(guān)節(jié)中的俯仰自由度，q6為扭轉(zhuǎn)自由度，nx為末端連桿坐標(biāo)系的x軸在自然坐標(biāo)系{o0}的x軸方向上的分量，ny為末端連桿坐標(biāo)系的x軸在自然坐標(biāo)系{o0}的y軸方向上的分量，nz為末端連桿坐標(biāo)系的x軸在自然坐標(biāo)系{o0}的z軸方向上的分量，ox為末端連桿坐標(biāo)系的y軸在自然坐標(biāo)系{o0}的x軸方向上的分量，oy為末端連桿坐標(biāo)系的y軸在自然坐標(biāo)系{o0}的y軸方向上的分量，oz為末端連桿坐標(biāo)系的y軸在自然坐標(biāo)系{o0}的z軸方向上的分量，ax為末端連桿坐標(biāo)系的z軸在自然坐標(biāo)系{o0}的x軸方向上的分量，ay為末端連桿坐標(biāo)系的z軸在自然坐標(biāo)系{o0}的y軸方向上的分量，az為末端連桿坐標(biāo)系的z軸在自然坐標(biāo)系{o0}的z軸方向上的分量，px為末端連桿坐標(biāo)系的原點在自然坐標(biāo)系{o0}的x軸方向上的位置坐標(biāo)，py為末端連桿坐標(biāo)系的原點在自然坐標(biāo)系{o0}的y軸方向上的位置坐標(biāo)，pz為末端連桿坐標(biāo)系的原點在自然坐標(biāo)系{o0}的z軸方向上的位置坐標(biāo)，l67＝l6+l7為坐標(biāo)系{o5-n}原點到坐標(biāo)系{o7}原點的距離。

13、進一步地，所述構(gòu)建迭代順序與求解方式包括三種方式，分別為：

14、方式一，根據(jù)a21等式求解q2，根據(jù)a11等式求解q3，根據(jù)a31等式調(diào)節(jié)q5，根據(jù)a22等式求解q6，根據(jù)a12等式調(diào)節(jié)q4，循環(huán)迭代預(yù)設(shè)次數(shù)后，根據(jù)a14等式求解q1；

15、方式二，根據(jù)a21等式求解q2，根據(jù)a11等式求解q3，根據(jù)a24等式求解q1，根據(jù)a22等式求解q6，根據(jù)a12等式調(diào)節(jié)q4，根據(jù)a34等式調(diào)節(jié)q5，循環(huán)迭代預(yù)設(shè)次數(shù)后，根據(jù)a14等式求解q1；

16、方式三，根據(jù)a14等式求解q1，根據(jù)a24等式求解q2，根據(jù)a34等式求解q3，根據(jù)a31等式調(diào)節(jié)q4，根據(jù)a21等式調(diào)節(jié)q5，根據(jù)a22等式求解q6，循環(huán)迭代預(yù)設(shè)次數(shù)后，求解出個自由度的最終解。

17、進一步地，根據(jù)求解出的自由度求得末端位置與姿態(tài)的理論值，根據(jù)末端位姿理論值與期望值之間的誤差小于指定值來決定預(yù)設(shè)次數(shù)。

18、進一步地，所述對所述位置向量和姿態(tài)向量與各自由度的映射關(guān)系進行迭代求解包括：

19、通過求解平移參數(shù)q1；

20、通過(l67nx-px+q1)sinq2+(py-l67ny)cosq2+l2sinq2＝a24求解非復(fù)合關(guān)節(jié)參數(shù)q2，同理求解q3和q6；

21、通過nxcosq2sinq3+nysinq2sinq3+nzcosq3＝a31求解復(fù)合關(guān)節(jié)參數(shù)q4，同理求解q4；

22、其中，l2為坐標(biāo)系{o1}原點與坐標(biāo)系{o2}原點之間的距離，l3為坐標(biāo)系{o2}原點與坐標(biāo)系{o3}原點之間的距離。

23、一種計算機可讀存儲介質(zhì)，所述的計算機可讀存儲介質(zhì)存儲有計算機程序，所述的計算機程序被處理器執(zhí)行時實現(xiàn)所述一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解方法的步驟。

24、一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解設(shè)備，包括存儲器、處理器以及存儲在所述存儲器中并可在所述處理器上運行的計算機程序，所述的處理器執(zhí)行所述的計算機程序時實現(xiàn)所述一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解方法的步驟。

25、本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點在于：

26、(1)本發(fā)明提出一種串聯(lián)復(fù)合關(guān)節(jié)逆運動學(xué)求解方法。針對多個重復(fù)串聯(lián)俯仰自由度與偏航自由度的復(fù)合關(guān)節(jié)，構(gòu)建復(fù)合矩陣，并通過矩陣中的參數(shù)表征串聯(lián)復(fù)合關(guān)節(jié)，通過該參數(shù)可分析出串聯(lián)復(fù)合關(guān)節(jié)的運動學(xué)特性；

27、(2)本發(fā)明通過分析表征參數(shù)，提出一種新型的迭代求解方式，通過幾次循環(huán)迭代求解或迭代調(diào)解求解出所有自由度數(shù)值，解決了復(fù)合關(guān)節(jié)矩陣維度多、復(fù)雜、無法用于逆運動學(xué)求解的問題。

28、(3)本發(fā)明構(gòu)建得到位置向量和姿態(tài)向量與各自由度的映射關(guān)系，通過這個包括3個4×4矩陣組建的公式，可得到16個等式，通過合理運用部分公式，可迭代求解出所有自由度的數(shù)值；

29、(4)本發(fā)明通過規(guī)劃求解順序，加快了求解速度。