呂恩利，蘇秋雙，王飛仁，羅毅智 ，曾伯陽，曾志雄

(1. 華南農(nóng)業(yè)大學(xué)工程學(xué)院，廣州市，510642； 2. 廣東機(jī)電職業(yè)技術(shù)學(xué)院，廣州市，510550)

0 引言

定位是移動機(jī)器人算法中必不可少的組成部分。在室外場景中，研究者們大都使用全球衛(wèi)星定位系統(tǒng)[1-2]為機(jī)器人提供定位，但畜牧機(jī)器人運(yùn)行在沒有衛(wèi)星信號的室內(nèi)，所以此類定位方法在畜牧機(jī)器人的應(yīng)用上受到了限制。在定位方面，室內(nèi)機(jī)器人大多使用激光雷達(dá)[3-5]、全景相機(jī)[6]和人工標(biāo)志物[7-8]等方式。在這些方法中，采用人工標(biāo)志物是一種低成本的方法，而在人工標(biāo)志物中AprilTag又是一種較為靈活和具有魯棒性的選擇[9]。

Manfred Klopschitz[10]等基于sfm技術(shù)先確定相機(jī)位姿，然后在此基礎(chǔ)上推算AprilTag位姿。Rafael Munoz-Salinas[11]等先通過AprilTag在各幀間的共視關(guān)系得到AprilTag之間的相對位姿，并以此為初值構(gòu)建關(guān)于投影關(guān)系的最小二乘問題來優(yōu)化AprilTag的位姿，最后再優(yōu)化得到相機(jī)位姿。Pfrommer等使用因子圖的方法在大場景中對AprilTag進(jìn)行建圖。不僅是在AprilTag位姿的測量上，在使用AprilTag為機(jī)器人提供定位上前人也有不少的研究，例如將AprilTag應(yīng)用在水下機(jī)器人和無人機(jī)的定位上[12-13],但在這些研究中AprilTag位姿都是先驗(yàn)的，他們的研究都沒有涉及AprilTag地圖的建立。

前人在建立AprilTag地圖和使用AprilTag定位上都做了不少的研究，但在移動畜牧機(jī)器人上為布置在環(huán)境中的AprilTag建立地圖，并利用該地圖為機(jī)器人提供定位的研究卻不多。本文先使用機(jī)器人上的單目相機(jī)和輪式里程計(jì)估計(jì)出機(jī)器人的狀態(tài)和布置在環(huán)境中的AprilTag的位姿；再以此為初值構(gòu)建有關(guān)于投影關(guān)系最小二乘問題，進(jìn)一步優(yōu)化機(jī)器人和AprilTag的位姿；最后得到精度較高的AprilTag地圖，并使用該地圖為機(jī)器人提供定位。

1 機(jī)器人及算法相關(guān)介紹

1.1 算法原理

本算法的流程如圖1所示，主要為兩大部分，第一部分為狀態(tài)估計(jì)部分，即圖中的s1到s5，第二部分為位姿優(yōu)化，即圖中的s6到s9。

位姿優(yōu)化由兩部分組成，一個(gè)是在沒有回環(huán)時(shí)利用局部地圖優(yōu)化抑制累計(jì)誤差在地圖中的增長，即圖1中的s7；另一部分是檢測到回環(huán)后利用全局優(yōu)化消除累計(jì)誤差以便能獲得高精度的AprilTag地圖，即圖1中的s8。

圖1 算法流程圖Fig. 1 Flow chart of algorithm

1.2 坐標(biāo)系建立

機(jī)器人坐標(biāo)系Tr：如圖2所示，Tr以機(jī)器人的幾何中心為原點(diǎn)，z軸垂直于地面豎直向上，x軸平行于地面指向機(jī)器人前進(jìn)方向，y軸的正方向指向前進(jìn)時(shí)的左側(cè)，Tr始終與機(jī)器人固連在一起。

全局坐標(biāo)系Tw：如圖2所示，Tw與t=0時(shí)刻的Tr重合，但與Tr不同的是其不會隨著機(jī)器人的運(yùn)動而發(fā)生變化，而是始終固定不動。Tw是整個(gè)算法的基準(zhǔn)，算法最終得到的機(jī)器人和Apriltag的位姿均是在這個(gè)坐標(biāo)系下的位姿。

