本發(fā)明屬于智能導(dǎo)航裝置領(lǐng)域，尤其涉及的是一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)。

背景技術(shù)：

近年來(lái)，在一些衛(wèi)星導(dǎo)航等導(dǎo)航系統(tǒng)無(wú)法探測(cè)的陸地領(lǐng)域，慣性導(dǎo)航(簡(jiǎn)稱(chēng)ins)充分發(fā)揮了自身的優(yōu)勢(shì)?，F(xiàn)有技術(shù)中的捷聯(lián)慣性導(dǎo)航系統(tǒng)由加速度計(jì)和陀螺儀組成，安裝在運(yùn)載體上，分別計(jì)算運(yùn)載體的加速度信息和角速度信息。但在實(shí)際應(yīng)用中，由于捷聯(lián)慣性導(dǎo)航模塊的傳感單一性，導(dǎo)航誤差產(chǎn)生頻繁并會(huì)累積放大。

并發(fā)建圖與定位系統(tǒng)(又稱(chēng)同時(shí)定位與地圖構(gòu)建，簡(jiǎn)稱(chēng)slam)被認(rèn)為是實(shí)現(xiàn)真正全自助移動(dòng)機(jī)器人的關(guān)鍵。其描述了由移動(dòng)機(jī)器人從未知的地點(diǎn)出發(fā)，利用激光雷達(dá)對(duì)周?chē)h(huán)境進(jìn)行掃描，對(duì)掃描的信息進(jìn)行處理得到自身的位置，并構(gòu)建出周?chē)h(huán)境的地圖信息，完成自身定位和特征地圖的構(gòu)建的問(wèn)題，以實(shí)現(xiàn)對(duì)信息不可用，完全未知的環(huán)境的探測(cè)。然而激光雷達(dá)受掃描時(shí)間及范圍限制，隨著機(jī)器人移動(dòng)速度加快，反饋延遲提高，建圖與方位識(shí)別誤差漸增，直至無(wú)法有效建模，嚴(yán)重影響對(duì)機(jī)器人的定位導(dǎo)航作用。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明為了解決現(xiàn)有技術(shù)中，機(jī)器人導(dǎo)航系統(tǒng)傳感器單一產(chǎn)生的誤差積累，本發(fā)明將同時(shí)定位與地圖構(gòu)建(slam)與慣性導(dǎo)航(ins)進(jìn)行誤差融合，并反饋校正，提供一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，使機(jī)器人定位與導(dǎo)航更精確可靠。

本發(fā)明的技術(shù)方案如下：

一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，其中，包括運(yùn)載體、慣性導(dǎo)航(簡(jiǎn)稱(chēng)ins)器件、并發(fā)建圖定位(簡(jiǎn)稱(chēng)slam)裝置和誤差融合裝置；所述慣性導(dǎo)航器件包括加速度計(jì)、陀螺儀和慣性導(dǎo)航驅(qū)動(dòng)裝置，所述并發(fā)建圖定位裝置包括激光雷達(dá)和環(huán)境地圖構(gòu)建裝置；所述誤差融合裝置包括線性離散并發(fā)建圖定位誤差模塊、慣性導(dǎo)航誤差模塊和加權(quán)融合模塊；

在所述慣性導(dǎo)航器件中，所述加速度計(jì)獲得運(yùn)載體加速度，所述陀螺儀獲得運(yùn)載體角速度，并將加速度與角速度信號(hào)通過(guò)通訊連接傳送至慣性導(dǎo)航誤差模塊和慣性導(dǎo)航驅(qū)動(dòng)裝置；所述慣性導(dǎo)航誤差模塊進(jìn)行誤差分析；所述慣性導(dǎo)航驅(qū)動(dòng)裝置處理得到位置、速度和角度控制參數(shù)，調(diào)整運(yùn)載體姿態(tài)；

在所述并發(fā)建圖定位裝置中，所述激光雷達(dá)獲得運(yùn)載體與環(huán)境間距離和方位角，并將距離和方位角通過(guò)通訊連接，傳送至線性離散并發(fā)建圖定位誤差模塊和環(huán)境地圖構(gòu)件裝置，分別進(jìn)行誤差分析和地圖構(gòu)建；

