智能搜救機器人在障礙地形的自主構型規劃

陳柏良,黃開宏,潘海南,肖軍浩,吳文啟,盧惠民

(國防科技大學 智能科學學院, 湖南 長沙 410073)

智能搜救機器人是一種可變構型機器人,其利用擺臂結構或子履帶結構調整自身構型并提高越障能力,更好地適應城市廢墟、戶外林地等障礙地形,如圖1所示。其代價是操作自由度較多(6個),如果完全依賴遠程人工遙控,會給操作人員帶來很大的認知負擔,容易導致控制失誤[1]。因此,越障過程中機器人構型的自主規劃是當前一個急需解決的問題。

圖1 NuBot-Rescue 四擺臂履帶機器人Fig.1 The NuBot-Rescue tracked robot with four flippers

根據實現方法,現有的構型規劃算法主要分為三類:底盤姿態控制法、簡化模型分析法和機器學習法。

底盤姿態控制法是根據地形起伏給出期望的底盤姿態(包括底盤俯仰角、擺臂角度等)并以此為擺臂控制目標。文獻[2]根據機器人三維全局路徑的起伏曲率來調整擺臂角度,使底盤與擺臂的整體構型接近路徑的包絡線。底盤姿態控制法可以實現基本的越障功能,但仍需要專門的越障構型來應對較為復雜的障礙,而且這些方法較少考慮越障動作的優化問題,存在局限性。

簡化模型分析法是針對擺臂履帶機器人與特定地形的接觸過程進行運動學與動力學分析[3],得到最優的越障過程。特定的地形通常指單個臺階[4-5]、樓梯[6]等結構化場景。文獻[4]中建立了機器人翻越單個臺階過程的運動學和動力學模型,并總結出使擺臂力矩最小的控制規律?；诤喕Ｐ偷姆椒ǚ治鲈敱M,但不能應對復雜、不規則、非結構化的地形等情況。

機器學習法既考慮了越障動作優化問題,也對不規則的地形具有一定適應性。其主要采用深度學習或強化學習方法獲得機器人在當前狀態下越障效果最好的機器人期望姿態或擺臂動作[7-10]。文獻[9-10]采用強化學習方法,以地形起伏信息和機器人姿態等為狀態量,以危險姿態、顛簸程度等指標為懲罰項,訓練出能夠自主上下樓梯的擺臂控制策略。文獻[8]將模仿學習與強化學習相結合,使機器人面對多種不同障礙時能夠快速地訓練出適應性較好的越障策略。然而,此類方法在未訓練過的地形以及極易傾覆的高難度地形中的適應性尚未得到充分驗證。

上述三種方法的共同特點都是針對越障過程中的機器人構型和姿態進行分析處理。機器人通過調整擺臂角度改變自身構型,從而控制自身姿態以翻越高難度的障礙,并減輕越障過程的顛簸。因此,機器人在越障時的構型變化取決于機器人當前的姿態以及在即將到達的地形上所期望的姿態。如果能夠提前預知機器人在即將到達的地形上的狀態,機器人就可以有效地規劃越障構型。目前主要有仿真分析法和迭代幾何法實現了履帶機器人的姿態預測。前者主要利用物理仿真引擎,得到機器人在穩定狀態下的仿真預測值[11]。其適用于任意地形和機器人外形,但計算量大、實時性差。迭代幾何法[12]則是根據機器人以及地形的起伏形狀,迭代尋找接觸點以確定姿態。這種方法能夠在復雜地形中預測姿態,但是現有研究并未考慮擺臂采用多種構型時的姿態預測問題。

因此,本文針對傳統方法無法兼顧優化越障過程和適應復雜障礙的問題,以及強化學習方法訓練過程不確定性強、高難度地形適應性存疑的不足,結合姿態預測對復雜地形的高適應性和動態規劃算法尋找最優解的特點,提出一種既能應對復雜地形又具有優化能力的搜救機器人自主構型規劃方法,該方法在仿真與實物實驗中均展現出了優異的自動控制效果。