二維碼(AprilTag)坐標(biāo)系Tmi：如圖2所示，Tmi(其中i代表該AprilTag的id號)的原點(diǎn)和AprilTag幾何中心重合，x軸正方向指向右側(cè)，y軸正方向向上，z軸正方向垂直于紙面指向外側(cè)，且每一張AprilTag都有一個(gè)對應(yīng)的二維碼(AprilTag)坐標(biāo)系。本文假設(shè)每張AprilTag的邊長均為Ts(單位：m)。

機(jī)器人狀態(tài)sr：由于機(jī)器人都在地板平面上運(yùn)動，所以在全局坐標(biāo)系中使用x，y(單位：m)和航向角θ(弧度制)便可以描述機(jī)器人的狀態(tài)，即sr=[xyθ]T。

圖2 各坐標(biāo)系示意圖Fig. 2 Diagram of each coordinate system

1.3 機(jī)器人結(jié)構(gòu)

機(jī)器人大體結(jié)構(gòu)如圖3所示，機(jī)器人整體的長寬高分別為0.73 m，0.52 m，0.3 m，內(nèi)部裝載著一臺工控機(jī)，用于電機(jī)的控制和傳感器數(shù)據(jù)的接收處理等其他計(jì)算工作。Apriltag貼在機(jī)器人行走路線上方的天花板上，同時(shí)為了能夠使得相機(jī)在天花板上擁有較大的成像范圍，相機(jī)是以一定角度斜向上安裝的，如圖3(a)所示。

如圖3(b)所示，機(jī)器人的驅(qū)動輪位于左右兩側(cè)，它們之間的間距為0.388 m，前后輪是支撐輪。工控機(jī)通過串口控制左右兩側(cè)電機(jī)，電機(jī)再驅(qū)動左右兩驅(qū)動輪；兩個(gè)電機(jī)均配備了編碼器，可以實(shí)時(shí)的測量出左右電機(jī)轉(zhuǎn)動量，結(jié)合輪子尺寸便可得到左右輪移動的距離。

(a) 移動機(jī)器人示意圖

(b) 移動機(jī)器人底面示意圖圖3 機(jī)器人大體結(jié)構(gòu)Fig. 3 General structure of robot

2 狀態(tài)估計(jì)

2.1 AprilTag識別及其在相機(jī)坐標(biāo)系中位姿估計(jì)

AprilTag的識別流程圖如圖4所示，在執(zhí)行具體的識別操作之前要對圖像做預(yù)處理，預(yù)處理包括將圖像轉(zhuǎn)換成灰度圖、高斯濾波和圖像的二值化。具體的識別過程可分為三個(gè)部分：直線提取、四邊形檢測和解碼檢測到的每個(gè)四邊形[9]。直線提取是通過計(jì)算每個(gè)像素的梯度，然后采用根據(jù)梯度的大小及方向進(jìn)行聚類的方法提取出圖像中直線[9]；在獲得圖中所有直線的基礎(chǔ)上，使用搜索樹的方法[9]檢測出它們所構(gòu)成的所有四邊形；最后通過對每一個(gè)檢測出來的四邊形做解碼[9]來確定AprilTag的id，同時(shí)也能去除掉不是AprilTag的四邊形，降低誤檢測率。

圖4 Apriltag的識別過程

Apriltag的在相機(jī)坐標(biāo)系中的位姿估計(jì)流程圖如圖5所示，利用Apriltag的四個(gè)角點(diǎn)在相機(jī)歸一化平面上的投影坐標(biāo)pij(i代表Apriltag的id號，j表示第幾個(gè)角點(diǎn))求解出Apriltag在相機(jī)坐標(biāo)系中的兩個(gè)局部極值，再以兩個(gè)局部極值為初值分別迭代求解出兩個(gè)更精確的值，最終比較兩個(gè)解的誤差，取誤差較小者作為最終結(jié)果。

圖5 AprilTag位姿估計(jì)的流程圖

如圖2所示，id為i的AprilTag的4個(gè)角點(diǎn)P1～P4在Tmi中的坐標(biāo)分別為Pi1=[-0.5×Ts0.5×Ts0]T，Pi2=[0.5×Ts0.5×Ts0]T,Pi3=[0.5×Ts-0.5×Ts0]T,Pi4=[-0.5×Ts-0.5×Ts0]T。由于Apriltag的四個(gè)角點(diǎn)在一個(gè)平面上，所以存在單應(yīng)矩陣H滿足式(1)。

pij?HPij(j=1,2,3,4)

(1)

式(1)中的pij表示某個(gè)角點(diǎn)在相機(jī)歸一化平面上的坐標(biāo)，由式(2)計(jì)算獲得。

