陳慶武，張志安，何　雨，韓明明，黃學功

(南京理工大學 機械工程學院，江蘇 南京 210094)

在機器人位姿跟蹤過程中，由于測量噪聲和系統(tǒng)噪聲的存在，測量系統(tǒng)通過傳感器獲取的信息只是針對客觀環(huán)境的片面描述[1]，而單一傳感器的可靠性以及輸入信息的精確性和充足性難以獲得保證. 針對這個問題，本文采用擴展卡爾曼濾波進行多傳感信息融合[2]，在提高位姿跟蹤精度的基礎上，通過擴展觀測矩陣，使得該算法能夠靈活適應于不同傳感器類型和不同傳感器數(shù)量的測量系統(tǒng)，使得程序代碼可快速部署于不同應用場合，解決移動機器人位姿跟蹤多傳感器測量系統(tǒng)的可擴展性問題，增加算法的靈活性.

1　改進擴展卡爾曼濾波算法

擴展卡爾曼濾波算法[3]主要應用于非線性測量系統(tǒng)[4]，通過對系統(tǒng)方程和觀測方程進行線性化處理來估計后驗，在進行擴展卡爾曼濾波時，所觀測非線性動態(tài)系統(tǒng)的狀態(tài)轉移概率由非線性函數(shù)g控制，即

xt=g(xt-1)+ξt-1,

(1)

式中：xt為觀測目標在t時刻的狀態(tài)向量，隨機變量ξt-1是一個高斯隨機向量，表示由狀態(tài)轉移引入的不確定性，其維數(shù)與狀態(tài)向量相同，均值為0，方差為Rt.

測量概率由非線性函數(shù)h控制，即

zt=h(xt)+δt,

(2)

式中：zt為觀測目標在t時刻的測量值；h為預測值映射到測量空間的非線性函數(shù)；δt是均值為0，方差為Qt的多變量高斯分布測量噪聲.

擴展卡爾曼濾波算法分為兩個階段： 預測和更新. 式(3)與(4)是進行當前狀態(tài)預測和誤差協(xié)方差前向更新的預測階段.

(3)

(4)

(5)

(6)

(7)

在位姿跟蹤類問題中，擴展卡爾曼濾波主要用于估計所觀測6自由度移動目標的位置、姿態(tài)和速度信息，假設xt是所觀測系統(tǒng)在t時刻的12維狀態(tài)向量，包含三維坐標及其對應軸的速度、三維角度及其對應軸的角速度，其表達式為

xt=[xyzx′y′z′φθψφ′θ′ψ′]T.

(8)

在特定傳感器類型和數(shù)量的實際工程應用場合中，所觀測的狀態(tài)一般都是上式狀態(tài)變量的一部分，標準的擴展卡爾曼濾波的觀測矩陣H必須通過測量模型h函數(shù)的雅克比矩陣[5]給出，為支持擴展傳感器的類型和數(shù)量，使得算法可重復快速部署使用，本文假定系統(tǒng)中所使用的每一個傳感器都可以對所有要預測的狀態(tài)變量進行更新，即可進行狀態(tài)空間的局部更新.

若某個傳感器某次測量只測得n個狀態(tài)變量值，則相應預測空間到測量空間的映射為一個具有n個非零變量、維數(shù)為n×12、秩為n的變換矩陣. 通過這種方式擴展觀測矩陣H，任何傳感器都可以對狀態(tài)向量進行局部更新而不用重新設計算法，使得基于擴展卡爾曼濾波的位姿跟蹤算法更加具有普遍性，增加測量系統(tǒng)的靈活性.

2　定位算法

假設地面移動平臺-大地系統(tǒng)運動學狀態(tài)，即運動學模型的目標觀測狀態(tài)為

xt=[xtytθtvtωt]T,

(9)

式中：xt和yt為機器人坐標系下t時刻機器人的坐標；θt為機器人坐標位置與原點連線和x軸方向的夾角；vt和ωt為機器人在t時刻的速度和角速度. 本文通過設計里程計模型來進行地面移動平臺狀態(tài)估計，通過設計慣性傳感器[6]IMU模型和全球定位GPS模型來進行狀態(tài)更新.

2.1　預測階段

對于兩輪或兩履帶差速驅動的地面移動機器人，假設其在運行過程中通過驅動電機編碼器得到的左側輪子速度為vl，右側輪子速度為vr，輪距為W，則機器人的線速度和角速度為

圖 1　地面移動機器人恒定v和ω無噪聲移動模型Fig.1　The constant v and ω noiseless movement model of ground mobile robot

(10)

假定在Δt時間內保持vt和ωt為恒定值，當ωt不等于0時，無人車以半徑r進行圓運動.

(11)

圓心坐標為

(12)

