本發(fā)明涉及一種自動掉頭路徑規(guī)劃及其控制方法，特別涉及一種用于農(nóng)機作業(yè)的gnss/ins/車輛組合導(dǎo)航方法。

背景技術(shù)：

隨著農(nóng)業(yè)自動化的發(fā)展、國家惠農(nóng)政策的普及以及農(nóng)民老齡化的比例上升，精準(zhǔn)農(nóng)業(yè)逐漸成為國際研究熱點，精準(zhǔn)農(nóng)業(yè)的普及可以解放雙手，提高工作效率、提供土地利用率、及時播種收割、節(jié)約種子等投入，達到減少勞動力、減少投入、增加產(chǎn)量的目標(biāo)。

在現(xiàn)有的導(dǎo)航系統(tǒng)中，最常用的技術(shù)為基于gnss/ins組合導(dǎo)航方法設(shè)計的衛(wèi)星組合導(dǎo)航系統(tǒng)，該系統(tǒng)包括慣性測量單元和gnss接收單元，導(dǎo)航方法具體的為，慣性測量單元測量載體的加速度和角速度，通過ins計算單元解算得到速度、位置和姿態(tài)信息，gnss輸出位置和速度信息，對gnss輸出的位置速度信息和ins計算單元輸出的位置速度姿態(tài)信息進行卡爾曼濾波，估計出ins的位置、速度和姿態(tài)誤差，以及慣性測量單元的誤差，將這些誤差反饋給ins計算單元，修正ins的導(dǎo)航解和傳感器誤差，從而抑制ins誤差隨時間的增長，獲得穩(wěn)定的導(dǎo)航解，但是在信號被遮擋等復(fù)雜環(huán)境下時，gnss信號會瞬間丟失，此時imu發(fā)散，導(dǎo)航精度差，同時，這樣的導(dǎo)航系統(tǒng)中需要提供高精度的imu才能獲得較高精度的姿態(tài)信息，對傳感器自身的要求高，成本高。

技術(shù)實現(xiàn)要素：

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的在于克服上述現(xiàn)有技術(shù)中的不足之處，解決現(xiàn)有技術(shù)中信號遮擋等復(fù)雜環(huán)境下導(dǎo)航精度差的技術(shù)問題，提供一種用于農(nóng)機作業(yè)的gnss/ins/車輛組合導(dǎo)航方法，本發(fā)明的導(dǎo)航精度高，對imu自身的要求低，成本低。

本發(fā)明的目的是這樣實現(xiàn)的：一種用于農(nóng)機作業(yè)的gnss/ins/車輛組合導(dǎo)航方法，具體包括以下步驟，

步驟1（ins解算）：對慣性測量單元進行零偏修正后，通過ins計算單元進行慣導(dǎo)解算，經(jīng)過卡爾曼濾波計算出狀態(tài)轉(zhuǎn)移矩陣，根據(jù)狀態(tài)轉(zhuǎn)移矩陣進行卡爾曼濾波一步預(yù)測得到系統(tǒng)協(xié)方差矩陣；

步驟2（gnss與ins的組合導(dǎo)航）：判斷gnss信息是否已經(jīng)更新，若gnss信息更新，gnss信息外推與imu（慣導(dǎo)測量單元）時間同步，慣導(dǎo)與gnss的位置、速度差值作為觀測量進行kalman濾波估計，kalman濾波估計更新步驟1中的狀態(tài)量和狀態(tài)協(xié)方差矩陣，構(gòu)成全維狀態(tài)觀測量，判斷載體的動靜狀態(tài),反饋修正平臺誤差和imu零偏誤差返回至步驟1中循環(huán)執(zhí)行；否則，當(dāng)前信息就為載體的位置、速度和姿態(tài)信息；

步驟3（車輛與ins的組合導(dǎo)航）：判斷車輛信息是否已經(jīng)更新，若車輛信息更新，車輛信息外推與imu更新時間同步，慣導(dǎo)與車輛的速度差值作為觀測量進行kalman濾波估計，得到狀態(tài)誤差修正量，用于反饋修正平臺誤差和imu零偏誤差；否則，當(dāng)前信息就為載體的位置、速度和姿態(tài)信息；

