基于機器學習與視覺的四足機器人步態精確監測方法

秦鵬舉, 蔣周翔, 蘇瑞, 宋鵬成, 馬紫怡

(北京信息科技大學機電工程學院, 北京 100192)

隨著機器人仿生學的發展,四足機器人逐漸成為移動式機器人領域的研究熱點。四足機器人憑借出色的運動性與環境適應性,被充分應用于軍事、生活、娛樂等場景,具有較大的發展潛能[1]。代表性的四足仿生機器人有波士頓動力公司研發的BigDog和SpotMini[2],麻省理工學院研發的Cheetah獵豹機器人[3]以及山東大學研發的Scalf四足機器人[4]等。

四足機器人的步態直接影響其運動能力和穩定性。隨著應用場景不斷拓展,對機器人性能的需求也日益提高,其中就包括了要求機器人步態軌跡長期穩定保持高精度。然而,長期行進會導致關節和傳動系統損耗,通過運動鏈的傳播積累會顯著影響足端位姿和步態軌跡精度[5]。解決方法中的關鍵一環就是如何對足端位姿進行精確監測。

在位姿測量領域,如激光跟蹤儀、雙球桿儀等在內的測量設備[6-7]通常以大體積工業機器人為對象,難以用于移動輕巧的足式機器人。相比之下,機器視覺不僅成本低廉,而且便捷快速,在機器人位姿測量中得到了廣泛的應用。Balanji等[8]提出了一種基于單目視覺的機器人運動學標定方法。Fu等[9]通過固連在機器人末端的棋盤格標記用于辨識其運動學參數。Wang等[10]基于單目視覺和ChArUco(chess augmented reality university of cordoba)標記對六足機器人進行了位姿測量。這些方法的數據來源均為相機所拍圖像,因此易受鏡頭畸變、光照差異、相機標定等因素影響,導致位姿測量精度不足。

面對上述現狀,以四足機器人的Trot步態(對角小跑步態)軌跡為對象,依據軌跡規劃結果選取步態監測點。采用雙目相機實時觀測足端靶標位姿,并基于機器學習方法將“不精確”位姿測量結果修正為足端“精確”步態位姿數據,從而實現精確步態監測。

1 系統設計

將雙目相機安裝在四足機器人前端,其視場范圍應包含兩個前足的完整步態軌跡,識別靶標采用ArUco[11](augmented reality university of cordoba)標記并安裝在機器人足端,如圖1所示。在還未長期運動而產生積累誤差時,建立足端名義運動學模型并計算足端“精確”的名義位姿,同時視覺采集獲得靶標“不精確”的測量位姿。以此構建深度神經網絡建立二者映射關系,即可通過實時觀測步態監測點處靶標位姿,準確預測足端位姿,以保證足端軌跡精度和步態運動周期性。

圖1 監測系統整體示意圖Fig.1 Schematic diagram of monitoring system

2 步態監測點選取

2.1 步態特征分析

由于Trot步態相比其他步態在能效、速度變化范圍、步態穩定性上均有明顯優勢,且應用最為廣泛[4],因此選擇Trot步態作為監測點選取對象,以保證監測方法具有廣泛適用性。

一個完整的步態周期分為擺動相和支撐相,而擺動相決定了機器人跨步步長與抬腿高度。為保持步態運動穩定性與流暢性,防止產生沖擊,擺動相足端落地與抬起瞬時速度應為0。因此四足機器人步態規劃可定義為復合擺線軌跡[12]

(1)

(2)

2.2 步態監測點選取

將復合擺線軌跡分解為沿x軸及沿z軸方向的運動,如圖2、圖3所示,分別為擺動相足端位置和速度變化曲線。

圖2 擺動相足端位置變化曲線Fig.2 Swing phase foot position variation curve

圖3 擺動相足端速度變化曲線Fig.3 Swing phase foot end velocity variation curve

