[發(fā)明專利]一種基于載體運(yùn)動(dòng)條件約束和單軸旋轉(zhuǎn)調(diào)制的車載自主導(dǎo)航方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010520820.9
• 申請(qǐng)日: 2020-06-09
• 公開(kāi)（公告）號(hào): CN111678514B
• 公開(kāi)（公告）日: 2023-03-28
• 發(fā)明（設(shè)計(jì)）人: 杜爽;甘旭東;張瑞琪;魯琪;王清林
• 申請(qǐng)（專利權(quán)）人: 電子科技大學(xué)
• 主分類號(hào): G01C21/18
• 分類號(hào): G01C21/18
• 代理公司: 電子科技大學(xué)專利中心 51203
• 代理人: 張冉

1.一種基于載體運(yùn)動(dòng)條件約束和單軸旋轉(zhuǎn)調(diào)制的車載自主導(dǎo)航方法，其特征在于，包括以下步驟：

步驟1.基于旋轉(zhuǎn)IMU的虛擬里程計(jì)計(jì)算載體前行速度和車輪旋轉(zhuǎn)角度；

將IMU安裝在車輪中心，使IMU隨著載體運(yùn)動(dòng)而旋轉(zhuǎn)；o-x_sy_sz_s代表傳感器坐標(biāo)系，o-x_by_bz_b代表載體坐標(biāo)系；當(dāng)載體運(yùn)動(dòng)時(shí)，o-x_sy_sz_s繞x_s軸旋轉(zhuǎn)；在傳感器坐標(biāo)系中，IMU實(shí)際輸出的比力和角速率/分別由式(1)和式(2)表示：

其中，為載體坐標(biāo)系至傳感器坐標(biāo)系的轉(zhuǎn)換矩陣，下標(biāo)b表示載體坐標(biāo)系，上標(biāo)s表示傳感器坐標(biāo)系，φ＝∫ωdt為車輪旋轉(zhuǎn)角度，ω為車輪旋轉(zhuǎn)角速度，t為時(shí)間；f^b為載體坐標(biāo)系下的比力，a^b和g^b分別為載體坐標(biāo)系下載體加速度矢量和當(dāng)?shù)刂亓铀俣仁噶?，?Sup>s為加速度計(jì)誤差；/為在載體坐標(biāo)系下載體坐標(biāo)系相對(duì)于慣性坐標(biāo)系的旋轉(zhuǎn)角速度，/為在傳感器坐標(biāo)系下載體坐標(biāo)系相對(duì)于傳感器坐標(biāo)系的旋轉(zhuǎn)角速度，d^s表示陀螺儀誤差；

y_s軸和z_s軸加速度計(jì)的實(shí)際輸出和/表示為：

其中，g為當(dāng)?shù)刂亓铀俣龋瑸檩d體坐標(biāo)系中y_b軸的加速度，/和/分別表示y_s軸和z_s軸的加速度計(jì)輸出誤差；

傳感器坐標(biāo)系中x_s軸陀螺儀實(shí)際輸出為：

其中，為x_s軸陀螺儀輸出誤差；/為載體坐標(biāo)系中x_b軸陀螺儀輸出；

y_s軸的加速度計(jì)實(shí)際輸出z_s軸的加速度計(jì)實(shí)際輸出/和x_s軸陀螺儀實(shí)際輸出/為：

其中，表示車輪旋轉(zhuǎn)角度φ的一階時(shí)間導(dǎo)數(shù)；

基于擴(kuò)展型卡爾曼濾波，通過(guò)IMU輸出來(lái)計(jì)算載體沿y_b軸速度和車輪旋轉(zhuǎn)角度φ；

系統(tǒng)狀態(tài)量x_o和系統(tǒng)方程分別由式(9)和(10)表示：

其中，r_w為車輪半徑，w_o為系統(tǒng)噪聲，δφ為車輪旋轉(zhuǎn)角度φ的誤差，/為載體沿y_b軸速度/的誤差，上標(biāo)T表示轉(zhuǎn)置，/表示系統(tǒng)狀態(tài)量x_o的一階時(shí)間導(dǎo)數(shù)；

經(jīng)過(guò)線性化之后的系統(tǒng)觀測(cè)模型由式(11)表示：

z_o＝H_ox_o+v (11)

其中，為線性化之后的系統(tǒng)觀測(cè)量，δf_y為y_s軸的加速度計(jì)輸出/的閉合差、δf_z為z_s軸的加速度計(jì)輸出/的閉合差，/為x_s軸陀螺儀輸出/的閉合差，v為觀測(cè)量噪聲；

