多特征融合的月面采樣遙操作視覺(jué)定位方法

劉傳凱，李東升，謝劍鋒，雷俊雄，袁春強(qiáng), 2，何錫明

1. 北京航天飛行控制中心，北京 100190

2. 航天飛行動(dòng)力學(xué)技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100190

3. 江西理工大學(xué) 電氣工程與自動(dòng)化學(xué)院，贛州 341000

2020年12月1日，嫦娥五號(hào)探測(cè)器成功在月球正面呂姆克山脈以北地區(qū)軟著陸，次日經(jīng)過(guò)約19 h月面工作，順利完成月球表面采樣，并已按預(yù)定形式將樣品封裝保存在上升器攜帶的貯存裝置中，完成了我國(guó)首次月球無(wú)人采樣封裝任務(wù)[1]。在進(jìn)行月面采樣封裝任務(wù)過(guò)程中，由于四自由度機(jī)械臂細(xì)長(zhǎng)柔性特點(diǎn)，使得采樣和放樣操作中機(jī)械臂采樣器末端難以準(zhǔn)確到達(dá)目標(biāo)位置，造成采樣碰撞和放樣外撒的風(fēng)險(xiǎn)。為此引入動(dòng)態(tài)視覺(jué)定位引導(dǎo)技術(shù)，控制機(jī)械臂實(shí)現(xiàn)末端精確操作，以確保采樣和放樣過(guò)程的精確性和安全性。

最早實(shí)施月面無(wú)人采樣的是蘇聯(lián)，于1970年發(fā)射月球16號(hào)探測(cè)器著陸月球表面，實(shí)現(xiàn)了人類首次月面無(wú)人采樣返回任務(wù)[2]。此次任務(wù)中的采樣機(jī)構(gòu)較為單一，主要采用0.9 m長(zhǎng)的鉆臂進(jìn)行定點(diǎn)鉆取采樣，并未考慮機(jī)構(gòu)形變對(duì)采樣的影響，相機(jī)圖像主要用來(lái)人工判讀采樣狀態(tài)，并未進(jìn)行定位引導(dǎo)。后續(xù)實(shí)施的月球20、24等月面采樣任務(wù)也基本沿用了月球16的探測(cè)模式[3-4]。應(yīng)用視覺(jué)測(cè)量手段引導(dǎo)機(jī)械臂采樣操作方面比較成功的案例是美國(guó)NASA的鳳凰號(hào)(Phoenix)和洞察號(hào)(Insight)火星探測(cè)任務(wù)[5-6]。鳳凰號(hào)探測(cè)器和洞察號(hào)探測(cè)器分別于2008年和2018年著陸火星表面，在火星表面實(shí)施采樣、儀器安放和科學(xué)探測(cè)等活動(dòng)，視覺(jué)引導(dǎo)的機(jī)械臂操作是支撐各類任務(wù)實(shí)施的關(guān)鍵[7-8]。

鳳凰號(hào)帶有四自由度機(jī)械臂，機(jī)械臂末端裝有相機(jī)RAC(Robotic Arm Camera)和配套使用的紅、綠、藍(lán)三色LED燈組成的照明系統(tǒng)以及裝載在桅桿上的表面立體成像儀(SSI)。RAC相機(jī)可以提供低至22 μm/像素分辨率的機(jī)械臂鏟斗近距離土壤偽真彩色圖像。鳳凰號(hào)配有視覺(jué)圖像處理與測(cè)量系統(tǒng)[9]，能夠完成RAC和SSI等相機(jī)圖像的處理與目標(biāo)定位，任務(wù)中主要采用視差測(cè)量和三角測(cè)量方法實(shí)現(xiàn)對(duì)目標(biāo)或地形網(wǎng)格數(shù)據(jù)的定位，輔助機(jī)械臂實(shí)現(xiàn)對(duì)火星表面樣品的采集任務(wù)[10]。

洞察號(hào)探測(cè)器在繼承鳳凰號(hào)探測(cè)器的基礎(chǔ)上，進(jìn)一步增強(qiáng)儀器安放和鉆取探測(cè)功能，深度開(kāi)展火星內(nèi)部狀態(tài)考察。洞察號(hào)探測(cè)器配備了廣角視圖的儀器環(huán)境相機(jī)[11](Instrument Context Camera，ICC)，用于監(jiān)視探測(cè)器部署過(guò)程；攜帶了一個(gè)四自由度機(jī)械臂(Instrument Deployment Arm，IDA)，并在機(jī)械臂上裝載一個(gè)單目相機(jī)(Instrument Deployment Camera，IDC)，用于獲取探測(cè)器以及周圍地形的圖像[12]，引導(dǎo)機(jī)械臂進(jìn)行儀器安放操作。跟鳳凰號(hào)相比，洞察號(hào)探測(cè)器對(duì)鳳凰號(hào)的視覺(jué)測(cè)量系統(tǒng)進(jìn)行改進(jìn)，增加了基準(zhǔn)定位法和立體定位法2種定位方法[13-14]。