其中，修正平臺誤差指位置、速度和姿態(tài)誤差，imu零偏誤差指慣性測量單元的零偏誤差；慣性測量單元包括加速度計和陀螺儀。

本發(fā)明工作時，當(dāng)慣導(dǎo)解算得到新的位置、速度、姿態(tài)信息，經(jīng)過卡爾曼濾波計算出狀態(tài)轉(zhuǎn)移矩陣，根據(jù)狀態(tài)轉(zhuǎn)移矩陣進行卡爾曼濾波一步預(yù)測得到狀態(tài)協(xié)方差矩陣，此時，若gnss信息得到新的位置和速度信息時，慣導(dǎo)與gnss的位置、速度差值作為觀測量進行kalman濾波估計，得到全維狀態(tài)量的誤差修正值，判斷載體動靜態(tài)，反饋修正平臺誤差和imu零偏誤差至ins解算步驟中的ins計算單元，同時，輸出經(jīng)過kalman濾波估計后解算出來的載體速度、位置和姿態(tài)信息；若gnss信息沒有更新，則當(dāng)前信息就是載體的位置、速度和姿態(tài)信息；當(dāng)慣導(dǎo)解算得到新的位置、速度和姿態(tài)信息，且里程計更新得到新的車輛信息，則慣導(dǎo)與車輛的速度差值作為觀測量進行kalman濾波估計，得到狀態(tài)誤差修正量，反饋修正平臺誤差和imu零偏誤差，反饋imu零偏誤差至imu的零偏修正，修正平臺誤差反饋至ins計算單元重新進行慣導(dǎo)解算以循環(huán)執(zhí)行，同時，輸出當(dāng)前載體速度、位置和姿態(tài)信息；若車輛信息的時間沒有更新，則直接輸出當(dāng)前載體速度、位置和姿態(tài)信息；本發(fā)明提供了gnss和ins組合導(dǎo)航以及ins和車輛組合導(dǎo)航方法，gnss/ins組合導(dǎo)航方法，動態(tài)初始對準(zhǔn)收斂速度快、精度高，當(dāng)gnss信號丟失時，該ins和車輛組合導(dǎo)航技術(shù)可以很好地抑制imu發(fā)散，提供高精度的位置、速度和姿態(tài)信息；可應(yīng)用于農(nóng)機在田間作業(yè)時自動駕駛的導(dǎo)航工作中。

作為本發(fā)明的進一步改進，所述步驟1中，ins計算單元進行慣導(dǎo)解算，解算過程如下，

a）速度解算：

（1-1）

其中，

為k時刻導(dǎo)航坐標(biāo)系下的載體速度；

為單位矩陣；

為導(dǎo)航系相對于慣性系在k-1到k時刻的角度變化量；

表示叉乘；

為k-1時刻姿態(tài)旋轉(zhuǎn)矩陣；

為k-1到k時刻機體系下載體速度增量；

為劃槳效應(yīng)速度補償量；

為旋轉(zhuǎn)效應(yīng)速度補償量；

為由重力和科里奧利引起的速度增量；

初始速度由gnss提供；

b）位置解算：

位置四元數(shù)解算：

（1-2）

其中：

（1-3）

為導(dǎo)航系k-1到k時刻等效旋轉(zhuǎn)矢量；

為k-1到k時刻地球自轉(zhuǎn)引起的等效旋轉(zhuǎn)矢量；

為k時刻位置四元數(shù)；

利用更新后的四元數(shù)直接轉(zhuǎn)換得到更新后的位置，四元數(shù)與位置之間關(guān)系為：

（1-4）

其中，

l為當(dāng)前位置緯度；

λ為當(dāng)前位置經(jīng)度；

ins的初始位置由gnss提供；

c）姿態(tài)解算：

姿態(tài)四元數(shù)解算：

（1-5）

其中：

（1-6）

為導(dǎo)航系k-1到k時刻等效旋轉(zhuǎn)矢量；

為機體系下等效旋轉(zhuǎn)矢量；

