基于激光雷達(dá)的火箭推進(jìn)劑加注機(jī)器人姿態(tài)估計(jì)研究

陸 煜 蔣 贊 洪 剛 頓向明

1. 上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院，上海，200240 2. 上海航天技術(shù)研究院第805研究所，上海，201108

0 引言

在運(yùn)載火箭發(fā)射的環(huán)節(jié)中，對(duì)火箭箭體進(jìn)行推進(jìn)劑燃料加注的操作是重要的一環(huán)，目前我國(guó)主要還是依靠人工來(lái)進(jìn)行燃料加注的操作[1]。由于推進(jìn)劑具有易揮發(fā)性、易燃易爆性、劇毒性和腐蝕性等化學(xué)性質(zhì)，因此對(duì)火箭進(jìn)行燃料加注的操作具有極大危險(xiǎn)性，且會(huì)持續(xù)損害工作人員的健康[2]。近年來(lái)，對(duì)火箭箭體進(jìn)行推進(jìn)劑燃料加注正逐漸用機(jī)器人代替人工加注，而其中加注機(jī)器人對(duì)火箭箭體活門(mén)的精準(zhǔn)定位和姿態(tài)估計(jì)是其成功對(duì)接的關(guān)鍵，是實(shí)現(xiàn)自動(dòng)化的重要一步，因此進(jìn)行火箭加注機(jī)器人對(duì)接的姿態(tài)估計(jì)研究具有積極意義[3]。

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

為解決上述火箭推進(jìn)劑加注機(jī)器人在對(duì)接過(guò)程中的姿態(tài)估計(jì)問(wèn)題，本文提出了一種基于單線激光雷達(dá)的姿態(tài)估計(jì)方法，在文獻(xiàn)[10]定位方法的基礎(chǔ)上，通過(guò)電機(jī)旋轉(zhuǎn)給激光雷達(dá)一個(gè)旋轉(zhuǎn)自由度，基于單線激光雷達(dá)獲取三維點(diǎn)云信息，并設(shè)計(jì)了一種對(duì)人工信標(biāo)進(jìn)行識(shí)別和姿態(tài)估計(jì)的算法。最后，通過(guò)實(shí)驗(yàn)驗(yàn)證了該算法的穩(wěn)定性、精確性和可靠性，該算法可在火箭停放位置不平整時(shí)檢測(cè)出偏差的姿態(tài)。

1 箭體活門(mén)的姿態(tài)估計(jì)策略

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

圖1 目標(biāo)板Fig.1 Target board

定位的整體策略如下：

(1)通過(guò)雷達(dá)掃描獲取單幀二維點(diǎn)云信息，從二維點(diǎn)云中提取直線拐點(diǎn)信息；

(2)電機(jī)帶動(dòng)雷達(dá)擺動(dòng)，重復(fù)步驟(1)獲取多幀的直線拐點(diǎn)信息；

(3)當(dāng)電機(jī)擺動(dòng)完一個(gè)周期后，將二維的拐點(diǎn)通過(guò)坐標(biāo)轉(zhuǎn)換到三維坐標(biāo)中，從而得到三維點(diǎn)云；

(4)從三維點(diǎn)云中識(shí)別出目標(biāo)板；

(5)計(jì)算得到目標(biāo)板相對(duì)于雷達(dá)的姿態(tài)；

(6)根據(jù)計(jì)算得到的姿態(tài)控制加注機(jī)器人逐步調(diào)整姿態(tài)，直到與信標(biāo)在同一平面；

(7)通過(guò)雷達(dá)二維點(diǎn)云對(duì)目標(biāo)板進(jìn)行定位和識(shí)別，并控制加注機(jī)器人靠近箭體活門(mén)對(duì)接。

2 目標(biāo)姿態(tài)估計(jì)算法

2.1 二維點(diǎn)云直線拐點(diǎn)提取

對(duì)掃描得到的二維點(diǎn)云數(shù)據(jù)進(jìn)行中值濾波，消除噪聲干擾，得到原始二維點(diǎn)云圖，見(jiàn)圖2。

圖2 原始二維點(diǎn)云圖Fig.2 Original 2D point cloud picture

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

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

(1)

式中，C0為根據(jù)雷達(dá)角度分辨力而人為調(diào)整的參數(shù)，實(shí)驗(yàn)中取C0=8 cm；α為雷達(dá)的角度分辨力。

