本發(fā)明涉及機器人控制技術領域，特別涉及一種高自由度機器人逆運動學的快速求解方法及系統(tǒng)。

背景技術：

機器人技術不僅可以應用于工業(yè)生產(chǎn)，而且可以服務于民眾生活，是一項很有應用前景的技術。機器人一般是由很多關節(jié)組成，通過控制每個關節(jié)變量，達到位姿變化的功能，例如移動、行走和抓取等等。在機器人學中，每個關節(jié)各提供一個自由度。通常來講，機器人的自由度越多(關節(jié)越多)，機器人功能就越強大，移動就越靈活。機器人運動學是機器人運動控制的基礎，包含正運動學和逆運動學。正運動學，即給定每個關節(jié)變量θ，求解機器人的位姿P；逆運動學，即給定機器人的位姿P，求解機器人各個關節(jié)的關節(jié)變量θ，如圖1所示。正運動學可以通過運動學方程求解，求解過程相對簡單，相反，逆運動學求解復雜、耗時，對于高自由度機器人情況更糟。目前，機器人逆運動學求解主要采用：解析法、數(shù)值法和機器學習法。

解析法，通過構(gòu)建逆運動學方程，可以很容易求解逆運動學問題。但是，對于任意機器人來講，構(gòu)建逆運動學方程非常復雜，并且很多情況是不存在逆運動學方程的。因此，解析法只能應用特定的機器人或機械臂中，并且機器人或機械臂的自由度只能很少。

數(shù)值法，一般需要通過多次迭代，尋找一個滿足一定精度要求的近似解。其中，運用最多的是基于雅克比的方法，相比其他數(shù)值方法，該方法更加精確、穩(wěn)定?；谘趴吮鹊那蠼夥椒ㄓ职瑑深悾貉趴吮葌文娣ê脱趴吮绒D(zhuǎn)置法。雅克比偽逆法收斂快，但是需要進行奇異值分解運算，復雜耗時，難并行；相反，雅克比轉(zhuǎn)置法需要很多次迭代，但是，每次迭代運算簡單，快速。

機器學習法，利用機器學習的方法對逆運動學方程進行近似，從而在有限時間內(nèi)得到近似解。但是，該方法最大的問題是近似解與精確解的偏差較大，遠大于數(shù)值法得到的近似解。同時，該方法需要大量數(shù)據(jù)進行訓練。

目前，最常用的逆運動學求解方法是基于雅克比的求解方法，但是對高自由度機器人來講，現(xiàn)有的基于雅克比的方法非常耗時，不能滿足機器人控制的實時性要求，為此，本發(fā)明提出一種適用于并行體系結(jié)構(gòu)中執(zhí)行的快速求解高自由度機器人逆運動學問題的方法。

原始雅克比轉(zhuǎn)置法在每次迭代過程中，首先生成一個參數(shù)值α，然后依據(jù)該參數(shù)更新關節(jié)變量θ，見附圖2，發(fā)明人發(fā)現(xiàn)該參考值α的選取對于求解速度有嚴重影響，為此提出一種并行投機的選取方法。該方法在每次迭代時，生成多個參數(shù)值(投機值)α1,α2,...αm，依據(jù)這些不同的參數(shù)值可以得到多個關節(jié)變量更新值，然后從中選出最接近目標解的參數(shù)值及關節(jié)變量更新值，由于多個參數(shù)值的計算及后續(xù)的關節(jié)變量更新值計算之間沒有依賴，可以通過并行結(jié)構(gòu)同時執(zhí)行，從而加速求解速度。

技術實現(xiàn)要素：

針對現(xiàn)有技術的不足，本發(fā)明提出一種高自由度機器人逆運動學的快速求解方法及系統(tǒng)。

本發(fā)明提出一種高自由度機器人逆運動學的快速求解方法，包括：

步驟1，將關節(jié)變量θ帶入機器人運動學方程中，獲得雅克比矩陣J，將所述雅克比矩陣J進行轉(zhuǎn)置，獲得雅克比轉(zhuǎn)置矩陣J^T；

步驟2，生成一組投機值，為每個投機值計算相應的關節(jié)變量更新值，將每個關節(jié)變量更新值帶入機器人正運動學方程中，獲得相應的位姿P_k，為每個位姿P_k計算其與目標位姿P的位姿偏差Δe_k，及位姿偏差Δe_k的模error_k；

