本發(fā)明涉及一種基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，屬于智能機(jī)器人的技術(shù)領(lǐng)域。

背景技術(shù)：

四足仿生機(jī)器人被認(rèn)為是最佳的足式機(jī)器人形式，四足步行機(jī)器人比兩足步行仿人機(jī)器人承載能力強(qiáng)、穩(wěn)定性好，具備更加良好的適應(yīng)復(fù)雜路況環(huán)境的能力，同時相比于六足、八足步行機(jī)器人，四足機(jī)器人的電子機(jī)械結(jié)構(gòu)設(shè)計簡單、易于控制，使其在軍事行動、反恐防暴以及工程建設(shè)等多種環(huán)境下具有廣泛的應(yīng)用前景。對四足機(jī)器人本體姿態(tài)進(jìn)行的研究，為機(jī)器人規(guī)劃協(xié)調(diào)層接下來的路徑規(guī)劃、步態(tài)生成、平衡控制等具有重要的工程價值及實際意義。

機(jī)器人位姿標(biāo)定的傳統(tǒng)算法主要有兩種：1.運(yùn)動學(xué)回路法，即在已知機(jī)器人初始位姿的情況下，通過測量裝置獲得機(jī)器人末時刻的位姿，然后通過求解機(jī)器人的運(yùn)動學(xué)方程來獲得機(jī)器人的運(yùn)動參數(shù)；2.軸線測量法，通過將機(jī)器人的關(guān)節(jié)軸線抽象成一條空間中的直線，然后以該軸線為基礎(chǔ)建立機(jī)器人的運(yùn)動學(xué)模型，利用空間幾何關(guān)系解出機(jī)器人運(yùn)動模型中的相關(guān)參數(shù)。

機(jī)器人姿態(tài)估計的一般步驟：首先，建立四足機(jī)器人本體的幾何結(jié)構(gòu)模型，建立載體坐標(biāo)系、地理坐標(biāo)系、平臺坐標(biāo)系以及導(dǎo)航坐標(biāo)系，通過四者之間的關(guān)系，推導(dǎo)出載體坐標(biāo)與導(dǎo)航坐標(biāo)之間的捷聯(lián)轉(zhuǎn)換矩陣。然后，根據(jù)四足機(jī)器人狀態(tài)感知系統(tǒng)協(xié)調(diào)規(guī)劃層的需求，結(jié)合機(jī)器人本體幾何結(jié)構(gòu)模型，對四足機(jī)器人本體姿態(tài)求解，通過四元數(shù)實時計算，得出機(jī)器人本體速度、位置以及姿態(tài)的實時計算模型，從而計算出機(jī)器人本體的速度、位置信息以及機(jī)器人本體航向角、俯仰角、翻滾角的姿態(tài)信息。

傳統(tǒng)位姿估計需要用到加速度計來獲得航向角、俯仰角、翻滾角的角加速度，但是隨著時間的累積，由于加速度計本身的噪聲影響帶來的精度問題，誤差會越來越大，而后續(xù)的累積誤差修正算法則會影響機(jī)器的實時性，增加系統(tǒng)運(yùn)算的時間，影響系統(tǒng)的性能。而采用更高精度的加速度計則會提高機(jī)器人的設(shè)計成本。

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

針對現(xiàn)有技術(shù)的不足，本發(fā)明提供一種基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法。

發(fā)明概述：

四足機(jī)器人Viroscope是一個以液壓為動力驅(qū)動的四足機(jī)器人，本發(fā)明為其在trot對角小跑步態(tài)時對其位姿進(jìn)行實時追蹤并向MCU傳導(dǎo)更新，運(yùn)行環(huán)境涵蓋了除實驗室外的曠野、山地以及夜間黑暗環(huán)境，實驗運(yùn)行軌跡為直線行走。

本發(fā)明的技術(shù)方案為：

一種基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，包括步驟如下：

1)在四足機(jī)器人本體背面配置兩個單目攝像頭，1號攝像頭和2號攝像頭；1號攝像頭指向正前方，2號攝像頭指向側(cè)方，兩個單目攝像頭方向滿足嚴(yán)格正交；機(jī)器人采取雙攝像頭替換了傳統(tǒng)設(shè)計中的高精度陀螺儀和加速度計，節(jié)省了機(jī)器人的生產(chǎn)成本。

2)圖像采集：

