基于衛(wèi)星定位和慣導(dǎo)融合的模擬測(cè)繪系統(tǒng)設(shè)計(jì)

(南京信息工程大學(xué) 電子與信息工程學(xué)院，江蘇 南京 210044)

測(cè)繪系統(tǒng)是通過(guò)測(cè)量手段獲得反映地面現(xiàn)狀的圖形和位置信息，測(cè)繪在地理信息系統(tǒng)、城市規(guī)劃、土地利用、防洪減災(zāi)、水電工程規(guī)劃與設(shè)計(jì)選址、地質(zhì)災(zāi)害評(píng)估、宏觀分析地形地貌和地質(zhì)構(gòu)造等領(lǐng)域有著重要作用[1]?，F(xiàn)有的測(cè)繪方式主要有：

① 直接從地面測(cè)量，所涉及儀器有水平導(dǎo)軌、測(cè)針、測(cè)針架和相對(duì)高程測(cè)量板等構(gòu)件，也可用車載GPS、全站儀等高端儀器。

② 根據(jù)航空或航天影像，通過(guò)攝影測(cè)量途徑獲取，如立體坐標(biāo)儀觀測(cè)及空三加密法、解析測(cè)圖、數(shù)字?jǐn)z影測(cè)量等。

③ 從現(xiàn)有地形圖上采集，如格網(wǎng)讀點(diǎn)法、數(shù)字化儀手扶跟蹤及掃描半自動(dòng)采集然后進(jìn)行繪圖[2]。

這些方式存在以下的問(wèn)題：方法①直接測(cè)量耗費(fèi)人力物力較大；方法②測(cè)量裝置復(fù)雜，成本高；方法③測(cè)量不精確，一般地圖存在數(shù)據(jù)老舊的問(wèn)題。

本文設(shè)計(jì)了一種基于衛(wèi)星定位和慣導(dǎo)融合的模擬測(cè)繪系統(tǒng)，用以對(duì)現(xiàn)有的測(cè)繪系統(tǒng)進(jìn)行補(bǔ)充，利用履帶車，通過(guò)慣性導(dǎo)航模塊和衛(wèi)星定位模塊進(jìn)行位置地理信息采集，結(jié)合算法得到地面數(shù)字高程模型(digital elevation model)[3]。

1 系統(tǒng)結(jié)構(gòu)

測(cè)繪系統(tǒng)結(jié)構(gòu)如圖1所示，包括3部分：數(shù)據(jù)采集子系統(tǒng)、通信子系統(tǒng)、地面站子系統(tǒng)。數(shù)據(jù)采集子系統(tǒng)借助移動(dòng)平臺(tái)完成地理信息采集，并接收地面站系統(tǒng)完成移動(dòng)車的控制。通信子系統(tǒng)借助4G網(wǎng)絡(luò)完成地理信息和圖像傳輸。地面站接收地理信息數(shù)據(jù)并處理形成地面高程模型，并可以對(duì)移動(dòng)平臺(tái)進(jìn)行遠(yuǎn)程控制。

圖1 測(cè)繪系統(tǒng)的結(jié)構(gòu)框圖

1.1 數(shù)據(jù)采集子系統(tǒng)設(shè)計(jì)

數(shù)據(jù)采集子系統(tǒng)以金屬履帶小車作為移動(dòng)平臺(tái)，航模電池供電，采用單片機(jī)STM32F103作為主控制器，BD/GPS雙精度模塊作為地理數(shù)據(jù)獲取的主模塊，輔之以九軸MPU結(jié)合慣導(dǎo)融合算法獲得三維數(shù)據(jù)。USB攝像頭用以獲取視頻圖像數(shù)據(jù)，樹莓派自帶WiFi模塊連入以太網(wǎng)負(fù)責(zé)子系統(tǒng)通信。數(shù)據(jù)采集子系統(tǒng)留有預(yù)留接口，可根據(jù)測(cè)量要求自行選擇添加測(cè)量模塊。

