專利名稱：：一種仿人機器人身體姿態(tài)角度實時估計方法
技術(shù)領(lǐng)域：
：本發(fā)明屬于機器人領(lǐng)域，尤其涉及一種基于六維力/力矩傳感器，加速度計和陀螺儀的仿人機器人姿態(tài)估計方法。
背景技術(shù)：
：仿人機器人是一種模仿人體骨骼結(jié)構(gòu)的機器人，它與其他機器人的最大區(qū)別在于其仿人的雙足運動方式。仿生分析表明，仿人機器人具有環(huán)境適應(yīng)能力強、動作靈活等優(yōu)點，因此，具有更加廣闊的應(yīng)用和發(fā)展前景。與輪式機器人和工業(yè)機器人不同，仿人機器人的支撐域更小，更容易摔倒。因此需要知道機器人的姿態(tài)角度，包括俯仰和滾動角度，以便檢測機器人的狀態(tài)，預(yù)防機器人摔倒。一種比較常用的姿態(tài)估計算法是先對角速度的積分經(jīng)過一個高通濾波器得到角度的高頻信號，再對經(jīng)加速度計算得到的角度經(jīng)過一個低通濾波器，得到姿態(tài)角度的低頻信號，這兩個角度信號的疊加可以得到完整的姿態(tài)角度(參見“K.Loffler,M.Gienger,F.Pfeiffer:SensorandControlDesignofaDynamicallyStableBipedRobot，InternationalConferenceonRoboticsandAutomation,September，2003，，)，但是該方法需要設(shè)定合適的低通濾波器和高通濾波器的參數(shù)，以達(dá)到準(zhǔn)確的估計，而且對于不同的步行模式，需要選擇不同的濾波器參數(shù)，因此該方法的適用范圍較小。由于姿態(tài)估計的非線性，另外一種常見的方法是，對姿態(tài)運動學(xué)模型在平衡點進行線性化處理，得到該運動學(xué)方程的線性模型，利用kalman濾波的方法估計該線性模型的狀態(tài)，從而得至IJ姿態(tài)角度(參見"AnAttitudeControlSystemforsmallunmannedhelicoptersbasedongyrometerandangularrategyroscope，，)。{Β亥方^去又寸于ill離平衡狀態(tài)的估計存在較大的誤差，并且測量的范圍較小。HenrikRehbinder和XiaomingHu提出了姿態(tài)估計的另一種方法(參見"Drift-freeattitudeestimationforacceleratedrigidbodies,Automatica,2004”)。在該方法中，利用重力加速度修正機器人的姿態(tài)角度。但是該方法需要機器人的橫向加速度較小。對于仿人機器人步行中的加速度變化，由于橫向加速度引入的模型誤差并不能忽略，所以該方法并不適用于仿人機器人的步行。中國發(fā)明專利申請200810224579.4提出了一個仿人機器人足部姿態(tài)實時檢測系統(tǒng)及方法，該姿態(tài)檢測方法利用三軸加速度信息計算得到靜態(tài)傾角信息；對采集的角速度信息進行計算得到角度的變化值；根據(jù)所述的靜態(tài)面傾角信息和所述的角度變化值，得到實時的傾角信息。但是由于機器人運動過程中與地面的碰撞，利用加速度計計算所得的靜態(tài)傾角信息并不準(zhǔn)確，而且該方案也沒辦法克服角速度的積分誤差。因此，有必要設(shè)計一種能夠?qū)崟r，快速并且能有效克服陀螺儀積分誤差的仿人機器人姿態(tài)角度估計方案。
發(fā)明內(nèi)容本發(fā)明的目的是針對現(xiàn)有技術(shù)的不足，提供了一種仿人機器人身體姿態(tài)角度實時估計方法。本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的一種仿人機器人身體姿態(tài)角度實時估計方法，仿人機器人姿態(tài)估計系統(tǒng)包括三軸陀螺儀、三軸加速度計、kalman濾波器和兩個六維力/力矩傳感器，其中，三軸陀螺儀、三軸加速度計、和兩個六維力/力矩傳感器分別與kalman濾波器相連；該方法包括以下幾個步驟(1)安裝六維力/力矩傳感器及三軸陀螺儀和三軸加速度計；其中，兩個六維力/力矩傳感器分別安裝在機器人的兩個踝關(guān)節(jié)上，三軸加速度計和三軸陀螺儀安裝在機器人的髖部中心點。(2)建立機器人姿態(tài)的運動學(xué)模型；(3)根據(jù)機器人姿態(tài)的運動學(xué)模型，建立帶噪聲的線性離散隨機系統(tǒng)模型，利用卡爾曼濾波的相應(yīng)方法，基于三軸加速度計的加速度數(shù)據(jù)，三軸陀螺儀的角速度數(shù)據(jù)和六維力/力矩傳感器三維力數(shù)據(jù)估計機器人的姿態(tài)狀態(tài)；(4)根據(jù)估計的機器人姿態(tài)信息，得到機器人實際的姿態(tài)角度；(5)根據(jù)得到的機器人實際的姿態(tài)角度，矯正機器人當(dāng)前的姿態(tài)。本發(fā)明的有益效果是，本發(fā)明仿人機器人身體姿態(tài)角度實時估計方法通過軟測量的技術(shù)實現(xiàn)，根據(jù)機器人姿態(tài)的運動學(xué)方程，利用kalman濾波的方法，得到機器人姿態(tài)狀態(tài)的最優(yōu)估計，從而計算出相應(yīng)的俯仰和滾動角度。本方法克服了角速度積分的漂移以及傳統(tǒng)線性化方法測量范圍較小的缺陷，實現(xiàn)了仿人機器人姿態(tài)實時準(zhǔn)確的估計。圖1是仿人機器人姿態(tài)估計系統(tǒng)的整體框圖；圖2是傳感器的安裝示意圖3是機器人穩(wěn)定行走時身體姿態(tài)的滾動角度數(shù)據(jù)圖；圖4是機器人穩(wěn)定行走時身體姿態(tài)的俯仰角度數(shù)據(jù)圖。具體實施例方式Kalman濾波的原理如下禾Ij用給定的系統(tǒng)輸出觀測序列權(quán)利要求1.一種仿人機器人身體姿態(tài)角度實時估計方法，仿人機器人姿態(tài)估計系統(tǒng)包括三軸陀螺儀、三軸加速度計、kalman濾波器和兩個六維力/力矩傳感器，其中，三軸陀螺儀、三軸加速度計、和兩個六維力/力矩傳感器分別與kalman濾波器相連；其特征在于，該方法包括以下幾個步驟(1)安裝六維力/力矩傳感器及三軸陀螺儀和三軸加速度計；其中，兩個六維力/力矩傳感器分別安裝在機器人的兩個踝關(guān)節(jié)上，三軸加速度計和三軸陀螺儀安裝在機器人的髖部中心點；(2)建立機器人姿態(tài)的運動學(xué)模型；(3)根據(jù)機器人姿態(tài)的運動學(xué)模型，建立帶噪聲的線性離散隨機系統(tǒng)模型，利用卡爾曼濾波的相應(yīng)方法，基于三軸加速度計的加速度數(shù)據(jù)，三軸陀螺儀的角速度數(shù)據(jù)和六維力/力矩傳感器三維力數(shù)據(jù)估計機器人的姿態(tài)狀態(tài)；(4)根據(jù)估計的機器人姿態(tài)信息，得到機器人實際的姿態(tài)角度；(5)根據(jù)得到的機器人實際的姿態(tài)角度，矯正機器人當(dāng)前的姿態(tài)。2.根據(jù)權(quán)利要求1所述仿人機器人身體姿態(tài)角度實時估計方法，其特征在于，所述步驟(2)建立的運動學(xué)模型為3.根據(jù)權(quán)利要求1所述仿人機器人身體姿態(tài)角度實時估計方法，其特征在于，所述步驟(2)建立的線性離散隨機系統(tǒng)模型為4.根據(jù)權(quán)利要求1所述仿人機器人身體姿態(tài)角度實時估計方法，其特征在于，所述步驟(4)得到的機器人實際的姿態(tài)角度為)，X(S)5X{9)分別表示狀態(tài)向量μ-D第7，8，9行的元素。(Ζ-tan-1β——sin其中，it；全文摘要本發(fā)明公開了一種仿人機器人身體姿態(tài)角度實時估計方法，該方法首先建立機器人姿態(tài)的運動學(xué)模型；然后根據(jù)機器人姿態(tài)的運動學(xué)模型，建立帶噪聲的線性離散隨機系統(tǒng)模型，利用卡爾曼濾波的相應(yīng)方法，基于三軸加速度計的加速度數(shù)據(jù)，三軸陀螺儀的角速度數(shù)據(jù)和六維力/力矩傳感器三維力數(shù)據(jù)估計機器人的姿態(tài)狀態(tài)，再根據(jù)估計的機器人姿態(tài)信息，得到機器人實際的姿態(tài)角度，從而矯正機器人當(dāng)前的姿態(tài)；本方法克服了角速度積分的漂移以及傳統(tǒng)線性化方法測量范圍較小的缺陷，實現(xiàn)了仿人機器人姿態(tài)實時準(zhǔn)確的估計。文檔編號G01C21/16GK102121828SQ20101059760公開日2011年7月13日申請日期2010年12月21日優(yōu)先權(quán)日2010年12月21日發(fā)明者熊蓉,相遠(yuǎn)飛申請人:浙江大學(xué)