觀察其運動特性可知,擺動相抬腿速度和落地速度均為0。在擺動足達到最高點時,z軸速度為0。因此選擇監測點時依據步態位置穩定以及無沖擊原則,以0T及0.5T為主要監測點,以0.25T為次要監測點。

3 步態位姿測量與計算

3.1 足端名義位姿計算

首先,基于D-H(Denavit-Hartenberg)參數法[13],建立四足機器人左前足運動學模型,定義基坐標系B位于機身中心,xB、yB、zB為基坐標系3個坐標軸,xi、yi、zi為左前足第i關節坐標系3個坐標軸,i=1,2,3;F為左前足末端坐標系,xF、yF、zF為左前足末端坐標系3個坐標軸;M為靶標坐標系,xM、yM、zM為靶標坐標系3個坐標軸;C為相機坐標系,xC、yC、zC為相機坐標系3個坐標軸。坐標系分布如圖4所示。

圖4 坐標系分布Fig.4 Coordinate system distribution

足端名義位姿通過基坐標系描述,足端坐標系F相對基坐標系B的正運動學變換矩陣為

(3)

式(3)中:若將BT1看作0T1,3TF看作3T4,則i-1Ti表示第i-1關節坐標系到i關節的齊次變換矩陣,可表示為

i-1Ti=Rx(αi)Tx(ai)Rz(θi)Tz(di)

(4)

式(4)中:Rx(·)、Rz(·)為旋轉矩陣;Tx(·)、Tz(·)為平移矩陣;αi、ai、θi、di為第i關節D-H參數[14]。

若將式(3)表征為

(5)

式(5)中:由nx、ny、nz、ox、oy、oz、ax、ay、az組成的旋轉矩陣與px、py、pz組成的位置向量,用于表示齊次變換矩陣BTF。

為提升機器學習泛化能力,將BTF矩陣中各元素使用式(6)變換得到對應足端名義位姿向量PF,以此作為機器學習的標簽進行有監督學習。其中,XF、YF、ZF為足端坐標系的位置坐標,θxF、θyF、θzF為足端坐標系繞基坐標系xB、yB、zB軸的旋轉歐拉角,atan2(y,x)為雙變量反正切函數。

(6)

3.2 足端靶標實際采集位姿計算

足端靶標實際采集位姿通過相機坐標系描述,相機到靶標坐標系實際變換矩陣為CTMa。為復現實際采集過程中視覺誤差影響用于仿真驗證,對CTMa進行如下變換。依據圖4建立的D-H坐標系分布,相機到基坐標系變換矩陣恒為CTB,左前足端到靶標坐標系變換矩陣恒為FTM,因此相機到靶標坐標系理想變換矩陣CTM為

(7)

若將視覺系統復雜誤差綜合模型[14]表示為TMe,可得

CTMa=CTMTMe

(8)

采用與式(5)和式(6)類似變換,將CTMa轉化為靶標實際位姿向量PMa=[XMa,YMa,ZMa,θxMa,θyMa,θzMa]T以此作為機器學習的特征進行有監督學習。其中,XMa、YMa、ZMa為相機坐標系下靶標坐標系的真實位置坐標,θxMa、θyMa、θzMa為靶標坐標系繞相機坐標系xC、yC、zC軸的旋轉歐拉角。

4 神經網絡設計

4.1 設計網絡結構

深度前饋神經網絡是目前機器學習與深度學習領域應用最廣泛的模型,擬設計神經網絡結構如圖5所示。相鄰節點使用全連接層搭建,采用ReLU激活函數進行修正,輸出層采用線性激活函數,使該網絡具有更加明顯收斂性與稀疏性。

圖5 神經網絡結構Fig.5 Neural network architecture

隱藏層數和節點數直接關系著網絡預測精度,對此,通常參考如式(9)所示經驗公式來初定最佳隱層節點數L的范圍[15]。

(9)

式(9)中:m、n為輸入層以及輸出層節點數;常數a=1,2,…,10。