系統(tǒng)采用3S航模電池提供11.1 V直流電壓，通過(guò)L298N模塊給履帶車的兩個(gè)編碼電機(jī)供電，如圖2所示；采用LM2596S DC-DC直流可調(diào)降壓-穩(wěn)壓電源模塊進(jìn)行電壓轉(zhuǎn)換，將11.1 V轉(zhuǎn)化成5 V給STM32F103單片機(jī)和樹莓派供電；在單片機(jī)最小系統(tǒng)板上有LM1117降壓電路，將5 V電壓轉(zhuǎn)為3.3 V，給圖3所示的BD/GPS雙精度模塊和圖4所示的慣導(dǎo)模塊(GY-99九軸MPU)供電。主控單片機(jī)通過(guò)輸出兩路PWM波至L298N模塊，從而驅(qū)動(dòng)編碼電機(jī)，帶動(dòng)數(shù)據(jù)采集終端移動(dòng)。單片機(jī)分別通過(guò)串口2(PA2和PA3)和串口3(PB10和PB11)獲取BD/GPS雙精度模塊和GY99模塊的數(shù)據(jù)，通過(guò)串口1(PA9和PA10)和樹莓派通信。如圖5所示，USB攝像頭直接與樹莓派的USB接口相連。

圖2 電機(jī)驅(qū)動(dòng)模塊電路原理圖

圖3 DEM數(shù)據(jù)探測(cè)的定位模塊電路原理圖

圖4 慣導(dǎo)模塊-九軸MPU電路原理圖

圖5 樹莓派和USB接口電路原理圖

數(shù)據(jù)采集子系統(tǒng)分為手動(dòng)控制模式和自主巡航模式。手動(dòng)控制模式下，操作人員使用地面站以第一視角的方式通過(guò)物聯(lián)網(wǎng)技術(shù)控制數(shù)據(jù)采集終端在指定區(qū)域運(yùn)行，采集地面高程信息，并將數(shù)據(jù)上傳至云服務(wù)器；自主巡航模式下操作人員使用地面站給數(shù)據(jù)采集終端設(shè)定好移動(dòng)路徑后，數(shù)據(jù)采集終端沿指定路徑行駛，采集相應(yīng)的高程信息，并將數(shù)據(jù)上傳至云服務(wù)器。在這兩種模式下，操作人員均可以使用地面站以第一視角實(shí)時(shí)掌握數(shù)據(jù)采集終端的運(yùn)行狀況，該子系統(tǒng)實(shí)物如圖6所示。

圖6 數(shù)據(jù)采集子系統(tǒng)硬件實(shí)物圖

1.2 通信子系統(tǒng)設(shè)計(jì)

通信子系統(tǒng)的通信鏈路為：主控STM32F103單片機(jī)和樹莓派之間通過(guò)串口通信，樹莓派WiFi模塊通過(guò)4G路由器，連入以太網(wǎng)，將數(shù)據(jù)上傳給云服務(wù)器，云服務(wù)器將數(shù)據(jù)初步處理后發(fā)送給PC客戶端，即地面站，整個(gè)通信鏈路均是雙向的。

主控STM32F103單片機(jī)主要采集地理信息，樹莓派主要采集圖像信息并將圖像信息和地理信息打包上傳；阿里云服務(wù)器相當(dāng)于數(shù)據(jù)中轉(zhuǎn)站對(duì)數(shù)據(jù)進(jìn)行解包和打包；PC客戶端可以請(qǐng)求相應(yīng)設(shè)備的視頻數(shù)據(jù)和采集數(shù)據(jù)。同時(shí)客戶端可以發(fā)送命令控制相應(yīng)的設(shè)備，并且客戶端可通過(guò)物聯(lián)網(wǎng)技術(shù)控制多臺(tái)設(shè)備[6]。

在整個(gè)通信鏈路中，數(shù)據(jù)以二進(jìn)制流進(jìn)行傳輸，方便各種系統(tǒng)的移植和接收。設(shè)計(jì)的數(shù)據(jù)包結(jié)構(gòu)如圖7所示，數(shù)據(jù)包包括隨機(jī)ID號(hào)(2字節(jié))、命令號(hào)(2字節(jié))、數(shù)據(jù)包總包數(shù)(2字節(jié))、當(dāng)前包號(hào)(2字節(jié))以及數(shù)據(jù)包本體內(nèi)容，其中隨機(jī)ID號(hào)和命令號(hào)用于判斷數(shù)據(jù)包的數(shù)據(jù)特征，方便系統(tǒng)區(qū)分地理信息數(shù)據(jù)、指令數(shù)據(jù)和視頻圖像數(shù)據(jù)，從而進(jìn)行解包和打包。

圖7 數(shù)據(jù)包結(jié)構(gòu)圖

1.3 地面站子系統(tǒng)

