[發(fā)明專利]基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010193890.8
• 申請(qǐng)日: 2020-03-19
• 公開(kāi)（公告）號(hào): CN111351482B
• 公開(kāi)（公告）日: 2023-08-18
• 發(fā)明（設(shè)計(jì)）人: 張永;徐賢鵬;秦偉;劉自力;黃成;馬國(guó)梁
• 申請(qǐng)（專利權(quán)）人: 南京理工大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20;G01S19/45;G01S19/47
• 代理公司: 南京理工大學(xué)專利中心 32203
• 代理人: 薛云燕

1.一種基于誤差狀態(tài)卡爾曼濾波的多旋翼飛行器組合導(dǎo)航方法，其特征在于，包括以下步驟：

步驟1，建立傳感器的測(cè)量模型，推導(dǎo)四元數(shù)形式的導(dǎo)航方程，并對(duì)該方程進(jìn)行線性化處理得到誤差狀態(tài)方程；

步驟2，基于GPS、磁力計(jì)和加速度計(jì)的測(cè)量信息，建立用于卡爾曼濾波估計(jì)的測(cè)量方程；

步驟3，結(jié)合慣性-GPS松組合模式，采用誤差狀態(tài)卡爾曼濾波器ESKF對(duì)誤差狀態(tài)進(jìn)行最優(yōu)估計(jì)，并實(shí)現(xiàn)對(duì)導(dǎo)航狀態(tài)的校正；

步驟1所述建立傳感器的測(cè)量模型，具體如下：

基于慣性+GPS組合導(dǎo)航的信息測(cè)量元件包括陀螺儀、加速度計(jì)、磁力計(jì)和GPS，做出以下4條假設(shè)：

(1)陀螺儀和加速度計(jì)測(cè)量信息的零位偏差為固定偏差；

(2)陀螺儀和加速度計(jì)的測(cè)量噪聲為高斯噪聲，滿足正態(tài)分布；

(3)磁力計(jì)不存在零位偏差，測(cè)量噪聲為高斯噪聲，滿足正態(tài)分布；

(4)GPS不存在零位偏差，測(cè)量噪聲為高斯噪聲，滿足正態(tài)分布；

基于以上假設(shè)，陀螺儀的測(cè)量模型為：

其中，ω_m角為速度測(cè)量值，為機(jī)體系相對(duì)于慣性系的角速度在機(jī)體系下的坐標(biāo)表示，b_g為陀螺儀的零偏；n_g為高斯噪聲，各個(gè)方向不相關(guān)，滿足正態(tài)分布：

其中，E(n_g)＝0為高斯噪聲的均值，為高斯噪聲的協(xié)方差矩陣；σ_g是高斯噪聲的標(biāo)準(zhǔn)差，I是3階單位陣；

加速度計(jì)的測(cè)量模型為：

其中，a_m是加速度的測(cè)量值，a_i是慣性加速度，g_i是重力加速度在慣性系下的坐標(biāo)表示，是慣性系到機(jī)體系的旋轉(zhuǎn)矩陣，b_a為加速度計(jì)的零偏；n_a表示高斯白噪聲，各個(gè)方向不相關(guān)，滿足正態(tài)分布：

其中，E(n_a)＝0為高斯白噪聲的均值，為高斯白噪聲的協(xié)方差矩陣；σ_a為角速度計(jì)噪聲的標(biāo)準(zhǔn)差；

磁力計(jì)的測(cè)量模型為：

其中，M_m是磁場(chǎng)強(qiáng)度的測(cè)量值，是導(dǎo)航系到機(jī)體系的旋轉(zhuǎn)矩陣，M_n是磁場(chǎng)強(qiáng)度在導(dǎo)航系下的坐標(biāo)表示；n_M是磁力計(jì)的測(cè)量噪聲，滿足正態(tài)分布：

其中，σ_m磁力計(jì)噪聲的標(biāo)準(zhǔn)差；

步驟1所述推導(dǎo)四元數(shù)形式的導(dǎo)航方程，并對(duì)該方程進(jìn)行線性化處理得到誤差狀態(tài)方程，具體如下：

