一種基於計算機視覺的四足機器人本體位姿跟蹤方法與流程
2023-05-22 01:46:16 2
本發明涉及一種基於計算機視覺的四足機器人本體位姿跟蹤方法，屬於智慧機器人的技術領域。
背景技術：
四足仿生機器人被認為是最佳的足式機器人形式，四足步行機器人比兩足步行仿人機器人承載能力強、穩定性好，具備更加良好的適應複雜路況環境的能力，同時相比於六足、八足步行機器人，四足機器人的電子機械結構設計簡單、易於控制，使其在軍事行動、反恐防暴以及工程建設等多種環境下具有廣泛的應用前景。對四足機器人本體姿態進行的研究，為機器人規劃協調層接下來的路徑規劃、步態生成、平衡控制等具有重要的工程價值及實際意義。
機器人位姿標定的傳統算法主要有兩種：1.運動學迴路法，即在已知機器人初始位姿的情況下，通過測量裝置獲得機器人末時刻的位姿，然後通過求解機器人的運動學方程來獲得機器人的運動參數；2.軸線測量法，通過將機器人的關節軸線抽象成一條空間中的直線，然後以該軸線為基礎建立機器人的運動學模型，利用空間幾何關係解出機器人運動模型中的相關參數。
機器人姿態估計的一般步驟：首先，建立四足機器人本體的幾何結構模型，建立載體坐標系、地理坐標系、平臺坐標系以及導航坐標系，通過四者之間的關係，推導出載體坐標與導航坐標之間的捷聯轉換矩陣。然後，根據四足機器人狀態感知系統協調規劃層的需求，結合機器人本體幾何結構模型，對四足機器人本體姿態求解，通過四元數實時計算，得出機器人本體速度、位置以及姿態的實時計算模型，從而計算出機器人本體的速度、位置信息以及機器人本體航向角、俯仰角、翻滾角的姿態信息。
傳統位姿估計需要用到加速度計來獲得航向角、俯仰角、翻滾角的角加速度，但是隨著時間的累積，由於加速度計本身的噪聲影響帶來的精度問題，誤差會越來越大，而後續的累積誤差修正算法則會影響機器的實時性，增加系統運算的時間，影響系統的性能。而採用更高精度的加速度計則會提高機器人的設計成本。
技術實現要素：
針對現有技術的不足，本發明提供一種基於計算機視覺的四足機器人本體位姿跟蹤方法。
發明概述：
四足機器人Viroscope是一個以液壓為動力驅動的四足機器人，本發明為其在trot對角小跑步態時對其位姿進行實時追蹤並向MCU傳導更新，運行環境涵蓋了除實驗室外的曠野、山地以及夜間黑暗環境，實驗運行軌跡為直線行走。
本發明的技術方案為：
一種基於計算機視覺的四足機器人本體位姿跟蹤方法，包括步驟如下：
1)在四足機器人本體背面配置兩個單目攝像頭，1號攝像頭和2號攝像頭；1號攝像頭指向正前方，2號攝像頭指向側方，兩個單目攝像頭方向滿足嚴格正交；機器人採取雙攝像頭替換了傳統設計中的高精度陀螺儀和加速度計，節省了機器人的生產成本。
2)圖像採集：
將機器人步態設置為對角小跑步態，運動路線設置為直線行走；運動開始時開啟1號攝像頭和2號攝像頭，每個攝像頭每1S內拍攝N張圖片；圖像採集不採用錄像的方式，而是使攝像頭每1S內拍攝N張圖片的方式，有效減少了圖像的數量，節省了運算量，改善了系統的實時性。
3)圖像匹配：
將兩個單目攝像頭採集到的第一幅圖像分別設定為各自的參考幀圖像，對參考幀圖像和當前幀圖像進行MSER檢驗，選取前C個區域進行區域標定，然後在標定的區域進行FAST特徵點檢測；在標定的區域進行FAST特徵點檢測，取代了傳統算法中對整幅圖像進行特徵點檢測的方法，使得提取出來的特徵點更加穩定，性能優越，而且FAST角點提取速度非常快，而傳統的特徵點提取還要篩去特徵點，篩去野點，防止誤匹配。在標定區域進行FAST特徵點檢測大大節省了時間，增強了匹配的可靠性，減小了特徵點之間的誤匹配概率，提高了對四足機器人本體運動參數估計的精度。
4)計算四足機器人運動參數：
機器人本體運動包括水平橫向位移Δx、水平縱向位移Δy、豎直位移Δh、俯仰角θ、翻滾角γ和航向角ψ；仰角θ向上為正值，向下為負值，定義域為[-90°～+90°]；翻滾角γ右傾為正值，左傾為負值，定義域為[-180°～+180°]；航向角ψ，定義域為[0°～360°]；其中，航向角ψ由加速度計測得；
統計匹配點，對1號攝像頭採集的圖像用最小二乘法求解上述超定方程得到翻滾角γ，對2號攝像頭採集的圖像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(x1,y1)為1號攝像頭採集的參考幀圖像的特徵點坐標，(x'1，y'1)為1號攝像頭採集的當前幀圖像的特徵點坐標；(x2，y2)為2號攝像頭採集的參考幀圖像的特徵點坐標，(x'2，y'2)為2號攝像頭採集的當前幀圖像的特徵點坐標；坐標係為圖像坐標系，x軸為圖像的水平軸，y軸為圖像的豎直軸，坐標系原點為圖像左上角第一點；Tx1為1號攝像頭幀間圖像的水平位移，Ty1為1號攝像頭幀間圖像的垂直位移；Tx2為2號攝像頭幀間圖像的水平位移，Ty2為2號攝像頭幀間圖像的垂直位移；
5)分別測量1號攝像頭距離障礙物的距離l1，2號攝像頭距離障礙物的距離l2，求解四足機器人本體運動位移；
運算上述方程，更新機器人運動估計參數，其中，(x0,y0)為機器人初始位姿坐標。至此，四足機器人本體位置姿態信息全部更新完畢；
優選的，針對曠野和夜間環境，選擇捷聯慣導作為姿態求解的備選系統；定義四元數Q表徵機器人當前狀態的姿態向量：
其中，
其中，φ表示等效角坐標系的旋轉角度；
實時計算四元數Q；輸入信號為陀螺儀的數字輸出信號其中i為x、y或z；b為機器人軀幹坐標系，ωib表示在機器人軀幹坐標系中，四足機器人饒x,y或z軸的角速度；然後採用二階龍格庫塔法計算：
其中，
K1、K2、Y為中間變量，T為運動過程中的更新周期；q(t)為四元數對時間t時刻的函數；q(t+T)為四元數對時間t+T時刻的函數；ωi為四足機器人饒x,y或z軸的角速度；所述陀螺儀設置在機器人軀幹中。
由此可得歐拉參數表示的等效旋轉矩陣：
根據機器人本體旋轉軸為一般軸的等效旋轉矩陣：
其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；
可得：
其中，rab為機器人本體旋轉軸為一般軸的等效旋轉矩陣的第a行第b列元素，a＝1,2,3；b＝1,2,3；
由旋轉矩陣中可得航向角ψ，俯仰角θ和翻滾角γ；
針對曠野和夜間等視覺不良的環境，我們通過接連慣導作為四足機器人姿態的備選系統對機器人本體姿態進行求解，避免了極端情況下視覺算法失效帶來的機器人姿態追蹤失敗的問題。
由於在某些特殊地形與極端條件下，(比如像曠野與夜間)基於計算機視覺的位姿跟蹤無法正常運行，此時將傳統捷聯式慣導系統作為位姿檢測備選方案，大大提高了四足機器人對環境的適應能力和對極端情況的應變能力，可以在多種環境下使用。
優選的，N的取值範圍為5～10；C的取值範圍為2～4。
優選的，在所述步驟3)和步驟4)之間還包括圖像誤匹配的步驟，具體方法如下：
計算第j副圖像幀與參考幀圖像的匹配分數
其中，Sj(m,n)為參考幀圖像在坐標(m,n)處的灰度值，T(m,n)為當前幀圖像在坐標(m,n)處的灰度值，坐標(m,n)所處的坐標係為圖像坐標系，圖像左上角起點為原點，x軸為圖像的水平軸，y軸為圖像的豎直軸，M是指圖像的橫坐標最大索引值，即圖像的寬度，N是指圖像的縱坐標的最大索引值，即圖像的長度；
設定第一閾值M1和第二閾值M2，當匹配分數低於M1，則判定圖像匹配失敗，丟棄當前幀圖像，如果M2幅圖像後依舊為圖像匹配失敗狀態，則選擇當前幀圖像作為參考幀圖像。
隨著四足機器人運動過程的推進，場景最終發生大幅度轉換，與參考幀圖像公共部分逐漸變小直至消失，這時會導致圖像匹配失敗；採取上述方法解決了場景切換帶來的匹配失敗的問題，使得機器人即使發生大幅度的轉向運動該算法依舊可以保持優良的性能。
進一步優選的，M1的取值範圍為10～20；M2的取值範圍為3～5。
優選的，所述四足機器人為Viroscope。
本發明的有益效果為：
1.本發明所述基於計算機視覺的四足機器人本體位姿跟蹤方法，通過給四足機器人本體配置兩個攝像頭，可以實時獲取機器人的翻滾角以及俯仰角，通過圖像匹配可以達到亞像素級別的精度，優化了傳統算法中的精確度問題，同時算法具有較高的魯棒性，對噪聲具有較強的抵抗能力，優化了傳統算法帶來的累計誤差問題；
2.本發明所述基於計算機視覺的四足機器人本體位姿跟蹤方法，捨棄了傳統算法帶來的累積誤差，算法並未涉及陀螺儀與加速度計的使用，無需根據地磁進行誤差補償，免除了傳統捷聯式慣導的累積誤差和地磁影響；同時在6幀/s的圖像採集標準下，位姿跟蹤精度達到亞像素精度，比傳統算法具有更加精確的機器人本體位姿反饋；
3.本發明所述基於計算機視覺的四足機器人本體位姿跟蹤方法，與傳統算法逆求解運動方程相比，耗時更短，速度更快，滿足機器人運動過程中的實時性要求；硬體上，節省了兩個高精度加速度計，有效節省了機器人製造的成本。
附圖說明
圖1為本發明所述四足機器人Viroscope的概念圖；
圖2為本發明所述基於計算機視覺的四足機器人本體位姿跟蹤方法的流程圖；
圖3為FAST特徵點檢測的效果圖；
圖4為對參考幀圖像和當前幀圖像進行MSER檢驗的效果圖；
圖5為機器人姿態解算示意圖；
圖6為機器人的運動軌跡圖；
圖7等效角坐標系。
具體實施方式
下面結合實施例和說明書附圖對本發明做進一步說明，但不限於此。
實施例1
如圖1-2所示。
一種基於計算機視覺的四足機器人本體位姿跟蹤方法，包括步驟如下：
1)在Viroscope四足機器人本體背面配置兩個單目攝像頭，1號攝像頭和2號攝像頭；1號攝像頭指向正前方，2號攝像頭指向側方，兩個單目攝像頭方向滿足嚴格正交；
2)圖像採集：
將機器人步態設置為對角小跑步態，運動路線設置為直線行走；運動開始時開啟1號攝像頭和2號攝像頭，每個攝像頭每1S內拍攝5張圖片；
3)圖像匹配：
將兩個單目攝像頭採集到的第一幅圖像分別設定為各自的參考幀圖像，對參考幀圖像和當前幀圖像進行MSER檢驗，選取前4個區域進行區域標定，然後在標定的區域進行FAST特徵點檢測；在標定的區域進行FAST特徵點檢測，取代了傳統算法中對整幅圖像進行特徵點檢測的方法，使得提取出來的特徵點更加穩定，性能優越，而且FAST角點提取速度非常快，而傳統的特徵點提取還要篩去特徵點，篩去野點，防止誤匹配。在標定區域進行FAST特徵點檢測大大節省了時間，增強了匹配的可靠性，減小了特徵點之間的誤匹配概率，提高了對四足機器人本體運動參數估計的精度。如圖3、4所示。
4)計算四足機器人運動參數：
機器人本體運動包括水平橫向位移Δx、水平縱向位移Δy、豎直位移Δh、俯仰角θ、翻滾角γ和航向角ψ；仰角θ向上為正值，向下為負值，定義域為[-90°～+90°]；翻滾角γ右傾為正值，左傾為負值，定義域為[-180°～+180°]；航向角ψ，定義域為[0°～360°]；其中，航向角ψ由加速度計測得；
統計匹配點，對1號攝像頭採集的圖像用最小二乘法求解上述超定方程得到翻滾角γ，對2號攝像頭採集的圖像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(x1,y1)為1號攝像頭採集的參考幀圖像的特徵點坐標，(x'1，y'1)為1號攝像頭採集的當前幀圖像的特徵點坐標；(x2，y2)為2號攝像頭採集的參考幀圖像的特徵點坐標，(x'2，y'2)為2號攝像頭採集的當前幀圖像的特徵點坐標；坐標係為圖像坐標系，x軸為圖像的水平軸，y軸為圖像的豎直軸，坐標系原點為圖像左上角第一點；Tx1為1號攝像頭幀間圖像的水平位移，Ty1為1號攝像頭幀間圖像的垂直位移；Tx2為2號攝像頭幀間圖像的水平位移，Ty2為2號攝像頭幀間圖像的垂直位移；
5)通過機器人雷射測距模塊分別測量1號攝像頭距離障礙物的距離l1，2號攝像頭距離障礙物的距離l2，求解四足機器人本體運動位移；如圖5所示。
運算上述方程，更新機器人運動估計參數，其中，(x0,y0)為機器人初始位姿坐標。至此，四足機器人本體位置姿態信息全部更新完畢；機器人本體運動軌跡見圖6。
實施例2
如實施例1所述的基於計算機視覺的四足機器人本體位姿跟蹤方法，所不同的是，針對曠野和夜間環境，選擇捷聯慣導作為姿態求解的備選系統；定義四元數Q表徵機器人當前狀態的姿態向量：
其中，
其中，φ表示等效角坐標系的旋轉角度；如圖7所示。
實時計算四元數Q；輸入信號為陀螺儀的數字輸出信號其中i為x、y或z；b為機器人軀幹坐標系，ωib表示在機器人軀幹坐標系中，四足機器人饒x,y或z軸的角速度；然後採用二階龍格庫塔法計算：
其中，
K1、K2、Y為中間變量，T為運動過程中的更新周期；q(t)為四元數對時間t時刻的函數；q(t+T)為四元數對時間t+T時刻的函數；ωi為四足機器人饒x,y或z軸的角速度；所述陀螺儀設置在機器人軀幹中。
由此可得歐拉參數表示的等效旋轉矩陣：
根據機器人本體旋轉軸為一般軸的等效旋轉矩陣：
其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；
可得：
其中，rab為機器人本體旋轉軸為一般軸的等效旋轉矩陣的第a行第b列元素，a＝1,2,3；b＝1,2,3；
由旋轉矩陣中可得航向角ψ，俯仰角θ和翻滾角γ；
θ＝arctan2(r21/cγ，r11/cγ)
。
γ＝arctan2(r32/cγ，r33/cγ)
針對曠野和夜間等視覺不良的環境，我們通過接連慣導作為四足機器人姿態的備選系統對機器人本體姿態進行求解，避免了極端情況下視覺算法失效帶來的機器人姿態追蹤失敗的問題。
由於在某些特殊地形與極端條件下，(比如像曠野與夜間)基於計算機視覺的位姿跟蹤無法正常運行，此時將傳統捷聯式慣導系統作為位姿檢測備選方案，大大提高了四足機器人對環境的適應能力和對極端情況的應變能力，可以在多種環境下使用。
實施例3
如實施例1所述的基於計算機視覺的四足機器人本體位姿跟蹤方法，所不同的是，在所述步驟3)和步驟4)之間還包括圖像誤匹配的步驟，具體方法如下：
計算第j副圖像幀與參考幀圖像的匹配分數
其中，Sj(m,n)為參考幀圖像在坐標(m,n)處的灰度值，T(m,n)為當前幀圖像在坐標(m,n)處的灰度值，坐標(m,n)所處的坐標係為圖像坐標系，圖像左上角起點為原點，x軸為圖像的水平軸，y軸為圖像的豎直軸，M是指圖像的橫坐標最大索引值，即圖像的寬度，N是指圖像的縱坐標的最大索引值，即圖像的長度；
設定第一閾值M1和第二閾值M2，M1＝20，M2＝5；當匹配分數低於M1，則判定圖像匹配失敗，丟棄當前幀圖像，如果M2幅圖像後依舊為圖像匹配失敗狀態，則選擇當前幀圖像作為參考幀圖像。
隨著四足機器人運動過程的推進，場景最終發生大幅度轉換，與參考幀圖像公共部分逐漸變小直至消失，這時會導致圖像匹配失敗；採取上述方法解決了場景切換帶來的匹配失敗的問題，使得機器人即使發生大幅度的轉向運動該算法依舊可以保持優良的性能。