基準(zhǔn)定位法主要包括2類，第1類是基于圓形單點(diǎn)基準(zhǔn)和AprilTag靶標(biāo)基準(zhǔn)的聯(lián)合定位方法[13]。其中單點(diǎn)基準(zhǔn)由簡(jiǎn)單的十字準(zhǔn)線和黑白相間的象限組成，便于在遠(yuǎn)距離和低分辨率條件下準(zhǔn)確地檢測(cè)到基準(zhǔn)圓心[15]；AprilTag是方形靶標(biāo)，中間有不對(duì)稱圖案，每個(gè)AprilTag靶標(biāo)有唯一編碼，通過(guò)檢測(cè)AprilTag的4個(gè)角及可選的中心點(diǎn)就可以準(zhǔn)確給出目標(biāo)儀器的位姿[16-17]。洞察號(hào)任務(wù)中采用橢圓擬合算法來(lái)檢測(cè)基準(zhǔn)點(diǎn)，基準(zhǔn)定位位置誤差約為±1.5 cm、姿態(tài)誤差約為±2°，在實(shí)際任務(wù)中定位位置誤差約為±0.5 cm。第2類是抓鉤定位法[15]，通過(guò)將機(jī)械臂上的抓鉤在儀器上方多次移動(dòng)拍攝圖像實(shí)現(xiàn)定位，任務(wù)中首先移動(dòng)到儀器定位抓鉤上方3 cm，再上下移動(dòng)1 cm，在每個(gè)姿勢(shì)拍攝圖像。利用圖像準(zhǔn)確估計(jì)垂直于機(jī)械臂切向誤差，通過(guò)高低圖像之間的運(yùn)動(dòng)估計(jì)出重力方向誤差，再通過(guò)機(jī)械臂抓鉤的影子和儀器定位抓鉤的相對(duì)關(guān)系估計(jì)徑向誤差，最終實(shí)現(xiàn)3個(gè)方向的定位。該方法主要用于完善洞察號(hào)上抓斗、鏟斗等各類儀器的定位。由于該定位方法采用近距離抵近觀測(cè)方法，因此定位同時(shí)便于確定機(jī)械臂對(duì)儀器的可達(dá)性和可操作性。

立體定位法是在洞察號(hào)部署過(guò)程中，通過(guò)移動(dòng)IDA和IDC一小段距離來(lái)模擬2個(gè)不同的攝像頭獲取立體像對(duì)，再根據(jù)生成的立體網(wǎng)格點(diǎn)提取儀器的位置。此方法通過(guò)設(shè)計(jì)IDA移動(dòng)的距離和相機(jī)拍攝位姿來(lái)保證2次拍攝的圖像中有足夠的目標(biāo)及其周圍的特征，以保證圖像匹配的關(guān)聯(lián)性[18]。此方法需要考慮相機(jī)的位姿定位精度和視場(chǎng)中是否包含標(biāo)尺信息等因素，主要用于某些特殊情況(如其他相機(jī)故障)下的目標(biāo)位姿獲取。基準(zhǔn)定位法和立體定位法的組合應(yīng)用于洞察號(hào)在火星表面的儀器安放與部署任務(wù)中，實(shí)現(xiàn)了儀器安放過(guò)程的準(zhǔn)確引導(dǎo)和儀器安放位姿的準(zhǔn)確測(cè)量。

中國(guó)嫦娥五號(hào)探測(cè)器攜帶細(xì)長(zhǎng)結(jié)構(gòu)的四自由度機(jī)械臂，在地面遙操作中心的控制下，利用該機(jī)械臂完成表取采樣、樣品放置、樣品罐抓取和樣品罐放入上升器密封罐等操作任務(wù)。由于細(xì)長(zhǎng)機(jī)械臂在月面微重力作用下的柔性形變不可預(yù)知，因此利用視覺(jué)引導(dǎo)的方式完成精確操作控制。在嫦娥五號(hào)著陸器、上升器和機(jī)械臂上帶有多個(gè)相機(jī)，如圖1所示，主要包括采樣區(qū)觀測(cè)雙目相機(jī)、采樣器相機(jī)、臂載相機(jī)[19]和多個(gè)監(jiān)視相機(jī)，利用這些相機(jī)測(cè)量引導(dǎo)完成對(duì)采樣、放樣、抓罐和放罐等操作任務(wù)。

圖1 著陸器、上升器和機(jī)械臂上的各類相機(jī)

本文針對(duì)采樣、放樣、抓罐和放罐等操作中的視覺(jué)引導(dǎo)問(wèn)題，提出了基于光束逆投影的多特征融合視覺(jué)定位算法框架，通過(guò)對(duì)靜態(tài)相機(jī)測(cè)量動(dòng)態(tài)目標(biāo)和動(dòng)態(tài)相機(jī)測(cè)量靜態(tài)目標(biāo)的過(guò)程進(jìn)行抽象建模，建立了雙目、單目相機(jī)對(duì)圓形目標(biāo)和靶標(biāo)的組合定位優(yōu)化模型，并分析了位姿誤差對(duì)定位迭代過(guò)程收斂性的影響，實(shí)現(xiàn)了不同操作應(yīng)用場(chǎng)景下適應(yīng)不同相機(jī)數(shù)量的多類特征組合定位。通過(guò)仿真和實(shí)際應(yīng)用實(shí)驗(yàn)，驗(yàn)證了上述框架和方法的有效性。