1 姿態預測

1.1 機器人與地形建模

研究對象為如圖1所示的智能搜救機器人,其在一般履帶底盤的基礎上增加了四個可以獨立轉動的子履帶(稱為擺臂)。這種機器人的左側與右側結構是相同的,在建模時可以將機器人左側與右側分別使用同樣的方法分析。當滾動角較小時,機器人模型可以簡化為圖2(a)所示的單側履帶模型:側向投影機器人的左側或右側得到三段式模型,中間段為底盤,左右兩段分別為后擺臂與前擺臂。在側向投影中,當擺臂具有與主履帶大致相同的厚度時,可以將地面向外膨脹履帶的彎曲半徑[5],得到進一步簡化的機器人無厚度線段模型,如圖2(b)所示。

(a) 單側履帶簡化模型(a) The single-side pose modelling

機器人下方的地形采用離散的點集表示,如圖2(b)中的地形點。通過機載激光雷達或深度相機可以感知局部地形并結合建圖算法[13-14]獲得原始點云;再將點云膨脹并重新采樣得到膨脹后的地形點集,記為集合T,其中的點記為p∈T。

1.2 姿態預測問題定義

機器人在地形中的姿態,是由機器人自身構型以及與地形的接觸點決定的。設地面參考系為[G],姿態預測問題可以描述為下列形式。

給定:

1)T:地形的點集。

2)xR:底盤中心在[G]中的x軸坐標。

3)θ1:機器人前擺臂角度,逆時針為正。

4)θ2:機器人后擺臂角度,順時針為正。

求機器人在靜態穩定狀態下的:

1)yR:底盤中心在[G]中的y軸坐標。

2)θR:機器人在[G]中的俯仰角,逆時針為正。

約束條件:

1)機器人質心的前后兩側各存在一個以上與地形的接觸點;

2)由接觸點確定的機器人姿態應當使機器人位于其范圍內所有地形點的上方。

圖2(b)展示了上述參數的具體意義。因此,求解姿態就是從點集T中找出滿足約束條件的地形點即接觸點,進而確定姿態參數。這里預測的是機器人在“穩定狀態”下的姿態,基于此可以提出兩個約束條件。條件1是機器人在地面上保持穩定的基本條件;條件2代表機器人的任一部位都不能與地面出現碰撞干涉的基本物理約束。

1.3 姿態預測問題的數學描述

1.3.1 機器人模型的數學描述

以[G]中的待預測位置(xR,0)為原點建立一個y軸與重力方向平行、x軸方向為機器人朝向的局部坐標系[L]。令機器人的底盤中心位置與原點重合,底盤與x軸平行,可以得到如圖3(a)所示的模型。該模型可以用分段直線描述各部件:

(1)

1.3.2 接觸關系的數學描述

地形點與機器人接觸可以表示為點在分段直線模型上,即機器人R在[L]中首先繞原點旋轉俯仰角φ,再沿y軸平移h變換為R′,并與地形點pc1(xc1,yc1)∈T、pc2(xc2,yc2)∈T接觸,如圖3(b)所示。

(a) 在局部坐標系中水平放置的機器人的分段直線模型(a) A segmented linear model of the robot placed horizontally in the local coordinate system

(b) 將機器人模型先旋轉φ,再平移h,與接觸點重合(b) The robot model is first rotated φ, then translated h to coincide with the contact points圖3 機器人模型以及機器人姿態與接觸點的關系Fig.3 Robot model and the relationship between robot pose and contact points

為滿足約束條件1,地形點需分別位于機器人質心前、后側。點在直線上的幾何關系可以用直線的齊次坐標向量與點的齊次坐標向量的點積為零表示。而pc1、pc2既可能位于擺臂上,也可能位于底盤上,因此對于pc1,有式(2)成立:

(2)

對于pc2,有式(3)成立:

(3)