步驟3，在模error_k的集合中選取最小值error_min，及其對應的位姿偏差Δe_min與關節(jié)變量更新值Δθ_min，并更新位姿偏差為Δe＝Δe_min，更新關節(jié)變量θ＝θ+Δθ_min；

步驟4，判斷error_min是否滿足error_min<Threshold，其中Threshold為預設的error_min閾值，如果是，則輸出關節(jié)變量θ并結(jié)束，否則，返至所述步驟1，繼續(xù)執(zhí)行。

所述步驟1之前還包括隨機生成一組初始值θ_init，并且令θ＝θ_init；

將關節(jié)變量θ帶入機器人正運動學方程中，獲得相應的位姿P_init；

計算位姿P_init與目標位姿P的位姿偏差Δe＝P-P_k，及位姿偏差Δe的模error；

判斷模error是否滿足error<Threshold，如果是，則輸出關節(jié)變量θ并結(jié)束，否則，執(zhí)行所述步驟1。

所述步驟2中每個投機值大于0，小于1。

所述步驟2中為每個投機值計算相應的關節(jié)變量更新值的公式為：

Δθ_k＝α_kJ^TΔe

其中Δθ_k為關節(jié)變量更新值，α_k為投機值，J^T為雅克比轉(zhuǎn)置矩陣，Δe為位姿偏差。

所述開啟m個計算線程，每個線程生成一個投機值，其中第k個線程生成投機值α_k，計算公式為：

其中α_k為投機值，Δe為位姿偏差，J^T為雅克比轉(zhuǎn)置矩陣，雅克比矩陣J。

本發(fā)明還提出一種高自由度機器人逆運動學的快速求解系統(tǒng)，包括：

獲得雅克比轉(zhuǎn)置矩陣模塊，用于將關節(jié)變量θ帶入機器人運動學方程中，獲得雅克比矩陣J，將所述雅克比矩陣J進行轉(zhuǎn)置，獲得雅克比轉(zhuǎn)置矩陣J^T；

獲得位姿偏差模塊，用于生成一組投機值，為每個投機值計算相應的關節(jié)變量更新值，將每個關節(jié)變量更新值帶入機器人正運動學方程中，獲得相應的位姿P_k，為每個位姿P_k計算其與目標位姿P的位姿偏差Δe_k，及位姿偏差Δe_k的模error_k；

更新關節(jié)變量模塊，用于在模error_k的集合中選取最小值error_min，及其對應的位姿偏差Δe_min與關節(jié)變量更新值Δθ_min，并更新位姿偏差為Δe＝Δe_min，更新關節(jié)變量θ＝θ+Δθ_min；

判斷模塊，用于判斷error_min是否滿足error_min<Threshold，其中Threshold為預設的error_min閾值，如果是，則輸出關節(jié)變量θ并結(jié)束，否則，返至所述獲得雅克比轉(zhuǎn)置矩陣模塊，繼續(xù)執(zhí)行。

所述獲得雅克比轉(zhuǎn)置矩陣模塊之前還包括隨機生成一組初始值θ_init，并且令θ＝θ_init；

將關節(jié)變量θ帶入機器人正運動學方程中，獲得相應的位姿P_init；

計算位姿P_init與目標位姿P的位姿偏差Δe＝P-P_k，及位姿偏差Δe的模error；

判斷模error是否滿足error<Threshold，如果是，則輸出關節(jié)變量θ并結(jié)束，否則，執(zhí)行所述步驟1。

所述獲得位姿偏差模塊中每個投機值大于0，小于1。

所述獲得位姿偏差模塊中為每個投機值計算相應的關節(jié)變量更新值的公式為：

Δθ_k＝α_kJ^TΔe

其中Δθ_k為關節(jié)變量更新值，α_k為投機值，J^T為雅克比轉(zhuǎn)置矩陣，Δe為位姿偏差。

所述開啟m個計算線程，每個線程生成一個投機值，其中第k個線程生成投機值α_k，計算公式為：

其中α_k為投機值，Δe為位姿偏差，J^T為雅克比轉(zhuǎn)置矩陣，雅克比矩陣J。由以上方案可知，本發(fā)明的優(yōu)點在于：