則t+Δt時刻的狀態(tài)向量為

(13)

當ωt等于0時，

(14)

可以通過式(13)與(14)得到運動模型的雅克比矩陣JA，擴展卡爾曼濾波模型的預測可以寫成

xt+1=g(xt),

(15)

(16)

(17)

處理噪聲的協(xié)方差矩陣表達式為

Q=E[N·NT]=E[GμμμTGT]=G.E[μμT]GT,

(18)

其中，

(19)

里程計模型的處理噪聲為

2.2　更新階段

當只用慣性測量元件測量三軸角度和三軸角速度時，映射函數(shù)為

(20)

將預測映射到IMU測量空間

HIMUxt=[φθψφ′θ′ψ′]T.

(21)

當只利用GPS測量移動機器人的坐標時，映射函數(shù)為

(22)

移動機器人的世界坐標系和GPS的UTM坐標系[7](universal transverse mercartor grid system，通用橫墨卡托格網系統(tǒng)坐標)之間的轉換關系如式(23)[8]所示，其中c代表余弦函數(shù)cos，s代表正弦函數(shù)sin，φ,θ和ψ分別代表UTM坐標系下的橫滾角、俯仰軸和偏航角，移動機器人在UTM坐標系下的初始位置由xUTM0,yUTM0和zUTM0表示.

(23)

里程計和GPS數(shù)據(jù)之間的轉化關系式為

(24)

3　實　驗

3.1　實驗設計

采用如圖 2 與 3 所示輪履復合移動平臺作為實驗平臺，該平臺可通過輪履切換機構改變驅動方式為輪子驅動或履帶驅動，驅動電機自帶2500線光電式增量編碼器，可以一定周期通過CAN總線向主控設備返回光電編碼器的位置信息，經過處理可得到電機轉速和里程計數(shù)據(jù).

圖 2　輪履復合移動平臺三維設計圖Fig.2　3D design of wheel-track mobile platform

圖 3　實驗平臺實物圖Fig.3　Entity of experimental platform

實驗采用的全球定位模塊為SkyTra公司的GPS+北斗雙模定位模塊S1216F8-BD，其封裝如圖 4 所示，可以多種波特率與外部設備進行串口數(shù)據(jù)傳輸，輸出的 GPS/北斗定位數(shù)據(jù)采用 NMEA-0183 協(xié)議(默認)，控制協(xié)議為 SkyTraq 協(xié)議.

慣性傳感器IMU采用美國“英美盛”公司推出的全球首款整合性6軸運動處理器件MPU6050，如圖 5 所示，該傳感器內部集成了3軸MEMS陀螺儀及3軸MEMS加速度計，可以通過IIC接口輸出完整的6軸運動信息.

圖 4　GPS和北斗雙定位模塊S1216及其天線Fig.4　GPS and Beidou dual positioning module S1216 and its antenna

圖 5　IMU(MPU6050)Fig.5　IMU (MPU6050)

本實驗采用上下位機的分層結構進行控制系統(tǒng)設計，下位機采用深圳大疆創(chuàng)新科技有限公司的stm32f427單片機主控板，通過多種總線接口與傳感器直接進行信息交互，再將得到的傳感器信息通過串口發(fā)送至上位機英偉達Jetson TX1開發(fā)板，上位機負責進行數(shù)據(jù)存儲并進行上述改進擴展卡爾曼濾波算法的運算. Jetson TX1開發(fā)板安裝有Ubuntu操作系統(tǒng)，在Ubuntu環(huán)境下運行ROS(robot operation system，機器人操作系統(tǒng))以進行程序開發(fā)[9]. 實驗整體硬件連接如圖 8 所示，GPS模塊、IMU模塊和電機驅動器分別通過串口線、IIC總線和CAN總線與stm32f4單片機進行數(shù)據(jù)交互，單片機再通過串口線將傳感器數(shù)據(jù)上傳至Jetson TX1開發(fā)板.

ROS是一個適用于機器人控制的開源程序框架，它的核心是節(jié)點，用來執(zhí)行某個相對簡單的任務或進程，多個節(jié)點之間可以互相針對特定的主題傳遞消息或者提供服務. 在ROS中，所有主題消息都可以被消息記錄包加上時間戳存儲到后綴名為“.bag”的文件中，且可以通過ROS提供的工具進行消息重復回放，用以將實驗數(shù)據(jù)存儲下來用于未來的分析、處理、開發(fā)和算法驗證.

圖 6　stm32f4單片機主控板Fig.6　stm32f4 microcontroller main control board

圖 7　英偉達Jetson TX1開發(fā)板Fig.7　NVIDIA Jetson TX1 development board