為k時刻姿態(tài)四元數(shù)；

利用更新后的姿態(tài)四元數(shù)直接轉(zhuǎn)換得到姿態(tài)矩陣：

（1-7）

其中，

為姿態(tài)矩陣方向余弦形式；

為姿態(tài)四元數(shù)的第x維；

利用姿態(tài)矩陣與方向角之間的關(guān)系可以得到姿態(tài)角：

（1-8）

（1-9）

其中，

r、p、h分別為翻轉(zhuǎn)角、俯仰角、航向角；

為姿態(tài)矩陣的第x/y維；

初始航向由gnss東向和北向速度解算，初始翻轉(zhuǎn)角和俯仰角設(shè)定為0，根據(jù)上述姿態(tài)角轉(zhuǎn)換矩陣初始化初始姿態(tài)陣；

此設(shè)計中，通過慣導(dǎo)解算計算出載體的速度、位置和姿態(tài)信息。

作為本發(fā)明的進一步改進，所述步驟1中，慣導(dǎo)解算后的經(jīng)過卡爾曼濾波計算出狀態(tài)轉(zhuǎn)移矩陣和一步預(yù)測，具體的為，

d）狀態(tài)轉(zhuǎn)移矩陣的計算：

選取位置誤差、速度誤差、平臺姿態(tài)角以及陀螺和加表的零偏誤差共15維作為濾波估計的狀態(tài)量，即

（2-1）

其中，為陀螺儀的三個軸向漂移，為加表三個軸向的偏置誤差，并不是對應(yīng)imu的xyz三軸零偏誤差，δl、δλ和δh為ins的三個位置誤差由地球坐標(biāo)系描述，δvn、δve和δvd為東北天三個方向上的速度誤差；

根據(jù)所選狀態(tài)量之間的關(guān)系可以得到狀態(tài)轉(zhuǎn)移矩陣如下，

（2-2）

其中，

f1為位置與位置之間狀態(tài)轉(zhuǎn)移矩陣；

f2為位置與速度之間狀態(tài)轉(zhuǎn)移矩陣；

f3為速度與位置之間狀態(tài)轉(zhuǎn)移矩陣；

f4為速度與速度之間狀態(tài)轉(zhuǎn)移矩陣；

f5為速度與姿態(tài)之間狀態(tài)轉(zhuǎn)移矩陣；

f6為速度與加表零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

f7為姿態(tài)與姿態(tài)之間狀態(tài)轉(zhuǎn)移矩陣；

f8為姿態(tài)與陀螺零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

f9為陀螺零偏誤差與陀螺零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

f10為加表零偏誤差與加表零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

e）一步預(yù)測：

一步預(yù)測n-1時刻到n時刻狀態(tài)量狀態(tài)協(xié)方差矩陣，

（2-3）

（2-4）

其中，

為n-1時刻狀態(tài)量x的估計值；

為n-1時刻狀態(tài)協(xié)方差矩陣；

為n-1時刻到n時刻狀態(tài)轉(zhuǎn)移矩陣；

q為系統(tǒng)噪聲矩陣；

為imu更新周期；

此設(shè)計中，通過以上步驟解算出狀態(tài)轉(zhuǎn)移矩陣和狀態(tài)協(xié)方差矩陣。

為了獲得高精度的同步時間，所述步驟2中，實現(xiàn)gnss信息與imu信息同步更新的方法為，gnss的pps（脈沖）信號的周期為1s，每個脈沖處系統(tǒng)晶振均重新計時，相當(dāng)于每1s校準(zhǔn)一次晶振，當(dāng)gnss信息更新時，將gnss信息外推到最近時刻的imu處；所述車輛與ins的組合導(dǎo)航中，實現(xiàn)里程計信息與imu信息同步更新的方法與gnss信息與imu信息同步更新的方法相同；此設(shè)計中，通過軟校準(zhǔn)的時間同步方法，實現(xiàn)gnss信息與ins信息的同步或者ins信息與里程計信息的同步，實現(xiàn)簡單，時間同步精度高，即使使用低精度的晶振也可以獲得高精度的同步時間。