步驟2.建立載體自主導(dǎo)航誤差方程

將比力f^s和旋轉(zhuǎn)角速度轉(zhuǎn)換至載體坐標(biāo)系，如式(12)和(13)所示：

其中，為傳感器坐標(biāo)系至載體坐標(biāo)系的轉(zhuǎn)換矩陣；

基于轉(zhuǎn)換至載體坐標(biāo)系的比力和角速率，利用傳統(tǒng)捷聯(lián)導(dǎo)航解算方程解算出載體位置rⁿ、速度vⁿ與姿態(tài)信息

(1)姿態(tài)誤差方程

捷聯(lián)慣性導(dǎo)航中姿態(tài)更新方程為：

其中，為/的一階時(shí)間導(dǎo)數(shù)，/為載體坐標(biāo)系至導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣，/是/的斜對(duì)稱矩陣，/為載體坐標(biāo)系下載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的旋轉(zhuǎn)角速度；

根據(jù)姿態(tài)誤差定義其中/為含有姿態(tài)誤差的轉(zhuǎn)換矩陣，I為單位矩陣，Eⁿ是姿態(tài)誤差εⁿ的斜對(duì)稱矩陣，εⁿ＝[ε_E ε_N ε_U]^T，ε_E、ε_N、ε_U分別代表姿態(tài)角誤差在東向、北向和天向的分量；

在小失準(zhǔn)角的情況下，通過(guò)對(duì)式(14)進(jìn)行擾動(dòng)分析推導(dǎo)得出姿態(tài)誤差方程：

其中，δrⁿ為位置誤差矢量，δλ和δh分別表示載體緯度誤差、經(jīng)度誤差和高度誤差；δvⁿ為速度誤差矢量，δvⁿ＝[δv_E δv_N δv_U]^T，δv_E、δv_N和δv_U分別代表了東向、北向和天向的速度誤差；d^b為載體坐標(biāo)系下的陀螺儀誤差矢量，d^b＝[d_x d_y d_z]^T，d_x、d_y和d_z分別表示x_b軸、y_b軸和z_b軸的陀螺儀誤差；F_εr，F(xiàn)_εv和F_εε分別代表姿態(tài)誤差變化率與位置誤差、速度誤差和姿態(tài)誤差的關(guān)系矩陣，可以表示為：

/

其中，M為載體所在位置子午圈的曲率半徑，N為載體所在位置卯酉圈的曲率半徑，ω_ie為地球自轉(zhuǎn)角速度值，h為載體高度，表示經(jīng)度λ的一階時(shí)間導(dǎo)數(shù)；

(2)速度誤差方程

導(dǎo)航坐標(biāo)系下速度更新方程表示為：

其中，為vⁿ的一階時(shí)間導(dǎo)數(shù)，vⁿ為載體相對(duì)于導(dǎo)航坐標(biāo)系的速度，/為地球自轉(zhuǎn)角速率/的斜對(duì)稱矩陣；/為旋轉(zhuǎn)角速度/的斜對(duì)稱矩陣，/為導(dǎo)航坐標(biāo)系下導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)角速度；gⁿ為導(dǎo)航坐標(biāo)系下的當(dāng)?shù)刂亓铀俣仁噶浚?/p>

根據(jù)速度誤差定義δvⁿ為速度誤差，/為含有誤差的速度，對(duì)式(16)進(jìn)行擾動(dòng)分析推導(dǎo)出速度誤差方程：

其中，為/的一階時(shí)間導(dǎo)數(shù)，γ^b為載體坐標(biāo)系下的加速度計(jì)誤差矢量，γ^b＝[γ_x γ_yγ_z]^T，γ_x、γ_y和γ_z表示x_b軸、y_b軸和z_b軸的加速度計(jì)誤差；F_vr、F_vv和F_vε分別代表速度誤差變化率與位置誤差、速度誤差和姿態(tài)誤差的關(guān)系矩陣，表示為：

其中，ν_E、v_N和v_U分別表示東向、北向和天向的載體的速度值，f_E、f_N和f_U分別表示東向、北向和天向的載體的比力值，γ表示隨載體維度和高度變化的當(dāng)?shù)刂亓铀俣龋?/p>

(3)位置誤差方程

捷聯(lián)慣性導(dǎo)航中位置更新方程：

其中，為載體緯度；

通過(guò)對(duì)式(18)進(jìn)行擾動(dòng)分析得出位置誤差方程：

