基于人體運(yùn)動建模的逆運(yùn)動學(xué)算法研究

逆運(yùn)動學(xué)是給定末端執(zhí)行器的目標(biāo)位置和姿態(tài)，求解機(jī)器人各個關(guān)節(jié)的關(guān)節(jié)角。逆運(yùn)動學(xué)在機(jī)器人的研究領(lǐng)域占有重要，它的好壞直接影響到后面的運(yùn)動分析、運(yùn)動控制和軌跡規(guī)劃。因此，設(shè)計性能優(yōu)越的逆運(yùn)動學(xué)求解算法成為仿生機(jī)器人研究中的一個熱點。

針對逆運(yùn)動學(xué)求解問題，國內(nèi)諸多外學(xué)者進(jìn)行了深入研究，并提出了一系列有效的方法。目前逆運(yùn)動學(xué)求解方法主要分為三大類：代數(shù)法、幾何法和數(shù)值法。代數(shù)法可以求解出所有解，但是這種方法計算復(fù)雜，適用于六自由度機(jī)器人求逆解。幾何法需要將機(jī)器人空間分解為平面來求解幾何參數(shù)，適用于機(jī)器人連桿參數(shù)滿足特定值情況下的求解。數(shù)值法可以很好的處理自由度較多的復(fù)雜運(yùn)動鏈，且在關(guān)節(jié)約束條件下，也具有很好的通用性和靈活性。由于人體逆運(yùn)動學(xué)問題較為復(fù)雜，數(shù)值法在反復(fù)迭代后所求得的解準(zhǔn)確度不高，因此需要對數(shù)值法的求解進(jìn)行優(yōu)化。目前常用的數(shù)值法是牛頓法、Newton-Raphson法、粒子濾波法、循環(huán)坐標(biāo)下降法算法(CCD)等。牛頓法當(dāng)矩陣有奇異值時無法求解；Newton-Raphson法在初始點離目標(biāo)點較遠(yuǎn)時無法求解；粒子濾波法計算復(fù)雜度高，而且會出現(xiàn)奇異值問題；傳統(tǒng)CCD算法在離目標(biāo)點較遠(yuǎn)時求解速度很快，而在近目標(biāo)點時求解速度急劇下降。針對上述方法只能求得特定情況下的人體運(yùn)動逆解問題，本文提出了一種更加普遍、快速的逆運(yùn)動求解算法。

本文的核心思想是將傳統(tǒng)CCD算法和BFGS算法結(jié)合，融合它們的優(yōu)點得到一個新的算法，從而達(dá)到快速求解的目的，并進(jìn)行了求解仿真，驗證了所提算法的可行性。

1  人體運(yùn)動模型的建立

1.1  運(yùn)動數(shù)據(jù)獲取

本文用NOKOV三維紅外被動式光學(xué)運(yùn)動捕捉系統(tǒng)來實時捕捉人體的下肢運(yùn)動數(shù)據(jù)，所用動作捕捉系統(tǒng)主要由6個相機(jī)、上位機(jī)、mark點、定位桿、校正桿等組成，每個mark點只要同時被兩個鏡頭捕捉到，便能記錄下mark點的坐標(biāo)，進(jìn)而得到特定點隨時間變化的連續(xù)運(yùn)動軌跡。運(yùn)動捕捉系統(tǒng)以每秒100幀的速率記錄所有標(biāo)記點的空間坐標(biāo)，并且其捕捉精度可達(dá)1mm。動作捕捉系統(tǒng)中mark點位置和相機(jī)的布置如圖1所示。

（a）mark點位置                  （b）相機(jī)布置

圖1  mark點位置和相機(jī)布置

1.2  建立關(guān)節(jié)模型 

人體下肢是一個極其復(fù)雜的系統(tǒng)，為了便于研究一般將下肢分為髖關(guān)節(jié)、膝關(guān)節(jié)、踝關(guān)節(jié)、大腿和小腿五部分。這樣就可以把關(guān)節(jié)看做為點，大腿和小腿看做是鏈，如圖2所示。每個關(guān)節(jié)都固定一個局部坐標(biāo)系，根據(jù)歐拉角的表示方法，從子關(guān)節(jié)局部坐標(biāo)系旋轉(zhuǎn)變換到父關(guān)節(jié)局部坐標(biāo)系的旋轉(zhuǎn)矩陣可以表示為

式中分別表示繞軸，軸，軸的旋轉(zhuǎn)角度。用表示子關(guān)節(jié)相對于父關(guān)節(jié)在軸，軸，軸的平移。則子關(guān)節(jié)相對于父關(guān)節(jié)的變換矩陣可以表示為

設(shè)子關(guān)節(jié)在局部坐標(biāo)系的坐標(biāo)表示為，子關(guān)節(jié)在父關(guān)節(jié)坐標(biāo)系中的坐標(biāo)表示為，則有

局部坐標(biāo)系之間的變換矩陣可以表示為：，這樣人體的運(yùn)動學(xué)問題轉(zhuǎn)化為關(guān)節(jié)模型的運(yùn)動方程求解問題。

圖2  人體下肢關(guān)節(jié)模型

2  逆運(yùn)動學(xué)求解算法的研究

2.1  循環(huán)坐標(biāo)下降（CCD）算法