為了進一步提高載體靜止時載體姿態(tài)信息的精度，所述步驟2中，判斷載體的動靜狀態(tài),反饋修正平臺誤差和imu零偏誤差具體的為，若載體靜止，將gnss輸出的速度約束為零并與imu測得的當(dāng)前速度作差，將其差值作為kalman濾波估計的觀測量，同時，約束載體航向，經(jīng)過kalman濾波估計得到狀態(tài)修正量，反饋經(jīng)過kalman濾波估計的修正平臺誤差和imu零偏誤差至ins計算單元進行解算，同時，輸出當(dāng)前載體的速度、位置和姿態(tài)信息；若載體為動態(tài)，則直接反饋修正平臺誤差和imu零偏誤差至ins計算單元循環(huán)執(zhí)行，同時，輸出當(dāng)前載體的速度、位置和姿態(tài)信息；此設(shè)計中，零速修正和航向鎖定技術(shù)，在載體靜止時可以很好地抑制航向漂移，提供高精度的姿態(tài)信息。

為了進一步修正觀測誤差，所述kalman濾波估計更新狀態(tài)量和狀態(tài)協(xié)方差矩陣，具體的為，

f）解算濾波增益：

（3-1）

其中，

為n時刻濾波增益；

為n時刻的觀測矩陣，由觀測量與狀態(tài)量之間的關(guān)系得到；

為n時刻觀測噪聲協(xié)方差矩陣，由gnss信息提供；

g）狀態(tài)量的更新：

（3-2）

其中，

為n時刻的狀態(tài)估計量；

為n時刻的觀測量即imu與gnss的位置速度差值；

h）狀態(tài)協(xié)方差矩陣的更新：

（3-3）

其中，

為n時刻狀態(tài)協(xié)方差矩陣；

為單位矩陣；

此設(shè)計中，在濾波估計時加入了質(zhì)量控制，即當(dāng)實際觀測誤差量與估計的觀測誤差相差較大時，對kalman濾波的觀測誤差進行進一步的修正。

為了進一步提高姿態(tài)角的精度，所述步驟3中，獲取車輛信息時，具體的為，將農(nóng)機運動模型簡化為簡單的四輪模型，以后軸中心為原點的前右下坐標(biāo)系，將車輛后軸中心的右和下兩個軸的速度約束為零，前向軸的速度由里程計測得；將車輛三軸的速度與ins的速度差值作為kalman濾波估計的觀測量，經(jīng)過kalman濾波估計后輸出修正平臺誤差和imu零偏誤差并反饋至ins計算單元解算循環(huán)執(zhí)行；此設(shè)計，可有效輔助航向信息，提供更高精度的姿態(tài)角，當(dāng)gnss信號丟失時，該技術(shù)可以很好地抑制imu發(fā)散，提供更高精度的位置、速度和姿態(tài)信息。

附圖說明

圖1為本發(fā)明中導(dǎo)航方法的總體流程圖。

圖2為本發(fā)明中慣性導(dǎo)航系統(tǒng)的解算流程圖。

圖3為本發(fā)明中g(shù)nss和ins組合導(dǎo)航技術(shù)流程。

圖4為本發(fā)明中ins和車輛組合導(dǎo)航技術(shù)流程。

具體實施方式

下面結(jié)合附圖對本發(fā)明作進一步的說明。

如圖1～4所示的一種用于農(nóng)機作業(yè)的gnss/ins/車輛組合導(dǎo)航方法，具體包括以下步驟，

步驟1（ins解算）：對慣性測量單元進行零偏修正后，通過ins計算單元進行慣導(dǎo)解算，經(jīng)過卡爾曼濾波計算出狀態(tài)轉(zhuǎn)移矩陣，根據(jù)狀態(tài)轉(zhuǎn)移矩陣進行卡爾曼濾波一步預(yù)測得到狀態(tài)協(xié)方差矩陣；