由于輸入和輸出分別是實際采集足端靶標位姿向量和足端名義位姿向量,故令n=m=6,隱層節點數可選L=5,6,…,14?？紤]到模擬相機采集足端靶標位姿時建立綜合誤差模型,引入多種干擾誤差,因此在初選節點數14基礎上進行調整,經過多次驗證,隱層節點數為16,隱藏層數為3。

4.2 生成訓練樣本

依據第2節推論,以0T以及0.5T為主要監測點,以0.25T為次要監測點。將各點對應關節角度值代入式(3)、式(4)可得足端坐標系矩陣BTF。如圖6所示,以每個步態監測點位置(px,py,pz)為中心,規劃邊長為100 mm的立方空間,在內部均勻取103個訓練點。然后在±5°范圍內均勻取10個值,生成103個不同的角度組合(δαF,δβF,δγF)。令足端坐標系BTF依次繞xF、yF、zF軸旋轉δαF、δβF、δγF角度,得到103個訓練點對應的足端坐標系位姿矩陣BTF。將BTF依次代入式(5)、式(6)求得訓練點對應的足端名義位姿向量PF。然后綜合式(5)～式(8)可計算出訓練點對應足端靶標實際采集位姿向量PMa。將PMa作為神經網絡輸入特征,PF作為輸出標簽,由此訓練所得神經網絡可根據監測點臨域內某點的足端靶標實際采集位姿預測出對應足端名義位姿。

5 仿真驗證

5.1 步態監測點仿真驗證

如表1所示,使用D-H參數在仿真軟件中建立四足機器人左前足模型,使用正運動學公式[式(3)]解算足端位姿,使用蒙特卡洛法仿真出左前足的工作空間如圖7所示。

表1 四足機器人左前足D-H參數設置

圖7 左前足工作空間Fig.7 Front left foot workspace

如表2所示,將數據代入式(1)、式(2),使用復合擺線形式進行Trot步態軌跡規劃后,利用反變換逆解算法[16]求出擺動腿各個旋轉關節的角度值。再代入式(3)生成左前足擺動相足端軌跡。進而在擺動相足端軌跡中提取出3個步態監測點如圖8所示,解算出各個步態監測點擺動腿各旋轉關節角度值如表3所示。

表2 Trot步態規劃參數設置

表3 步態監測點對應關節角度值

圖8 步態監測點示意圖Fig.8 Schematic diagram of gait monitoring points

仿真結果表明,經過Trot步態規劃后的足端軌跡與復合擺線軌跡一致,觀察監測點1處的足端位置,恰處于擺動相抬腿落地時刻,且速度為0。符合步態位置穩定以及無沖擊原則,滿足使用需求。

5.2 神經網絡仿真驗證

5.2.1 神經網絡訓練

神經網絡仿真驗證根據3種典型影響因素建立了靶標采集位姿綜合復雜誤差模型TMe,模擬采集環境影響,驗證基于機器學習與視覺的四足機器人步態精確監測的準確性,仿真流程如圖9所示。

圖9 神經網絡仿真流程圖Fig.9 Neural network simulation flowchart

擬采用步態監測點1作為仿真驗證對象,相較于其他監測點,其更靠近視場邊界,受畸變以及光照影響相對明顯,更加典型。依照4.2節訓練樣本的生成方法,使用表3中步態監測點1對應的關節角度值作為仿真驗證的對象,求得鄰域內足端名義位姿向量PF,再根據3.2節所分析綜合誤差帶來的影響,依照仿真流程圖,對足端靶標實際采集位姿進行仿真驗證,首先依據真實情況,為相機到機器人基坐標系CTB與足端到靶標坐標系FTM賦值,如表4、表5所示。綜合誤差模型包含相機標定、相機畸變、光照差異3種典型影響因素,然后為綜合誤差模型TMe賦值,如表6所示。據此計算得到足端靶標實際采集位姿向量PMa。

表4 相機到基坐標系的變換矩陣CTB賦值Table 4 Camera to base transformation matrix CTB assignment

表5 足端到靶標坐標系變換矩陣FTM賦值Table 5 Foot to target transformation matrix FTM assignment

