基于激光雷達的火箭推進劑加注機器人姿態估計研究

陸 煜 蔣 贊 洪 剛 頓向明

1. 上海交通大學機械與動力工程學院，上海，200240 2. 上海航天技術研究院第805研究所，上海，201108

0 引言

在運載火箭發射的環節中，對火箭箭體進行推進劑燃料加注的操作是重要的一環，目前我國主要還是依靠人工來進行燃料加注的操作[1]。由于推進劑具有易揮發性、易燃易爆性、劇毒性和腐蝕性等化學性質，因此對火箭進行燃料加注的操作具有極大危險性，且會持續損害工作人員的健康[2]。近年來，對火箭箭體進行推進劑燃料加注正逐漸用機器人代替人工加注，而其中加注機器人對火箭箭體活門的精準定位和姿態估計是其成功對接的關鍵，是實現自動化的重要一步，因此進行火箭加注機器人對接的姿態估計研究具有積極意義[3]。

目前，美國、俄羅斯及中國等航天大國都在積極進行火箭推進劑加注機器人的研究。美國基于火箭箭體的加注對接技術[4-6]，將加注對接裝置安裝于火箭箭體，在火箭發射時加注對接裝置會依靠慣性自主掉落。這種方法操作簡單，成本低，但并非完全自動化。俄羅斯火箭臍帶自動對接是采用機械導向的方法來進行定位[7-8]，這種方法同樣操作簡單，但是僅靠機械定位時，對火箭箭體的初始姿態精度要求極高。趙帥鋒等[9]開發的自動對接機器人可利用攝像頭獲取環境信息，并通過視覺算法計算得到箭體活門的位置姿態信息，但采用視覺算法受光照和火箭泄漏的四氧化二氮影響較大。游嘉偉等[10]采用單線激光雷達識別人工信標的方式定位，這種方法的魯棒性和精度高，然而由于單線激光雷達只能獲取二維平面點云，因此要求目標板初始位置與雷達在同一平面，這同樣對火箭箭體的初始姿態有較高要求。

為解決上述火箭推進劑加注機器人在對接過程中的姿態估計問題，本文提出了一種基于單線激光雷達的姿態估計方法，在文獻[10]定位方法的基礎上，通過電機旋轉給激光雷達一個旋轉自由度，基于單線激光雷達獲取三維點云信息，并設計了一種對人工信標進行識別和姿態估計的算法。最后，通過實驗驗證了該算法的穩定性、精確性和可靠性，該算法可在火箭停放位置不平整時檢測出偏差的姿態。

1 箭體活門的姿態估計策略

對于箭體活門的定位，主要通過設計一個形狀特殊又易于識別的人工信標，將其固定安裝于火箭箭體上，然后通過傳感器識別信標并定位。本研究設計的人工信標為一塊目標板，其模型如圖1所示。本研究采用單線激光雷達[11]作為識別信標的傳感器，在能夠準確識別目標的基礎上，相比于視覺，單線激光雷達受光照和環境的影響較小，且精度更高，而相比于多線激光雷達，它體積小、價格便宜、算法簡易、實時性高。由于單線激光雷達只能獲取一個平面的點云信息，故通過加裝電機給雷達一個旋轉自由度，使其能夠獲取三維點云信息，并計算出目標板相對于雷達的相對姿態。

圖1 目標板Fig.1 Target board

定位的整體策略如下：

(1)通過雷達掃描獲取單幀二維點云信息，從二維點云中提取直線拐點信息；

(2)電機帶動雷達擺動，重復步驟(1)獲取多幀的直線拐點信息；

(3)當電機擺動完一個周期后，將二維的拐點通過坐標轉換到三維坐標中，從而得到三維點云；

(4)從三維點云中識別出目標板；

(5)計算得到目標板相對于雷達的姿態；

(6)根據計算得到的姿態控制加注機器人逐步調整姿態，直到與信標在同一平面；

(7)通過雷達二維點云對目標板進行定位和識別，并控制加注機器人靠近箭體活門對接。

2 目標姿態估計算法

2.1 二維點云直線拐點提取

對掃描得到的二維點云數據進行中值濾波，消除噪聲干擾，得到原始二維點云圖，見圖2。

圖2 原始二維點云圖Fig.2 Original 2D point cloud picture

激光雷達掃描同一物體得到的兩相鄰反射點之間的距離變化較小。根據這一特點，若相鄰兩點之間的間距小于某個閾值TV，則可將其歸為同一個簇類(即聚類)，后續將對每個簇類進行特征提取。設激光雷達掃描一圈得到的反射點集為{ai}，每個點ai在雷達極坐標系下的坐標為(Di，θi)，其中Di為位置偏移量，θi為旋轉角度。由于激光雷達點云有近密遠疏的特性，故本算法中使用的閾值公式需要考慮反射點的位置偏移量，可表示為