步驟2（gnss與ins的組合導(dǎo)航）：判斷gnss信息是否已經(jīng)更新，若gnss信息更新，gnss信息外推與imu（慣導(dǎo)測量單元）時間同步，慣導(dǎo)與gnss的位置、速度差值作為觀測量進行kalman濾波估計，kalman濾波估計更新步驟1中的狀態(tài)量和狀態(tài)協(xié)方差矩陣，構(gòu)成全維狀態(tài)觀測量，判斷載體的動靜狀態(tài),反饋修正平臺誤差和imu零偏誤差返回至步驟1中循環(huán)執(zhí)行；否則，當(dāng)前信息就為載體的位置、速度和姿態(tài)信息；

步驟3（車輛與ins的組合導(dǎo)航）：判斷車輛信息是否已經(jīng)更新，若車輛信息更新，車輛信息外推與imu更新時間同步，慣導(dǎo)與車輛的速度差值作為觀測量進行kalman濾波估計，得到狀態(tài)誤差修正量，用于反饋修正平臺誤差和imu零偏誤差；否則，當(dāng)前信息就為載體的位置、速度和姿態(tài)信息；

其中，修正平臺誤差指位置、速度和姿態(tài)誤差，imu零偏誤差指慣性測量單元的零偏誤差；慣性測量單元包括加速度計和陀螺儀；

步驟1中，ins計算單元進行慣導(dǎo)解算，解算過程如下，

a）速度解算：

（1-1）

其中，

為k時刻導(dǎo)航坐標(biāo)系下的載體速度；

為單位矩陣；

為導(dǎo)航系相對于慣性系在k-1到k時刻的角度變化量；

表示叉乘；

為k-1時刻姿態(tài)旋轉(zhuǎn)矩陣；

為k-1到k時刻機體系下載體速度增量；

為劃槳效應(yīng)速度補償量；

為旋轉(zhuǎn)效應(yīng)速度補償量；

為由重力和科里奧利引起的速度增量；

初始速度由gnss提供；

b）位置解算：

位置四元數(shù)解算：

（1-2）

其中：

（1-3）

為導(dǎo)航系k-1到k時刻等效旋轉(zhuǎn)矢量；

為k-1到k時刻地球自轉(zhuǎn)引起的等效旋轉(zhuǎn)矢量；

為k時刻位置四元數(shù)；

利用更新后的四元數(shù)直接轉(zhuǎn)換得到更新后的位置，四元數(shù)與位置之間關(guān)系為：

（1-4）

其中，

l為當(dāng)前位置緯度；

λ為當(dāng)前位置經(jīng)度；

ins的初始位置由gnss提供；

c）姿態(tài)解算：

姿態(tài)四元數(shù)解算：

（1-5）

其中：

（1-6）

為導(dǎo)航系k-1到k時刻等效旋轉(zhuǎn)矢量；

為機體系下等效旋轉(zhuǎn)矢量；

為k時刻姿態(tài)四元數(shù)；

利用更新后的姿態(tài)四元數(shù)直接轉(zhuǎn)換得到姿態(tài)矩陣：

（1-7）

其中，

為姿態(tài)矩陣方向余弦形式；

為姿態(tài)四元數(shù)的第x維；

利用姿態(tài)矩陣與方向角之間的關(guān)系可以得到姿態(tài)角：

（1-8）

（1-9）

其中，

r、p、h分別為翻轉(zhuǎn)角、俯仰角、航向角；

為姿態(tài)矩陣的第x/y維；

初始航向由gnss東向和北向速度解算，初始翻轉(zhuǎn)角和俯仰角設(shè)定為0，根據(jù)上述姿態(tài)角轉(zhuǎn)換矩陣初始化初始姿態(tài)陣；

步驟1中，慣導(dǎo)解算后的經(jīng)過卡爾曼濾波計算出狀態(tài)轉(zhuǎn)移矩陣和一步預(yù)測，具體的為，

d）狀態(tài)轉(zhuǎn)移矩陣的計算：

選取位置誤差、速度誤差、平臺姿態(tài)角以及陀螺和加表的零偏誤差共15維作為濾波估計的狀態(tài)量，即

（2-1）

