共軛梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法
【專利摘要】本發(fā)明涉及共軛梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，首先采集傳感器信息獲取四旋翼飛行狀態(tài)，然后利用共軛梯度方法進(jìn)行觀測(cè)模型建模，進(jìn)而進(jìn)行過程模型建模，將共軛梯度方法得到的四元數(shù)作為擴(kuò)展卡爾曼濾波的測(cè)量值；最后利用擴(kuò)展卡爾曼濾波法得到最佳的四元數(shù)，解算出四旋翼的三個(gè)姿態(tài)角。相比單純的梯度方法和互補(bǔ)濾波方法，本發(fā)明的擴(kuò)展卡爾曼濾波法考慮到了系統(tǒng)誤差和傳感器的測(cè)量噪聲，因此估計(jì)的四元數(shù)有更好的準(zhǔn)確性。利用共軛梯度方法得到觀測(cè)四元數(shù)運(yùn)用到擴(kuò)展卡爾曼濾波中，可以避免計(jì)算復(fù)雜的線性化觀測(cè)模型。
【專利說明】
共軛梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種四旋翼無人機(jī)姿態(tài)航向數(shù)據(jù)測(cè)量方法，特別是涉及一種共輒梯度 與擴(kuò)展卡爾曼結(jié)合的對(duì)四旋翼無人機(jī)（以下簡(jiǎn)稱四旋翼）姿態(tài)解算方法，主要用于軍事偵 察，農(nóng)業(yè)植保，電力巡檢，測(cè)繪，航拍等。
【背景技術(shù)】
[0002] 隨著機(jī)器人和航天航空技術(shù)的不斷發(fā)展，四旋翼逐漸成為了國(guó)內(nèi)外學(xué)者和航拍發(fā) 燒友們的研究熱點(diǎn)。四旋翼是一種具有垂直起降與空中懸停等特殊飛行能力的多旋翼無人 飛行器。在飛行過程中，通過改變四個(gè)旋翼的轉(zhuǎn)速，可以改變各種飛行姿態(tài)。正因?yàn)樗男?無人機(jī)具有垂直起降、空中定點(diǎn)懸停、可以實(shí)現(xiàn)六自由度的飛行等獨(dú)特的飛行能力，使其在 軍事領(lǐng)域和民用領(lǐng)域得到了廣泛的應(yīng)用。
[0003] 姿態(tài)解算是四旋翼的核心部分，其擔(dān)負(fù)著為四旋翼實(shí)時(shí)提供可靠的飛行狀態(tài)數(shù)據(jù) 的重任。目前運(yùn)用比較廣泛的姿態(tài)解算方法主要是互補(bǔ)濾波、卡爾曼濾波法和梯度下降法 的方法?；パa(bǔ)濾波方法和梯度下降方法計(jì)算簡(jiǎn)單，利用了加速度傳感器和地磁傳感器去補(bǔ) 償由于陀螺儀傳感器隨時(shí)間產(chǎn)生漂移造成的誤差，但是都沒有考慮系統(tǒng)本身的噪聲和傳感 器的測(cè)量誤差?；パa(bǔ)濾波方法需要選取動(dòng)態(tài)的權(quán)值去適應(yīng)不同的飛行狀態(tài)，大部分運(yùn)用者 選擇給一個(gè)比較大的權(quán)值去適應(yīng)快速飛行，這將會(huì)導(dǎo)致飛行器靜態(tài)性能欠佳?？柭鼮V波 方法中考慮了系統(tǒng)的測(cè)量誤差和傳感器的測(cè)量噪聲，相比前兩種方法有更好的準(zhǔn)確性，且 是運(yùn)用最廣泛的姿態(tài)解算方法。一般的卡爾曼方法中需要線性化觀測(cè)模型，而線性化觀測(cè) 模型需要面臨求解雅各比矩陣的問題，這將會(huì)帶來很大的計(jì)算量。

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