將機(jī)器人步態(tài)設(shè)置為對角小跑步態(tài)，運(yùn)動路線設(shè)置為直線行走；運(yùn)動開始時開啟1號攝像頭和2號攝像頭，每個攝像頭每1S內(nèi)拍攝N張圖片；圖像采集不采用錄像的方式，而是使攝像頭每1S內(nèi)拍攝N張圖片的方式，有效減少了圖像的數(shù)量，節(jié)省了運(yùn)算量，改善了系統(tǒng)的實時性。

3)圖像匹配：

將兩個單目攝像頭采集到的第一幅圖像分別設(shè)定為各自的參考幀圖像，對參考幀圖像和當(dāng)前幀圖像進(jìn)行MSER檢驗，選取前C個區(qū)域進(jìn)行區(qū)域標(biāo)定，然后在標(biāo)定的區(qū)域進(jìn)行FAST特征點檢測；在標(biāo)定的區(qū)域進(jìn)行FAST特征點檢測，取代了傳統(tǒng)算法中對整幅圖像進(jìn)行特征點檢測的方法，使得提取出來的特征點更加穩(wěn)定，性能優(yōu)越，而且FAST角點提取速度非?？?，而傳統(tǒng)的特征點提取還要篩去特征點，篩去野點，防止誤匹配。在標(biāo)定區(qū)域進(jìn)行FAST特征點檢測大大節(jié)省了時間，增強(qiáng)了匹配的可靠性，減小了特征點之間的誤匹配概率，提高了對四足機(jī)器人本體運(yùn)動參數(shù)估計的精度。

4)計算四足機(jī)器人運(yùn)動參數(shù)：

機(jī)器人本體運(yùn)動包括水平橫向位移Δx、水平縱向位移Δy、豎直位移Δh、俯仰角θ、翻滾角γ和航向角ψ；仰角θ向上為正值，向下為負(fù)值，定義域為[-90°～+90°]；翻滾角γ右傾為正值，左傾為負(fù)值，定義域為[-180°～+180°]；航向角ψ，定義域為[0°～360°]；其中，航向角ψ由加速度計測得；

統(tǒng)計匹配點，對1號攝像頭采集的圖像用最小二乘法求解上述超定方程得到翻滾角γ，對2號攝像頭采集的圖像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(x₁,y₁)為1號攝像頭采集的參考幀圖像的特征點坐標(biāo)，(x'₁，y'₁)為1號攝像頭采集的當(dāng)前幀圖像的特征點坐標(biāo)；(x₂，y₂)為2號攝像頭采集的參考幀圖像的特征點坐標(biāo)，(x'₂，y'₂)為2號攝像頭采集的當(dāng)前幀圖像的特征點坐標(biāo)；坐標(biāo)系為圖像坐標(biāo)系，x軸為圖像的水平軸，y軸為圖像的豎直軸，坐標(biāo)系原點為圖像左上角第一點；T_x1為1號攝像頭幀間圖像的水平位移，T_y1為1號攝像頭幀間圖像的垂直位移；T_x2為2號攝像頭幀間圖像的水平位移，T_y2為2號攝像頭幀間圖像的垂直位移；

5)分別測量1號攝像頭距離障礙物的距離l₁，2號攝像頭距離障礙物的距離l₂，求解四足機(jī)器人本體運(yùn)動位移；

運(yùn)算上述方程，更新機(jī)器人運(yùn)動估計參數(shù)，其中，(x₀,y₀)為機(jī)器人初始位姿坐標(biāo)。至此，四足機(jī)器人本體位置姿態(tài)信息全部更新完畢；

優(yōu)選的，針對曠野和夜間環(huán)境，選擇捷聯(lián)慣導(dǎo)作為姿態(tài)求解的備選系統(tǒng)；定義四元數(shù)Q表征機(jī)器人當(dāng)前狀態(tài)的姿態(tài)向量：

其中，

其中，φ表示等效角坐標(biāo)系的旋轉(zhuǎn)角度；

實時計算四元數(shù)Q；輸入信號為陀螺儀的數(shù)字輸出信號其中i為x、y或z；b為機(jī)器人軀干坐標(biāo)系，ω_ib表示在機(jī)器人軀干坐標(biāo)系中，四足機(jī)器人饒x,y或z軸的角速度；然后采用二階龍格庫塔法計算：

其中，

K1、K2、Y為中間變量，T為運(yùn)動過程中的更新周期；q(t)為四元數(shù)對時間t時刻的函數(shù)；q(t+T)為四元數(shù)對時間t+T時刻的函數(shù)；ω_i為四足機(jī)器人饒x,y或z軸的角速度；所述陀螺儀設(shè)置在機(jī)器人軀干中。

