[發(fā)明專利]一種基于計算機視覺的四足機器人本體位姿跟蹤方法

有效

• 申請?zhí)枺?/td>: 201610979615.2
• 申請日: 2016-11-08
• 公開（公告）號: CN106525049B
• 公開（公告）日: 2019-06-28
• 發(fā)明（設(shè)計）人: 楊明強;鄭慶河;王曉辰;張慶銳
• 申請（專利權(quán)）人: 山東大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 濟南金迪知識產(chǎn)權(quán)代理有限公司 37219
• 代理人: 葉亞林

1.一種基于計算機視覺的四足機器人本體位姿跟蹤方法，其特征在于，包括步驟如下：

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

2)圖像采集：

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

3)圖像匹配：

將兩個單目攝像頭采集到的第一幅圖像分別設(shè)定為各自的參考幀圖像，對參考幀圖像和當前幀圖像進行MSER檢驗，選取前C個區(qū)域進行區(qū)域標定，然后在標定的區(qū)域進行FAST特征點檢測；

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

機器人本體運動包括水平橫向位移Δx、水平縱向位移Δy、豎直位移Δh、俯仰角θ、翻滾角γ和航向角ψ；俯仰角θ向上為正值，向下為負值，定義域為[-90°～+90°]；翻滾角γ右傾為正值，左傾為負值，定義域為[-180°～+180°]；航向角ψ，定義域為[0°～360°]；

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

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

運算上述方程，更新機器人運動估計參數(shù)，其中，(x₀,y₀)為機器人初始位姿坐標。

2.根據(jù)權(quán)利要求1所述的基于計算機視覺的四足機器人本體位姿跟蹤方法，其特征在于，針對曠野和夜間環(huán)境，選擇捷聯(lián)慣導作為姿態(tài)求解的備選系統(tǒng)；定義四元數(shù)Q表征機器人當前狀態(tài)的姿態(tài)向量：

其中，

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

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

其中，

K1、K2、Y為中間變量，T為運動過程中的更新周期；q(t)為四元數(shù)對時間t時刻的函數(shù)；q(t+T)為四元數(shù)對時間t+T時刻的函數(shù)；ω_i為四足機器人饒x,y或z軸的角速度；

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

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

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

可得：

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

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

3.根據(jù)權(quán)利要求1所述的基于計算機視覺的四足機器人本體位姿跟蹤方法，其特征在于，N的取值范圍為5～10；C的取值范圍為2～4。