在所述誤差融合裝置中，所述線性離散并發(fā)建圖定位誤差模塊和所述慣性導(dǎo)航誤差模塊分別將誤差通過(guò)通訊連接傳送至所述加權(quán)融合模塊，所述加權(quán)融合模塊分析獲得姿態(tài)誤差和地圖誤差，姿態(tài)誤差通過(guò)通訊連接反饋至慣性導(dǎo)航驅(qū)動(dòng)裝置校正位置、速度和角度控制參數(shù)，地圖誤差通過(guò)通訊連接傳送至環(huán)境地圖構(gòu)建裝置校正環(huán)境地圖。

進(jìn)一步，在線性離散并發(fā)建圖定位誤差模塊中，其采用的狀態(tài)方程：

δx(k+1)＝f(k)δx(k)+g(k)w(k)，

δx(k)表示誤差狀態(tài)向量，運(yùn)載體與地圖誤差狀態(tài)表示為：

δx(k)＝[δxv(k),δxm(k)]^t，

運(yùn)載體誤差狀態(tài)δxv(k)在導(dǎo)航坐標(biāo)系中表示為：

δxv(k)＝[δpⁿ(k),δvⁿ(k),δψⁿ(k)]^t，

地圖誤差狀態(tài)δxm(k)在導(dǎo)航坐標(biāo)系中表示為：

f(k)表示系統(tǒng)轉(zhuǎn)移矩陣，g(k)表示系統(tǒng)噪聲輸入矩陣，w(k)表示系統(tǒng)誤差向量，代表器件本身誤差，誤差強(qiáng)度為q(k)。

進(jìn)一步，所述慣性導(dǎo)航誤差模塊中，所述慣性導(dǎo)航和靜態(tài)環(huán)境模型為：

δpⁿ為位置誤差，δvⁿ為速度誤差，δψⁿ為姿態(tài)誤差，f^b和ω^b分別為慣性導(dǎo)航器件測(cè)量所得的加速度和角速度，δf^b和δω^b表示慣性導(dǎo)航器件的測(cè)量誤差，

表示由四元數(shù)組成的方向余弦矩陣，表示為：

σδf和σδω分別表示加速度和角速度的噪聲強(qiáng)度。

進(jìn)一步，所述慣性導(dǎo)航誤差模塊中，所述線性觀測(cè)模型為：

δz(k)＝h(k)δx(k)+v(k)

其中，h(k)是線性觀測(cè)雅克比矩陣，v(k)是觀測(cè)噪聲，噪聲強(qiáng)度矩陣為r(k)，誤差噪聲輸入為測(cè)量值與觀測(cè)值之差：

在導(dǎo)航坐標(biāo)系中初始路標(biāo)位置為：

其中，pⁿ(k)為k時(shí)刻路標(biāo)在導(dǎo)航坐標(biāo)系中的位置，為傳感器在以運(yùn)載體為中心的傳感器坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)器的的偏移量，為由傳感器坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)器的方向余弦矩陣，為在傳感器坐標(biāo)系中路標(biāo)的相對(duì)位置，表示為：

ρ，和θ分別是航向角、俯仰角和偏航角，由激光雷達(dá)可得：

其中，

通過(guò)計(jì)算方程的雅克比矩陣，線性離散模型可得：

所有的噪聲為零均值高斯噪聲，在預(yù)測(cè)過(guò)程中沒(méi)有誤差傳遞：

δx(k|k-1)＝f(k)δx(k-1|k-1)＝0

δx(k|k)＝δx(k|k-1)+w(k)ν(k)＝w(k)ν(k)

v(k)＝z(k)-h(k)δx(k|k-1)＝z(k)。

優(yōu)選的，在慣性導(dǎo)航器件和慣性導(dǎo)航誤差模塊中，導(dǎo)航坐標(biāo)使用東-北-天坐標(biāo)系，誤差狀態(tài)為9維狀態(tài)變量，分別為3維姿態(tài)誤差，3維速度誤差和3維位置誤差：