由此可得歐拉參數(shù)表示的等效旋轉(zhuǎn)矩陣：

根據(jù)機(jī)器人本體旋轉(zhuǎn)軸為一般軸的等效旋轉(zhuǎn)矩陣：

其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；

可得：

其中，r_ab為機(jī)器人本體旋轉(zhuǎn)軸為一般軸的等效旋轉(zhuǎn)矩陣的第a行第b列元素，a＝1,2,3；b＝1,2,3；

由旋轉(zhuǎn)矩陣中可得航向角ψ，俯仰角θ和翻滾角γ；

針對曠野和夜間等視覺不良的環(huán)境，我們通過接連慣導(dǎo)作為四足機(jī)器人姿態(tài)的備選系統(tǒng)對機(jī)器人本體姿態(tài)進(jìn)行求解，避免了極端情況下視覺算法失效帶來的機(jī)器人姿態(tài)追蹤失敗的問題。

由于在某些特殊地形與極端條件下，(比如像曠野與夜間)基于計算機(jī)視覺的位姿跟蹤無法正常運(yùn)行，此時將傳統(tǒng)捷聯(lián)式慣導(dǎo)系統(tǒng)作為位姿檢測備選方案，大大提高了四足機(jī)器人對環(huán)境的適應(yīng)能力和對極端情況的應(yīng)變能力，可以在多種環(huán)境下使用。

優(yōu)選的，N的取值范圍為5～10；C的取值范圍為2～4。

優(yōu)選的，在所述步驟3)和步驟4)之間還包括圖像誤匹配的步驟，具體方法如下：

計算第j副圖像幀與參考幀圖像的匹配分?jǐn)?shù)

其中，S_j(m,n)為參考幀圖像在坐標(biāo)(m,n)處的灰度值，T(m,n)為當(dāng)前幀圖像在坐標(biāo)(m,n)處的灰度值，坐標(biāo)(m,n)所處的坐標(biāo)系為圖像坐標(biāo)系，圖像左上角起點為原點，x軸為圖像的水平軸，y軸為圖像的豎直軸，M是指圖像的橫坐標(biāo)最大索引值，即圖像的寬度，N是指圖像的縱坐標(biāo)的最大索引值，即圖像的長度；

設(shè)定第一閾值M₁和第二閾值M₂，當(dāng)匹配分?jǐn)?shù)低于M₁，則判定圖像匹配失敗，丟棄當(dāng)前幀圖像，如果M₂幅圖像后依舊為圖像匹配失敗狀態(tài)，則選擇當(dāng)前幀圖像作為參考幀圖像。

隨著四足機(jī)器人運(yùn)動過程的推進(jìn)，場景最終發(fā)生大幅度轉(zhuǎn)換，與參考幀圖像公共部分逐漸變小直至消失，這時會導(dǎo)致圖像匹配失??；采取上述方法解決了場景切換帶來的匹配失敗的問題，使得機(jī)器人即使發(fā)生大幅度的轉(zhuǎn)向運(yùn)動該算法依舊可以保持優(yōu)良的性能。

進(jìn)一步優(yōu)選的，M₁的取值范圍為10～20；M₂的取值范圍為3～5。

優(yōu)選的，所述四足機(jī)器人為Viroscope。

本發(fā)明的有益效果為：

1.本發(fā)明所述基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，通過給四足機(jī)器人本體配置兩個攝像頭，可以實時獲取機(jī)器人的翻滾角以及俯仰角，通過圖像匹配可以達(dá)到亞像素級別的精度，優(yōu)化了傳統(tǒng)算法中的精確度問題，同時算法具有較高的魯棒性，對噪聲具有較強(qiáng)的抵抗能力，優(yōu)化了傳統(tǒng)算法帶來的累計誤差問題；

2.本發(fā)明所述基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，舍棄了傳統(tǒng)算法帶來的累積誤差，算法并未涉及陀螺儀與加速度計的使用，無需根據(jù)地磁進(jìn)行誤差補(bǔ)償，免除了傳統(tǒng)捷聯(lián)式慣導(dǎo)的累積誤差和地磁影響；同時在6幀/s的圖像采集標(biāo)準(zhǔn)下，位姿跟蹤精度達(dá)到亞像素精度，比傳統(tǒng)算法具有更加精確的機(jī)器人本體位姿反饋；

3.本發(fā)明所述基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，與傳統(tǒng)算法逆求解運(yùn)動方程相比，耗時更短，速度更快，滿足機(jī)器人運(yùn)動過程中的實時性要求；硬件上，節(jié)省了兩個高精度加速度計，有效節(jié)省了機(jī)器人制造的成本。