pij=CP-1pij′

(2)

式中：Cp——相機(jī)內(nèi)參矩陣，本文認(rèn)為內(nèi)參矩陣是已知的；

pij′——點(diǎn)Pij在像素平面上的投影。

(3)

(4)

(5)

2.2 基于EKF的機(jī)器人狀態(tài)估計(jì)

如圖6所示，假設(shè)機(jī)器人經(jīng)過一段時(shí)間的運(yùn)動后狀態(tài)由sr1轉(zhuǎn)移到了狀態(tài)sr2,在這一運(yùn)動過程中，航向角的變化記為α(弧度制，本文的角度表示均為弧度制)，左右輪式里程計(jì)所記錄的左右輪走過的距離分別為l,r(單位：m)。圖6中的向量n1,n2分別是狀態(tài)sr1，sr2時(shí)指向機(jī)器人前進(jìn)方向的單位向量，m1,m2分別是垂直于n1,n2的單位向量，R為轉(zhuǎn)向半徑(單位：m)，機(jī)器人左右輪寬為d(單位：m)。不妨假設(shè)狀態(tài)sr1和sr2如式(6)所示。

sr1T=[x1y1θ1],sr2T=[x2y2θ2]

(6)

圖6 運(yùn)動模型Fig. 6 Motion model

由幾何關(guān)系得式(7),由圖6可得式(8)。

R=ld/(r-l),α=(r-l)/d

(7)

(8)

將式(7)代入式(8)并整理得，當(dāng)l≠r時(shí)狀態(tài)轉(zhuǎn)移方程如式(9)所示。當(dāng)l=r時(shí)，狀態(tài)轉(zhuǎn)移方程如式(10)所示。式(9)和式(10)一起組成了狀態(tài)轉(zhuǎn)移方程，其中ε是均值為0高斯分布。

(9)

(10)

(11)

(12)

式中：Cke——相機(jī)內(nèi)參矩陣的齊次形式。

結(jié)合式(11)和式(12)，id號為2的AprilTag的觀測數(shù)據(jù)可由式(13)給出。

(13)

其中δ是均值為0的高斯分布。

同理可求id號為3的AprilTag的理想觀測g3，由此得機(jī)器人的觀測方程為式(14)。

(14)

(15)

3 位姿優(yōu)化

僅依靠上述方法估計(jì)AprilTag的位姿和機(jī)器人的狀態(tài)，然后再依據(jù)式(15)增量式地?cái)U(kuò)展AprilTag地圖的做法會使得累計(jì)誤差在地圖中快速增長，無法滿足建圖與定位精度的需求。為避免這一情況的出現(xiàn)，本文引入了位姿優(yōu)化。

3.1 局部地圖優(yōu)化

局部地圖優(yōu)化只優(yōu)化最新關(guān)鍵幀(當(dāng)前關(guān)鍵Fc)所對應(yīng)的機(jī)器人狀態(tài)及最新關(guān)鍵幀所觀測到的AprilTag的位姿。假設(shè)機(jī)器人在狀態(tài)sr2時(shí)恰好滿足關(guān)鍵幀生成條件，但并沒有觸發(fā)回環(huán)，其產(chǎn)生的關(guān)鍵幀記作F3，如圖7所示。圖7中標(biāo)號1～4的正方形表示對應(yīng)id號的AprilTag，F(xiàn)k表示第k個(gè)關(guān)鍵幀，每個(gè)關(guān)鍵幀中帶編號的四邊形是該id號的AprilTag在該關(guān)鍵幀上的投影。

圖7 觀測示意圖Fig. 7 Schematic diagram of observations

(16)

將式(16)代入式(12)中得式(17)。

(17)

由于每個(gè)旋轉(zhuǎn)平移矩陣都可以表示為一個(gè)李代數(shù)，所以每一個(gè)集合Mpartial都對應(yīng)著一個(gè)李代數(shù)的集合Mζ。圖7的例子中，對應(yīng)關(guān)鍵幀F(xiàn)3的李代數(shù)集合如式(18)所示。

Mζ3={ζ2,ζ3,ζ4}

(18)

局部優(yōu)化的目的在于找到一組李代數(shù)Mbest使Mζ中對應(yīng)的AprilTag的4個(gè)角點(diǎn)在所有屬于集合F={Fpartial,Fc}的關(guān)鍵幀中的重投影誤差最小，同時(shí)也為Fc找到一個(gè)狀態(tài)srbest使得Mpartial中所有AprilTag在當(dāng)前幀中的重投影誤差最小。