[0004] 本發(fā)明所要解決的技術(shù)問題是，針對(duì)四旋翼姿態(tài)解算的問題，提供一種能夠?qū)崟r(shí) 獲取姿態(tài)且精度較高的姿態(tài)解算方法，從而更好的控制四旋翼的飛行狀態(tài)。
[0005] 為解決上述技術(shù)問題，本發(fā)明采用的技術(shù)方案是：
[0006] -種共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，所述四旋翼包含單 片機(jī)以及與單片機(jī)相連的陀螺儀、加速度傳感器、地磁傳感器，單片機(jī)與地面系統(tǒng)上位機(jī)通 訊連接;其特征在于包括如下步驟：
[0007] 步驟SI:采集傳感器信息:在機(jī)體坐標(biāo)系下，通過陀螺儀、加速度傳感器、地磁傳感 器采集四旋翼飛行狀態(tài)信息，其中，陀螺儀測(cè)量四旋翼三軸角速度，加速度傳感器測(cè)量四旋 翼加速度在三軸上的投影，地磁傳感器測(cè)量四旋翼所在位置的三軸磁感應(yīng)強(qiáng)度;所述機(jī)體 坐標(biāo)系是固連于四旋翼的參考坐標(biāo)系；
[0008] 步驟S2:分別對(duì)陀螺儀、加速度計(jì)做去零飄處理：
[0009] 步驟S3:采用巴特沃斯低通濾波器濾除機(jī)體加速度對(duì)重力加速度在三軸上的投影 的影響;對(duì)三軸磁感應(yīng)強(qiáng)度做歸零歸一處理；
[0010] 步驟S4:利用共輒梯度方法建立對(duì)擴(kuò)展卡爾曼濾波觀測(cè)模型；
[0011] 步驟S5:建立擴(kuò)展卡爾曼濾波器的過程模型:將共輒梯度法求出的使誤差函數(shù)e(q) 最小的四元數(shù)作為測(cè)量值，設(shè)計(jì)擴(kuò)展卡爾曼濾波器，利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四 元數(shù)；
[0012] 步驟S6:聯(lián)立歐拉方向余弦矩陣和四元數(shù)矩陣，得到三個(gè)利用四元數(shù)形成的姿態(tài) 角方程;將步驟S5估算出的最優(yōu)四元數(shù)帶入三個(gè)姿態(tài)角方程解算出四旋翼的三個(gè)姿態(tài)角： 航向角γ，俯仰角Θ，橫滾角％,
[0013] 上述技術(shù)方案中，步驟S2對(duì)陀螺儀、加速度計(jì)做去零飄處理的具體做法是將四旋 翼保持水平，取200次值取平均值，這個(gè)平均值就為零飄值，以后取得的值都減去該零飄值。
[0014] 上述技術(shù)方案中，步驟S3對(duì)地磁傳感器測(cè)得的三軸磁感應(yīng)強(qiáng)度做歸零歸一處理的 具體過程如下：
[0015] 首先，將安裝有地磁傳感器的四旋翼水平放置，然后不斷轉(zhuǎn)動(dòng)四旋翼讓地磁傳感 器旋轉(zhuǎn)出一個(gè)圓球，同時(shí)旋轉(zhuǎn)過程中下位機(jī)不斷的將數(shù)據(jù)發(fā)送給上位機(jī);進(jìn)而，上位機(jī)對(duì)接 收到的數(shù)據(jù)進(jìn)行處理，分別找出X軸、y軸和Z軸各單個(gè)軸的最大值和最小值，最后進(jìn)行歸零 歸一計(jì)算：歸零，就是要使單個(gè)軸的最大值和最小值的絕對(duì)值相等，或者說在坐標(biāo)軸上對(duì) 稱;歸一，就是使兩個(gè)軸的數(shù)據(jù)范圍趨于一致;計(jì)算公式如下：
[0016] ⑴
[0017] (2)
[0018] (3.)
[0019] 式中，X軸為長(zhǎng)軸，y軸為短軸;Hxi、Hyi、Hzi為X軸、y軸和z軸各單個(gè)軸經(jīng)過歸零歸一 處理后的標(biāo)定值；Hsxi、Hsyi、Hszi分別表示在X軸、y軸、z軸采集到的三軸磁感應(yīng)強(qiáng)度； HsXmax、HsXmin、Hsymax、Hsymin、HsZmax、HsZmin分別表示在X軸、y軸、Z軸采集到的三軸磁感應(yīng)強(qiáng) 度的最大值和最小值。
[0020] 上述技術(shù)方案中，步驟S4用共輒梯度方法建立對(duì)擴(kuò)展卡爾曼濾波觀測(cè)模型過程如 下:將理想狀態(tài)均勻重力場(chǎng)和地磁場(chǎng)作為參考方向，通過導(dǎo)航坐標(biāo)系下四元數(shù)轉(zhuǎn)換到機(jī)體 坐標(biāo)系下得到三軸加速度[a'b x a'by a'bz]T和磁感應(yīng)強(qiáng)度[m'bx m'by m'bz]T;將轉(zhuǎn)換得到的 三軸加速度減去機(jī)體坐標(biāo)系下測(cè)得的重力加速度在三軸上的投影，且將轉(zhuǎn)換得到的磁感應(yīng) 強(qiáng)度減去機(jī)體坐標(biāo)系下測(cè)得的三軸磁感應(yīng)強(qiáng)度，得到測(cè)量誤差函數(shù)e( q)，利用共輒梯度法求 出使測(cè)量誤差函數(shù)e(q)最小的測(cè)量四元數(shù)，即利用共輒梯度方法建立擴(kuò)展卡爾曼濾波觀測(cè) 模型。
[0021] 上述技術(shù)方案中，步驟S5建立對(duì)擴(kuò)展卡爾曼濾波的過程模型的具體過程如下：
[0022] 首先由四元數(shù)的微分值彳與角速度w的關(guān)系可以得到擴(kuò)展卡爾曼濾波的狀態(tài)等式：
[0023] (17)
[0024] (18)
[0025] 〇[￥]為狀態(tài)轉(zhuǎn)移矩陣，1、^、￥2分別表示陀螺儀傳感器測(cè)量的三軸角速度;#表示 四元數(shù)的微分。將連續(xù)時(shí)間模型轉(zhuǎn)化為離散時(shí)間模型，令系統(tǒng)采樣時(shí)間為Ts，離散時(shí)間模型 為：
[0026] (19)
[0027]式中q(k)表示第k次采樣，對(duì)exp( QkTs)進(jìn)行泰勒級(jí)數(shù)展開保留一階和二階項(xiàng)，可得 公式(20)·
[0028]
(20)
[0029] 式中：
[0030]
(21)
[0031] 13*3表示三維單位向量，wx、w y、Wz分別表示陀螺儀傳感器測(cè)量的三軸角速度。
[0032] 上述技術(shù)方案中，步驟S5利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四元數(shù)包括如下步 驟:首先利用步驟S5的過程模型建模估算出下一個(gè)時(shí)刻的四元數(shù)，然后進(jìn)行誤差協(xié)方差更 新，第三步利用更新的協(xié)方差計(jì)算出卡爾曼增益，第四步利用步驟S4中觀測(cè)模型求解出的 測(cè)量四元數(shù)減去第一步估計(jì)出來的下一個(gè)時(shí)刻的四元數(shù)，乘上第三步求出的卡爾曼增益， 加上上一個(gè)時(shí)刻的四元數(shù)就是擴(kuò)展卡爾曼濾波器求出的最優(yōu)四元數(shù);之后更新下一時(shí)刻協(xié) 方差為下一次計(jì)算做準(zhǔn)備。
[0033] 本發(fā)明的擴(kuò)展卡爾曼濾波法考慮到了系統(tǒng)誤差和傳感器的測(cè)量噪聲，因此估計(jì)的 四元數(shù)有更好的準(zhǔn)確性。利用共輒梯度方法得到觀測(cè)四元數(shù)運(yùn)用到擴(kuò)展卡爾曼濾波中，可 以避免計(jì)算復(fù)雜的線性化觀測(cè)模型。
[0034] 本發(fā)明選擇的共輒梯度與擴(kuò)展卡爾曼結(jié)合的姿態(tài)解算方法，利用共輒梯度方法求 出觀測(cè)四元數(shù)運(yùn)用于擴(kuò)展卡爾曼濾波中，不需要考慮線性化觀測(cè)模型的問題。共輒梯度法 相比梯度下降方法，下降的步長(zhǎng)為實(shí)時(shí)修正的動(dòng)態(tài)值，精準(zhǔn)度更高，更容易跟蹤各種飛行狀 ??τ O
[0035] 除此之外，本發(fā)明運(yùn)用了目前最流行的四元數(shù)的方法去表示機(jī)體的轉(zhuǎn)動(dòng)，四元數(shù) 法運(yùn)用在R4空間中，相比歐拉角表示機(jī)體轉(zhuǎn)動(dòng)的方法，四元數(shù)的方法更容易表示機(jī)體任何 的轉(zhuǎn)動(dòng)，且不會(huì)出現(xiàn)奇異性問題。
[0036] 綜上所述，本發(fā)明的共輒梯度法與擴(kuò)展卡爾曼結(jié)合的四旋翼姿態(tài)解算方法，主要 優(yōu)點(diǎn)有三個(gè):其一利用多傳感器數(shù)據(jù)融合，對(duì)低成本傳感器飄移問題進(jìn)行修正，提高測(cè)量精 度。其二利用了共輒梯度方法建立擴(kuò)展卡爾曼濾波的觀測(cè)模型，共輒梯度方法是一階迭代 方法相比其它需要線性化觀測(cè)模型，求解復(fù)雜的雅各比矩陣的方法計(jì)算量大大減小。讓方 法的實(shí)時(shí)性有了很好的保證，為更好的控制四旋翼飛行器的姿態(tài)做好準(zhǔn)備。其三運(yùn)用擴(kuò)展 卡爾曼方法考慮了系統(tǒng)本身噪聲和傳感器的測(cè)量誤差，提高測(cè)量精度，利用了共輒梯度的 方法能夠動(dòng)態(tài)的調(diào)整下降方向和步長(zhǎng)可以很好的適應(yīng)不同的飛行狀態(tài)。
【附圖說明】
[0037] 圖1為本發(fā)明涉及的導(dǎo)航坐標(biāo)系與機(jī)體坐標(biāo)系關(guān)系圖；
[0038]圖2為本發(fā)明涉及的導(dǎo)航坐標(biāo)系經(jīng)過三次轉(zhuǎn)動(dòng)到機(jī)體坐標(biāo)系圖；
[0039] 圖3為本發(fā)明共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法方框圖；
[0040] 圖4為本發(fā)明共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法測(cè)量流程 圖；
[0041] 圖5為本發(fā)明共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法測(cè)量結(jié)果圖 (滾轉(zhuǎn)角隨時(shí)間變化曲線）。
[0042] 圖6為本發(fā)明共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法測(cè)量結(jié)果圖 (俯仰角隨時(shí)間變化曲線）。
【具體實(shí)施方式】
[0043] 下面對(duì)照附圖1-6和實(shí)施例詳細(xì)說明本發(fā)明，本發(fā)明并不僅僅限于所給附圖和實(shí) 施例。
[0044] 根據(jù)本發(fā)明實(shí)施的共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法總體 流程如圖3和4。其特征在于包括如下步驟：
[0045] 步驟Sl:采集傳感器信息:在機(jī)體坐標(biāo)系下，通過陀螺儀、加速度傳感器、地磁傳感 器采集四旋翼狀態(tài)信息，其中，陀螺儀測(cè)量四旋翼三軸角速度，加速度傳感器測(cè)量四旋翼加 速度在三軸上的投影，地磁傳感器測(cè)量四旋翼所在位置的三軸磁感應(yīng)強(qiáng)度;機(jī)體坐標(biāo)系是 固連于四旋翼的參考坐標(biāo)系；
[0046] 步驟S2:分別對(duì)陀螺儀、加速度計(jì)做去零飄處理：
[0047]步驟S3:采用巴特沃斯低通濾波器濾除機(jī)體加速度對(duì)重力加速度在三軸上的投影 的影響;對(duì)三軸磁感應(yīng)強(qiáng)度做歸零歸一處理；
[0048]加速度傳感器又叫重力加速度計(jì)，它可以測(cè)量飛行器重力在三軸上的投影;但是 實(shí)際測(cè)量的重力加速度中包含有機(jī)體加速度，由于機(jī)體加速度是高頻噪聲，所以采用巴特 沃斯低通濾波器濾除機(jī)體加速度的影響;經(jīng)過實(shí)驗(yàn)選擇截止頻率為5HZ的巴特沃斯低通濾 波器；由于地磁傳感器本身的特性不是完全對(duì)稱的，所以需要采用歸零歸一的方法對(duì)地磁 傳感器測(cè)得的三軸磁感應(yīng)強(qiáng)度進(jìn)行處理；
[0049]步驟S4:將理想狀態(tài)均勻重力場(chǎng)和地磁場(chǎng)作為參考方向，通過導(dǎo)航坐標(biāo)系下四元 數(shù)轉(zhuǎn)換到機(jī)體坐標(biāo)系下得到三軸加速度[a'bx a'by a'bz]T和磁感應(yīng)強(qiáng)度[m'bx m'by m'bz]T; 將轉(zhuǎn)換得到的三軸加速度減去機(jī)體坐標(biāo)系下測(cè)得的重力加速度在三軸上的投影，且將轉(zhuǎn)換 得到的磁感應(yīng)強(qiáng)度減去機(jī)體坐標(biāo)系下測(cè)得的三軸磁感應(yīng)強(qiáng)度，得到測(cè)量誤差函數(shù)e( q)，利用 共輒梯度法求出使測(cè)量誤差函數(shù)最小的四元數(shù)，即利用共輒梯度方法建立擴(kuò)展卡爾曼 濾波觀測(cè)模型；
[0050] 步驟S5:建立擴(kuò)展卡爾曼濾波器的過程模型:將共輒梯度法求出的使誤差函數(shù)e(q) 最小的四元數(shù)作為測(cè)量值，設(shè)計(jì)擴(kuò)展卡爾曼濾波器，利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四 元數(shù)；
[0051] 步驟S6:聯(lián)立歐拉方向余弦矩陣和四元數(shù)矩陣，得到三個(gè)利用四元數(shù)形成的姿態(tài) 角方程;將步驟S5估算出的最優(yōu)四元數(shù)帶入三個(gè)姿態(tài)角方程求出三個(gè)姿態(tài)角:航向角γ，俯 仰角Θ，橫滾角
[0052]上述技術(shù)方案中，步驟S2對(duì)陀螺儀、加速度計(jì)做去零飄處理的具體做法是將四旋 翼無人機(jī)保持水平，取200次值取平均值，這個(gè)平均值就為零飄值，以后取得的值都需要減 去這個(gè)零飄值。
[0053]上述技術(shù)方案中，步驟S3對(duì)地磁傳感器測(cè)得的三軸磁感應(yīng)強(qiáng)度做歸零歸一處理的 具體過程如下：
[0054] 首先，將安裝有地磁傳感器的四旋翼水平放置，然后不斷轉(zhuǎn)動(dòng)四旋翼讓地磁傳感 器旋轉(zhuǎn)出一個(gè)圓球，同時(shí)旋轉(zhuǎn)過程中下位機(jī)(下位機(jī)是四旋翼上的單片機(jī)，三大傳感器集成 在單片機(jī)中，下位機(jī)與電腦上位機(jī)通訊連接)不斷的將數(shù)據(jù)發(fā)送給上位機(jī);進(jìn)而，上位機(jī)對(duì) 接收到的數(shù)據(jù)進(jìn)行處理，分別找出X軸、y軸和Z軸各單個(gè)軸的最大值和最小值，最后進(jìn)行歸 零歸一計(jì)算:歸零，就是要使單個(gè)軸的最大值和最小值的絕對(duì)值相等，或者說在坐標(biāo)軸上對(duì) 稱；歸一，就是伸兩個(gè)軸的數(shù)據(jù)范闈趨干一敔：計(jì)筧公式如下：
[0055] (1)
[0056] (2>
[0057] (3.)
[0058] 式中，X軸為長(zhǎng)軸，y軸為短軸;Hxi、Hyi、Hzi為X軸、y軸和z軸各單個(gè)軸經(jīng)過歸零歸一 處理后的標(biāo)定值；Hsxi、Hsyi、Hszi分別表示在X軸、y軸、z軸采集到的三軸磁感應(yīng)強(qiáng)度； HsXmax、HsXmin、Hsymax、Hsymin、HsZmax、HsZmin分別表示在X軸、y軸、Z軸采集到的三軸磁感應(yīng)強(qiáng) 度的最大值和最小值。
[0059] 上述技術(shù)方案中，步驟S4利用共輒梯度方法建立對(duì)擴(kuò)展卡爾曼濾波觀測(cè)模型的具 體步驟如下：
[0060] 首先在理想情況下，認(rèn)為重力的參考量為fg=[0 0 g]T，地磁場(chǎng)的參考量為H= [Hn 0 Hd]τ;其中g(shù)表不重力加速度，Hn和Hd分別是地磁場(chǎng)在北東地坐標(biāo)系中北向分量和垂直分 量;單位化可：
[0061] 如圖1所示定義導(dǎo)航坐標(biāo)系(η系)和機(jī)體坐標(biāo)系(b系）。導(dǎo)航系統(tǒng)中，機(jī)體坐標(biāo)系相 對(duì)導(dǎo)航坐標(biāo)系的轉(zhuǎn)動(dòng)可以用轉(zhuǎn)動(dòng)四元數(shù)表示，假設(shè)起始狀態(tài)Ve = XEi+yE j+ZEk，式中Xe、yE、Ze 分別表示待轉(zhuǎn)動(dòng)的量在三軸上的投影;經(jīng)過一次四旋翼的轉(zhuǎn)動(dòng)q到達(dá)終止?fàn)顟B(tài)Vb = x'i+y'j +z'k，（這里是用四元數(shù)的方式來表示四旋翼的一次旋轉(zhuǎn)q，然后利用四元數(shù)微分方程更新 新的四元數(shù)，下面提到的耶^:^^陽(yáng)都是經(jīng)過四元數(shù)微分方程更新后的四元數(shù)：由于四旋 翼無人機(jī)的是處于一個(gè)不斷運(yùn)動(dòng)的狀態(tài)，所以每個(gè)周期q都是需要通過四元數(shù)的微分方程 來更新的，因而q也表示四元數(shù)微分方程更新后的四元數(shù)。啟動(dòng)前給q-個(gè)初始值[1 0 0 0]，后面每個(gè)周期更新一次通過微分方程來更新。）其中：q = qQ+qii+q2j+q3k，qo、qi、q2、q3表 示四元數(shù)的分量，i、j、k表示三個(gè)矢量；，其中X'、y'、z'表示轉(zhuǎn)動(dòng)后的量分別在三軸上的投 影，根據(jù)四元數(shù)轉(zhuǎn)動(dòng)的映像方式為：
[0062]
c4)
[0063] 式中cf為q的共輒四元數(shù)即標(biāo)量相同矢量相反，表示為q* = q〇-qii-q2j-q3k。將上式 展開，可得如式(5):
[0064]
(5)
[0066]
[0065] 根據(jù)公式(5)，將理想情況下，重力的參考量和地磁場(chǎng)的參考量轉(zhuǎn)換到機(jī)體坐標(biāo)系 中，如式(6)和(7):
[0067]
[0068] 式(6)和式（7)中么、#分別表示單位化后的理想情況下重力場(chǎng)和磁感應(yīng)強(qiáng)度在 三軸上的分量，4、氧^分別表示單位化后北東地坐標(biāo)系中北向分量和垂直分量，q表示四 元數(shù)微分方程更新后的四元數(shù)，其中則、9 1、屯43表示四元數(shù)的分量彳為9的共輒四元數(shù)。將 式(6)和式(7)所得數(shù)據(jù)與加速度傳感器和地磁傳感器在機(jī)體坐標(biāo)系測(cè)得的加速度和磁感 應(yīng)強(qiáng)度相減，可得測(cè)量誤差函數(shù)e( q):
[0069] Θ(q) - [a bx-Sbx ? by-Sby ? bz-Sbz ΠΙ bx-IIlbx ΠΙ by-IIlby ΠΙ bz-IIlbz ] ( 8 )
[0070] 式(8)中，36\1丫、31)2、11?^、11?^、11?)2表示加速度傳感器和地磁傳感器在機(jī)體坐標(biāo)系下 測(cè)得的加速度和磁感應(yīng)強(qiáng)度^'^^^^'^^'^^^^'^是式⑷~⑴計(jì)算出的理想情況 下機(jī)體的加速度和磁感應(yīng)強(qiáng)度。則對(duì)四元數(shù)的優(yōu)化問題可轉(zhuǎn)換為求目標(biāo)函數(shù).Z w=Aa9,最 小值的問題。
[0071 ]根據(jù)共輒梯度法的計(jì)算公式：
[0072] Z(k+i) = Z(k)+X(k+i)d(k+i) (9)
[0073]式中，A(k+1)為最優(yōu)步長(zhǎng)，d(k+i)為下降的方向，Ζ〇〇表示上一時(shí)刻四元數(shù)，Z(k+i)表示 下一時(shí)刻估算出來的四元數(shù)，k表示第k次迭代。
[0074]根據(jù)FR共輒梯度法：
[0075]
IO^
[0076] 式中W為一中間變量，JTSe(q)的雅格比矩陣，具體展開后的雅格比矩陣如式 (11)：
[0077]
[0078] 式中Hn和//y分別表示單位化后北東地坐標(biāo)系中北方向的北向分量和垂直方向的 垂直分量，η表示北方向，d表示垂直方向；其中9〇、91、92、93表示四元數(shù)的分量。
[0079] 根據(jù)FR共輒梯度法，利用下面公式選擇步長(zhǎng)，以及下降方向：
[0080] di = -gi (12)
[0081] dk+i = _gk+i+&+idk (13)
[0082] Ek^l=Vfkn (14)
[0083] (15)
[0084] 16)
[0085] 上面5個(gè)公式為共輒梯度法選擇合適步長(zhǎng)以及最快下降方向的核心公式，式中 gk+1，&+1為兩個(gè)中間變量，Wa+1為公式（10)經(jīng)過k+Ι次運(yùn)算出的結(jié)果，d k+1表示第k+Ι次下降 的步長(zhǎng)，Ak+1為第k+Ι次下降的方向，Cl1為第一次下降的方向， gl為中間變量gk+i的初值；
[0086] 至此為步驟S4利用共輒梯度法求解誤差函數(shù)的最小值，也就是對(duì)擴(kuò)展卡爾曼濾波 的觀測(cè)模型建模，從而得到的一組姿態(tài)四元數(shù)，這組四元數(shù)是基于加速度傳感器和地磁傳 感器表征出來的四元數(shù)，作為擴(kuò)展卡爾曼濾波的測(cè)量值。
[0087] 步驟S5建立對(duì)擴(kuò)展卡爾曼濾波的過程模型的具體過程如下：
[0088] 首先由四元數(shù)的微分值4與角速度w的關(guān)系可以得到卡爾曼濾波的狀態(tài)等式：
[0089] (17)
[0090] (18；
[0091] 〇[￥]為狀態(tài)轉(zhuǎn)移矩陣，1、％、￥2分別表示陀螺儀傳感器測(cè)量的三軸角速度;纟表示 四元數(shù)的微分。將連續(xù)時(shí)間模型轉(zhuǎn)化為離散時(shí)間模型，令系統(tǒng)采樣時(shí)間為T s，離散時(shí)間模型
為：
[0092]
[0093]式中q(k)表示第k次采樣，對(duì)exp( QkTs)進(jìn)行泰勒級(jí)數(shù)展開保留一階和二階項(xiàng)，可得 公式(20):
[0094] (20)
[0095]
[0096]
[0097] 13*3表示三維單位向量，wx、w y、Wz分別表示陀螺儀傳感器測(cè)量的三軸角速度，至此 完成對(duì)擴(kuò)展卡爾曼濾波的過程模型建模，對(duì)過程模型的建模主要用于擴(kuò)展卡爾曼濾波的第 一步，也就是估計(jì)下一個(gè)時(shí)刻的四元數(shù)。之后就是需要利用擴(kuò)展卡爾曼濾波器估算出最優(yōu) 四元數(shù):首先利用步驟S5的過程模型建模估算出下一個(gè)時(shí)刻的四元數(shù)也就是公式(22)，然 后進(jìn)行第二步利用公式(23)進(jìn)行誤差協(xié)方差更新，第三步利用公式(24)更新的協(xié)方差計(jì)算 出卡爾曼增益，第四步利用步驟S4的觀測(cè)模型求解出的測(cè)量四元數(shù)減去第一步估計(jì)出來的 下一個(gè)時(shí)刻的四元數(shù)，乘上第三步求出的卡爾曼增益，加上上一個(gè)時(shí)刻的四元數(shù)就是擴(kuò)展 卡爾曼濾波器求出的最優(yōu)四元數(shù)，第五步利用公式(26)更新下一時(shí)刻協(xié)方差為下一次計(jì)算 做準(zhǔn)備。擴(kuò)展卡爾曼濾波器就是這樣不斷的把協(xié)方差遞歸，從而能求出最優(yōu)四元數(shù)。下面給 出利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四元數(shù)的五個(gè)具體步驟：
[0098] 第一步:根據(jù)陀螺儀測(cè)量的k時(shí)刻角速度更新四旋翼k+Ι時(shí)刻的狀態(tài)四元數(shù)，如下：
[0099] ?_(?+η =-4(?,?(?-> (22)
[0100] 式(22)對(duì)應(yīng)公式(20)，％:)表示四元數(shù)，Ak表示exp (QkTs)泰勒級(jí)數(shù)展開后的一階 和二階項(xiàng)義+υ表示預(yù)測(cè)的k+Ι時(shí)刻四元數(shù)。
[0101] 第二步：四旋翼狀態(tài)向前推算誤差協(xié)方差更新，如下：
[0102] P-(k+1)=A(k)P(k)AT(k)+Q(k+l) (23)
[0103] 式中，P(k)表不k時(shí)刻的協(xié)方差矩陣，Q(k+i)表不四維過程噪聲，<表不六〇〇的轉(zhuǎn)置矩 陣，表示預(yù)測(cè)的k+Ι時(shí)刻的協(xié)方差矩陣。
[0104] 第三步:濾波器增益系數(shù)的求解，如下：
[0105；
124)
[0106] 式中，Kk+i表示卡爾曼增益，R(k+i)表示四維的測(cè)量噪聲，H(k+i)表示單位矩陣， 表示預(yù)測(cè)的k+Ι時(shí)刻的協(xié)方差矩陣。
[0107] 笛四步：枏據(jù)共軛梯庠法得剞觀測(cè)更新方程，如下：
[0108]
(25)
[0109] 式中，4+1表示卡爾曼濾波器估計(jì)的最優(yōu)四元數(shù)，Zk+1為步驟S4中共輒梯度方法求 解出的四元數(shù)，％η表示預(yù)測(cè)的k+Ι時(shí)刻四元數(shù)，Η (1?+υ表示單位矩陣；
[0110]第五步:協(xié)方差更新，如下：
[0111]
(26)
[0112] 式中，I為四維單位矩陣，表示預(yù)測(cè)的k+i時(shí)刻的協(xié)方差矩陣，H(k+1)表示單位矩 陣。
[0113] 至此，利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四元數(shù)過程結(jié)束，接下來就是利用估算 出最優(yōu)四元數(shù)求解出歐拉角的過程。
[0114] 上述技術(shù)方案中，步驟S6利用估算出最優(yōu)四元數(shù)解算姿態(tài)角方程的具體過程如 下：
[0115] 定義四旋翼繞ζ軸轉(zhuǎn)過的角度P為航向角;繞y軸轉(zhuǎn)過的角度γ為橫滾角;繞X軸轉(zhuǎn) 過的角度Θ為俯仰角;假設(shè)四旋翼依次經(jīng)過下面三次轉(zhuǎn)動(dòng)從導(dǎo)航坐標(biāo)系運(yùn)動(dòng)到機(jī)體坐標(biāo)系： 先繞ζ軸轉(zhuǎn)過0角度;再繞y軸轉(zhuǎn)過Θ角度;最后繞X軸轉(zhuǎn)過γ角度；
[0116] 1)繞ζ軸轉(zhuǎn)動(dòng)^ (如圖2)
[0117]
[0118]
[0119]
[0120]
[0121]
[0122]這里把四旋翼的一次轉(zhuǎn)動(dòng)用歐拉角的方式分解為三次轉(zhuǎn)動(dòng)，式中x，y，z表示轉(zhuǎn)動(dòng) 前機(jī)體坐標(biāo)系的X，y，Z軸，經(jīng)過第一次繞Z軸轉(zhuǎn)動(dòng)機(jī)體坐標(biāo)系變?yōu)閄i，yi，Zi，經(jīng)過第二次繞yi 轉(zhuǎn)動(dòng)機(jī)體坐標(biāo)系變?yōu)閄2，y2，Z2，最后經(jīng)過第三次繞X2轉(zhuǎn)動(dòng)機(jī)體坐標(biāo)系變?yōu)?X3，y3，Z3。
[0123] S#立H術(shù)=汝鮮動(dòng)可得，
[0124]
(27)
[0125] 則可得歐枋方向會(huì)弦矩陣如下：
[0126]
(2.8)
[0127] 根據(jù)式(5)和(28)可得：
[0128] ?29)
[0129]
[0130] (3丨）
[0131] 圖5為本發(fā)明共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法測(cè)量結(jié)果圖 (橫滾角隨時(shí)間變化曲線）。
[0132] 圖6為本發(fā)明共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法測(cè)量結(jié)果圖 (俯仰角隨時(shí)間變化曲線）。
[0133] 圖5和6中，縱軸表示相應(yīng)的角度變化，橫軸是采樣次數(shù)（隨時(shí)間變化每隔4ms采樣 一次）。其中細(xì)實(shí)線部分為沒有進(jìn)行融合算法計(jì)算出來的姿態(tài)隨時(shí)間變化曲線，粗實(shí)線部分 為本發(fā)明的姿態(tài)解算方法，從圖形中可以明顯看出本發(fā)明的姿態(tài)解算方法靜態(tài)性能和動(dòng)態(tài) 性能良好，不會(huì)隨時(shí)間變化產(chǎn)生漂移誤差，曲線光滑沒有延時(shí)，能夠很好的跟蹤姿態(tài)的變 化。
【主權(quán)項(xiàng)】
1. 一種共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，所述四旋翼包含單片 機(jī)以及與單片機(jī)相連的陀螺儀、加速度傳感器、地磁傳感器，單片機(jī)與地面系統(tǒng)上位機(jī)通訊 連接;其特征在于包括如下步驟： 步驟SI:采集傳感器信息:在機(jī)體坐標(biāo)系下，通過陀螺儀、加速度傳感器、地磁傳感器采 集四旋翼飛行狀態(tài)信息，其中，陀螺儀測(cè)量四旋翼三軸角速度，加速度傳感器測(cè)量四旋翼加 速度在三軸上的投影，地磁傳感器測(cè)量四旋翼所在位置的三軸磁感應(yīng)強(qiáng)度;所述機(jī)體坐標(biāo) 系是固連于四旋翼的參考坐標(biāo)系； 步驟S2:分別對(duì)陀螺儀、加速度計(jì)做去零飄處理： 步驟S3:采用巴特沃斯低通濾波器濾除機(jī)體加速度對(duì)重力加速度在三軸上的投影的影 響;對(duì)三軸磁感應(yīng)強(qiáng)度做歸零歸一處理； 步驟S4:利用共輒梯度方法建立對(duì)擴(kuò)展卡爾曼濾波觀測(cè)模型； 步驟S5:建立擴(kuò)展卡爾曼濾波器的過程模型:將共輒梯度法求出的使誤差函數(shù)e(q)最小 的四元數(shù)作為測(cè)量值，設(shè)計(jì)擴(kuò)展卡爾曼濾波器，利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四元數(shù)； 步驟S6:聯(lián)立歐拉方向余弦矩陣和四元數(shù)矩陣，得到三個(gè)利用四元數(shù)形成的姿態(tài)角方 程;將步驟S5估算出的最優(yōu)四元數(shù)帶入三個(gè)姿態(tài)角方程解算出四旋翼的三個(gè)姿態(tài)角:航向 角γ，俯仰角Θ，橫滾角0。2. 根據(jù)權(quán)利要求1所述的共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，其 特征在于：步驟S2對(duì)陀螺儀、加速度計(jì)做去零飄處理的具體做法是將四旋翼保持水平，取 200次值取平均值，這個(gè)平均值就為零飄值，以后取得的值都減去該零飄值。3. 根據(jù)權(quán)利要求2所述的共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，其 特征在于:步驟S3對(duì)地磁傳感器測(cè)得的三軸磁感應(yīng)強(qiáng)度做歸零歸一處理的具體過程如下： 首先，將安裝有地磁傳感器的四旋翼水平放置，然后不斷轉(zhuǎn)動(dòng)四旋翼讓地磁傳感器旋 轉(zhuǎn)出一個(gè)圓球，同時(shí)旋轉(zhuǎn)過程中下位機(jī)不斷的將數(shù)據(jù)發(fā)送給上位機(jī);進(jìn)而，上位機(jī)對(duì)接收到 的數(shù)據(jù)進(jìn)行處理，分別找出X軸、y軸和ζ軸各單個(gè)軸的最大值和最小值，最后進(jìn)行歸零歸一 計(jì)算：歸零，就是要使單個(gè)軸的最大值和最小值的絕對(duì)值相等，或者說在坐標(biāo)軸上對(duì)稱；歸 一，就是使兩個(gè)軸的數(shù)據(jù)范圍趨于一致;計(jì)算公式如下：(1) ⑵： (3) 式中，X軸為長(zhǎng)軸，y軸為短軸;Hxi、Hyi、Hzi為X軸、y軸和z軸各單個(gè)軸經(jīng)過歸零歸一處理 后的標(biāo)定值;Hsxi、Hsyi、Hszi分別表示在X軸、y軸、z軸采集到的三軸磁感應(yīng)強(qiáng)度;Hsxmax、 HsXmin、Hsymax、Hsymin、HsZmax、HsZmin分別表示在X軸、y軸、Z軸采集到的三軸磁感應(yīng)強(qiáng)度的最 大值和最小值。4. 根據(jù)權(quán)利要求3所述的共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，其 特征在于:步驟S4用共輒梯度方法建立對(duì)擴(kuò)展卡爾曼濾波觀測(cè)模型過程如下:將理想狀態(tài) 均勻重力場(chǎng)和地磁場(chǎng)作為參考方向，通過導(dǎo)航坐標(biāo)系下四元數(shù)轉(zhuǎn)換到機(jī)體坐標(biāo)系下得到三 軸加速度[a'bx a'by a'bz]T和磁感應(yīng)強(qiáng)度[m'bx m'by m'bz]Tj#轉(zhuǎn)換得到的三軸加速度減去 機(jī)體坐標(biāo)系下測(cè)得的重力加速度在三軸上的投影，且將轉(zhuǎn)換得到的磁感應(yīng)強(qiáng)度減去機(jī)體坐 標(biāo)系下測(cè)得的三軸磁感應(yīng)強(qiáng)度，得到測(cè)量誤差函數(shù)e( q)，利用共輒梯度法求出使測(cè)量誤差函 數(shù)e(q)最小的測(cè)量四元數(shù)，即利用共輒梯度方法建立擴(kuò)展卡爾曼濾波觀測(cè)模型。5. 根據(jù)權(quán)利要求4所述的共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，其 特征在于:步驟S5建立對(duì)擴(kuò)展卡爾曼濾波的過程模型的具體過程如下： 首先由四元數(shù)的微分值纟與角速度w的關(guān)系可以得到擴(kuò)展卡爾曼濾波的狀態(tài)等式：(17) (18) Ω[?]為狀態(tài)轉(zhuǎn)移矩陣，Wx、Wy、Wz分別表示陀螺儀傳感器測(cè)量的三軸角速度表示四元 數(shù)的微分;將連續(xù)時(shí)間模型轉(zhuǎn)化為離散時(shí)間模型，令系統(tǒng)采樣時(shí)間為Ts，離散時(shí)間模型為： q(k+i) = exp( Ω kTs)*qk (19)式中q〇〇表示第k次采樣，對(duì)exp( QkTs)進(jìn)行泰勒級(jí)數(shù)展開保留一階和二階項(xiàng)，可得公式 (20)： (20) 式中： Δ Θ2= (wxTs)2+(wyTs)2+(wzTs)2 (21) 13*3表示三維單位向量，Wx、Wy、Wz分別表示陀螺儀傳感器測(cè)量的三軸角速度。6. 根據(jù)權(quán)利要求5所述的共輒梯度與擴(kuò)展卡爾曼濾波結(jié)合的四旋翼姿態(tài)解算方法，其 特征在于:步驟S5利用擴(kuò)展卡爾曼濾波器估算出最優(yōu)四元數(shù)包括如下步驟:首先利用步驟 S5的過程模型建模估算出下一個(gè)時(shí)刻的四元數(shù)，然后進(jìn)行誤差協(xié)方差更新，第三步利用更 新的協(xié)方差計(jì)算出卡爾曼增益，第四步利用步驟S4中觀測(cè)模型求解出的測(cè)量四元數(shù)減去第 一步估計(jì)出來的下一個(gè)時(shí)刻的四元數(shù)，乘上第三步求出的卡爾曼增益，加上上一個(gè)時(shí)刻的 四元數(shù)就是擴(kuò)展卡爾曼濾波器求出的最優(yōu)四元數(shù);之后更新下一時(shí)刻協(xié)方差為下一次計(jì)算 做準(zhǔn)備。
【文檔編號(hào)】G01C21/20GK105890598SQ201610216667
【公開日】2016年8月24日
【申請(qǐng)日】2016年4月8日
【發(fā)明人】陳洋, 龍文, 吳懷宇, 程磊, 王正熙
【申請(qǐng)人】武漢科技大學(xué)