本實驗將利用ROS的消息記錄包對實驗過程中的傳感器數(shù)據(jù)進行存儲，通過離線回放實驗數(shù)據(jù)進行不同傳感器配置下的算法可行性驗證及其效率比較. 按照圖 8 實驗硬件連接簡圖構建實驗平臺，在平臺上安裝兩個GPS模塊和兩個IMU模塊.

圖 8　實驗硬件連接簡圖Fig.8　Diagram of experimental hardware connection

數(shù)據(jù)處理過程的數(shù)據(jù)流向如圖 9 中虛線所示，圖 9 中實線箭頭部分為實驗數(shù)據(jù)采集時的數(shù)據(jù)流向，通過遙控實驗平臺在某校廣場進行閉環(huán)運動，從事先標記的地點出發(fā)，stm32f4單片機同時對驅動電機、兩個GPS模塊和2個IMU模塊進行數(shù)據(jù)采集，并將數(shù)據(jù)上傳給Jetson TX1上的ROS，ROS利用ROSBAG消息記錄與回放功能包記錄所有傳感器數(shù)據(jù)并進行存儲，實驗平臺繞廣場一周回到出發(fā)點后，實驗數(shù)據(jù)采集完畢并保存在上位機中.

圖 9　實驗數(shù)據(jù)流向及流程圖Fig.9　Experiment data flow and flowsheet

3.2　實驗結果分析

通過ROS消息記錄與回放功能包回放采集到的多傳感器數(shù)據(jù)，設計五種不同傳感器組合并使用同一個改進Kalman濾波算法應用方案進行數(shù)據(jù)處理，分別得到五種傳感器配置下的算法定位信息，以此來判斷算法對不同傳感器種類和數(shù)量測量系統(tǒng)的兼容性. 對兩個GPS模塊的定位信息求平均值作為實驗平臺的標準定位信息，通過算法定位信息與標準定位信息進行比較判斷算法的定位精度. 將數(shù)據(jù)處理得到的不同傳感器類型組合下算法定位數(shù)據(jù)導入到谷歌地圖[10]中，如圖 10～12 所示，分別表示由里程計、里程計和IMU、里程計與IMU和GPS傳感器配置組成的測量系統(tǒng)經過改進擴展卡爾曼濾波算法所得到的定位信息，三種傳感器組合所得到的路徑圖大致匹配，說明上述算法可以對不同傳感器組合達到很好的兼容性.

由數(shù)據(jù)處理得到的五種不同傳感器組合下算法定位信息誤差分析如表 1 所示，分別計算機器人坐標系下起點坐標與終點坐標的誤差值、算法定位信息與兩個GPS模塊定位信息求平均值得到的標準定位信息的位置測量標準差. 圖 13 與 14 分別為起點與終點坐標誤差與位置測量標準差.

表 1　不同傳感器配置的測量系統(tǒng)定位誤差

圖 10　里程計-路徑圖Fig.10　Path map of odometer

圖 11　里程計和IMU-路徑圖Fig.11　Path map of odometer and IMU

圖 12　里程計、IMU和GPS-路徑圖Fig.12　Path map of odometer, IMU and GPS

圖 13　起點與終點坐標誤差Fig.13　Coordinate errors between start and end position

圖 14　位置測量標準差Fig.14　Standard deviation of position measurement

從圖 13 與 14 可以看出，當只使用驅動電機的里程計數(shù)據(jù)時，測量系統(tǒng)的定位誤差已超過可接受范圍，隨著傳感器種類的增加，融合多種不同傳感器信息的定位精度得到顯著提升，當增加同一種傳感器數(shù)量時，經過算法處理的定位精度也得到進一步提高. 綜上所述，不同傳感器組合下，起點與終點坐標在機器人坐標系下的x軸和y軸方向誤差隨著傳感器種類和數(shù)量的增加而逐漸減小，實驗過程的位置測量標準差也隨之降低. 說明通過擴展算法的觀測矩陣不僅能對不同傳感器組合下的測量系統(tǒng)達到很好的兼容性，并且隨著傳感器的增加，算法定位精度也隨之提高.

4　結束語

本文針對移動機器人位姿跟蹤過程中測量系統(tǒng)傳感器信息處理問題進行了詳細研究. 首先介紹了擴展卡爾曼濾波算法，通過擴展觀測矩陣引入傳感器信息對觀測向量進行局部更新的方法，在保證定位精度的前提下，實現(xiàn)算法對不同傳感器類型和數(shù)量測量系統(tǒng)的兼容性. 然后，進行地面移動機器人位姿跟蹤算法模型設計，建立里程計模型與傳感器數(shù)學模型，搭建實驗平臺進行算法的可行性和可靠性驗證. 理論分析和實驗結果表明，該方法能提高測量系統(tǒng)的靈活性和可靠性.