第l個(gè)關(guān)鍵幀的重投影誤差可表示為式(19)。

(19)

式中：a——第l幀中總共觀測到了a張AprilTag；

srl——該幀對應(yīng)的機(jī)器人狀態(tài)；

pij——角點(diǎn)在像素平面上的投影。

結(jié)合式(19)，局部地圖優(yōu)化的目標(biāo)函數(shù)可以由式(20)給出。

(20)

式(20)是一個(gè)最小二乘問題，這個(gè)問題的求解可以使用LM方法[17]，其初值由估計(jì)部分所提供。

3.2 全局優(yōu)化

本算法在創(chuàng)建AprilTag地圖過程中，會按照每張AprilTag出現(xiàn)的順序?yàn)樗鼈冑x予一個(gè)序號。當(dāng)某一關(guān)鍵幀中所觀測到的AprilTag序號的差大于一定閾值時(shí)，便認(rèn)為是回環(huán)了，此時(shí)便會啟動全局優(yōu)化。與局部地圖優(yōu)化類似，全局優(yōu)化也是一個(gè)有關(guān)于重投影誤差的最小二乘問題，也要確定兩個(gè)集合Fglobal和Mglobal。

由于整個(gè)建圖過程是以機(jī)器人起始狀態(tài)為基準(zhǔn)的，即以第一個(gè)關(guān)鍵幀為基準(zhǔn)，所以第一個(gè)關(guān)鍵幀F(xiàn)1對應(yīng)的機(jī)器人狀態(tài)在全局優(yōu)化中是被固定的。在全局優(yōu)化中Fglobal和Mglobal分別為所有關(guān)鍵幀的集合和所有被觀測到的AprilTag的集合，同時(shí)與局部地圖優(yōu)化類似Mglobal也對應(yīng)著一個(gè)李代數(shù)的集合Mgζ。

類似的，全局優(yōu)化的目的在于為每個(gè)關(guān)鍵幀(F1除外)找到一個(gè)對應(yīng)的機(jī)器人狀態(tài)sr(它們的集合記為sgbest)和一個(gè)李代數(shù)的集合Mgbest，使得所有的AprilTag的4個(gè)角點(diǎn)在所有關(guān)鍵幀中的重投影誤差最小。結(jié)合計(jì)算某一關(guān)鍵幀中的重投影誤差的式(19)，全局優(yōu)化的目標(biāo)函數(shù)如式(21)所示。

(21)

類似于式(20)，式(21)也可以使用LM方法求解[17]，其初值由局部地圖優(yōu)化提供。

4 試驗(yàn)與分析

機(jī)器人本體如圖8所示，圖中的nav350是用以提供機(jī)器人位置真值的傳感器，其參數(shù)如表1所示。為驗(yàn)證本文所提算法，在一長約36 m，寬約10 m的區(qū)域內(nèi)布置了126張AprilTag。布置時(shí)應(yīng)保證任何時(shí)候相機(jī)均能觀測到兩張及以上的AprilTag，即控制AprilTag在環(huán)境中布置的密度。本文根據(jù)相機(jī)的成像范圍，沿著機(jī)器人行走的方向大約每0.8 m布置一張AprilTag。

表1 nav350主要技術(shù)參數(shù)表Tab. 1 Main technical parameters of nav350

圖8 機(jī)器人俯視圖和側(cè)視圖

圖9 試驗(yàn)環(huán)境及AprilTag提取的效果Fig. 9 Environmentof experiment and effect of AprilTag extraction

4.1 局部地圖優(yōu)化對累計(jì)誤差的抑制

如圖10(a)所示，在僅依靠估計(jì)部分估計(jì)機(jī)器人位姿的情況下，經(jīng)過4 200幀后，算法給出的路徑和真實(shí)的路徑便有了明顯的差距，此時(shí)路徑的平均誤差為0.324 3 m，最大誤差2.043 m。引入了局部地圖優(yōu)化的算法得到的結(jié)果平均誤差為0.042 2 m，最大誤差0.339 8 m，和真實(shí)路徑差距不大?？梢娋植康貓D優(yōu)化在建圖過程中對累計(jì)誤差的有明顯的抑制作用。