循環(huán)坐標(biāo)下降（CCD）算法是一種啟發(fā)式迭代方法，該方法根據(jù)運(yùn)動鏈末端位置信息，依次調(diào)整各個關(guān)節(jié)的旋轉(zhuǎn)角度，逐步使末端位置逼近目標(biāo)位置。該算法同時考慮了各個關(guān)節(jié)的約束條件，對于大范圍運(yùn)動收斂快速、穩(wěn)定，因此在機(jī)器人運(yùn)動研究領(lǐng)域得到了廣泛應(yīng)用。

算法的基本思想如下：

假設(shè)當(dāng)前位置的世界坐標(biāo)和姿態(tài)分別為：和，其中為各個旋轉(zhuǎn)角度組成的向量。設(shè)要達(dá)到的末端位置和姿態(tài)為：和，這樣問題就轉(zhuǎn)化為找到一個合適的向量，使當(dāng)前位姿到達(dá)目標(biāo)位姿，即，。

定義位置誤差為：

定義姿態(tài)誤差為：

總的誤差為：

從而轉(zhuǎn)化為求合適的向量，使得總的誤差最小。關(guān)節(jié)的優(yōu)化目標(biāo)函數(shù)為：

設(shè)和為加權(quán)因子，取值為任意正實數(shù)，，，為當(dāng)前坐標(biāo)原點的位置矢量，為第個關(guān)節(jié)相對于基坐標(biāo)的單位向量，則常系數(shù)和可以表示為：

取得極大值的條件為

由，結(jié)合上式可以求出關(guān)節(jié)的最優(yōu)調(diào)整值，從而獲得合適的向量，并得到各個關(guān)節(jié)的旋轉(zhuǎn)角度，由于每個關(guān)節(jié)有一定的關(guān)節(jié)約束，即關(guān)節(jié)旋轉(zhuǎn)范圍，如果，則旋轉(zhuǎn)角度??；如果，則旋轉(zhuǎn)角度為；如果，則旋轉(zhuǎn)角度取。這樣得到的旋轉(zhuǎn)角度可以滿足實際要求，由新的旋轉(zhuǎn)角度得到新的人體運(yùn)動模型。

2.2  BFGS算法

BFGS算法是一種應(yīng)用最為廣泛的擬牛頓算法，不需要計算二階導(dǎo)數(shù)，而且保存了牛頓算法的超線性收斂性。本文將BFGS算法融入到循環(huán)坐標(biāo)下降算法中，BFGS算法的校正公式為：

BFGS算法應(yīng)用二階導(dǎo)數(shù)信息具有更快的收斂速度，接近目標(biāo)點時收斂速度很快，而當(dāng)目標(biāo)點較遠(yuǎn)時則速度較慢。本文將2種算法的優(yōu)點結(jié)合起來。首先利用循環(huán)坐標(biāo)下降算法進(jìn)行大范圍的移動，在目標(biāo)點小范圍內(nèi)采用BFGS算法。

3  實驗流程及結(jié)果分析

本文所用實驗的具體流程如下：

Step1：輸入人體下肢運(yùn)動的骨架模型和人體運(yùn)動的初始值，設(shè)定人體運(yùn)動的目標(biāo)位置和局部坐標(biāo)系、迭代停止閾值和轉(zhuǎn)向BFGS算法的閾值；

Step2：計算出人體各個關(guān)節(jié)的世界坐標(biāo)，運(yùn)用式計算出總的誤差，如果總誤差小于閾值，轉(zhuǎn)到Step5,否則執(zhí)行下一步，直到總誤差小于閾值，轉(zhuǎn)到Step4,執(zhí)行BFGS算法；

Step3：對當(dāng)前模型運(yùn)用改進(jìn)循環(huán)坐標(biāo)下降算法，更新人體運(yùn)動數(shù)據(jù)，轉(zhuǎn)到Step2；

Step4：對當(dāng)前模型執(zhí)行BFGS算法，直到誤差小于設(shè)置閾值，轉(zhuǎn)到下一步；

Step5：繪制人體運(yùn)動圖形。

為了驗證算法的有效性，設(shè)目標(biāo)點位姿為：=[161.5614,965.4743,－5.0585],=[0.0990,－0.9951,0]，=[－0.9951,0.0990,0]，=[0,0,1]，一個精確解。圖3為傳統(tǒng)CCD算法和CCD-BFGS算法平均執(zhí)行時間的對比，從圖中可以看出本文提出的CCD-BFGS算法明顯優(yōu)于傳統(tǒng)CCD算法。

圖3  傳統(tǒng)算法與CCD-BFGS算法對比

4  結(jié)語

針對人體運(yùn)動建模中逆運(yùn)動學(xué)求解問題，提出一種CCD算法和BFGS算法結(jié)合的新的算法，并通過仿真驗證了算法的性能。仿真結(jié)果表明，CCD-BFGS算法不僅克服了傳統(tǒng)CCD算法近目標(biāo)點收斂慢的缺點，而且具有較強(qiáng)的魯棒性。本算法思想還可以應(yīng)用于運(yùn)動壓縮和合成等方向，同時在推動仿生機(jī)器人的運(yùn)動仿真研究有一定的借鑒意義。

本文來源：《企業(yè)科技與發(fā)展》：http://xwlcp.cn/w/qk/21223.html