夏紅兵

(江蘇航運(yùn)職業(yè)技術(shù)學(xué)院, 江蘇 南通 226010)

0 引 言

協(xié)同導(dǎo)航定位系統(tǒng)的出現(xiàn)，標(biāo)志著人類太空文明的開始。在國(guó)外，關(guān)于較為先進(jìn)的定位系統(tǒng)有俄羅斯的GLONASSG 系統(tǒng)，歐盟的GALILEO，以及美國(guó)的GPS 系統(tǒng)[1–3]。而我國(guó)北斗導(dǎo)航系統(tǒng)（以下簡(jiǎn)稱"北斗系統(tǒng)"）是一種專注于國(guó)際安全和社會(huì)經(jīng)濟(jì)發(fā)展，自主創(chuàng)建，20 世紀(jì)末獨(dú)立運(yùn)作的協(xié)同導(dǎo)航定位系統(tǒng)。據(jù)此，本文以北斗系統(tǒng)為例，以SLAM算法作為輔助手段，通過(guò)對(duì)無(wú)人艇展開相關(guān)設(shè)計(jì)，并將該協(xié)同導(dǎo)航定位系統(tǒng)應(yīng)用其中，旨在分析其導(dǎo)航數(shù)據(jù)的誤差，為后續(xù)研究奠定一個(gè)較為詳備的理論基礎(chǔ)。

1 SLAM 算法基礎(chǔ)

1.1 SLAM 算法簡(jiǎn)介

目前關(guān)于無(wú)人艇技術(shù)的發(fā)展日新月異，各科研團(tuán)隊(duì)也已經(jīng)顯示出很強(qiáng)的研究能力。無(wú)人艇技術(shù)的發(fā)展，極大地降低了出海作業(yè)中工作人員的安全問(wèn)題，這種依據(jù)人類實(shí)際需求而進(jìn)行的相關(guān)設(shè)計(jì)與調(diào)整，不僅可以協(xié)助人類避免危險(xiǎn)環(huán)境，節(jié)約駕駛成本，還可以借助無(wú)人艇在高壓強(qiáng)海底進(jìn)行相關(guān)作業(yè)。

然而，若是想要實(shí)現(xiàn)無(wú)人艇在如此復(fù)雜外部環(huán)境中的自主運(yùn)行，就必須能夠在這些復(fù)雜場(chǎng)景中安全的進(jìn)行地圖繪制和導(dǎo)航。大多數(shù)研究學(xué)者都認(rèn)為解決SLAM 問(wèn)題是實(shí)現(xiàn)完全自主無(wú)人艇控制的關(guān)鍵先決條件，這個(gè)問(wèn)題一直是過(guò)去十多年學(xué)界研究的重點(diǎn)，SLAM 問(wèn)題也引起了人們對(duì)無(wú)人艇研究的重視。

現(xiàn)如今，隨著時(shí)代的發(fā)展以及相關(guān)技術(shù)的不斷進(jìn)步，關(guān)于SLAM 算法也得到了較為矚目的提升，并可以很好地解決了無(wú)人艇先前移入環(huán)境時(shí)位姿預(yù)估不可實(shí)現(xiàn)等問(wèn)題。比如有設(shè)計(jì)人員可以基于無(wú)人艇的外形特征、工作環(huán)境，以及水域?qū)嶋H運(yùn)動(dòng)情況，對(duì)無(wú)人艇進(jìn)行針對(duì)性的研發(fā)，在確保無(wú)人艇觀察到的信息不被破壞的同時(shí)，還可以借助SLAM 為無(wú)人艇重建全局環(huán)境地圖，由此引導(dǎo)無(wú)人艇得以在安全行駛路徑中開展工作[4–6]。

基于以上研究經(jīng)驗(yàn)，經(jīng)整理后認(rèn)為，要想實(shí)現(xiàn)SLAM 算法的過(guò)程，可以按照如圖1 所示的SLAM思路，將其分為預(yù)測(cè)和更新2 個(gè)階段。

圖1 SLAM 算法實(shí)現(xiàn)過(guò)程Fig. 1 Implementation process of SLAM algorithm

1.2 SLAM 程序設(shè)計(jì)

本文開發(fā)了一個(gè)基于SLAM 程序的完整的無(wú)人艇軟件系統(tǒng)，以解決無(wú)人艇協(xié)同導(dǎo)航定位方案和映射任務(wù)。

無(wú)人艇執(zhí)行各SLAM 任務(wù)的聯(lián)系如圖2 所示，當(dāng)無(wú)人艇進(jìn)行協(xié)同導(dǎo)航定位和映射任務(wù)時(shí)，遠(yuǎn)程無(wú)人艇可以在未知環(huán)境中使用用戶終端PC 和鍵盤進(jìn)行控制，并顯示了各個(gè)關(guān)鍵節(jié)點(diǎn)與實(shí)現(xiàn)無(wú)人艇SLAM 的主題之間的關(guān)系。

圖2 無(wú)人艇執(zhí)行各SLAM 任務(wù)的聯(lián)系Fig. 2 Connection of Unmanned Craft to Perform Various SLAM Tasks