地面站子系統(tǒng)為PC客戶端，用以進(jìn)行后臺(tái)設(shè)置和數(shù)據(jù)處理。后臺(tái)設(shè)置主要用以登錄服務(wù)器，輸入服務(wù)器的IP地址和端口號(hào)后登錄服務(wù)器，同時(shí)設(shè)置好數(shù)據(jù)的存儲(chǔ)位置，完成后臺(tái)基本設(shè)置。

數(shù)據(jù)處理主要實(shí)現(xiàn)高程曲線繪制和自主巡航的路徑顯示[7]，地面站聯(lián)網(wǎng)后，接入網(wǎng)頁(yè)地圖，用戶在地圖上規(guī)劃路線后結(jié)點(diǎn)，以綠線顯示，如圖8所示，用戶按“開始巡回”按鈕控制數(shù)據(jù)采集子系統(tǒng)進(jìn)行采集數(shù)據(jù)。地面站接收到數(shù)據(jù)并處理后生成高度與時(shí)間曲線，進(jìn)一步處理后生成地面數(shù)字高程模型圖，路線數(shù)據(jù)以經(jīng)緯坐標(biāo)的形式發(fā)送給數(shù)據(jù)采集終端，終端主控執(zhí)行自主巡航功能，同時(shí)再將實(shí)際的行駛路線反饋給地面站，在網(wǎng)頁(yè)地圖上以藍(lán)線顯示出來(lái)。

圖8 自主巡航路徑顯示界面

地面站還實(shí)時(shí)顯示數(shù)據(jù)采集終端傳回的第一視角圖像，并實(shí)現(xiàn)手動(dòng)控制數(shù)據(jù)采集終端功能，通過(guò)PC機(jī)鍵盤控制履帶車的移動(dòng)，鍵盤W、A、S、D及空格5個(gè)鍵，代表前進(jìn)、左拐、后退、右拐和停止。

1.4 慣導(dǎo)融合的實(shí)現(xiàn)

為提高履帶車巡航精度，使自主巡航路線穩(wěn)定、準(zhǔn)確，采用捷聯(lián)式慣導(dǎo)技術(shù)[8]，對(duì)應(yīng)位姿解算方法如圖9所示[9]。數(shù)據(jù)采集終端利用慣性傳感器GY99模塊測(cè)量系統(tǒng)運(yùn)動(dòng)信息，再通過(guò)對(duì)運(yùn)動(dòng)信息的解算得出系統(tǒng)相對(duì)全局參考坐標(biāo)系的姿態(tài)航向、位移及速度信息[10]。

圖9 捷聯(lián)慣導(dǎo)位姿算法

設(shè)計(jì)采用位置型捷聯(lián)慣導(dǎo)系統(tǒng)，GY99模塊可直接讀取角位移信號(hào)，慣性導(dǎo)航所用的坐標(biāo)系是根據(jù)定位和指南針?biāo)玫淖鴺?biāo)系，是基于地球本身的[11]。

R(b,n)矩陣中，φ表示俯仰角(Pitch)，θ表示橫滾角(Roll)，ψ表示航向角(Yaw)，代入式A(n)=R(b,n)A(b)即可得到導(dǎo)航坐標(biāo)系下的加速度信息，為避免程序中的大量三角函數(shù)運(yùn)算，改使用四元數(shù)旋轉(zhuǎn)矩陣

(1)

代替R(b,n)，可得

(2) 對(duì)重力加速度進(jìn)行補(bǔ)償?shù)玫饺旨铀俣?，記?/p>

(2)

式中，g為重力加速度。

(3) 對(duì)全局加速度進(jìn)行一次積分得到速度，記作

(3)

離散化后得到

(4)

式中，V(k)是連續(xù)量V(e)的離散化表達(dá)；A(j)是A(e)的離散化表達(dá)；Kia是對(duì)加速度求累加和的比例系數(shù)。

(4) 對(duì)速度再進(jìn)行一次積分得到位移，記作

(5)

離散化后得到

(6)

式中，S(k)是連續(xù)量S(e)的離散化表達(dá)，V(j)是V(e)的離散化表達(dá)，Kiv是對(duì)速度求累加和的比例系數(shù)。

(5) 對(duì)加速度、速度、位移進(jìn)行三階互補(bǔ)融合修正。具體融合方法為：

① 將BD/GPS雙精度模塊測(cè)得的系統(tǒng)位移的變化量與積分求得的位移變化量作差，記作