表6 綜合誤差模型TMe參數賦值Table 6 Integrated error model TMe parameter assignment

仿真生成監測點1鄰域訓練樣本如圖10所示。

紅色點陣為靶標坐標;藍色點陣為足端關節坐標

觀察兩組點陣之間存在一定程度的差異,說明訓練樣本融合了靶標安裝偏置、光照差異以及視覺系統導致的測量誤差。隨后將靶標采集位姿向量PMa作為模型特征輸入,足端名義位姿向量PF作為模型標簽輸出,經過歸一化特征處理,取訓練樣本中的900個目標點作為訓練集,其余100個目標點作為驗證集。其中模型最大訓練次數為2 000,學習率為0.01,訓練目標最小誤差為10-6。采用4.1節所搭建的模型結構,網絡訓練結果如圖11所示。在步態監測點1處的網絡訓練結果均方誤差可達到1.7×10-5,可見神經網絡能夠準確地學習視覺系統誤差到靶標位姿采集誤差的映射關系。

圖11 神經網絡訓練結果Fig.11 Neural network training results

5.2.2 神經網絡測試

在步態運行過程中,由誤差積累導致的足端偏移范圍較小,為模擬真實條件且驗證神經網絡的可信度,據此在步態監測點1較小鄰域內隨機生成更為密集、方位角范圍更加離散的測試樣本進行仿真測試。監測點1鄰域的測試樣本如圖12所示。該點陣相較于訓練樣本更加復雜,因此步態監測精度效果驗證更加可靠。

圖12 測試樣本點位分布Fig.12 Distribution of test sample points

5.2.3 驗證結果與分析

將測試樣本中的足端靶標實際采集位姿向量PMa′輸入步態監測點1訓練后的網絡中,得到足端名義位姿向量預測值PMap。在步態監測過程中,僅需要保證其位置精度,但該方法亦預測了其姿態。因此主要對足端位置預測精度進行驗證。將預測值PMap位置坐標與測試樣本足端名義位姿向量PF′位置坐標進行解算,求得預測點位與真實點位之間的距離誤差如圖13所示。數據點越藍表明距離誤差越小,觀察可知,面對更加密集、復雜的測試點集,相機采集靶標位姿的預測點位置與真實足端位置誤差均處于亞毫米級別,最大點位距離誤差不超過0.05 mm。

圖13 預測點與真實點之間距離誤差Fig.13 Distance error between predicted and actual point

為更加詳細的評價監測精度,隨機抽取100個測試樣本在步態監測點1處的預測點位與真實點位誤差如圖14所示,可以看出,預測誤差集中分布在0～0.01 mm范圍內,最大預測誤差不超過0.03 mm。

圖14 隨機樣本預測誤差Fig.14 Random sample prediction error

所有測試樣本在步態監測點1處的預測點位與真實點位誤差分布直方圖如圖15所示,仿真結果表明,監測精度誤差平均值達到0.006 mm,最大值僅為0.057 mm,在超過99.68%的測試樣本中,相機拍攝靶標的復雜誤差數據,經過神經網絡訓練后對足端位置預測精度能達到0.025 mm。因此該方法可以滿足對機器人足端位姿預測精度的需求,能夠完成對步態軌跡的實時監測,保證其完成精確的步態運動。

圖15 預測誤差分布直方圖Fig.15 Histogram of prediction error distribution

6 結論

針對四足機器人步態運動精度問題,使用了機器視覺方法進行步態監測,提出引入機器學習的方法對視覺系統測量誤差進行學習修正,以相機采集靶標位姿預測足端真實位姿。仿真結果表明,新方法具有較高的預測精度,能滿足步態實時高精度監測的需求,進而為足式機器人實現精確的足端定位以及保持步態周期性提供了理論依據和技術支撐。

由于實際的視覺采集過程還存在一些其他因素干擾,因此下一步研究計劃是搭建實驗平臺驗證新方法在現實場景下的測量精度,以針對該方法的實際應用做進一步優化與完善。