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

有效

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

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)境地圖。

2.根據(jù)權(quán)利要求1所述的并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，其特征在于，在線性離散并發(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，

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

δpⁿ為位置誤差，δVⁿ為速度誤差，δψⁿ為姿態(tài)誤差，F(xiàn)(k)表示系統(tǒng)轉(zhuǎn)移矩陣，G(k)表示系統(tǒng)噪聲輸入矩陣，w(k)表示系統(tǒng)誤差向量，代表器件本身誤差，誤差強(qiáng)度為Q(k)。

3.根據(jù)權(quán)利要求2所述的并發(fā)建圖定位與慣性導(dǎo)航的組合誤差融合系統(tǒng)，其特征在于，所述慣性導(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ù)組成的方向余弦矩陣進(jìn)行表示，方向余弦矩陣表示為：

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