本發(fā)明易并行化：本發(fā)明提出的快速求解方法改進了雅克比轉(zhuǎn)置法，使其適合在并行體系結(jié)構(gòu)中執(zhí)行，例如多核處理器、圖像處理器等；

高實時性：通過在并行體系結(jié)構(gòu)中執(zhí)行該算法，可以有效加速逆運動學求解，從而使得該方法可以在很短時間內(nèi)得到滿意結(jié)果。

附圖說明

圖1是逆運動學求解示意圖；

圖2是原始雅克比轉(zhuǎn)置法流程圖；

圖3是逆運動學的快速求解方法流程圖；

圖4是高自由度機器人示意圖。

具體實施方式

以下為本發(fā)明的整體流程，如圖3所示，本發(fā)明方法包括：

步驟一，隨機生成一組初始值θ_init，并且令θ＝θ_init；

步驟二，將關節(jié)變量θ帶入機器人正運動學方程中，求出相應的位姿P_init＝f(θ)；

步驟三，計算位姿P_init與目標位姿P的偏差Δe＝P-P_k，及偏差Δe的模error；

步驟四，判斷error是否滿足精度要求，即error<Threshold，如果是，輸出關節(jié)變量θ并結(jié)束，否則，繼續(xù)執(zhí)行步驟五。

步驟五，將關節(jié)變量θ帶入機器人運動學方程中，求出雅克比矩陣J；

步驟六，將雅克比矩陣轉(zhuǎn)置，獲得雅克比轉(zhuǎn)置矩陣J^T；

步驟七，生成一組投機值α₁,α₂,α₃,...α_m，且每個投機值大于0，小于1；

步驟八，為每個投機值α_k，計算相應的關節(jié)變量更新值Δθ_k＝α_kJ^TΔe；

步驟九，將每個關節(jié)變量更新值Δθ_k帶入機器人正運動學方程中，求出相應的位姿P_k＝f(θ+Δθ_k)；

步驟十，為每個位姿P_k計算其與目標位姿P的位姿偏差Δe_k＝P-P_k，及位姿偏差Δe_k的模error_k；

步驟十一，在error₁,error₂,...error_m中選取最小值error_min，及其對應的位姿偏差Δe_min和關節(jié)變量更新值Δθ_min，并更新位姿偏差Δe＝Δe_min，更新關節(jié)變量θ＝θ+Δθ_min；

步驟十二，判斷error_min是否滿足精度要求，即error_min<Threshold，如果是，輸出關節(jié)變量θ并結(jié)束，否則，返至步驟五，繼續(xù)執(zhí)行。

以下為本發(fā)明的一種高效的并行投機機制，附圖3所示：

步驟一，開啟m個計算線程；

步驟二，每個線程生成一個投機值，其中第k個線程生成投機值α_k，計算方式如下：

步驟三，每個線程根據(jù)自身生成的投機值α_k，進行位姿計算P_k＝f(θ+Δθ_k)和位姿偏差計算Δe_k＝P-P_k；

步驟四，收集所有線程計算所得的位姿偏差Δe₁，Δe₂,…Δe_m，選取其中模最小的位姿偏差Δe_min及其對應的投機值α_min；

步驟五，更新關節(jié)變量θ＝θ+α_kJ^TΔe和位姿偏差Δe＝Δe_min。

下面將結(jié)合附圖對本發(fā)明進行詳細說明。

本發(fā)明的應用環(huán)境為高自由度機器人。在該機器人中具有很多關節(jié)，同時裝備可以并行計算的處理器，例如多核處理器(多核CPU)、圖像處理器(GPU)或者定制FPGA等。圖4顯示了一個具有20個關節(jié)(自由度)的機器人，同時配備一個圖像處理器GPU用于執(zhí)行逆運動學求解算法。

圖4中，給定抓手的空間位置P(x_o,y_o,z_o)，然后求解各個關節(jié)的角度θ(θ₁,θ₂,...θ₂₀)。

步驟一，處理器GPU隨機生成20個數(shù)值組成一組初始值θ_init，并且令θ＝θ_init；

步驟二，處理器GPU將關節(jié)變量θ帶入機器人運動學方程中，求出相應的位姿P_init＝f(θ)；