其中:lr′i+1r′i(i=1,2,3)為以齊次坐標向量形式表示的變換后的機器人模型R′,其含義為經過r′i+1、r′兩點的直線;pcj(j=1,2)為以齊次坐標向量形式表示的接觸點,如式(4)所示。

(4)

式中,(xr′i,yr′i)為r′i的坐標,是圖3(a)中初始狀態下的ri經過旋轉和平移變換后得到的,如式(5):

(5)

在約束條件1中,接觸點還需要滿足分布在質心前后的要求,即式(2)、式(3)中接觸點的x軸坐標取值范圍要遠離質心。機器人模型質心在[L]中的坐標(xcom,ycom)可以表示為式(6):

(6)

其中,(xb,yb)是底盤質心的坐標,lf是擺臂質心到擺臂轉軸的距離,mB和mF是機器人單側底盤以及單個擺臂的質量。假設底盤質心與底盤形狀中心重合即(xb,yb)=(0,0),則有式(7):

(7)

與此同時,所有位于變換之后機器人范圍內的地形點p還需要滿足約束條件2的要求?？梢杂胠r′i+1r′i(i=1,2,3)與p的點積正負表示點相對直線的位置,因此該條件可以轉化為式(8):

(8)

需要注意的是,變換后的機器人模型擺臂末端點r′1、r′4有可能位于底盤下方即xr′2>xr′1或xr′4>xr′3,此時式(8)中的符號取反。

最終滿足所有條件的候選接觸點pc1、pc2所對應的(h,φ)即為機器人在當前位置的預測姿態參數:(yR,θR)=(h,φ)。

聯立式(2)～(5)、式(8)可以得到如下形式的姿態預測問題的數學描述:

求解:h,φ,pc1(xc1,yc1)∈T1,pc2(xc2,yc2)∈T2

條件:

(9)

(10)

(11)

(12)

該問題并不總是有可行解,而無效解對應的機器人構型與地形位置,將在后續的構型規劃中被視為危險狀態進行規避。

1.4 姿態預測問題的解析求解方法

至此,姿態預測問題的數學模型已經建立,實際運用時需要高效的求解算法。當地形點總數量有限時,可以利用式(2)、式(3)組成的等式方程組快速地遍歷求解局部地形點集中所有候選接觸點對(pc1,pc2)對應的機器人姿態解析解,并根據區間范圍條件、不等式條件篩選得到滿足所有約束條件的姿態?；谶@種思路,設計了一種可以適用于復雜地形的履帶機器人姿態預測方法。

首先從地形點集T1、T2中選取一對候選接觸點(pc1,pc2),由于接觸點既可能位于擺臂上也可能位于底盤上,因此式(2)、式(3)可以根據下列情況各自提出一個等式組成待求解的等式方程組:

1)pc1在前擺臂上,pc2在后擺臂上;

2)pc1在前擺臂上,pc2在底盤上;

3)pc1在底盤上,pc2在后擺臂上;

4)pc1在底盤上,pc2在底盤上。

這些方程組的基本形式是一致的,接下來以第二種情況為例,其他三種情況將對應參數替換即可。將式(4)、式(5)代入由式(2)第一行等式、式(3)第一行等式組成的方程組,令z=tan(φ/2),則sinφ=2z/(1+z2),cosφ=(1-z2)/(1+z2)。整理得到關于h和z的方程組如式(13):

(13)

其中的系數Ai、Bi、Ci、Xi、Yi(i=1,2)為只與機器人模型的原坐標(xri,yri)、候選接觸點坐標(xci,yci) 有關的常數。將該方程組視為關于z的多項式,如果z的解存在,則這兩個多項式之間必然存在公因式,即兩者的結式為零。假設式(13)的最高項系數(Ai-Xih)均不為零,展開結式可以得到關于h的四次方程,利用多項式求根算法可以求出h的解;進一步利用二次方程求根公式與反正切函數得到h對應的φ。采用相同的方法,可以求出其他三種情況下的解。根據接觸點x軸坐標的范圍條件以及等式約束條件,可從這四種情況中找到一組滿足接觸點條件的姿態參數(h,φ)。