其中，為陀螺儀的三個軸向漂移，為加表三個軸向的偏置誤差，并不是對應(yīng)imu的xyz三軸零偏誤差，δl、δλ和δh為ins的三個位置誤差由地球坐標(biāo)系描述，δvn、δve和δvd為東北天三個方向上的速度誤差；

根據(jù)所選狀態(tài)量之間的關(guān)系可以得到狀態(tài)轉(zhuǎn)移矩陣如下，

（2-2）

其中，

f1為位置與位置之間狀態(tài)轉(zhuǎn)移矩陣；

f2為位置與速度之間狀態(tài)轉(zhuǎn)移矩陣；

f3為速度與位置之間狀態(tài)轉(zhuǎn)移矩陣；

f4為速度與速度之間狀態(tài)轉(zhuǎn)移矩陣；

f5為速度與姿態(tài)之間狀態(tài)轉(zhuǎn)移矩陣；

f6為速度與加表零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

f7為姿態(tài)與姿態(tài)之間狀態(tài)轉(zhuǎn)移矩陣；

f8為姿態(tài)與陀螺零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

f9為陀螺零偏誤差與陀螺零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

f10為加表零偏誤差與加表零偏誤差之間狀態(tài)轉(zhuǎn)移矩陣；

e）一步預(yù)測：

一步預(yù)測n-1時刻到n時刻狀態(tài)量狀態(tài)協(xié)方差矩陣，

（2-3）

（2-4）

其中，

為n-1時刻狀態(tài)量x的估計值；

為n-1時刻狀態(tài)協(xié)方差矩陣；

為n-1時刻到n時刻狀態(tài)轉(zhuǎn)移矩陣；

q為系統(tǒng)噪聲矩陣；

為imu更新周期；

gnss信息相較于imu信息更新較慢，當(dāng)gnss信息更新時，imu不一定同步更新，所以要嚴(yán)格同步imu與gnss時間；實現(xiàn)gnss信息與imu信息同步更新的方法為，gnss的pps（脈沖）信號的周期為1s，每個脈沖處系統(tǒng)晶振均重新計時，相當(dāng)于每1s校準(zhǔn)一次晶振，當(dāng)gnss信息更新時，將gnss信息外推到最近時刻的imu處；車輛信息相較于imu信息更新較慢，當(dāng)車輛信息更新時，imu不一定同步更新，所以要嚴(yán)格同步imu與車輛信息更新的時間，實現(xiàn)車輛信息與imu信息同步更新的方法與gnss信息與imu信息同步更新的方法相同；此設(shè)計中，通過軟校準(zhǔn)的時間同步方法，實現(xiàn)gnss信息與ins信息的同步或者ins信息與里程計信息的同步，實現(xiàn)簡單，時間同步精度高，即使使用低精度的晶振也可以獲得高精度的同步時間。

為了進一步提高載體靜止時載體姿態(tài)信息的精度，步驟3中，判斷載體的動靜狀態(tài),反饋修正平臺誤差和imu零偏誤差具體的為，若載體靜止，將gnss輸出的速度約束為零并與imu測得的當(dāng)前速度作差，將其差值作為kalman濾波估計的觀測量，同時，約束載體航向，經(jīng)過kalman濾波估計得到狀態(tài)修正量，反饋經(jīng)過kalman濾波估計的修正平臺誤差和imu零偏誤差至ins計算單元進行解算，同時，輸出當(dāng)前載體的速度、位置和姿態(tài)信息；若載體為動態(tài)，則直接反饋修正平臺誤差和imu零偏誤差至ins計算單元循環(huán)執(zhí)行，同時，輸出當(dāng)前載體的速度、位置和姿態(tài)信息；

為了進一步修正觀測誤差，kalman濾波估計更新狀態(tài)量和狀態(tài)協(xié)方差矩陣，具體的為，

f）解算濾波增益：

（3-1）

其中，

為n時刻濾波增益；

為n時刻的觀測矩陣，由觀測量與狀態(tài)量之間的關(guān)系得到；

為n時刻觀測噪聲協(xié)方差矩陣，由gnss信息提供；

g）狀態(tài)量的更新：