導(dǎo)航坐標(biāo)狀態(tài)方程為：

x(k+1)＝ax(k)+w(k)

其中，a為系數(shù)矩陣w(k)為系統(tǒng)噪聲，

ω為姿態(tài)矩陣，ωbx,ωby,ωbz載體坐標(biāo)系中的角速度，l表示緯度，ve,vn,vd表示東北天三個(gè)方向的速度，r表示地球半徑；

其觀測(cè)方程：在觀測(cè)方程中，觀測(cè)誤差為測(cè)量值與預(yù)測(cè)值之差：

導(dǎo)航坐標(biāo)觀測(cè)模型表示為：

z(k)＝bx(k)+v(k)，

其中，b為系數(shù)矩陣b＝[b1b2b3]^t，b1＝[diag[111]03×303×3]，b2＝[03×3diag[111]03×3]，b3＝[03×303×3diag[rcoslr1]]。

優(yōu)選的，所述加權(quán)融合模塊包括標(biāo)量加權(quán)融合裝置，

所述標(biāo)量加權(quán)融合裝置對(duì)線性離散并發(fā)建圖定位誤差模塊和慣性導(dǎo)航誤差模塊傳送的誤差進(jìn)行融合的狀態(tài)方程為：

兩個(gè)加權(quán)系數(shù)由以下方程計(jì)算獲得：

其中，tr表示矩陣的跡，p1和p2分別表示x和δx的協(xié)方差，p12表示x和δx的互協(xié)方差，取最優(yōu)估計(jì)誤差協(xié)方差矩陣可得：

且，trp0≤trpi,i＝1,2。

本發(fā)明所采用標(biāo)量加權(quán)融合裝置，相較于現(xiàn)有技術(shù)矩陣加權(quán)相比，本發(fā)明裝置模塊僅需要處理標(biāo)量加權(quán)系數(shù)，減少了計(jì)算量。尤其，在本發(fā)明中慣性導(dǎo)航維數(shù)較高，更有效地減少了處理量，顯著提高了融合反饋效率。更重要的是，其使本發(fā)明誤差融合校正機(jī)器人姿態(tài)及環(huán)境地圖的反饋延遲大大縮短。該效果正反饋?zhàn)饔糜趕lam和ins裝置，使姿態(tài)調(diào)整和繪圖校正流程縮短，形成良性循環(huán)，進(jìn)一步從源頭上減少了誤差積累。

本發(fā)明綜合技術(shù)方案及綜合效果包括：

本發(fā)明利用slam裝置和ins器件對(duì)周?chē)h(huán)境進(jìn)行掃描。slam將激光雷達(dá)作主傳感器，掃描周?chē)h(huán)境內(nèi)障礙物，得到運(yùn)載體與障礙物之間的距離以及方位角。ins利用慣性導(dǎo)航器件，加速度計(jì)測(cè)量得到運(yùn)載體的線性加速度，陀螺儀測(cè)量得到運(yùn)載體的角速度，通過(guò)ins驅(qū)動(dòng)裝置處理得到運(yùn)載體位置、速度和角度信息。然后利用各相應(yīng)slam誤差模塊和ins誤差模塊分別對(duì)誤差數(shù)據(jù)分析處理，并建立誤差狀態(tài)方程和觀測(cè)方程，將slam裝置得到的環(huán)境信息與ins裝置得到的環(huán)境信息利用融合裝置，采用加權(quán)平均的方法進(jìn)行誤差融合，再利用融合裝置得到的誤差修正運(yùn)載體的位置預(yù)測(cè)，使運(yùn)載體移動(dòng)機(jī)器人的定位與構(gòu)圖更加精確。

附圖說(shuō)明

圖1為本發(fā)明裝置運(yùn)轉(zhuǎn)流程框圖。

具體實(shí)施方式

以下結(jié)合附圖和具體實(shí)施例，對(duì)本發(fā)明進(jìn)行詳細(xì)說(shuō)明。

實(shí)施例1

本發(fā)明主要針對(duì)機(jī)器人智能運(yùn)載導(dǎo)航，發(fā)明了一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，該裝置及運(yùn)轉(zhuǎn)流程圖如圖1所示，