目標(biāo)板的水平投影如圖3所示，目標(biāo)板由三條線段組成，這個(gè)形狀特殊，可區(qū)別于環(huán)境其他物體，又易于通過(guò)二維激光雷達(dá)識(shí)別。根據(jù)這個(gè)形狀特點(diǎn)，目標(biāo)板的目標(biāo)識(shí)別主要是提取點(diǎn)云中的直線和直線拐點(diǎn)特征。假設(shè)目標(biāo)板所在的簇類包含的點(diǎn)數(shù)為k，則將簇類第一個(gè)點(diǎn)記為a1，最后一個(gè)點(diǎn)記為ak，如圖4所示。

圖3 目標(biāo)板水平投影Fig.3 Horizontal projection of target board

圖4 拐點(diǎn)提取Fig.4 Finding turning point

拐點(diǎn)提取算法的流程如下：

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

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

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

(4) 將點(diǎn)a1改為點(diǎn)ak，重復(fù)步驟(2)、步驟(3)；

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

如圖5所示，提取的拐點(diǎn)為圖中白色的點(diǎn)。通過(guò)拐點(diǎn)提取算法識(shí)別了符合目標(biāo)板截面形狀的簇類，并提取了線段的拐點(diǎn)，對(duì)每一幀二維數(shù)據(jù)僅保留拐點(diǎn)信息來(lái)進(jìn)行后續(xù)姿態(tài)識(shí)別，節(jié)省了內(nèi)存空間和計(jì)算時(shí)間。

圖5 二維拐點(diǎn)Fig.5 2D turning point

2.2 二維拐點(diǎn)變換為三維拐點(diǎn)簇

將每幀二維雷達(dá)點(diǎn)云利用拐點(diǎn)提取算法得到的拐點(diǎn)通過(guò)坐標(biāo)變換轉(zhuǎn)為三維點(diǎn)云數(shù)據(jù)，并進(jìn)行數(shù)據(jù)關(guān)聯(lián)，從而獲得三維的拐點(diǎn)簇，后續(xù)對(duì)每個(gè)拐點(diǎn)簇的點(diǎn)進(jìn)行線段擬合，并進(jìn)行目標(biāo)識(shí)別。三維雷達(dá)坐標(biāo)的原點(diǎn)取二維雷達(dá)坐標(biāo)的原點(diǎn)，x軸、y軸方向分別為電機(jī)停在零點(diǎn)時(shí)雷達(dá)0°和90°的方向，z軸方向根據(jù)右手法則，指向雷達(dá)正上方。

將二維雷達(dá)坐標(biāo)轉(zhuǎn)為三維雷達(dá)坐標(biāo)，首先需要將雷達(dá)數(shù)據(jù)與電機(jī)編碼器數(shù)據(jù)根據(jù)數(shù)據(jù)采集 的時(shí)間戳進(jìn)行時(shí)間同步，每幀雷達(dá)數(shù)據(jù)將匹配一個(gè)電機(jī)轉(zhuǎn)角β，并根據(jù)下式將每個(gè)拐點(diǎn)從雷達(dá)二維極坐標(biāo)(Di,θi)轉(zhuǎn)換到三維雷達(dá)坐標(biāo)(xi,yi,zi)：

(2)

之后，對(duì)每幀拐點(diǎn)采用最近鄰域法[12]進(jìn)行數(shù)據(jù)關(guān)聯(lián)，將多幀拐點(diǎn)關(guān)聯(lián)成多個(gè)拐點(diǎn)簇。最近鄰域法是一種簡(jiǎn)單的數(shù)據(jù)關(guān)聯(lián)算法，它認(rèn)為前一幀數(shù)據(jù)中的某點(diǎn)距離后一幀數(shù)據(jù)中最近的點(diǎn)即為其相關(guān)聯(lián)的點(diǎn)。這種關(guān)聯(lián)算法的優(yōu)點(diǎn)是實(shí)現(xiàn)簡(jiǎn)單、計(jì)算快速，但這不是一種最優(yōu)關(guān)聯(lián)算法，易發(fā)生錯(cuò)誤關(guān)聯(lián)。由于是對(duì)提取的拐點(diǎn)進(jìn)行數(shù)據(jù)關(guān)聯(lián)，各個(gè)拐點(diǎn)間的距離相比關(guān)聯(lián)閾值有較大的差異，錯(cuò)誤關(guān)聯(lián)發(fā)生的可能性低，且對(duì)后續(xù)擬合直線影響不大，故采用該算法進(jìn)行分析研究。數(shù)據(jù)關(guān)聯(lián)算法的流程如下：