其中，為/的誤差，F(xiàn)_rr為位置誤差變化率和速度誤差的關(guān)系矩陣，F(xiàn)_rv為位置誤差和速度誤差的關(guān)系矩陣，表示為：

(4)MEMSIMU傳感器誤差方程

MEMS加速度計(jì)和陀螺儀誤差建模為一階高斯馬可夫隨機(jī)過(guò)程，如式(20)所示：

其中，為η的一階時(shí)間導(dǎo)數(shù)，η為加速度計(jì)或陀螺儀誤差，α為一階高斯馬爾科夫隨機(jī)過(guò)程中的相關(guān)系數(shù)，w為驅(qū)動(dòng)白噪聲；

加速度計(jì)和陀螺儀誤差方程：

其中，為γ^b的一階時(shí)間導(dǎo)數(shù)，/為d^b的一階時(shí)間導(dǎo)數(shù)，/為/的一階時(shí)間導(dǎo)數(shù)，/為γ^s的一階時(shí)間導(dǎo)數(shù)，γ^s為加速度計(jì)誤差，/是/的斜對(duì)稱矩陣，/為載體坐標(biāo)系下傳感器坐標(biāo)系相對(duì)于載體坐標(biāo)系的旋轉(zhuǎn)角速度，α_f為加速度計(jì)一階高斯馬爾可夫隨機(jī)模型的相關(guān)系數(shù)，/α_fx、α_fy、α_fz分別為α_f在x_s軸、y_s軸、z_s軸方向的分量，w_f為加速度計(jì)的高斯馬爾可夫模型的驅(qū)動(dòng)白噪聲，w_f＝[w_fx y_fy w_fz]^T，w_fx、w_fy、w_fz分別為w_f在x_s軸、y_s軸、z_s軸方向的分量；/為d^s的一階時(shí)間導(dǎo)數(shù)，d^s為陀螺儀誤差，α_ω為陀螺儀一階高斯馬爾可夫隨機(jī)模型的相關(guān)系數(shù)，/α_ωx、α_ωy、α_ωz分別為α_ω在x_s軸、y_s軸、z_s軸方向的分量，w_ω為陀螺儀的高斯馬爾可夫模型的驅(qū)動(dòng)白噪聲，w_ω＝[w_ωx w_ωy w_ωz]^T，w_ωx、w_ωy、w_ωz分別為w_ω在x_s軸、y_s軸、z_s軸方向的分量；

步驟3.載體運(yùn)動(dòng)約束條件下的自主導(dǎo)航模型建立

利用擴(kuò)展型卡爾曼濾波計(jì)算出慣性系統(tǒng)的位置誤差、速度誤差、姿態(tài)誤差以及加速度計(jì)和陀螺儀誤差；

首先建立系統(tǒng)狀態(tài)方程；根據(jù)車載慣性導(dǎo)航系統(tǒng)特性，選取位置誤差、速度誤差、姿態(tài)誤差、加速度計(jì)和陀螺儀誤差組成15維狀態(tài)量，如式(23)所示：

x＝[δrⁿ δvⁿ εⁿ γ^b d^b]^T (23)

根據(jù)式(15)、(17)、(19)、(21)和(22)，系統(tǒng)狀態(tài)方程由式(24)表示：

其中，表示x的一階時(shí)間導(dǎo)數(shù)，/w為系統(tǒng)驅(qū)動(dòng)白噪聲，w_r、w_v和w_ε分別代表狀態(tài)方程中，對(duì)應(yīng)位置誤差、速度誤差和姿態(tài)誤差的驅(qū)動(dòng)白噪聲；0_3×3為3×3的零矩陣；

其次建立系統(tǒng)觀測(cè)方程；載體坐標(biāo)系o-x_by_bz_b下的速度矢量表示為：基于擾動(dòng)分析得出載體坐標(biāo)系下的速度誤差δv^b：

其中，v^b是vⁿ的斜對(duì)稱矩陣，表示載體坐標(biāo)系下的速度；為導(dǎo)航坐標(biāo)系至載體坐標(biāo)系的轉(zhuǎn)換矩陣；

系統(tǒng)觀測(cè)量方程為：

z＝Hx+v (26)

其中，z＝δv^b，v為觀測(cè)量噪聲；

基于系統(tǒng)狀態(tài)方程和系統(tǒng)觀測(cè)量方程，利用擴(kuò)展型卡爾曼濾波計(jì)算得到位置誤差、速度誤差和姿態(tài)誤差，來(lái)修正慣性導(dǎo)航系統(tǒng)中的位置、速度和姿態(tài)狀態(tài)量。