一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，其中，包括運(yùn)載體、慣性導(dǎo)航器件、并發(fā)建圖定位裝置和誤差融合裝置；所述慣性導(dǎo)航器件包括加速度計(jì)、陀螺儀和慣性導(dǎo)航驅(qū)動(dòng)裝置，所述并發(fā)建圖定位裝置包括激光雷達(dá)和環(huán)境地圖構(gòu)建裝置；所述誤差融合裝置包括線性離散并發(fā)建圖定位誤差模塊、慣性導(dǎo)航誤差模塊和加權(quán)融合模塊；

在所述慣性導(dǎo)航器件中，所述加速度計(jì)獲得運(yùn)載體加速度，所述陀螺儀獲得運(yùn)載體角速度，并將加速度與角速度信號(hào)通過(guò)通訊連接傳送至慣性導(dǎo)航誤差模塊和慣性導(dǎo)航驅(qū)動(dòng)裝置；所述慣性導(dǎo)航誤差模塊進(jìn)行誤差分析；所述慣性導(dǎo)航驅(qū)動(dòng)裝置處理得到位置、速度和角度控制參數(shù)，調(diào)整運(yùn)載體姿態(tài)；

在所述并發(fā)建圖定位裝置中，所述激光雷達(dá)獲得運(yùn)載體與環(huán)境間距離和方位角，并將距離和方位角通過(guò)通訊連接，傳送至線性離散并發(fā)建圖定位誤差模塊和環(huán)境地圖構(gòu)件裝置，分別進(jìn)行誤差分析和地圖構(gòu)建；

在所述誤差融合裝置中，所述線性離散并發(fā)建圖定位誤差模塊和所述慣性導(dǎo)航誤差模塊分別將誤差通過(guò)通訊連接傳送至所述加權(quán)融合模塊，所述加權(quán)融合模塊分析獲得姿態(tài)誤差和地圖誤差，姿態(tài)誤差通過(guò)通訊連接反饋至慣性導(dǎo)航驅(qū)動(dòng)裝置校正位置、速度和角度控制參數(shù)，地圖誤差通過(guò)通訊連接傳送至環(huán)境地圖構(gòu)建裝置校正環(huán)境地圖。

在線性離散并發(fā)建圖定位誤差模塊中，其采用的狀態(tài)方程：δx(k+1)＝f(k)δx(k)+g(k)w(k)

δx(k)表示誤差狀態(tài)向量，運(yùn)載體與地圖誤差狀態(tài)表示為：

δx(k)＝[δxv(k),δxm(k)]^t

運(yùn)載體誤差狀態(tài)δxv(k)在導(dǎo)航坐標(biāo)系中表示為：

δxv(k)＝[δpⁿ(k),δvⁿ(k),δψⁿ(k)]^t

地圖誤差狀態(tài)δxm(k)在導(dǎo)航坐標(biāo)系中表示為：

f(k)表示系統(tǒng)轉(zhuǎn)移矩陣，g(k)表示系統(tǒng)噪聲輸入矩陣，w(k)表示系統(tǒng)誤差向量，代表器件本身誤差，誤差強(qiáng)度為q(k)。

所述慣性導(dǎo)航誤差模塊中，所述慣性導(dǎo)航和靜態(tài)環(huán)境模型為：

δpⁿ為位置誤差，δvⁿ為速度誤差，δψⁿ為姿態(tài)誤差，f^b和ω^b分別為慣性導(dǎo)航器件測(cè)量所得的加速度和角速度，δf^b和δω^b表示慣性導(dǎo)航器件的測(cè)量誤差，

表示由四元數(shù)組成的方向余弦矩陣，表示為：

σδf和σδω分別表示加速度和角速度的噪聲強(qiáng)度。

所述慣性導(dǎo)航誤差模塊中，所述線性觀測(cè)模型為：

δz(k)＝h(k)δx(k)+v(k)

其中，h(k)是線性觀測(cè)雅克比矩陣，v(k)是觀測(cè)噪聲，噪聲強(qiáng)度矩陣為r(k)，誤差噪聲輸入為測(cè)量值與觀測(cè)值之差：