(1)計(jì)算第k幀所有拐點(diǎn)與第k-1幀某一拐點(diǎn)之間的距離，選取距離最小且未關(guān)聯(lián)的拐點(diǎn)；

(2)若距離小于設(shè)定的關(guān)聯(lián)閾值TD，則將這兩點(diǎn)關(guān)聯(lián)；

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

(4)對(duì)已關(guān)聯(lián)的點(diǎn)進(jìn)行卡爾曼濾波預(yù)測(cè)[13]，對(duì)未關(guān)聯(lián)的點(diǎn)記丟失次數(shù)加1，當(dāng)丟失次數(shù)過(guò)多則認(rèn)為目標(biāo)丟失；

(5)在電機(jī)擺動(dòng)的一個(gè)周期內(nèi)，對(duì)所有相鄰兩幀雷達(dá)數(shù)據(jù)重復(fù)步驟(1)～步驟(4)，以獲取多個(gè)關(guān)聯(lián)的拐點(diǎn)簇。

由于電機(jī)轉(zhuǎn)動(dòng)緩慢，z軸方向的點(diǎn)相比x軸、y軸的點(diǎn)排列較密，因此兩點(diǎn)之間的距離d2采用加權(quán)距離計(jì)算公式，根據(jù)點(diǎn)云密度關(guān)系，將z軸權(quán)重值設(shè)置為x軸與y軸權(quán)重值的1/2，如下式所示：

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

(3)

關(guān)聯(lián)閾值TD同樣需要考慮雷達(dá)點(diǎn)的位置偏移量，采用的公式為

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

(4)

式中，C2、C3為根據(jù)電機(jī)轉(zhuǎn)速而人為調(diào)整的參數(shù)，實(shí)驗(yàn)中分別取C2=1 cm,C3=0.01。

2.3 目標(biāo)板的識(shí)別與姿態(tài)估計(jì)

利用上述算法可得到雷達(dá)擺動(dòng)一個(gè)掃描周期內(nèi)的三維拐點(diǎn)簇，如圖6中的白點(diǎn)所示。假設(shè)第j條直線Lj可以表示為

Lj=(Pj,vj)

(5)

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

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

圖6 目標(biāo)板識(shí)別Fig.6 Recognition of target board

則目標(biāo)板識(shí)別算法的流程如下：

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

(2)計(jì)算所有擬合直線的vj與三維坐標(biāo)系z(mì)軸方向上單位向量的點(diǎn)乘積，若大于設(shè)定的常數(shù)閾值則剔除該拐點(diǎn)簇；

(3) 選取第k個(gè)拐點(diǎn)簇，計(jì)算點(diǎn)Pk到直線Lk+1的距離；

(4) 若兩個(gè)拐點(diǎn)簇?cái)M合直線的距離與目標(biāo)板某一段長(zhǎng)度M、L、N相差在設(shè)定的閾值內(nèi)，則認(rèn)為該段距離長(zhǎng)度是目標(biāo)板的一段長(zhǎng)度；

(5) 重復(fù)步驟(3)、步驟(4)，如果找到4個(gè)拐點(diǎn)簇，且相鄰兩個(gè)拐點(diǎn)簇?cái)M合直線的距離與目標(biāo)板的三段長(zhǎng)度相近，則認(rèn)為這4個(gè)拐點(diǎn)簇為目標(biāo)板。

3 實(shí)驗(yàn)與分析

為測(cè)試姿態(tài)估計(jì)算法的可靠性與準(zhǔn)確性，設(shè)計(jì)了基于二維激光雷達(dá)和閉環(huán)伺服電機(jī)的掃描裝置(圖7)，并根據(jù)算法編寫(xiě)了程序。二維激光雷達(dá)為SLAMTIC公司的RPLIDAR A2，掃描范圍為0°～360°，采樣頻率達(dá)每秒8000次。閉環(huán)伺服電機(jī)為NiMotion公司的STM5776A，編碼器精度達(dá)0.1°。實(shí)驗(yàn)場(chǎng)景僅將掃描裝置放置于目標(biāo)板前的一定距離進(jìn)行掃描，由于實(shí)際火箭對(duì)接過(guò)程所處環(huán)境較為簡(jiǎn)單，故不考慮目標(biāo)板遮擋的情況。

圖7 雷達(dá)掃描裝置Fig.7 Lidar scanning device