（3-2）

其中，

為n時刻的狀態(tài)估計量；

為n時刻的觀測量即imu與gnss的位置速度差值；

h）狀態(tài)協(xié)方差矩陣的更新：

（3-3）

其中，

為n時刻狀態(tài)協(xié)方差矩陣；

為單位矩陣；

為了進一步提高姿態(tài)角的精度，步驟3中，將農(nóng)機運動模型簡化為簡單的四輪模型，以后軸中心為原點的前右下坐標(biāo)系，將車輛后軸中心的右和下兩個軸的速度約束為零，前向軸的速度由里程計測得，將車輛三軸的速度與ins的速度差值作為kalman濾波估計的觀測量，經(jīng)過kalman濾波估計后輸出修正平臺誤差和imu零偏誤差并反饋至ins計算單元解算循環(huán)執(zhí)行。

本發(fā)明工作時，當(dāng)慣導(dǎo)解算得到新的位置、速度、姿態(tài)信息，經(jīng)過卡爾曼濾波計算出狀態(tài)轉(zhuǎn)移矩陣，根據(jù)狀態(tài)轉(zhuǎn)移矩陣進行卡爾曼濾波一步預(yù)測得到狀態(tài)協(xié)方差矩陣，此時，若gnss信息得到新的位置和速度信息時，慣導(dǎo)與gnss的位置、速度差值作為觀測量進行kalman濾波估計，得到15維狀態(tài)量的誤差修正值，判斷載體動靜態(tài)，若此時載體靜止，則利用將gnss輸出的速度約束為零并與慣導(dǎo)元件測得的當(dāng)前速度作差，將其差值作為觀測量，進一步進行kalman濾波估計得到狀態(tài)修正量，然后反饋修正平臺誤差和imu零偏誤差；若載體為動態(tài)，則直接反饋imu零偏誤差進行零偏修正，以及修正平臺誤差至步驟1中的ins計算單元進行位置、速度和姿態(tài)的解算，同時，輸出當(dāng)前經(jīng)過kalman濾波估計后解算出來的載體速度、位置和姿態(tài)信息；若gnss信息沒有更新，則當(dāng)前信息就是載體的位置、速度和姿態(tài)信息；當(dāng)慣導(dǎo)解算得到新的位置、速度和姿態(tài)信息，且里程計更新得到新的車輛信息，則慣導(dǎo)與車輛的速度差值作為觀測量進行kalman濾波估計，得到狀態(tài)誤差修正量，反饋修正平臺誤差和imu零偏誤差，反饋imu零偏誤差至imu的零偏修正，修正平臺誤差反饋至步驟1中的ins計算單元重新進行慣導(dǎo)解算以循環(huán)執(zhí)行，同時，輸出當(dāng)前載體速度、位置和姿態(tài)信息；若車輛信息沒有更新，則直接輸出當(dāng)前載體速度、位置和姿態(tài)信息；本發(fā)明提供了gnss和ins組合導(dǎo)航以及ins和車輛組合導(dǎo)航方法，gnss/ins組合導(dǎo)航方法，動態(tài)初始對準(zhǔn)收斂速度快、精度高，當(dāng)gnss信號丟失時，該ins和車輛組合導(dǎo)航技術(shù)可以很好地抑制imu發(fā)散，提供高精度的位置、速度和姿態(tài)信息；采用的零速修正和航向鎖定技術(shù)，在載體靜止時可以很好地抑制航向漂移，提供高精度的姿態(tài)信息；使用軟校準(zhǔn)時間同步方法，實現(xiàn)簡單，時間同步精度高，利用低精度的晶振可以獲得高精度的同步時間；可應(yīng)用于農(nóng)機在田間作業(yè)時自動駕駛的導(dǎo)航工作中。

本發(fā)明并不局限于上述實施例，在本發(fā)明公開的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域的技術(shù)人員根據(jù)所公開的技術(shù)內(nèi)容，不需要創(chuàng)造性的勞動就可以對其中的一些技術(shù)特征作出一些替換和變形，這些替換和變形均在本發(fā)明保護范圍內(nèi)。