基于上述解析求解方法,在給定一對候選接觸點坐標后,可以計算出滿足約束條件1的姿態參數。在此之后,根據式(8)判斷是否滿足約束條件2。如果不滿足,則重新從T1、T2中選取一對候選接觸點,最終找到機器人在給定地形上采取指定前后擺臂角度(構型)時的單側機器人姿態。

姿態預測算法的整體流程如圖4所示。當T1、T2中地形點總數為30～50時,指定xR以及θ1、θ2時的單個姿態的平均求解時間約為0.75 ms,即每秒預測1 000～1 500個姿態,能夠滿足實時進行構型規劃的需求;采用點集的形式描述地面,使算法能夠應對各種上下起伏不規則的地形。

圖4 姿態預測算法框圖Fig.4 Framework of pose prediction algorithm

2 構型規劃

基于快速的姿態預測算法,進一步設計越障構型規劃算法。算法以傳統的動態規劃方法為基礎,尋找局部感知范圍內的最優越障構型序列;結合滾動優化的思想設計不斷更新構型序列的規劃器,實現機器人越障整體過程的優化。

2.1 機器人狀態與動作評價

動態規劃算法需要首先對越障過程中的機器人狀態以及狀態之間的轉移過程進行合理的評價。

圖5 機器人下壕溝過程的構型規劃示意圖Fig.5 Diagram of robot configuration planning when going down the trench

根據人類操作員的經驗,設計了代價函數C來評價在狀態SP,θ1,θ2時采取動作(a1,a2)的代價:

(14)

其中,Cθ(SP,θ1,θ2,SP+1,θ1+a1,θ2+a2)為前后兩個狀態間俯仰角變化的代價,Cy(SP,θ1,θ2)為當前狀態下質心高度與底盤范圍內地面平均高度的偏差的代價,Cs(SP,θ1,θ2)為當前狀態下俯仰方向穩定性的代價,Ct(SP,θ1,θ2)為當前狀態下擺臂力矩的代價,Cr(a1,a2)為擺臂調整幅度的代價。

利用式(14)對離散狀態空間中所有的狀態與動作進行評價,作為構型規劃的依據。

2.2 動態規劃

在獲得每個路徑點處機器人各種構型時的狀態以及采取各種動作時的代價值后,運用動態規劃算法尋找機器人狀態空間中連接起始狀態和目標狀態的最優狀態轉移路徑,使得式(15)的累計代價值最小。

Call(SP,θ1,θ2)=min{Call(SP-1,θ1-a1,θ2-a2)+

C(P-1,θ1-a1,θ2-a2)}

(15)

2.3 基于滾動優化的構型序列更新

機器人的感知范圍有限導致構型規劃的距離有限,因此運用滾動優化的方式在前進的同時不斷更新構型序列,如圖5中的3個階段所示。

2.4 非對稱地形構型規劃策略

左右非對稱的地形會使機器人傾斜,導致姿態預測算法的基本條件“滾動角度較小”無法滿足,同時也將影響滾轉方向的穩定性,使機器人更容易側翻。圖6中機器人滾轉方向的穩定性可以使用力角穩定性指標[15]分析,如式(16):

(16)

其中:θsl、θsr分別指機器人左側與右側的接觸點形成的翻轉軸法向與合力f(不包括支持力,靜態穩定時主要是重力)的夾角;dsl、dsr指左側與右側的翻轉軸到合力方向的距離。

當所處地形相對機器人左右高度差H較大時,如圖6(a)所示,機器人滾轉角θRr很大,極易側翻。此時將地形較低一側的擺臂下壓可以提高機器人穩定性,如圖6(b)所示;且機器人被抬起使滾轉角接近0°時,能滿足姿態預測算法的假設。

圖6 機器人在單側臺階上的滾轉穩定性分析示意圖Fig.6 Diagram of the rolling stability analysis of the robot on single-side steps