TV=C0+C1min(Di,Di+1)

(1)

式中，C0為根據雷達角度分辨力而人為調整的參數，實驗中取C0=8 cm；α為雷達的角度分辨力。

目標板的水平投影如圖3所示，目標板由三條線段組成，這個形狀特殊，可區別于環境其他物體，又易于通過二維激光雷達識別。根據這個形狀特點，目標板的目標識別主要是提取點云中的直線和直線拐點特征。假設目標板所在的簇類包含的點數為k，則將簇類第一個點記為a1，最后一個點記為ak，如圖4所示。

圖3 目標板水平投影Fig.3 Horizontal projection of target board

圖4 拐點提取Fig.4 Finding turning point

拐點提取算法的流程如下：

(1)選取中間點ak/2(k/2向下取整)；

(2)計算點a1與點ak/2之間所有點到直線a1ak/2的距離；

(3) 將其中距離超過閾值且距離最大的點作為拐點；

(4) 將點a1改為點ak，重復步驟(2)、步驟(3)；

(5) 若在上述步驟中找到了拐點，則將點a1和點ak也作為拐點。

如圖5所示，提取的拐點為圖中白色的點。通過拐點提取算法識別了符合目標板截面形狀的簇類，并提取了線段的拐點，對每一幀二維數據僅保留拐點信息來進行后續姿態識別，節省了內存空間和計算時間。

圖5 二維拐點Fig.5 2D turning point

2.2 二維拐點變換為三維拐點簇

將每幀二維雷達點云利用拐點提取算法得到的拐點通過坐標變換轉為三維點云數據，并進行數據關聯，從而獲得三維的拐點簇，后續對每個拐點簇的點進行線段擬合，并進行目標識別。三維雷達坐標的原點取二維雷達坐標的原點，x軸、y軸方向分別為電機停在零點時雷達0°和90°的方向，z軸方向根據右手法則，指向雷達正上方。

將二維雷達坐標轉為三維雷達坐標，首先需要將雷達數據與電機編碼器數據根據數據采集 的時間戳進行時間同步，每幀雷達數據將匹配一個電機轉角β，并根據下式將每個拐點從雷達二維極坐標(Di,θi)轉換到三維雷達坐標(xi,yi,zi)：

(2)

之后，對每幀拐點采用最近鄰域法[12]進行數據關聯，將多幀拐點關聯成多個拐點簇。最近鄰域法是一種簡單的數據關聯算法，它認為前一幀數據中的某點距離后一幀數據中最近的點即為其相關聯的點。這種關聯算法的優點是實現簡單、計算快速，但這不是一種最優關聯算法，易發生錯誤關聯。由于是對提取的拐點進行數據關聯，各個拐點間的距離相比關聯閾值有較大的差異，錯誤關聯發生的可能性低，且對后續擬合直線影響不大，故采用該算法進行分析研究。數據關聯算法的流程如下：

(1)計算第k幀所有拐點與第k-1幀某一拐點之間的距離，選取距離最小且未關聯的拐點；

(2)若距離小于設定的關聯閾值TD，則將這兩點關聯；

(3)選擇第k-1幀另一拐點，重復步驟(1)、步驟(2)，直到遍歷完第k-1幀的所有拐點；

(4)對已關聯的點進行卡爾曼濾波預測[13]，對未關聯的點記丟失次數加1，當丟失次數過多則認為目標丟失；

(5)在電機擺動的一個周期內，對所有相鄰兩幀雷達數據重復步驟(1)～步驟(4)，以獲取多個關聯的拐點簇。

由于電機轉動緩慢，z軸方向的點相比x軸、y軸的點排列較密，因此兩點之間的距離d2采用加權距離計算公式，根據點云密度關系，將z軸權重值設置為x軸與y軸權重值的1/2，如下式所示：

d2=0.4(xk-xk-1)2+0.4(yk-yk-1)2+
0.2(zk-zk-1)2

(3)

關聯閾值TD同樣需要考慮雷達點的位置偏移量，采用的公式為

TD=C2+C3min(Di,Di+1)

(4)

式中，C2、C3為根據電機轉速而人為調整的參數，實驗中分別取C2=1 cm,C3=0.01。

2.3 目標板的識別與姿態估計

利用上述算法可得到雷達擺動一個掃描周期內的三維拐點簇，如圖6中的白點所示。假設第j條直線Lj可以表示為