在導(dǎo)航坐標(biāo)系中初始路標(biāo)位置為：

其中，pⁿ(k)為k時(shí)刻路標(biāo)在導(dǎo)航坐標(biāo)系中的位置，為傳感器在以運(yùn)載體為中心的傳感器坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)器的的偏移量，為由傳感器坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)器的方向余弦矩陣，為在傳感器坐標(biāo)系中路標(biāo)的相對(duì)位置，表示為：

ρ，和θ分別是航向角、俯仰角和偏航角，由激光雷達(dá)可得：

其中，

通過(guò)計(jì)算方程的雅克比矩陣，線性離散模型可得：

所有的噪聲為零均值高斯噪聲，在預(yù)測(cè)過(guò)程中沒(méi)有誤差傳遞：

δx(k|k-1)＝f(k)δx(k-1|k-1)＝0

δx(k|k)＝δx(k|k-1)+w(k)ν(k)＝w(k)ν(k)

v(k)＝z(k)-h(k)δx(k|k-1)＝z(k)。

在慣性導(dǎo)航器件和慣性導(dǎo)航誤差模塊中，導(dǎo)航坐標(biāo)使用東-北-天坐標(biāo)系，誤差狀態(tài)為9維狀態(tài)變量，分別為3維姿態(tài)誤差，3維速度誤差和3維位置誤差：

導(dǎo)航坐標(biāo)狀態(tài)方程為：

x(k+1)＝ax(k)+w(k)

其中，a為系數(shù)矩陣w(k)為系統(tǒng)噪聲，

ω為姿態(tài)矩陣，ωbx,ωby,ωbz載體坐標(biāo)系中的角速度，l表示緯度，ve,vn,vd表示東北天三個(gè)方向的速度，r表示地球半徑；

其觀測(cè)方程：在觀測(cè)方程中，觀測(cè)誤差為測(cè)量值與預(yù)測(cè)值之差：

導(dǎo)航坐標(biāo)觀測(cè)模型表示為：

z(k)＝bx(k)+v(k)，

其中，b為系數(shù)矩陣b＝[b1b2b3]^t，b1＝[diag[111]03×303×3]，b2＝[03×3diag[111]03×3]，b3＝[03×303×3diag[rcoslr1]]。

所述加權(quán)融合模塊包括標(biāo)量加權(quán)融合裝置，

所述標(biāo)量加權(quán)融合裝置對(duì)線性離散并發(fā)建圖定位誤差模塊和慣性導(dǎo)航誤差模塊傳送的誤差進(jìn)行融合的狀態(tài)方程為：

兩個(gè)加權(quán)系數(shù)由以下方程計(jì)算獲得：

其中，tr表示矩陣的跡，p1和p2分別表示x和δx的協(xié)方差，p12表示x和δx的互協(xié)方差，取最優(yōu)估計(jì)誤差協(xié)方差矩陣可得：

且，trp0≤trpi,i＝1,2。

本實(shí)施例所采用標(biāo)量加權(quán)融合裝置，相較于現(xiàn)有技術(shù)矩陣加權(quán)相比，本實(shí)施例裝置模塊僅需要處理標(biāo)量加權(quán)系數(shù)，減少了計(jì)算量。尤其，在本實(shí)施例中慣性導(dǎo)航維數(shù)較高，更有效地減少了處理量，顯著提高了融合反饋效率。更重要的是，其使本實(shí)施例誤差融合校正機(jī)器人姿態(tài)及環(huán)境地圖的反饋延遲大大縮短。該效果正反饋?zhàn)饔糜趕lam和ins裝置，使姿態(tài)調(diào)整和繪圖校正流程縮短，形成良性循環(huán)，進(jìn)一步從源頭上減少了誤差積累。

應(yīng)當(dāng)理解的是，對(duì)本領(lǐng)域普通技術(shù)人員來(lái)說(shuō)，可以根據(jù)上述說(shuō)明加以改進(jìn)或變換，而所有這些改進(jìn)和變換都應(yīng)屬于本發(fā)明所附權(quán)利要求的保護(hù)范圍。