ΔS(e)=SBG(e)-S(e)

(7)

式中，SBG(e)表示由BD/GPS雙精度模塊測(cè)得的系統(tǒng)位移的變化量。

② 對(duì)ΔS(e)進(jìn)行微分得到

(8)

離散化后得到

ΔV(k)=Kds[ΔS(k)-ΔS(k-1)]

(9)

式中，ΔV(k)是ΔV(e)的離散化表達(dá);ΔS(k)是ΔS(e)的離散化表達(dá);Kds是ΔS(k)一階差分系數(shù)。

③ 對(duì)ΔV(e)進(jìn)行微分得到

(10)

離散化后得到

ΔA(k)=Kdv[ΔV(k)-ΔV(k-1)]

(11)

式中，ΔA(k)是ΔA(e)的離散化，Kdv是ΔV(k)的一階差分系數(shù)。將ΔA(e)與A(e)融合后進(jìn)行第三步，將ΔV(e)與V(e)融合后進(jìn)行第四步，將ΔS(e)與S(e)融合后輸出位移信息記作SO(e)。

按上述步驟，得到圖10所示位置型捷聯(lián)式慣導(dǎo)系統(tǒng)的算法結(jié)構(gòu)框圖。

1.5 慣導(dǎo)精度的提升

慣導(dǎo)誤差主要存在于初始對(duì)準(zhǔn)精度和慣性器件的誤差[13]。慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)精度直接影響著系統(tǒng)導(dǎo)航精度，系統(tǒng)誤差是隨時(shí)間積累，為了提高導(dǎo)航精度，使用卡爾曼濾波器進(jìn)行慣導(dǎo)初始對(duì)準(zhǔn)，通過(guò)外部基準(zhǔn)得到系統(tǒng)觀測(cè)值，推算出狀態(tài)值最優(yōu)解，進(jìn)行補(bǔ)償。

慣性器件有陀螺儀和加速度計(jì)，陀螺儀的誤差是常值漂移和白噪聲，加速度計(jì)誤差是常值誤差和白噪聲。由于慣性器件本身存在誤差，導(dǎo)致慣導(dǎo)系統(tǒng)參數(shù)也存在一定誤差，包括速度誤差、位置誤差等。慣性器件的常值誤差可以通過(guò)實(shí)驗(yàn)進(jìn)行估計(jì)，進(jìn)行補(bǔ)償。而白噪聲采用卡爾曼濾波算法進(jìn)行濾除?？柭鼮V波算法如式(12)：

X(k|k-1)=A·X(k-1|k-1)+B·U(k)

(12)

式中,X(k|k-1)為上一狀態(tài)預(yù)測(cè)的結(jié)果;X(k-1|k-1)為上一時(shí)刻的最優(yōu)預(yù)測(cè)值;A為系數(shù)矩陣;U(k)為現(xiàn)狀態(tài)的控制量。

在實(shí)際測(cè)試中，使用均方根誤差來(lái)描述慣導(dǎo)裝置的速度、角速度、航向角、姿態(tài)角精度，記做RMS。在測(cè)試多次的情況下，均方根誤差用式(13)統(tǒng)計(jì)獲得：

(13)

式中，n為有效試驗(yàn)次數(shù)；mi為第i次試驗(yàn)的采樣點(diǎn)數(shù)；j、xij和x0ij為第i次試驗(yàn)的第j個(gè)采樣時(shí)刻、時(shí)刻的測(cè)量值和時(shí)刻的真值。

1.6 DEM模型的產(chǎn)生

地面站將接收到的離散的三維坐標(biāo)點(diǎn)以經(jīng)度坐標(biāo)為列(x軸)、緯度坐標(biāo)為行(y軸)排序后生成一個(gè)m×n的二維矩陣A，高程值z(mì)為其函數(shù)值，然后按一定順序?qū)⒕仃嘇分成若干個(gè)2×2子矩陣，組成一個(gè)四邊形，使得相鄰兩個(gè)2×2矩陣(四邊形)共享一行(或列)的數(shù)據(jù)。利用一個(gè)子矩陣中的4個(gè)元素來(lái)確定該子矩陣對(duì)應(yīng)區(qū)域內(nèi)的雙線性多項(xiàng)式，其函數(shù)形式為

z(x,y)=a0+a1x+a2y+a3xy

(14)