從圖10(b)可以看出，隨著幀數(shù)(時(shí)間的推移)的增加，僅靠估計(jì)部分得到的路徑與真實(shí)路徑的差距越來越大，平均誤差增大到0.893 4 m，最大誤差為5.06 m。雖然了引入了局部地圖優(yōu)化的算法得到的路徑在經(jīng)歷了2 700幀后也開始與真實(shí)路徑有了差距，平均誤差為0.064 7 m，最大誤差為0.339 8 m，但是其差距遠(yuǎn)小于前者。

(a) 4 200幀時(shí)的對比圖

(b) 6 900幀時(shí)的對比圖圖10 定位效果對比Fig. 10 Comparison of positioning effect

4.2 AprilTag地圖精度試驗(yàn)

由于直接測量AprilTag在全局坐標(biāo)系中的位姿難度極高，所以直接將所建立的AprilTag地圖與真值作對比是不現(xiàn)實(shí)的，本文將使用一種間接的方法來衡量所建立地圖的精度。如果將所建立的AprilTag地圖中的每個(gè)AprilTag作為路標(biāo)點(diǎn)，在只運(yùn)行機(jī)器人狀態(tài)估計(jì)部分的情況下，機(jī)器人的定位精度便和AprilTag地圖精度正相關(guān)，所以本文使用基于地圖的定位(僅做狀態(tài)估計(jì))效果來衡量地圖的精度。

建圖時(shí)所有關(guān)鍵幀組成的路徑和真實(shí)的路徑對比圖如圖11所示，本算法給出的路徑的平均誤差為0.051 9 m，最大誤差為0.223 3 m。

圖11 關(guān)鍵幀對應(yīng)的機(jī)器人狀態(tài)與真值的對比圖Fig. 11 Comparison diagram of robot state and truth value corresponding to key frame

在使用本算法給出的AprilTag地圖的前提下，僅對機(jī)器人做狀態(tài)估計(jì)得到的路徑和真實(shí)值的對比圖如圖12所示，該定位效果的平均誤差為0.075 86 m，最大誤差為0.303 93 m，與建圖時(shí)的誤差相差不大。

圖12 基于本算法給出的地圖的定位效果Fig. 12 Positioning effect of the map based on this algorithm

5 結(jié)論

1) 本文研究了在相機(jī)坐標(biāo)系中估計(jì)AprilTag位姿的過程，先是利用AprilTag坐標(biāo)系中角點(diǎn)的坐標(biāo)和它們在相機(jī)歸一化平面上的投影坐標(biāo)計(jì)算出對應(yīng)的單應(yīng)矩陣；然后通過單應(yīng)矩陣恢復(fù)出對應(yīng)的旋轉(zhuǎn)和平移。同時(shí)為了避免陷入局部極值，還估計(jì)出了第二組潛在解，最后以兩組解為初值進(jìn)行迭代求解，取誤差較小的解作為結(jié)果。

2) EKF估計(jì)機(jī)器人的狀態(tài)：根據(jù)機(jī)器人的運(yùn)動模型給出了狀態(tài)轉(zhuǎn)移方程，同時(shí)也給出了機(jī)器人的觀測方程，結(jié)合兩個(gè)方程使用EKF估計(jì)機(jī)器人的狀態(tài)。試驗(yàn)表明僅使用EKF估計(jì)機(jī)器人狀態(tài)時(shí)會出現(xiàn)誤差的累積。

3) 為了抑制和消除誤差，本文引入了局部地圖優(yōu)化和全局優(yōu)化。在沒有回環(huán)時(shí)通過局部地圖優(yōu)化抑制誤差的產(chǎn)生，檢測到回環(huán)后使用全局優(yōu)化來消除誤差。文中分別給出了局部地圖優(yōu)化和全局優(yōu)化時(shí)，目標(biāo)函數(shù)的建立過程，同時(shí)采用LM方法來求解該問題。

4) 在建圖過程中使用的局部地圖的優(yōu)化能顯著的抑制累計(jì)誤差的產(chǎn)生，且隨著建圖范圍的增大，這種抑制效果越發(fā)明顯。所以在大范圍建立AprilTag地圖時(shí)，特別是在有大回環(huán)的情況下，本文所提出的局部地圖優(yōu)化是必要的。

5) 在建圖時(shí)各關(guān)鍵幀位置的平均誤差為0.051 9 m，最大誤差0.223 3 m；使用該地圖為畜牧機(jī)器人定位時(shí)，其定位的平均誤差為0.075 86 m，最大誤差為0.303 93 m，能滿足工作需求?；诘貓D的定位效果也間接驗(yàn)證了該建圖算法能建立一張較為準(zhǔn)確的AprilTag地圖。