在創(chuàng)建無(wú)人艇SLAM 系統(tǒng)時(shí)，本文創(chuàng)建了用于無(wú)人艇與客戶端末端計(jì)算機(jī)之間的通信ROS 網(wǎng)絡(luò)。同時(shí)在無(wú)人艇操作端，也通過(guò)PC 方式為技術(shù)人員提供了節(jié)點(diǎn)“/ keyop”，“/ slam_gmapping”和“/ rviz”，方便其工作同時(shí)優(yōu)化硬件配置的影響。此外，每個(gè)坐標(biāo)系之間的坐標(biāo)變換比率在主題“/ tf”中公布?！?Slam_gmapping”節(jié)點(diǎn)訂閱“/tf”和“/scan”主題，并基于時(shí)變調(diào)節(jié)因子EKF-SLAM 算法，以通過(guò)放置無(wú)人艇來(lái)創(chuàng)建漸變遞增的柵格地圖。

2 無(wú)人艇坐標(biāo)系模型建立

2.1 無(wú)人艇數(shù)學(xué)模型

為無(wú)人艇創(chuàng)建SLAM 算法問(wèn)題模型，考慮到由于每個(gè)模型的準(zhǔn)確性，都將直接影響到SLAM 系統(tǒng)的誤差評(píng)估，從而影響后續(xù)的協(xié)同導(dǎo)航定位系統(tǒng)性能。因此借助SLAM 算法開始部署無(wú)人艇時(shí)，首先創(chuàng)建3 個(gè)坐標(biāo)系，其中無(wú)人艇的局部坐標(biāo)系為X-O-Y，所有坐標(biāo)系都源于無(wú)人艇的原始位置，局部坐標(biāo)系的坐標(biāo)位于機(jī)體的幾何中心。

為了確定無(wú)人艇在環(huán)境中的位置變化，本文假設(shè)其在各種水域中，都可以借助坐標(biāo)系確定其自身的位置。

如圖3 所示，本文在整體的規(guī)劃思路上，是希望無(wú)人艇可以在各種水域中得以移動(dòng)，本文將其移動(dòng)的方向固定為無(wú)人艇坐標(biāo)系X軸方向，然后再將航行角度與X軸的夾角進(jìn)行計(jì)算，得到其在全局坐標(biāo)系中的位置（x(t)，y(t)，θ(t)）。由于無(wú)人艇在各種水域中只存在平動(dòng)和轉(zhuǎn)動(dòng)，因此得出以下公式，來(lái)表示無(wú)人艇在全局坐標(biāo)系中的位姿。

圖3 無(wú)人艇數(shù)學(xué)模型示意圖Fig. 3 Schematic diagram of the mathematical model of an unmanned boat

2.2 無(wú)人艇運(yùn)動(dòng)模型

按照以上思路設(shè)計(jì)出來(lái)的無(wú)人艇，再對(duì)其運(yùn)動(dòng)模型進(jìn)行構(gòu)建，可以讓該無(wú)人艇能于短時(shí)間內(nèi)進(jìn)行受控圓周運(yùn)動(dòng)，以便改變其運(yùn)動(dòng)軌跡。不過(guò)也有考慮到系統(tǒng)誤差，因此在進(jìn)行運(yùn)動(dòng)模型的設(shè)計(jì)中，依據(jù)無(wú)人艇的理論運(yùn)動(dòng)與無(wú)人艇輸入控制量，得到運(yùn)動(dòng)誤差，因此有必要通過(guò)合理的計(jì)算減小其誤差。最終，使用差動(dòng)驅(qū)動(dòng)對(duì)無(wú)人艇進(jìn)行轉(zhuǎn)換編碼，得出其增量式編碼器△t內(nèi)的平均速度：

用l與r分別表示左輪和右輪的速度，則：

無(wú)人艇的合速度可以表示為：

假設(shè)在t-1 時(shí)刻，無(wú)人艇的位姿為(xt?1,yt?1,θt?1)T，在 ?t時(shí)間內(nèi)，以恒定線速度v和角速度ω作為輸入，在t時(shí)刻時(shí)，無(wú)人艇的位姿為(xt,xt,θt)T。

按以上思路，可以求出無(wú)人艇的線速度v 和角速度 ω，無(wú)人艇進(jìn)行半徑為R的圓弧運(yùn)動(dòng)，半徑R的計(jì)算如下所示：

可知，無(wú)人艇的運(yùn)動(dòng)模型方程如下：

3 協(xié)同導(dǎo)航定位系統(tǒng)誤差分析

3.1 協(xié)同導(dǎo)航定位系統(tǒng)可見性的計(jì)算

設(shè)時(shí)刻t 無(wú)人艇的位置向量為：

協(xié)同導(dǎo)航定位系統(tǒng)位置的向量為：

則無(wú)人艇的歸一化位置坐標(biāo)向量為：

此時(shí)無(wú)人艇的位置方位仰角為：

其中：