步驟三，處理器GPU計算初始位姿P_init(x_init,y_init,z_init)與目標位姿P(x_o,y_o,z_o)的偏差Δe＝P-P_k＝(x_o-x_init,y_o-y_init,z_o-z_init)及偏差Δe的模error；

步驟四，判斷error是否滿足精度要求，即error<Threshold，如果是，處理器GPI輸出關節(jié)變量θ并結(jié)束，否則，執(zhí)行步驟五。

步驟五，將關節(jié)變量θ帶入機器人運動學方程中，求出雅克比矩陣J；

步驟六，將雅克比矩陣轉(zhuǎn)置，獲得雅克比轉(zhuǎn)置矩陣J^T；

步驟七，生成m個投機值α₁,α₂,α₃,...α_m，且每個投機值大于0，小于1；

步驟八，處理器GPU為每個投機值α_k分配一個計算線程，每個線程負責計算相應的關節(jié)變量更新值Δθ_k＝α_kJ^TΔe；

步驟九，將每個關節(jié)變量更新值Δθ_k帶入機器人正運動學方程中，求出相應的位姿P_k＝f(θ+Δθ_k)；

步驟十，為每個位姿P_k計算其與目標位姿P的位姿偏差Δe_k＝P-P_k，及位姿偏差Δe_k的模error_k；

步驟十一，在error₁,error₂,...error_m中選取最小值error_min，及其對應的位姿偏差Δe_min和關節(jié)變量更新值Δθ_min，并更新位姿偏差Δe＝Δe_min，更新關節(jié)變量θ＝θ+Δθ_min；

步驟十二，判斷error_min是否滿足精度要求，即error_min<Threshold，如果是，輸出關節(jié)變量θ并結(jié)束，否則，返至步驟五，繼續(xù)執(zhí)行。

本發(fā)明還提出一種高自由度機器人逆運動學的快速求解系統(tǒng)，包括：

獲得雅克比轉(zhuǎn)置矩陣模塊，用于將關節(jié)變量θ帶入機器人運動學方程中，獲得雅克比矩陣J，將所述雅克比矩陣J進行轉(zhuǎn)置，獲得雅克比轉(zhuǎn)置矩陣J^T；

獲得位姿偏差模塊，用于生成一組投機值，為每個投機值計算相應的關節(jié)變量更新值，將每個關節(jié)變量更新值帶入機器人正運動學方程中，獲得相應的位姿P_k，為每個位姿P_k計算其與目標位姿P的位姿偏差Δe_k，及位姿偏差Δe_k的模error_k；

更新關節(jié)變量模塊，用于在模error_k的集合中選取最小值error_min，及其對應的位姿偏差Δe_min與關節(jié)變量更新值Δθ_min，并更新位姿偏差為Δe＝Δe_min，更新關節(jié)變量θ＝θ+Δθ_min；

判斷模塊，用于判斷error_min是否滿足error_min<Threshold(其中Threshold為預設的error_min閾值，根據(jù)設計要求可以為0.1,0.05等數(shù)值)，如果是，則輸出關節(jié)變量θ并結(jié)束，否則，返至所述獲得雅克比轉(zhuǎn)置矩陣模塊，繼續(xù)執(zhí)行。

所述獲得雅克比轉(zhuǎn)置矩陣模塊之前還包括隨機生成一組初始值θ_init，并且令θ＝θ_init；

將關節(jié)變量θ帶入機器人正運動學方程中，獲得相應的位姿P_init；

計算位姿P_init與目標位姿P的位姿偏差Δe＝P-P_k，及位姿偏差Δe的模error；

判斷模error是否滿足error<Threshold，如果是，則輸出關節(jié)變量θ并結(jié)束，否則，執(zhí)行所述步驟1。

所述獲得位姿偏差模塊中每個投機值大于0，小于1。

所述獲得位姿偏差模塊中為每個投機值計算相應的關節(jié)變量更新值的公式為：

Δθ_k＝α_kJ^TΔe

其中Δθ_k為關節(jié)變量更新值，α_k為投機值，J^T為雅克比轉(zhuǎn)置矩陣，Δe為位姿偏差。

所述開啟m個計算線程，每個線程生成一個投機值，其中第k個線程生成投機值α_k，計算公式為：

其中α_k為投機值，Δe為位姿偏差，J^T為雅克比轉(zhuǎn)置矩陣，雅克比矩陣J。