基于誤差狀態(tài)卡爾曼濾波器ESKF的工作原理，將真實(shí)狀態(tài)向量x視為標(biāo)稱狀態(tài)向量x_nom和誤差狀態(tài)向量δx的組合，其中誤差狀態(tài)表示標(biāo)稱狀態(tài)與真實(shí)狀態(tài)之間的差，產(chǎn)生以下關(guān)系：

其中，⊕表示除四元數(shù)狀態(tài)之外的所有屬于空間狀態(tài)變量的典型加法運(yùn)算即標(biāo)量和矢量的加減運(yùn)算；

四元數(shù)誤差狀態(tài)在機(jī)體系中的定義為：

其中q是真實(shí)四元數(shù)，q_nom是標(biāo)稱四元數(shù)，δq是四元數(shù)偏差，為四元數(shù)運(yùn)算：

所述用于導(dǎo)航信息估計(jì)的ESKF狀態(tài)由包含19個(gè)元素的真實(shí)狀態(tài)向量x，包含19個(gè)元素的標(biāo)稱狀態(tài)向量x_nom和包含18個(gè)元素的誤差狀態(tài)向量δx組成，表示如下：

其中，r＝[x_n,y_n,z_n]^T是多旋翼在導(dǎo)航系下的位置，v＝[v_xn,v_yn,v_zn]^T是多旋翼在導(dǎo)航系下的速度；q＝[q₀?q₁?q₂?q₃]^T為四元數(shù)，表示多旋翼的姿態(tài)；b_g＝[b_ωx,b_ωy,b_ωz]^T是三軸陀螺儀的固定零偏，b_a＝[b_ax,b_ay,b_az]^T是三軸加速度計(jì)的固定零偏，g＝[0,0,g]^T是重力加速度在導(dǎo)航系下的表示；

角速度ω和旋轉(zhuǎn)偏差矢量δθ是在機(jī)體坐標(biāo)系下的表示，是機(jī)體系到導(dǎo)航系的旋轉(zhuǎn)矩陣；

所述標(biāo)稱狀態(tài)運(yùn)動(dòng)學(xué)方程為：

其中，k表示t_k時(shí)刻，k+1表示t_k+1時(shí)刻，x_k＝x(t_k),t_k＝kΔt，Δt＝t_k+1-t_k；

所述誤差狀態(tài)運(yùn)動(dòng)學(xué)方程為：

其中，δg表示重力加速度誤差；v_i,θ_i,b_ai,b_gi是速度噪聲、角度噪聲、加速度偏差噪聲和陀螺儀偏差噪聲的離散化能量；v_i,θ_i,b_ai,b_gi的均值是0，協(xié)方差矩陣通過(guò)對(duì)n_a,n_g,b_a,b_g的方差在間隔時(shí)間Δt內(nèi)進(jìn)行積分獲得：

其中，σ_a,σ_g根據(jù)IMU數(shù)據(jù)表中的信息確定，σ_a單位為m/s²，σ_g單位為rad/s；σ_wa是加速度偏差噪聲的標(biāo)準(zhǔn)差，σ_wg是陀螺儀偏差噪聲的標(biāo)準(zhǔn)差，V_i是速度噪聲的協(xié)方差矩陣、Θ_i是角度噪聲的協(xié)方差矩陣、A_i是加速度偏差噪聲的協(xié)方差矩陣、Ω_i是陀螺儀偏差噪聲的協(xié)方差矩陣；

所述擾動(dòng)向量w_i＝[v_i,θ_i,a_i,ω_i]^T，那么誤差狀態(tài)方程為：

其中，F(xiàn)_x是誤差狀態(tài)轉(zhuǎn)移矩陣，F(xiàn)_i是擾動(dòng)向量w_i的雅可比矩陣，F(xiàn)_x和F_i表達(dá)式如下式所示：

誤差狀態(tài)卡爾曼濾波器的誤差狀態(tài)的更新方程為：

誤差狀態(tài)卡爾曼濾波器的協(xié)方差P_k的更新方程為：

其中，Q_i是系統(tǒng)噪聲的協(xié)方差矩陣，表達(dá)式如下式所示：