根據(jù)以上理論推導(dǎo)，經(jīng)過(guò)仿真計(jì)算得：當(dāng)天可以同時(shí)觀察到9 顆可見協(xié)同導(dǎo)航定位系統(tǒng)，對(duì)上述的協(xié)同導(dǎo)航系統(tǒng)進(jìn)行3 次測(cè)試模擬，得到3 種測(cè)試巡航模式下的航跡分布圖如圖4 所示，可以發(fā)現(xiàn)無(wú)人艇可以很好的對(duì)軌跡進(jìn)行導(dǎo)航和跟蹤。

圖4 3 種測(cè)試模式下的航跡分布圖Fig. 4 Track distribution diagrams in three test modes

3.2 可見星空間位置坐標(biāo)的計(jì)算

根據(jù)協(xié)同導(dǎo)航定位系統(tǒng)電文提供的軌道參數(shù)，按一定的公式可計(jì)算出觀測(cè)瞬間無(wú)人艇在坐標(biāo)系中的位置。具體計(jì)算步驟如下：

首先，根據(jù)開普勒第三定律，對(duì)計(jì)算協(xié)同導(dǎo)航定位系統(tǒng)運(yùn)行的平均角速度n進(jìn)行計(jì)算：

其中的實(shí)際平均角速度為：

然后，考慮協(xié)同導(dǎo)航定位系統(tǒng)的軌道參數(shù)是相對(duì)于參考時(shí)間的，計(jì)算觀測(cè)時(shí)刻的平近點(diǎn)角Mk：

計(jì)算偏近點(diǎn)角Ek：

計(jì)算真近點(diǎn)角Vk，由于：

因此：

應(yīng)用SPSS 18.0統(tǒng)計(jì)軟件對(duì)數(shù)據(jù)進(jìn)行統(tǒng)計(jì)學(xué)分析。計(jì)量數(shù)據(jù)采用±s表示，計(jì)數(shù)資料采用頻數(shù)百分比表示，兩組數(shù)據(jù)比較采用獨(dú)立樣本t檢驗(yàn)以及卡方檢驗(yàn)，多組比較采用方差分析以及LSD兩兩比較，重復(fù)測(cè)量數(shù)據(jù)采用重復(fù)測(cè)量的方差分析。P＜0.05為差異有統(tǒng)計(jì)學(xué)意義。

計(jì)算升交點(diǎn)角距?k：

式中：w為導(dǎo)航電文中給出的近地點(diǎn)角距，據(jù)此計(jì)算攝動(dòng)改正項(xiàng)：

最后，計(jì)算協(xié)同導(dǎo)航定位系統(tǒng)在軌道平面坐標(biāo)系的坐標(biāo)：

3.3 協(xié)同導(dǎo)航定位參考點(diǎn)位置坐標(biāo)的計(jì)算

解算方程寫為矩陣形式為：

根據(jù)觀測(cè)協(xié)同導(dǎo)航定位系統(tǒng)個(gè)數(shù)，定位解算方程可以分為以下3 種情況：

1）當(dāng)觀測(cè)量n＜4 時(shí)，無(wú)解；

2）當(dāng)n=4 時(shí)，AX-L=0，故有唯一解，X=A-1L。

3）當(dāng)n＞4 時(shí)，方程個(gè)數(shù)大于未知數(shù)個(gè)數(shù)。此時(shí)，可用最小二乘法獲得無(wú)人艇位置和時(shí)間。

本次同時(shí)觀察到9 顆可見協(xié)同導(dǎo)航定位系統(tǒng)，因此可以采用最小二乘法對(duì)無(wú)人艇位置進(jìn)行定位計(jì)算。依照上述的協(xié)同導(dǎo)航定位系統(tǒng)坐標(biāo)，編寫程序，設(shè)置初始值。高度角：α=10°，采樣率：s=10 min。共仿真12 個(gè)小時(shí)，仿真結(jié)果如表1 所示。

表1 協(xié)同導(dǎo)航定位仿真結(jié)果Tab. 1 Simulation results of collaborative navigation and positioning

3.4 坐標(biāo)誤差分析

通過(guò)以上分析，初步計(jì)算出了“協(xié)同導(dǎo)航定位參考點(diǎn)”的位置坐標(biāo)。為了分析數(shù)據(jù)誤差，再次利用接收機(jī)于同一地點(diǎn)得到一組真實(shí)的位置坐標(biāo)數(shù)據(jù)。通過(guò)比較，兩者誤差如表2 所示。

表2 誤差結(jié)果對(duì)比Tab. 2 Comparison of error results

說(shuō)明該仿真程序能夠較為精確的實(shí)現(xiàn)對(duì)無(wú)人艇協(xié)同導(dǎo)航定位參考點(diǎn)的定位，達(dá)到了設(shè)計(jì)目的。

4 結(jié) 語(yǔ)

采用協(xié)同導(dǎo)航定位系統(tǒng)的無(wú)人艇具有良好的系統(tǒng)性能，能夠滿足高精度高速度的作戰(zhàn)要求，而國(guó)外在協(xié)同導(dǎo)航定位系統(tǒng)技術(shù)上，也都取得了較為廣泛的應(yīng)用，并逐步得到了廣泛傳播。