附圖說明

圖1為本發(fā)明所述四足機(jī)器人Viroscope的概念圖；

圖2為本發(fā)明所述基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法的流程圖；

圖3為FAST特征點檢測的效果圖；

圖4為對參考幀圖像和當(dāng)前幀圖像進(jìn)行MSER檢驗的效果圖；

圖5為機(jī)器人姿態(tài)解算示意圖；

圖6為機(jī)器人的運(yùn)動軌跡圖；

圖7等效角坐標(biāo)系。

具體實施方式

下面結(jié)合實施例和說明書附圖對本發(fā)明做進(jìn)一步說明，但不限于此。

實施例1

如圖1-2所示。

一種基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，包括步驟如下：

1)在Viroscope四足機(jī)器人本體背面配置兩個單目攝像頭，1號攝像頭和2號攝像頭；1號攝像頭指向正前方，2號攝像頭指向側(cè)方，兩個單目攝像頭方向滿足嚴(yán)格正交；

2)圖像采集：

將機(jī)器人步態(tài)設(shè)置為對角小跑步態(tài)，運(yùn)動路線設(shè)置為直線行走；運(yùn)動開始時開啟1號攝像頭和2號攝像頭，每個攝像頭每1S內(nèi)拍攝5張圖片；

3)圖像匹配：

將兩個單目攝像頭采集到的第一幅圖像分別設(shè)定為各自的參考幀圖像，對參考幀圖像和當(dāng)前幀圖像進(jìn)行MSER檢驗，選取前4個區(qū)域進(jìn)行區(qū)域標(biāo)定，然后在標(biāo)定的區(qū)域進(jìn)行FAST特征點檢測；在標(biāo)定的區(qū)域進(jìn)行FAST特征點檢測，取代了傳統(tǒng)算法中對整幅圖像進(jìn)行特征點檢測的方法，使得提取出來的特征點更加穩(wěn)定，性能優(yōu)越，而且FAST角點提取速度非?？?，而傳統(tǒng)的特征點提取還要篩去特征點，篩去野點，防止誤匹配。在標(biāo)定區(qū)域進(jìn)行FAST特征點檢測大大節(jié)省了時間，增強(qiáng)了匹配的可靠性，減小了特征點之間的誤匹配概率，提高了對四足機(jī)器人本體運(yùn)動參數(shù)估計的精度。如圖3、4所示。

4)計算四足機(jī)器人運(yùn)動參數(shù)：

機(jī)器人本體運(yùn)動包括水平橫向位移Δx、水平縱向位移Δy、豎直位移Δh、俯仰角θ、翻滾角γ和航向角ψ；仰角θ向上為正值，向下為負(fù)值，定義域為[-90°～+90°]；翻滾角γ右傾為正值，左傾為負(fù)值，定義域為[-180°～+180°]；航向角ψ，定義域為[0°～360°]；其中，航向角ψ由加速度計測得；

統(tǒng)計匹配點，對1號攝像頭采集的圖像用最小二乘法求解上述超定方程得到翻滾角γ，對2號攝像頭采集的圖像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(x₁,y₁)為1號攝像頭采集的參考幀圖像的特征點坐標(biāo)，(x'₁，y'₁)為1號攝像頭采集的當(dāng)前幀圖像的特征點坐標(biāo)；(x₂，y₂)為2號攝像頭采集的參考幀圖像的特征點坐標(biāo)，(x'₂，y'₂)為2號攝像頭采集的當(dāng)前幀圖像的特征點坐標(biāo)；坐標(biāo)系為圖像坐標(biāo)系，x軸為圖像的水平軸，y軸為圖像的豎直軸，坐標(biāo)系原點為圖像左上角第一點；T_x1為1號攝像頭幀間圖像的水平位移，T_y1為1號攝像頭幀間圖像的垂直位移；T_x2為2號攝像頭幀間圖像的水平位移，T_y2為2號攝像頭幀間圖像的垂直位移；

5)通過機(jī)器人激光測距模塊分別測量1號攝像頭距離障礙物的距離l₁，2號攝像頭距離障礙物的距離l₂，求解四足機(jī)器人本體運(yùn)動位移；如圖5所示。

運(yùn)算上述方程，更新機(jī)器人運(yùn)動估計參數(shù)，其中，(x₀,y₀)為機(jī)器人初始位姿坐標(biāo)。至此，四足機(jī)器人本體位置姿態(tài)信息全部更新完畢；機(jī)器人本體運(yùn)動軌跡見圖6。