本文第1節(jié)介紹多特征融合視覺(jué)定位模型，第2節(jié)進(jìn)行位姿誤差對(duì)定位模型收斂性的影響分析，第3節(jié)介紹仿真實(shí)驗(yàn)和嫦娥五號(hào)任務(wù)中的應(yīng)用情況。最后給出本文結(jié)論。

1 多特征融合視覺(jué)定位建模

嫦娥五號(hào)在月面執(zhí)行采樣、放樣、抓罐、放罐等任務(wù)的過(guò)程中，受細(xì)長(zhǎng)結(jié)構(gòu)機(jī)械臂柔性形變的影響，機(jī)械臂末端操作存在一定偏差，難以滿足采樣、放樣、抓罐和放罐等任務(wù)的精度要求，需要依靠視覺(jué)相機(jī)的引導(dǎo)才能實(shí)現(xiàn)機(jī)械臂末端的精確操作控制。由于機(jī)械臂操作過(guò)程涉及的環(huán)節(jié)和場(chǎng)景較多，每個(gè)環(huán)節(jié)對(duì)操作精度的要求不同，各個(gè)場(chǎng)景下可使用的相機(jī)及其組合模式較多且各相機(jī)視場(chǎng)范圍和觀測(cè)特征各不相同，對(duì)相機(jī)的統(tǒng)一規(guī)劃使用、實(shí)現(xiàn)各類操作下的高精度測(cè)量引導(dǎo)，是月面采樣封裝任務(wù)的一個(gè)難題。本節(jié)針對(duì)上述問(wèn)題，提出了基于光束逆投影的多特征融合視覺(jué)定位算法框架，從靜態(tài)相機(jī)對(duì)動(dòng)態(tài)目標(biāo)測(cè)量和動(dòng)態(tài)相機(jī)對(duì)靜態(tài)目標(biāo)測(cè)量2個(gè)層面，設(shè)計(jì)了多場(chǎng)景下的相機(jī)測(cè)量引導(dǎo)模式，為多相機(jī)的統(tǒng)一規(guī)劃管理和問(wèn)題的簡(jiǎn)化解決提供了支撐。

基于光束逆投影的多特征融合框架設(shè)計(jì)如下。假設(shè)空間中有N個(gè)相機(jī)，其中N1個(gè)靜態(tài)相機(jī)，即固定在著陸器或上升器上、不隨機(jī)械臂運(yùn)動(dòng)的相機(jī)；N2個(gè)動(dòng)態(tài)相機(jī)，即臂載相機(jī)或者采樣器相機(jī)。在相機(jī)視場(chǎng)中包含自然特征(如圓形目標(biāo))、靶標(biāo)特征2類特征，自然特征的幾何尺寸已知、靶標(biāo)之間的相對(duì)三維坐標(biāo)已知。相機(jī)對(duì)2類特征的測(cè)量問(wèn)題如圖2所示，相機(jī)對(duì)2類特征的相對(duì)測(cè)量均可通過(guò)統(tǒng)一的測(cè)量模型實(shí)現(xiàn),描述如下：

(1)

(2)

圖2 多特征融合視覺(jué)測(cè)量示意圖

圖3 基于光束逆投影的目標(biāo)特征點(diǎn)幾何表征

(3)

(4)

nk=Rz(φk)Rx(ψk)n0=

[sinφksinψk,-cosφksinψk,cosψk]T

(5)

(6)

(7)

在此基礎(chǔ)上，根據(jù)已知靶標(biāo)特征的相對(duì)位置和自然特征的幾何形狀特性構(gòu)建誤差方程。假設(shè)僅有靶標(biāo)和圓形目標(biāo)2類特征，則光束逆投影誤差可以寫(xiě)作：

(8)

(9)

2 定位模型的收斂性分析

2.1 目標(biāo)位姿變化對(duì)特征誤差函數(shù)的影響

圖4 誤差函數(shù)隨圓形目標(biāo)位置誤差的變化

圖5 誤差函數(shù)隨圓形目標(biāo)姿態(tài)角度誤差的變化

圖6 不同對(duì)應(yīng)的空間圓與實(shí)際目標(biāo)圓

圖7 圓形目標(biāo)誤差函數(shù)在不同真值情況下的變化

同理改變圓形目標(biāo)的位置真值，仿真位置真值變化對(duì)誤差函數(shù)曲面形狀和收斂性的影響，發(fā)現(xiàn)目標(biāo)位置的真值為不同值時(shí)，位置變化對(duì)誤差函數(shù)的仿真曲面形狀基本不變。因此，在位置與姿態(tài)取不同真值時(shí)，位置真值變化對(duì)誤差函數(shù)的影響較小，姿態(tài)真值的變化則會(huì)改變誤差函數(shù)曲面的形狀，當(dāng)取特定值時(shí)，誤差函數(shù)曲面近似對(duì)稱。