Lj=(Pj,vj)

(5)

Pj=(px,py,pz)vj=(vx,vy,vz)

式中，Pj為直線j上的某點;vj為直線j的單位方向向量。

圖6 目標板識別Fig.6 Recognition of target board

則目標板識別算法的流程如下：

(1)對每個拐點簇的點采用最小二乘法進行直線擬合，可以得到每條直線的Pj和vj；

(2)計算所有擬合直線的vj與三維坐標系z軸方向上單位向量的點乘積，若大于設定的常數閾值則剔除該拐點簇；

(3) 選取第k個拐點簇，計算點Pk到直線Lk+1的距離；

(4) 若兩個拐點簇擬合直線的距離與目標板某一段長度M、L、N相差在設定的閾值內，則認為該段距離長度是目標板的一段長度；

(5) 重復步驟(3)、步驟(4)，如果找到4個拐點簇，且相鄰兩個拐點簇擬合直線的距離與目標板的三段長度相近，則認為這4個拐點簇為目標板。

3 實驗與分析

為測試姿態估計算法的可靠性與準確性，設計了基于二維激光雷達和閉環伺服電機的掃描裝置(圖7)，并根據算法編寫了程序。二維激光雷達為SLAMTIC公司的RPLIDAR A2，掃描范圍為0°～360°，采樣頻率達每秒8000次。閉環伺服電機為NiMotion公司的STM5776A，編碼器精度達0.1°。實驗場景僅將掃描裝置放置于目標板前的一定距離進行掃描，由于實際火箭對接過程所處環境較為簡單，故不考慮目標板遮擋的情況。

圖7 雷達掃描裝置Fig.7 Lidar scanning device

目標板的識別是整個算法的第一步，從環境中準確地識別出目標板是后續姿態估計算法的基礎。目標識別算法的檢測方法為：將目標板放置于距離雷達0.5 m處，人為改變目標板的初始姿態，運行程序執行目標識別算法，對目標進行100次目標識別，并計算識別準確率。將文獻[10]中基于二維點云的目標板識別原始算法與本文提出的基于三維點云的目標識別算法的目標識別結果進行對比分析，并對實驗結果進行討論。

目標板識別算法結果如表1所示，由實驗結果可知，基于二維點云的目標識別算法對目標板初始姿態的精度要求較高，當目標板的俯仰角或翻滾角較大時，線段夾角和線段長度的判斷閾值失效，從而導致目標識別率降低；而本文提出的算法將掃描得到的三維拐點簇進行了卡爾曼濾波處理，誤差較小，且將擬合直線間的距離作為識別判斷條件，可以在目標板初始姿態偏移的情況下準確識別結果。

表1 目標板識別準確率

為檢驗本文算法對目標板姿態估計的精度和可靠性，設計了姿態估計實驗。實驗過程中，將目標板放置于距離雷達0.5 m處，人為改變目標板的初始姿態，運行程序執行目標識別和姿態估計算法，對目標進行100次姿態估計，計算出估計的姿態與真(TV)的均值誤差(ME)和標準誤差(SE)。

目標板的姿態估計結果如表2所示。文獻[10]中設計的火箭推進劑加注機器人的最大允許誤差為4°，由表2實驗結果可見，本文算法的姿態估計誤差值在4°以內，具有較高的準確性，滿足對接的精度要求。

表2 目標板姿態估計

4 結論

(1)為解決運載火箭燃料推進劑加注過程中加注機器人與箭體活門對接環節中產生的姿態估計問題，提出了一種基于二維激光雷達識別人工信標的姿態估計方法。通過電機帶動二維激光雷達旋轉，獲取三維點云信息，并從中識別出人工設置的目標板，對其進行姿態估計來獲得箭體活門與加注機器人的相對姿態，根據姿態調整加注機器人與箭體活門間的姿態關系，使得雷達掃描平面與目標板橫截面平行。

(2)根據需求搭建了實驗裝置，測試并驗證了激光雷達目標識別和姿態估計算法的識別準確率及姿態估計精度。實驗結果表明，該算法能夠在目標板處于不同姿態時準確地識別目標板，且對目標板的姿態估計精度較高，最大允許誤差在4°以內，證明了該算法的可靠性與準確性。

(3)相比原始的二維雷達目標板識別算法，本文算法有如下優點：①能夠在目標板初始姿態不平整的狀況下完成目標板的準確識別和姿態估計；②只保留二維拐點信息進行三維目標識別，節省了內存和計算時間。

綜上，本文算法能夠確保火箭加注機器人與箭體活門的準確對接，具有一定的實際應用價值。