實施例2

如實施例1所述的基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，所不同的是，針對曠野和夜間環(huán)境，選擇捷聯(lián)慣導(dǎo)作為姿態(tài)求解的備選系統(tǒng)；定義四元數(shù)Q表征機(jī)器人當(dāng)前狀態(tài)的姿態(tài)向量：

其中，

其中，φ表示等效角坐標(biāo)系的旋轉(zhuǎn)角度；如圖7所示。

實時計算四元數(shù)Q；輸入信號為陀螺儀的數(shù)字輸出信號其中i為x、y或z；b為機(jī)器人軀干坐標(biāo)系，ω_ib表示在機(jī)器人軀干坐標(biāo)系中，四足機(jī)器人饒x,y或z軸的角速度；然后采用二階龍格庫塔法計算：

其中，

K1、K2、Y為中間變量，T為運(yùn)動過程中的更新周期；q(t)為四元數(shù)對時間t時刻的函數(shù)；q(t+T)為四元數(shù)對時間t+T時刻的函數(shù)；ω_i為四足機(jī)器人饒x,y或z軸的角速度；所述陀螺儀設(shè)置在機(jī)器人軀干中。

由此可得歐拉參數(shù)表示的等效旋轉(zhuǎn)矩陣：

根據(jù)機(jī)器人本體旋轉(zhuǎn)軸為一般軸的等效旋轉(zhuǎn)矩陣：

其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；

可得：

其中，r_ab為機(jī)器人本體旋轉(zhuǎn)軸為一般軸的等效旋轉(zhuǎn)矩陣的第a行第b列元素，a＝1,2,3；b＝1,2,3；

由旋轉(zhuǎn)矩陣中可得航向角ψ，俯仰角θ和翻滾角γ；

θ＝arctan2(r₂₁/cγ，r₁₁/cγ)

。

γ＝arctan2(r₃₂/cγ，r₃₃/cγ)

針對曠野和夜間等視覺不良的環(huán)境，我們通過接連慣導(dǎo)作為四足機(jī)器人姿態(tài)的備選系統(tǒng)對機(jī)器人本體姿態(tài)進(jìn)行求解，避免了極端情況下視覺算法失效帶來的機(jī)器人姿態(tài)追蹤失敗的問題。

由于在某些特殊地形與極端條件下，(比如像曠野與夜間)基于計算機(jī)視覺的位姿跟蹤無法正常運(yùn)行，此時將傳統(tǒng)捷聯(lián)式慣導(dǎo)系統(tǒng)作為位姿檢測備選方案，大大提高了四足機(jī)器人對環(huán)境的適應(yīng)能力和對極端情況的應(yīng)變能力，可以在多種環(huán)境下使用。

實施例3

如實施例1所述的基于計算機(jī)視覺的四足機(jī)器人本體位姿跟蹤方法，所不同的是，在所述步驟3)和步驟4)之間還包括圖像誤匹配的步驟，具體方法如下：

計算第j副圖像幀與參考幀圖像的匹配分?jǐn)?shù)

其中，S_j(m,n)為參考幀圖像在坐標(biāo)(m,n)處的灰度值，T(m,n)為當(dāng)前幀圖像在坐標(biāo)(m,n)處的灰度值，坐標(biāo)(m,n)所處的坐標(biāo)系為圖像坐標(biāo)系，圖像左上角起點為原點，x軸為圖像的水平軸，y軸為圖像的豎直軸，M是指圖像的橫坐標(biāo)最大索引值，即圖像的寬度，N是指圖像的縱坐標(biāo)的最大索引值，即圖像的長度；

設(shè)定第一閾值M₁和第二閾值M₂，M₁＝20，M₂＝5；當(dāng)匹配分?jǐn)?shù)低于M₁，則判定圖像匹配失敗，丟棄當(dāng)前幀圖像，如果M₂幅圖像后依舊為圖像匹配失敗狀態(tài)，則選擇當(dāng)前幀圖像作為參考幀圖像。

隨著四足機(jī)器人運(yùn)動過程的推進(jìn)，場景最終發(fā)生大幅度轉(zhuǎn)換，與參考幀圖像公共部分逐漸變小直至消失，這時會導(dǎo)致圖像匹配失?。徊扇∩鲜龇椒ń鉀Q了場景切換帶來的匹配失敗的問題，使得機(jī)器人即使發(fā)生大幅度的轉(zhuǎn)向運(yùn)動該算法依舊可以保持優(yōu)良的性能。