從圖8可以得知，當(dāng)目標(biāo)姿態(tài)誤差范圍擴(kuò)大到(-π/5，π/5)≈(-0.63,0.63)時(shí)，誤差函數(shù)曲面的碗狀特性不再明顯、收斂性變差，這表明當(dāng)目標(biāo)姿態(tài)偏移初始姿態(tài)較大時(shí)，會(huì)使得現(xiàn)誤差函數(shù)不能很好地收斂或收斂速度較慢，所以在進(jìn)行實(shí)際的月面采樣定位過(guò)程中，目標(biāo)姿態(tài)的初值選取不能與真值偏移過(guò)大，應(yīng)控制在一定誤差范圍內(nèi)(如π/10以內(nèi))，否則可能無(wú)法獲取準(zhǔn)確的視覺(jué)定位結(jié)果。

圖8 圓形目標(biāo)誤差函數(shù)隨姿態(tài)變化時(shí)的仿真結(jié)果

從圖9中可以看出，當(dāng)存在一定姿態(tài)誤差時(shí)，位置變化對(duì)誤差函數(shù)的影響增大，但是不會(huì)改變誤差函數(shù)為碗狀的特性，說(shuō)明在存在較小的姿態(tài)誤差情況下并不影響誤差函數(shù)的收斂。但姿態(tài)誤差較大時(shí)，如圖10所示，誤差函數(shù)曲面圖的形狀也發(fā)生了較大改變，曲面的凹凸性變?nèi)酰諗克俣茸兟艺`差函數(shù)的收斂點(diǎn)發(fā)生了偏移。這說(shuō)明跟位置變化相比較，姿態(tài)誤差增大到一定程度時(shí)，會(huì)使誤差函數(shù)收斂性變差，影響視覺(jué)定位結(jié)果的正確性。

圖9 姿態(tài)偏移π/10時(shí)誤差函數(shù)隨目標(biāo)位置變化情況

圖10 姿態(tài)偏移π/3時(shí)誤差函數(shù)隨目標(biāo)位置變化情況

2.2 相機(jī)位姿變化對(duì)特征誤差函數(shù)的影響

圖11和圖14表明，隨著相機(jī)位置逐漸偏離或靠近相機(jī)坐標(biāo)真值，多特征融合的光束逆投影誤差函數(shù)值呈漸增或漸減趨勢(shì)，誤差函數(shù)值變化趨勢(shì)在自變量大范圍改變情況下仍然呈碗狀特性，表明誤差函數(shù)保持較好的收斂性，而權(quán)重因子α=0.5相較于α=0.1的光束逆投影誤差函數(shù)收斂趨勢(shì)更加陡峭，收斂速度更快。

圖11 多特征誤差隨相機(jī)位置誤差改變的趨勢(shì)圖(α=0.1)

圖12和圖15表明，相機(jī)姿態(tài)角αi和βi在一個(gè)較小范圍內(nèi)改變時(shí)，誤差函數(shù)具有良好的收斂性，當(dāng)考慮滾轉(zhuǎn)角γi對(duì)誤差函數(shù)產(chǎn)生作用時(shí)，誤差函數(shù)值變化趨勢(shì)不再呈一個(gè)碗狀而是呈滑梯狀，且均自γi軸的正半軸滑向負(fù)半軸，說(shuō)明以下3點(diǎn)：① 與其余2個(gè)角度相比，滾轉(zhuǎn)角γi對(duì)誤差函數(shù)造成的影響更大；② 增大滾轉(zhuǎn)角γi對(duì)誤差函數(shù)的影響遠(yuǎn)大于其減小對(duì)誤差函數(shù)的影響；③ 增大目標(biāo)的姿態(tài)誤差可能導(dǎo)致殘差函數(shù)收斂性質(zhì)變差，容易出現(xiàn)收斂到局部極小值現(xiàn)象。同樣地，權(quán)重因子α=0.5相較于α=0.1的光束逆投影誤差函數(shù)收斂趨勢(shì)更加陡峭，收斂速度更快。

圖12 多特征誤差隨相機(jī)姿態(tài)角改變的趨勢(shì)圖(α=0.1)

圖13 存在相機(jī)姿態(tài)誤差情況下多特征誤差隨相機(jī)位置改變的趨勢(shì)圖(α=0.1)

圖14 多特征誤差隨相機(jī)位置誤差改變的趨勢(shì)圖(α=0.5)

圖13和圖16是相機(jī)姿態(tài)角存在(Δα,Δβ,Δγ)=(π/20,π/20,π/20)的誤差時(shí)，相機(jī)位置偏移改變對(duì)誤差函數(shù)值變化趨勢(shì)的影響。可以看出相機(jī)姿態(tài)誤差使得相機(jī)的位置偏移誤差對(duì)誤差函數(shù)產(chǎn)生的影響更加顯著，相機(jī)在z方向的位置偏移Xw(x,y,z)對(duì)誤差函數(shù)產(chǎn)生的影響增大，使得xz方向位置偏差和yz方向的位置偏差仿真曲面的深度增加。相機(jī)存在姿態(tài)誤差時(shí)位置偏差對(duì)誤差函數(shù)的影響增大且收斂點(diǎn)的位置發(fā)生偏移，姿態(tài)誤差變化越大，收斂點(diǎn)偏移越多，所以在定位計(jì)算時(shí)姿態(tài)準(zhǔn)確才能保證最終定位結(jié)果的準(zhǔn)確性。

圖15 多特征誤差隨相機(jī)姿態(tài)誤差改變的趨勢(shì)圖(α=0.5)

圖16 存在相機(jī)姿態(tài)誤差情況下多特征誤差隨相機(jī)位置誤差改變的趨勢(shì)圖(α=0.5)

2.3 多類特征視覺(jué)定位情況的比對(duì)分析

通過(guò)2.1和2.2節(jié)的仿真分析可以得到以下3條結(jié)論，基于這些結(jié)論指導(dǎo)開(kāi)展多特征融合定位實(shí)驗(yàn)，實(shí)現(xiàn)目標(biāo)的位姿求解。

1) 位置誤差的適應(yīng)性。基于光束逆投影的誤差函數(shù)模型對(duì)目標(biāo)位置或相機(jī)位置的偏差具有很好的適應(yīng)性，即使初始位置誤差較大，也通過(guò)迭代實(shí)現(xiàn)收斂，求得精確的視覺(jué)定位結(jié)果。

2) 姿態(tài)誤差的容許能力。基于光束逆投影的誤差函數(shù)對(duì)應(yīng)的曲面是非凸的多極值點(diǎn)曲面，對(duì)目標(biāo)姿態(tài)和相機(jī)姿態(tài)偏差的容許能力較弱，如果初值選取超出了一定范圍，就會(huì)發(fā)生收斂錯(cuò)誤或者不收斂的情況。在實(shí)際任務(wù)中應(yīng)盡可能準(zhǔn)確地選取姿態(tài)初值，減小姿態(tài)迭代步長(zhǎng)，以保證迭代過(guò)程始終在局部凸化的鄰域內(nèi)進(jìn)行，確保最終定位結(jié)果的正確性。

3) 位姿誤差耦合影響關(guān)系。對(duì)于位置誤差和姿態(tài)誤差同時(shí)存在的情況，目標(biāo)和相機(jī)姿態(tài)誤差的存在會(huì)增大位置誤差對(duì)誤差函數(shù)的影響且使誤差函數(shù)的收斂點(diǎn)發(fā)生偏移；姿態(tài)誤差變化越大，對(duì)位置誤差影響力增加的越大，收斂點(diǎn)也偏移得越多。位置誤差和姿態(tài)誤差耦合影響誤差函數(shù)曲面的形狀，使得局部鄰域的凸性變差，迭代過(guò)程中更易跳出局部鄰域，因此在迭代求優(yōu)時(shí)應(yīng)合理選擇自變量改變的步長(zhǎng)，以確保在局部鄰域內(nèi)收斂到正確的位姿。

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

嫦娥五號(hào)月面采樣封裝過(guò)程中，針對(duì)觸月、采樣、放樣、抓罐及放罐等要求，利用視覺(jué)定位方法引導(dǎo)機(jī)械臂精調(diào)操作，主要采用了基于自然特征和合作靶標(biāo)組合的視覺(jué)定位方法，而基于自然特征定位方法又分成了單目橢圓視覺(jué)定位法和雙目橢圓視覺(jué)定位法。本節(jié)以月面采樣精調(diào)和放樣精調(diào)為例介紹融合單目、雙目橢圓特征的機(jī)械臂末端觸月圓盤(pán)位姿視覺(jué)定位實(shí)驗(yàn)和融合橢圓、靶標(biāo)特征的機(jī)械臂臂載相機(jī)位姿視覺(jué)定位實(shí)驗(yàn)。

3.1 融合單目、雙目橢圓特征的視覺(jué)定位實(shí)驗(yàn)

為了驗(yàn)證融合單目、雙目橢圓特征的視覺(jué)定位算法在真實(shí)月面采樣環(huán)境下的精確性和穩(wěn)定性，本文建立了月面數(shù)字仿真實(shí)驗(yàn)平臺(tái)，使用月面著陸器A-B靜態(tài)監(jiān)視相機(jī)拍攝數(shù)字仿真環(huán)境下月面采樣機(jī)械臂末端圖像作為實(shí)驗(yàn)圖像，圖17是融合單目、雙目橢圓特征的月面數(shù)字仿真環(huán)境下對(duì)機(jī)械臂末端觸月圓盤(pán)外側(cè)中心點(diǎn)定位實(shí)驗(yàn)流程圖。

圖17 融合單、雙目橢圓特征視覺(jué)定位實(shí)驗(yàn)流程圖

表1為使用單目視覺(jué)和雙目視覺(jué)的定位算法解算出6組機(jī)械臂末端觸月圓盤(pán)的位姿結(jié)果與真值的對(duì)比情況，由于仿真中未考慮機(jī)械臂柔性形變，因此真實(shí)位姿可根據(jù)機(jī)械臂關(guān)節(jié)角數(shù)據(jù)通過(guò)機(jī)械臂正運(yùn)動(dòng)學(xué)求解得到。圓心三維坐標(biāo)為Xw(x,y,z)，單位為mm；機(jī)械臂末端圓盤(pán)(圓平面)姿態(tài)角分別使用偏航角φ和滾轉(zhuǎn)角ψ表示。

圖18是6組基于橢圓特征單目與雙目視覺(jué)定位實(shí)驗(yàn)結(jié)果誤差的比對(duì)圖。從數(shù)字仿真實(shí)驗(yàn)結(jié)果看，基于雙目監(jiān)視相機(jī)對(duì)機(jī)械臂末端圓形目標(biāo)定位精度較高，6組定位試驗(yàn)中圓心三維坐標(biāo)最大誤差優(yōu)于10 mm，姿態(tài)角的最大誤差不超過(guò)0.5°，而基于單目視覺(jué)定位的方法定位精度相對(duì)較低，圓心三維坐標(biāo)最大的單向誤差<30 mm，姿態(tài)角的最大誤差不超過(guò)1°。

通過(guò)上述數(shù)字仿真實(shí)驗(yàn)驗(yàn)證了單目視覺(jué)定位和雙目視覺(jué)定位的精度，證明了雙目橢圓目標(biāo)定位算法的優(yōu)越性。

實(shí)際應(yīng)用中，單目橢圓定位采用解析求解方法實(shí)現(xiàn)，其定位結(jié)果在雙目特征定位誤差模型的收斂域內(nèi)，滿足第2節(jié)多特征融合視覺(jué)定位的初值選取要求，因此可利用單目橢圓定位結(jié)果作為雙目橢圓特征算法的初始值，迭代實(shí)現(xiàn)定位結(jié)果的精化求解。

在嫦娥五號(hào)任務(wù)中，在S1、S2、S5點(diǎn)總共進(jìn)行了2次觸月精調(diào)、12次采樣精調(diào)，其中在S1點(diǎn)附近進(jìn)行了5次采樣精調(diào)，在S2點(diǎn)進(jìn)行了6次采樣，在S5點(diǎn)進(jìn)行1次采樣。

圖19是在S1點(diǎn)處進(jìn)行第1次表取采樣時(shí)，機(jī)械臂末端采樣器到達(dá)采樣點(diǎn)，著陸器攜帶的A-B靜態(tài)監(jiān)視相機(jī)拍攝機(jī)械臂末端觸月圓盤(pán)進(jìn)行橢圓輪廓特征提取處理后的圖像。

表1 數(shù)字仿真環(huán)境下實(shí)驗(yàn)結(jié)果

利用基于單、雙目橢圓特征定位方法實(shí)現(xiàn)了月面采樣的精調(diào)引導(dǎo)，采樣精調(diào)過(guò)程中在精調(diào)起始點(diǎn)處進(jìn)行定位，定位結(jié)果為機(jī)械臂末端與規(guī)劃目標(biāo)采樣點(diǎn)的偏移在機(jī)械臂坐標(biāo)系下的精調(diào)量。在采樣S1點(diǎn)、S2點(diǎn)、S3點(diǎn)3次采樣精調(diào)結(jié)果的比較如圖20所示。

圖18 單目橢圓特征與雙目橢圓特征定位結(jié)果誤差比對(duì)

圖19 第1次月表采樣機(jī)械臂末端圓盤(pán)輪廓特征提取效果圖

圖20 真實(shí)月面采樣精調(diào)量計(jì)算結(jié)果比較

與實(shí)際采樣精調(diào)策略中的精調(diào)量比對(duì)結(jié)果表明，使用單目視覺(jué)方法獲得的定位結(jié)果在x、y、z方向的偏差分別不超過(guò)37.2 mm、16.2 mm、18.2 mm，使用雙目視覺(jué)方法獲得的定位結(jié)果在x、y、z方向的偏差不超過(guò)9.6 mm、11.6 mm、10.7 mm，雙目定位結(jié)果的穩(wěn)定性較好，優(yōu)于單目定位結(jié)果。定位結(jié)果與實(shí)際精調(diào)策略存在差異的主要原因是：采樣精調(diào)策略的設(shè)計(jì)中近似忽略x、y方向的誤差，將這2個(gè)方向的調(diào)整量設(shè)為0；實(shí)際精調(diào)中主要考慮z方向下移至采樣位置的移動(dòng)距離，考慮采樣目標(biāo)位置會(huì)隨著采樣器的下移進(jìn)行逐步調(diào)整，z方向定位得到的移動(dòng)量與精調(diào)策略應(yīng)存在一定的偏差。

3.2 融合橢圓、靶標(biāo)特征的視覺(jué)定位實(shí)驗(yàn)

為了驗(yàn)證融合橢圓、靶標(biāo)特征的視覺(jué)定位方法的有效性和精確性，本節(jié)針對(duì)2類特征的聯(lián)合優(yōu)化模型以月面放樣精調(diào)為例分別進(jìn)行MATLAB數(shù)值仿真實(shí)驗(yàn)和月面數(shù)字仿真實(shí)驗(yàn)，驗(yàn)證融合定位算法的精度，最后介紹真實(shí)月面放樣精調(diào)實(shí)施情況。

融合橢圓特征和靶標(biāo)特征的數(shù)值仿真實(shí)驗(yàn)使用MATLAB在三維空間中分別構(gòu)建離散點(diǎn)序列，組成圓形輪廓特征和靶標(biāo)特征。根據(jù)相機(jī)成像投影關(guān)系將罐口圓特征和合作靶標(biāo)特征投影至機(jī)械臂臂載相機(jī)像平面，形成橢圓特征像素坐標(biāo)和靶標(biāo)特征像素坐標(biāo)，對(duì)罐口圓成像和靶標(biāo)特征成像后的投影橢圓特征和靶標(biāo)特征點(diǎn)添加均值為0，標(biāo)準(zhǔn)差為σ=1的高斯白噪聲來(lái)提取特征時(shí)的不確定性和誤差，獲取所需的像素坐標(biāo)。

實(shí)驗(yàn)結(jié)果表明，隨著機(jī)械臂臂載相機(jī)與密封罐之間的距離逐漸減小，即采樣器逐漸接近放樣位置時(shí)，相機(jī)的x和z方向的位置誤差逐漸降低，而y方向的誤差值因相機(jī)y方向坐標(biāo)未改變而呈局部區(qū)間震蕩起伏狀，3個(gè)方向的誤差均優(yōu)于2 mm。其中x和z方向測(cè)量誤差逐步減小主要原因?yàn)椋阂皇菧y(cè)量距離減小導(dǎo)致圓形密封罐在臂載相機(jī)像素平面成像區(qū)域增大，特征區(qū)域所占的像素?cái)?shù)增多，橢圓像素改變跟隨相機(jī)位置的改變更加明顯；二是成像橢圓的大小隨測(cè)量距離減小而變大，從而使相同等級(jí)的噪聲對(duì)大橢圓的影響減小。因此使得測(cè)量誤差減小、精度增加，隨著動(dòng)態(tài)臂載相機(jī)從遠(yuǎn)處逐漸接近密封罐罐口，利用多特征融合的視覺(jué)定位方法的平均誤差逐漸降低，單向測(cè)量精度優(yōu)于2 mm，滿足放樣任務(wù)精度需求。

圖21 多特征融合數(shù)值仿真實(shí)驗(yàn)結(jié)果圖

進(jìn)一步考慮基于月面數(shù)字仿真實(shí)驗(yàn)平臺(tái)的多特征融合定位精度驗(yàn)證情況。首先根據(jù)放樣操作要求，模擬機(jī)械臂臂載相機(jī)拍攝數(shù)字仿真環(huán)境中的圓形罐口和合作靶標(biāo)，再?gòu)膱D像中提取橢圓特征和靶標(biāo)特征對(duì)機(jī)械臂末端臂載相機(jī)進(jìn)行定位，圖22給出了2類特征的提取效果圖。

表2為使用本文方法解算出的機(jī)械臂臂載相機(jī)位姿結(jié)果與其真值的對(duì)比，其中真實(shí)位姿根據(jù)機(jī)械臂正運(yùn)動(dòng)學(xué)求解得到，其中機(jī)械臂臂載相機(jī)光心三維坐標(biāo)為Xw(x,y,z)；臂載相機(jī)的姿態(tài)角為偏航角α、俯仰角β和滾轉(zhuǎn)角γ。

從月面數(shù)字仿真實(shí)驗(yàn)結(jié)果看，本文提出的基于橢圓特征和靶標(biāo)特征融合的視覺(jué)定位方法定位的最大單向位置誤差<2 mm，姿態(tài)角的最大誤差不超過(guò)0.5°，驗(yàn)證了本方法的有效性和精確性。

需要注意的是，月面數(shù)字仿真實(shí)驗(yàn)的精度略低于仿真合成圖像實(shí)驗(yàn)精度，分析原因主要是由攝像機(jī)的標(biāo)定誤差(包含相機(jī)內(nèi)、外參標(biāo)定誤差)、圖像噪聲和特征提取誤差造成，仿真合成圖像實(shí)驗(yàn)采用了考慮一定噪聲影響的特征像素坐標(biāo)和理想的相機(jī)參數(shù)，而月面數(shù)字仿真實(shí)驗(yàn)?zāi)M了成像、特征提取和定位的真實(shí)過(guò)程，考慮的誤差因素更多，與真實(shí)情況也更加接近。另一方面，控制機(jī)械臂臂載相機(jī)移動(dòng)至不同的位置和姿態(tài)處重復(fù)多次實(shí)驗(yàn)，得到的結(jié)果精度基本相同，證明了多特征融合定位算法的魯棒性。

圖22 橢圓輪廓特征和靶標(biāo)特征提取效果圖

實(shí)際應(yīng)用中，通常是首先根據(jù)機(jī)械臂無(wú)形變狀態(tài)的正運(yùn)動(dòng)學(xué)模型近似估計(jì)相機(jī)在機(jī)械臂坐標(biāo)系中的大致位姿，經(jīng)分析因機(jī)械臂柔性和關(guān)節(jié)間隙引起的誤差量在多特征融合定位誤差模型的收斂域內(nèi)，滿足第2節(jié)多特征融合視覺(jué)定位的初值選取要求，因此可利用基于正運(yùn)動(dòng)學(xué)的位姿估算結(jié)果作為多特征融合定位算法的初始值，迭代實(shí)現(xiàn)定位結(jié)果的精化求解。

為了進(jìn)一步說(shuō)明本文算法的有效性，下面將該算法在實(shí)際月面放樣中的應(yīng)用情況進(jìn)行說(shuō)明。首先真實(shí)月面放樣精調(diào)過(guò)程中對(duì)橢圓罐口和棋盤(pán)格靶標(biāo)的特征提取圖像數(shù)據(jù)如圖23所示。在此基礎(chǔ)上進(jìn)一步對(duì)放樣精調(diào)過(guò)程及其與真實(shí)精調(diào)策略比較的偏差情況進(jìn)行描述。

在月面實(shí)際放樣精調(diào)過(guò)程中在精調(diào)起始點(diǎn)、中間過(guò)程點(diǎn)和到位點(diǎn)進(jìn)行視覺(jué)定位，定位結(jié)果為機(jī)械臂末端與目標(biāo)放樣點(diǎn)偏移在機(jī)械臂坐標(biāo)系下的精調(diào)量。以12次放樣起始點(diǎn)精調(diào)量計(jì)算為例介紹融合橢圓與靶標(biāo)特征的真實(shí)月面放樣精調(diào)結(jié)果與實(shí)際精調(diào)策略偏差對(duì)比如圖24所示。

表2 橢圓特征和靶標(biāo)特征融合月面數(shù)字仿真實(shí)驗(yàn)結(jié)果

圖23 多特征融合定位的特征提取效果圖

圖24 12次放樣精調(diào)起始點(diǎn)定位的精調(diào)量與實(shí)際精調(diào)策略的偏差

融合橢圓與靶標(biāo)特征視覺(jué)定位方法獲取的精調(diào)量和精調(diào)策略中的精調(diào)量定位精度進(jìn)行比對(duì)分析結(jié)果表明使用融合橢圓與靶標(biāo)特征定位方法計(jì)算得到的定位結(jié)果與實(shí)際精調(diào)策略中的精調(diào)量在x、y、z3個(gè)方向的定位偏差范圍均小于2 mm,實(shí)現(xiàn)了對(duì)放樣過(guò)程的精確引導(dǎo)與精調(diào)策略一致性復(fù)核。

4 結(jié) 論

本文針對(duì)月面采樣封裝任務(wù)中的精確采樣、放樣、抓罐、放罐等操作任務(wù)，設(shè)計(jì)了多特征融合的視覺(jué)引導(dǎo)到位方法，對(duì)不同的操作過(guò)程選擇圓形目標(biāo)、靶標(biāo)及其組合等不同類型特征，對(duì)目標(biāo)和相機(jī)光心相對(duì)于機(jī)械臂坐標(biāo)系的位姿進(jìn)行測(cè)量計(jì)算。轉(zhuǎn)換計(jì)算采樣器相對(duì)機(jī)械臂坐標(biāo)系的精確位置，引導(dǎo)機(jī)械臂實(shí)施精確操作。主要貢獻(xiàn)在于：

1) 提出了基于光束逆投影變換的多特征融合定位的統(tǒng)一模型，將圓形目標(biāo)特征和靶標(biāo)特征統(tǒng)一到同一框架下，構(gòu)建了基于位姿偏移的誤差最小化優(yōu)化函數(shù)，設(shè)計(jì)了不同類特征定位誤差的同尺度統(tǒng)一與迭代優(yōu)化策略，實(shí)現(xiàn)了已知量和待求量靈活可變的迭代優(yōu)化定位求解。

2) 針對(duì)不同操作應(yīng)用場(chǎng)景實(shí)現(xiàn)了基于單個(gè)或多個(gè)相機(jī)的不同類型特征組合定位過(guò)程，設(shè)計(jì)了多類不同形式的實(shí)驗(yàn)驗(yàn)證方式，實(shí)驗(yàn)結(jié)果表明本文提出的算法在雙目條件下對(duì)遠(yuǎn)距離小目標(biāo)定位誤差優(yōu)于2 cm，對(duì)近距離目標(biāo)的相對(duì)定位精度優(yōu)于2 mm，精度與美國(guó)洞察號(hào)儀器安放等操作的視覺(jué)定位精度水平相當(dāng)。

本文提出的方法滿足了嫦娥五號(hào)任務(wù)中的采樣、放樣、抓罐和放罐等多類操作要求，實(shí)現(xiàn)了采樣全程的操作精確定位引導(dǎo)，成功支撐了嫦娥五號(hào)月面采樣封裝任務(wù)的順利實(shí)施。該方法也將進(jìn)一步應(yīng)用于中國(guó)嫦娥六號(hào)月面采樣任務(wù)中，引導(dǎo)月面采樣機(jī)械臂實(shí)現(xiàn)更加多元的精確操作。

致 謝

論文在北京航天飛行控制中心空間遙操作團(tuán)隊(duì)的通力協(xié)助下得以順利完成, 在此深表感謝。