目標(biāo)板的識(shí)別是整個(gè)算法的第一步，從環(huán)境中準(zhǔn)確地識(shí)別出目標(biāo)板是后續(xù)姿態(tài)估計(jì)算法的基礎(chǔ)。目標(biāo)識(shí)別算法的檢測(cè)方法為：將目標(biāo)板放置于距離雷達(dá)0.5 m處，人為改變目標(biāo)板的初始姿態(tài)，運(yùn)行程序執(zhí)行目標(biāo)識(shí)別算法，對(duì)目標(biāo)進(jìn)行100次目標(biāo)識(shí)別，并計(jì)算識(shí)別準(zhǔn)確率。將文獻(xiàn)[10]中基于二維點(diǎn)云的目標(biāo)板識(shí)別原始算法與本文提出的基于三維點(diǎn)云的目標(biāo)識(shí)別算法的目標(biāo)識(shí)別結(jié)果進(jìn)行對(duì)比分析，并對(duì)實(shí)驗(yàn)結(jié)果進(jìn)行討論。

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

表1 目標(biāo)板識(shí)別準(zhǔn)確率

為檢驗(yàn)本文算法對(duì)目標(biāo)板姿態(tài)估計(jì)的精度和可靠性，設(shè)計(jì)了姿態(tài)估計(jì)實(shí)驗(yàn)。實(shí)驗(yàn)過(guò)程中，將目標(biāo)板放置于距離雷達(dá)0.5 m處，人為改變目標(biāo)板的初始姿態(tài)，運(yùn)行程序執(zhí)行目標(biāo)識(shí)別和姿態(tài)估計(jì)算法，對(duì)目標(biāo)進(jìn)行100次姿態(tài)估計(jì)，計(jì)算出估計(jì)的姿態(tài)與真(TV)的均值誤差(ME)和標(biāo)準(zhǔn)誤差(SE)。

目標(biāo)板的姿態(tài)估計(jì)結(jié)果如表2所示。文獻(xiàn)[10]中設(shè)計(jì)的火箭推進(jìn)劑加注機(jī)器人的最大允許誤差為4°，由表2實(shí)驗(yàn)結(jié)果可見(jiàn)，本文算法的姿態(tài)估計(jì)誤差值在4°以內(nèi)，具有較高的準(zhǔn)確性，滿足對(duì)接的精度要求。

表2 目標(biāo)板姿態(tài)估計(jì)

4 結(jié)論

(1)為解決運(yùn)載火箭燃料推進(jìn)劑加注過(guò)程中加注機(jī)器人與箭體活門(mén)對(duì)接環(huán)節(jié)中產(chǎn)生的姿態(tài)估計(jì)問(wèn)題，提出了一種基于二維激光雷達(dá)識(shí)別人工信標(biāo)的姿態(tài)估計(jì)方法。通過(guò)電機(jī)帶動(dòng)二維激光雷達(dá)旋轉(zhuǎn)，獲取三維點(diǎn)云信息，并從中識(shí)別出人工設(shè)置的目標(biāo)板，對(duì)其進(jìn)行姿態(tài)估計(jì)來(lái)獲得箭體活門(mén)與加注機(jī)器人的相對(duì)姿態(tài)，根據(jù)姿態(tài)調(diào)整加注機(jī)器人與箭體活門(mén)間的姿態(tài)關(guān)系，使得雷達(dá)掃描平面與目標(biāo)板橫截面平行。

(2)根據(jù)需求搭建了實(shí)驗(yàn)裝置，測(cè)試并驗(yàn)證了激光雷達(dá)目標(biāo)識(shí)別和姿態(tài)估計(jì)算法的識(shí)別準(zhǔn)確率及姿態(tài)估計(jì)精度。實(shí)驗(yàn)結(jié)果表明，該算法能夠在目標(biāo)板處于不同姿態(tài)時(shí)準(zhǔn)確地識(shí)別目標(biāo)板，且對(duì)目標(biāo)板的姿態(tài)估計(jì)精度較高，最大允許誤差在4°以內(nèi)，證明了該算法的可靠性與準(zhǔn)確性。

(3)相比原始的二維雷達(dá)目標(biāo)板識(shí)別算法，本文算法有如下優(yōu)點(diǎn)：①能夠在目標(biāo)板初始姿態(tài)不平整的狀況下完成目標(biāo)板的準(zhǔn)確識(shí)別和姿態(tài)估計(jì)；②只保留二維拐點(diǎn)信息進(jìn)行三維目標(biāo)識(shí)別，節(jié)省了內(nèi)存和計(jì)算時(shí)間。

綜上，本文算法能夠確保火箭加注機(jī)器人與箭體活門(mén)的準(zhǔn)確對(duì)接，具有一定的實(shí)際應(yīng)用價(jià)值。