式中，a0，a1，a2，a3為待定系數(shù)。設(shè)4個(gè)元素對(duì)應(yīng)的三維坐標(biāo)為P1(x1,y1,z1)，P2(x2,y2,z2)，P3(x3,y3,z3)，P4(x4,y4,z4)，代入式(14),得

解得

將求得參數(shù)值代入函數(shù)，內(nèi)插出該子矩陣對(duì)應(yīng)區(qū)域內(nèi)的內(nèi)插點(diǎn)的高程。內(nèi)插出高程后，建立DEM，輸出數(shù)字高程模型圖。

2 系統(tǒng)測(cè)試

設(shè)計(jì)測(cè)繪系統(tǒng)能夠?qū)崿F(xiàn)自主巡航功能，即按照設(shè)定好的路徑智能移動(dòng)，采集相關(guān)地理信息數(shù)據(jù)。

首先，通過(guò)BD/GPS雙精度定位模塊獲得數(shù)據(jù)采集終端的位置坐標(biāo)信息，然后結(jié)合車載陀螺儀、加速度計(jì)的數(shù)據(jù)采用慣導(dǎo)融合算法測(cè)算具體地形下的位移、速度等數(shù)據(jù)信息，進(jìn)一步對(duì)履帶車作出準(zhǔn)確定位。最后，在基于地球本身坐標(biāo)定位基礎(chǔ)上，采用PID算法，實(shí)現(xiàn)履帶車在設(shè)定的路線下自主巡航，同時(shí)在自主巡航過(guò)程中采集相關(guān)地理信息。圖11為自主巡航的控制界面，當(dāng)設(shè)定好巡航路徑后，履帶車可以在地圖選中的點(diǎn)之間進(jìn)行自主巡航，并反饋回實(shí)時(shí)坐標(biāo)。

圖11 自主巡航控制界面

利用測(cè)繪系統(tǒng)采集的地理位置、地面高程信息及履帶車第一視角拍攝的圖片信息，通過(guò)云服務(wù)器傳至PC客戶端，再由PC客戶端相應(yīng)的算法處理得到地面DEM。圖12為一段時(shí)間內(nèi)測(cè)試中地面站顯示地面高程信息數(shù)據(jù)，坐標(biāo)為實(shí)時(shí)時(shí)間(min：s)，縱坐標(biāo)為地面高程(m)。

圖12 地面高程信息圖

將測(cè)量的地理信息數(shù)據(jù)以經(jīng)緯度和高度為坐標(biāo)軸采用內(nèi)插算法進(jìn)行處理，可得圖13的地面DEM，三維立體圖x、y、z軸對(duì)應(yīng)測(cè)量點(diǎn)經(jīng)度、緯度、高程信息。采用的定位BP/GPS模塊型號(hào)為s1216，能提供具體經(jīng)緯度信息，模塊獲取的經(jīng)緯度的精度為小數(shù)點(diǎn)后5位。根據(jù)雙精度模塊提供的數(shù)據(jù)，衛(wèi)星定位精度約為11.1 m。為了提高DEM數(shù)據(jù)的精度，進(jìn)行慣導(dǎo)融合，對(duì)衛(wèi)星定位最小的精度進(jìn)行等分處理，在每一等分處對(duì)采集數(shù)據(jù)進(jìn)行處理，使測(cè)量結(jié)果更準(zhǔn)確。

3 結(jié)束語(yǔ)

設(shè)計(jì)了一種基于衛(wèi)星定位和慣導(dǎo)融合的模擬測(cè)繪系統(tǒng)，結(jié)合了衛(wèi)星定位技術(shù)、自主巡航算法、卡爾曼濾波算法、慣導(dǎo)融合技術(shù)、DEM內(nèi)插算法、物聯(lián)網(wǎng)技術(shù)，實(shí)現(xiàn)了DEM測(cè)繪系統(tǒng)及其多功能應(yīng)用。測(cè)繪方案是以履帶車作為載體的測(cè)繪方式，能滿足部分的地形，但如遇臺(tái)階等復(fù)雜地形，會(huì)出現(xiàn)無(wú)法測(cè)量的情況，需手動(dòng)調(diào)整。測(cè)繪系統(tǒng)雖有一定的測(cè)繪局限性，但其智能化的自主巡航功能、云端數(shù)據(jù)分析功能有廣泛的工程應(yīng)用前景，后續(xù)將與更加靈活的機(jī)器人相結(jié)合，以期滿足更高的測(cè)繪要求。

圖13 地面DEM