[發(fā)明專利]一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201710186418.X
• 申請(qǐng)日: 2017-03-24
• 公開（公告）號(hào): CN106969784B
• 公開（公告）日: 2019-08-13
• 發(fā)明（設(shè)計(jì)）人: 姜向遠(yuǎn);任鵬;許敏
• 申請(qǐng)（專利權(quán)）人: 山東大學(xué)
• 主分類號(hào): G01C25/00
• 分類號(hào): G01C25/00
• 代理公司: 北京匯捷知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11531
• 代理人: 于鵬

本發(fā)明公開了一種并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，屬于智能導(dǎo)航裝置領(lǐ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)融合模塊；使機(jī)器人定位與導(dǎo)航更精確可靠。

技術(shù)領(lǐng)域

本發(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)稱INS)充分發(fā)揮了自身的優(yōu)勢(shì)。現(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)(又稱同時(shí)定位與地圖構(gòu)建，簡(jiǎn)稱SLAM)被認(rèn)為是實(shí)現(xiàn)真正全自助移動(dòng)機(jī)器人的關(guān)鍵。其描述了由移動(dòng)機(jī)器人從未知的地點(diǎn)出發(fā)，利用激光雷達(dá)對(duì)周圍環(huán)境進(jìn)行掃描，對(duì)掃描的信息進(jìn)行處理得到自身的位置，并構(gòu)建出周圍環(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)航作用。

發(fā)明內(nèi)容

本發(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)稱INS)器件、并發(fā)建圖定位(簡(jiǎn)稱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)＝[δx_v(k),δx_m(k)]^T，

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

δx_v(k)＝[δpⁿ(k),δvⁿ(k),δψⁿ(k)]^T，