因此,設計了一種輔助側擺臂角度計算方法,以減少機器人在面對不對稱障礙時的滾轉角變化,在保持穩定的同時讓姿態預測算法能夠有效運行。首先分析局部地形中不同方位的起伏程度,選擇地形較高的方位中的點云數據作為越障側姿態預測與構型規劃的輸入,地形較低的方位中的點云數據作為輔助側擺臂角度計算的輸入。如圖7所示,當灰色圓點Ttl表示的左側地形比紅色方形Ttr表示的右側地形平均值高時,黑色線段表示的機器人左側為越障側,淡藍色表示的機器人右側為輔助側。利用Ttl進行姿態預測與構型規劃,可以獲得在當前位置處期望的機器人左側姿態(yR,θR),接觸點pcl1、pcl2,以及左側擺臂角度θfl1、θfl2。此時若要使滾轉角接近0°,則右側的擺臂需要下壓將車體撐起,使右側擺臂轉軸rr1、rr2與左側擺臂轉軸的側向投影位置相近。因此右側(輔助側)擺臂角度θfr1、θfr2的計算只需在給定擺臂轉軸位置的條件下,求得滿足擺臂與地形接觸、擺臂不與地形碰撞干涉、擺臂角度在允許范圍內三個條件的擺臂角度即可,此時輔助側擺臂與地形的接觸點為pcr1、pcr2。

圖7 機器人攀爬左側臺階時的輔助側擺臂動作示意圖Fig.7 Diagram of the assistant side flipper action when the robot is climbing the left side step

綜上所述,以姿態預測為基礎,設計了基于動態規劃并能滾動更新的機器人構型規劃器,并提出了一種能夠適應非對稱地形的規劃策略。

3 實驗與討論

為了測試所提出的機器人自主構型規劃算法(以下簡稱構型規劃算法)的效果,在2個Gazebo仿真場景與3個真實場景中進行了實驗,并與基于深度強化學習的方法[10]進行了對比。

3.1 實驗設置與對比指標

3.1.1 參數設置

如圖1所示,實驗使用的NuBot-Rescue機器人的基本物理參數為:底盤軸距B=0.552 m,擺臂軸距F=0.336 m。通過機載激光雷達結合建圖算法[16]輸出局部地形點云;通過慣性測量單元(inertial measurement unit,IMU)感知機器人姿態。Gazebo仿真中的機器人與實物機器人具有相同的物理參數和傳感器配置。仿真履帶運動控制方法采用文獻[17]提出的“不可變形履帶的快速仿真方法”。

系統軟硬件架構如圖8所示。采用機器人操作系統(ROS)將所有程序運行在機載電腦(Intel i9-9900T,16 GB)中,主要包括:同步定位與建圖節點(5 Hz)、姿態預測與構型規劃節點(1～2 Hz)、電機控制節點(100 Hz)、人工控制節點(100 Hz)、交互界面。機載電腦從激光雷達、IMU、相機、電機驅動器等機載設備獲得環境感知信息與機器人狀態感知信息;通過無線通信將感知信息傳遞到遙控端,并接收前進方向、速度等人工控制指令信息。在電機控制節點中將人工控制指令與規劃的構型序列融合,輸出擺臂、履帶的控制指令。

在算法的參數方面,局部地形感知域為以機器人底盤中心向前1.5 m、向后0.5 m,左右各0.5 m的長方形范圍;預測路徑點間距0.2 m,路徑點數量至多4個;擺臂角度分辨率k=10°,擺臂可調整單位數b=3,擺臂活動范圍-70°～70°。

3.1.2 對比指標

(17)

3.2 仿真實驗

3.2.1 臺階場景

0.4 m高的單臺階是機器人能夠通過的極限高度,用于測試算法對高難度對稱地形的適應性。主要包括上、下臺階兩個過程,如圖9所示。

圖9 0.4 m單臺階場景越障過程Fig.9 Processes of the robot moving through the0.4 m single-step scenarios in simulation

量化指標如表1所示,對比強化學習方法和人工操作[10],構型規劃算法在單臺階越障場景中具有比強化學習方法更好的通行平穩程度和相近的通行時間,且都明顯優于人工操作。

3.2.2 陡峭樓梯場景

陡峭樓梯場景由上33.7°樓梯和下45°樓梯兩部分組成,其中上樓梯時臺階高度0.2 m,臺階寬度0.3 m,共6級;下樓梯時臺階高度0.2 m,臺階寬度0.2 m,共6級。整體過程如圖10所示。

圖10 陡峭樓梯場景越障過程Fig.10 Processes of the robot moving through the steep staircase scenarios in simulation

表1 仿真臺階與樓梯場景中的不同算法指標對比

3.3 實物實驗

3.3.1 單側臺階場景

在單側臺階場景實物實驗中,機器人需要攀爬位于左側的高0.3 m臺階,如圖11(a)所示。運行中的量化指標如表2所示。結果表明構型規劃算法能夠使機器人穩定地攀爬具有較高難度的單側臺階,且滾轉方向的搖晃程度較小。

3.3.2 斜向樓梯場景

在斜向樓梯場景中,機器人需要斜向通過樓梯地形,每級樓梯長0.38 m、高0.12 m,共8級。具體效果如圖11(b)～(c)所示。

3.3.3 廢墟場景

廢墟場景由水泥磚塊、家具、樹枝、麻袋等物體堆砌而成。機器人需要正向、反向通過廢墟,整體水平移動距離約8 m。具體效果如圖12所示。

綜上所述,在仿真實驗中,構型規劃算法能夠使機器人順利通過高難度的障礙地形,且相比深度強化學習算法具有更好的效果;在實物實驗中,進一步測試了算法的實際運行性能,結果表明算法能夠有效適應難度較高的單側臺階,輕松地斜向上下樓梯,平穩地通過復雜的廢墟地形。

(a) 0.3 m單側臺階場景越障過程(a) Obstacle crossing process in the 0.3 m single-side-step scenario

(b) 斜向上樓梯場景越障過程(b) Obstacle crossing process in the up-side oblique stairs scenario

(c) 斜向下樓梯場景越障過程(c) Obstacle crossing process in the down-side oblique stairs scenario圖11 機器人在真實單側臺階與斜向樓梯場景的越障過程Fig.11 Processes of the robot moving through the single-side-step and oblique stairs scenarios in real-world

表2 實物單側臺階與斜向樓梯場景的量化指標

(a) 廢墟場景正向越障過程(a) Obstacle crossing process along forward direction in ruins scenario

(b) 廢墟場景反向越障過程(b) Obstacle crossing process along reverse direction in ruins scenario圖12 機器人在廢墟場景的越障過程Fig.12 Processes of the robot moving through the ruins scenario in real-world

表3 實物廢墟場景的量化指標

4 結論

本文提出了一種適用于障礙地形的搜救機器人越障構型規劃方法。該方法以高效的姿態預測算法為核心,將不規則的地形剖面描述為離散的點云,并建立了履帶機器人單側姿態預測問題模型;進一步設計了該問題的解析求解算法,使得求解時間大為縮短;結合動態規劃方法與滾動優化思想,構建了姿態預測—狀態評價—構型規劃三個步驟滾動進行的越障構型規劃算法,并針對非對稱地形設計了構型規劃策略,獲得了優化的機器人越障構型序列和對應的擺臂動作。最后在多個場景中進行了實驗,并與深度強化學習方法進行了對比,仿真與實物實驗結果表明該算法能夠使機器人在復雜地形上實現優異的越障效果。在未來的工作中,將研究滾轉角較大時機器人整體姿態預測算法,以更好地適應非對稱地形;進一步提高姿態預測算法與構型規劃算法的協同程度,探索效率更高、適用性更強的自主越障方法。