基于gps的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法
【專利摘要】本發(fā)明公開了一種基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，包括采用導(dǎo)航傳感器組合測(cè)量得到農(nóng)業(yè)機(jī)械在WGS-84坐標(biāo)系下的原始緯度、經(jīng)度、高度坐標(biāo)和前進(jìn)速度，并測(cè)量得到農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、橫擺角速度和前進(jìn)方向加速度；將WGS-84坐標(biāo)系下的原始緯度、經(jīng)度和高度坐標(biāo)轉(zhuǎn)換到NEU大地導(dǎo)航坐標(biāo)系下得到初步導(dǎo)航定位位置值；將橫擺角速度經(jīng)積分后轉(zhuǎn)換為航向角度測(cè)量值；采用改進(jìn)的卡爾曼濾波器對(duì)獲得的橫滾角度、俯仰角度、前進(jìn)速度、前進(jìn)方向加速度、初步定位位置值和航向角度測(cè)量值進(jìn)行融合處理，獲得農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。本發(fā)明方法可形成更連續(xù)和穩(wěn)定的農(nóng)業(yè)機(jī)械導(dǎo)航數(shù)據(jù)，使得航向角度的估計(jì)更加精確。
【專利說明】基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種多傳感器信息融合方法，尤其是一種基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，屬于農(nóng)業(yè)機(jī)械的GPS導(dǎo)航定位【技術(shù)領(lǐng)域】。
【背景技術(shù)】
[0002]隨著GPS、北斗等衛(wèi)星導(dǎo)航定位技術(shù)、電子技術(shù)、機(jī)器視覺技術(shù)、現(xiàn)代傳感測(cè)試技術(shù)和自動(dòng)控制技術(shù)的不斷發(fā)展、順應(yīng)現(xiàn)代農(nóng)業(yè)生產(chǎn)的發(fā)展趨勢(shì)，基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航技術(shù)與裝備迅速發(fā)展，成為現(xiàn)代智能農(nóng)業(yè)機(jī)械與裝備的重要組成部分，對(duì)現(xiàn)代農(nóng)業(yè)建設(shè)提供了良好的技術(shù)支持。農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航技術(shù)在歐美日等發(fā)達(dá)國(guó)際已經(jīng)實(shí)現(xiàn)產(chǎn)業(yè)化，產(chǎn)生了良好的經(jīng)濟(jì)效益和社會(huì)效益?；贕PS的農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航技術(shù)產(chǎn)品可有效減輕駕駛員的工作負(fù)擔(dān)，避免疊行或者漏行作業(yè)，提高作業(yè)質(zhì)量和土地利用率，顯著提升農(nóng)業(yè)機(jī)械的自動(dòng)化水平，對(duì)我國(guó)現(xiàn)代農(nóng)業(yè)的建設(shè)將起到有力的推動(dòng)作用。
[0003]基于GPS的農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航涉及的主要導(dǎo)航信息包括:定位信息和航向角度信息。已有研究成果主要采用RTK-GPS接收機(jī)和慣性測(cè)量單元MU獲取上述導(dǎo)航信息，其成本較高。農(nóng)田表面高度變化大，從而影響GPS天線的定位精度，需要進(jìn)行傾斜校正；農(nóng)業(yè)機(jī)械航向角度信息無法直接精確測(cè)量，需要采用最小二乘方法或者卡爾曼濾波方法估計(jì)獲得。已有研究成果對(duì)傾斜校正和卡爾曼濾波都提出了不同解決方法。但存在的問題是:傾斜校正和卡爾曼濾波是各自獨(dú)立處理的，利用卡爾曼濾波器對(duì)航向角度的估計(jì)結(jié)果對(duì)GPS天線進(jìn)行傾斜校正，容易導(dǎo)致卡爾曼濾波發(fā)散。

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

[0004]本發(fā)明的目的，是為了解決上述現(xiàn)有技術(shù)的缺陷，提供一種低成本、高精度的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，該方法將傾斜校正整合進(jìn)卡爾曼濾波器中，得到農(nóng)業(yè)機(jī)械更加精確和穩(wěn)定的實(shí)時(shí)位姿信息。
[0005]本發(fā)明的目的可以通過采取如下技術(shù)方案達(dá)到:
[0006]基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于所述方法包括以下步驟:
[0007]I)采用導(dǎo)航傳感器組合測(cè)量得到農(nóng)業(yè)機(jī)械在WGS-84坐標(biāo)系下的原始緯度、經(jīng)度、高度坐標(biāo)和前進(jìn)速度，并測(cè)量得到農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、橫擺角速度和前進(jìn)方向加速度；
[0008]2)將WGS-84坐標(biāo)系下的原始緯度、經(jīng)度和高度坐標(biāo)轉(zhuǎn)換到NEU大地導(dǎo)航坐標(biāo)系下得到初步導(dǎo)航定位位置值，將橫擺角速度經(jīng)積分后轉(zhuǎn)換為航向角度測(cè)量值；
[0009]3)采用改進(jìn)的卡爾曼濾波器對(duì)步驟I)獲得的橫滾角度、俯仰角度、前進(jìn)速度和前進(jìn)方向加速度，以及對(duì)步驟2)獲得的初步定位位置值和航向角度測(cè)量值進(jìn)行融合處理，獲得農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。
[0010]作為一種實(shí)施方案，步驟I)所述導(dǎo)航傳感器組合包括OEM GPS板卡和微型姿態(tài)航向參考系統(tǒng)，其中農(nóng)業(yè)機(jī)械在WGS-84坐標(biāo)系下的原始緯度、經(jīng)度、高度坐標(biāo)和前進(jìn)速度采用OEM GPS板卡測(cè)量，農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、橫擺角速度和前進(jìn)方向加速度采用微型姿態(tài)航向參考系統(tǒng)測(cè)量。
[0011]作為一種實(shí)施方案，步驟2)所述將WGS-84坐標(biāo)系下的原始緯度、經(jīng)度和高度坐標(biāo)轉(zhuǎn)換到NEU大地導(dǎo)航坐標(biāo)系下得到初步導(dǎo)航定位位置值采用高斯投影變換或切平面法實(shí)現(xiàn)。
[0012]作為一種實(shí)施方案，步驟3)所述卡爾曼濾波器的模型中，狀態(tài)向量為農(nóng)業(yè)機(jī)械導(dǎo)航控制點(diǎn)位置的平面坐標(biāo)(Xyy。)、速度偏差ε v、加速度偏差ε a和航向角偏差ε ψ ;測(cè)量向量為GPS天線位置的平面坐標(biāo)(xg，yg)。
[0013]作為一種實(shí)施方案，步驟3)所述卡爾曼濾波器的模型中，狀態(tài)轉(zhuǎn)移方程是基于位置、速度和加速度的航位推算原理構(gòu)建的；測(cè)量方程是基于車體坐標(biāo)系和大地導(dǎo)航坐標(biāo)系的坐標(biāo)轉(zhuǎn)換公式構(gòu)建的。
[0014]作為一種實(shí)施方案，步驟3)所述卡爾曼濾波器的模型中，狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣根據(jù)步驟I)獲得的農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、前進(jìn)速度、前進(jìn)方向加速度信息和由橫擺角速度累積得到的航向角度，進(jìn)行實(shí)時(shí)更新；測(cè)量噪聲方差矩陣根據(jù)OEM GPS板卡的定位質(zhì)量標(biāo)識(shí)gps_flag自適應(yīng)調(diào)節(jié)。
[0015]作為一種實(shí)施方案，所述方法還包括:
[0016]若步驟2)獲得的航向角度測(cè)量值與步驟3)經(jīng)過卡爾曼濾波后的航向角度精確估計(jì)值的差值超過設(shè)定閾值，則采用卡爾曼濾波后的航向角度精確估計(jì)值更新橫擺角速率的積分初值，以防止積分發(fā)散，同時(shí)重新初始化卡爾曼濾波器。
[0017]本發(fā)明相對(duì)于現(xiàn)有技術(shù)具有如下的有益效果:
[0018]1、本發(fā)明基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法將車體坐標(biāo)和大地導(dǎo)航坐標(biāo)的轉(zhuǎn)換公式整合進(jìn)卡爾曼濾波器中，使卡爾曼濾波器得到改進(jìn)，既有利于為農(nóng)業(yè)機(jī)械導(dǎo)航控制提供更為精確的航向角數(shù)據(jù)，也有利于增強(qiáng)卡爾曼濾波器的穩(wěn)定性和適用性。
[0019]2、本發(fā)明基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法采用的改進(jìn)卡爾曼濾波器可形成更連續(xù)和穩(wěn)定的農(nóng)業(yè)機(jī)械導(dǎo)航數(shù)據(jù)，使得航向角度的估計(jì)更加精確。
[0020]3、本發(fā)明基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法采用的改進(jìn)卡爾曼濾波器，有效整合了多導(dǎo)航傳感器信息融合的處理，有利于軟件的模塊化編程。
【專利附圖】

【附圖說明】
[0021]圖1為本發(fā)明實(shí)施例1的多傳感器信息融合方法的流程示意圖。
[0022]圖2為本發(fā)明實(shí)施例1的卡爾曼濾波器直線跟蹤軌跡仿真結(jié)果I。
[0023]圖3為本發(fā)明實(shí)施例1的卡爾曼濾波器直線跟蹤航向角度仿真結(jié)果I。
[0024]圖4為本發(fā)明實(shí)施例1的卡爾曼濾波器直線跟蹤軌跡仿真結(jié)果2。
[0025]圖5為本發(fā)明實(shí)施例1的卡爾曼濾波器直線跟蹤航向角度仿真結(jié)果2。
[0026]圖6為本發(fā)明實(shí)施例1的卡爾曼濾波器直線跟蹤軌跡仿真結(jié)果3。
[0027]圖7為本發(fā)明實(shí)施例1的卡爾曼濾波器直線跟蹤航向角度仿真結(jié)果3。
[0028]圖8為本發(fā)明實(shí)施例1的卡爾曼濾波器地頭轉(zhuǎn)向曲線跟蹤軌跡仿真結(jié)果。[0029]圖9為本發(fā)明實(shí)施例1的卡爾曼濾波器地頭轉(zhuǎn)向曲線跟蹤航向角度仿真結(jié)果。
【具體實(shí)施方式】
[0030]實(shí)施例1:
[0031]本實(shí)施例采用的農(nóng)業(yè)機(jī)械為雷沃M904-D輪式拖拉機(jī)，其導(dǎo)航系統(tǒng)主要包括--導(dǎo)航傳感器組合、嵌入式開發(fā)板和顯示終端；其中，導(dǎo)航傳感器組合包括OEM RTK GPS板卡(Trimble BD970)和微型姿態(tài)航向參考系統(tǒng)AHRS (XSENS MTi)，GPS天線安裝于拖拉機(jī)駕駛室頂部，處于拖拉機(jī)的中軸線上，AHRS的傳感器坐標(biāo)系三軸指向與拖拉機(jī)車體坐標(biāo)系三軸指向相同，安裝在拖拉機(jī)座椅下方；嵌入式開發(fā)板與導(dǎo)航傳感器組合通過RS232通訊接口連接，用于實(shí)現(xiàn)數(shù)據(jù)采集、數(shù)據(jù)預(yù)處理和信息融合等軟件程序的運(yùn)行；導(dǎo)航傳感器組合的數(shù)據(jù)采集頻率為10Hz，數(shù)據(jù)采集、數(shù)據(jù)預(yù)處理和信息融合軟件程序采用ADS1.2編程工具在Windows XP操作系統(tǒng)下開發(fā)完成,嵌入式開發(fā)板將融合處理后的結(jié)果通過RS232通訊接口發(fā)送給顯示終端實(shí)時(shí)顯示并保存；顯示終端的操作系統(tǒng)為WinCE6.0，其軟件程序采用Microsoft Visual Studio2005集成開發(fā)工具在Windows XP操作系統(tǒng)下開發(fā)完成。
[0032]如圖1所示，本實(shí)施例的多傳感器信息融合方法，包括以下步驟:
[0033]I)采用導(dǎo)航傳感器組合測(cè)量得到拖拉機(jī)在WGS-84坐標(biāo)系下的原始緯度、經(jīng)度、高度坐標(biāo)和前進(jìn)速度，并測(cè)量得到拖拉機(jī)的原始橫滾角度、俯仰角度、橫擺角速度和前進(jìn)方向加速度；
[0034]2)將WGS-84坐標(biāo)系下的原始緯度、經(jīng)度和高度坐標(biāo)轉(zhuǎn)換到NEU大地導(dǎo)航坐標(biāo)系下得到初步導(dǎo)航定位位置值，可以采用高斯投影變換或切平面法實(shí)現(xiàn)，在本實(shí)施例采用高斯投影變換；將橫擺角速度經(jīng)積分后轉(zhuǎn)換為航向角度測(cè)量值；
[0035]3)采用改進(jìn)的卡爾曼`濾波器對(duì)步驟I)獲得的橫滾角度、俯仰角度、前進(jìn)速度和前進(jìn)方向加速度，以及對(duì)步驟2)獲得的初步定位位置值和航向角度測(cè)量值進(jìn)行融合處理，獲得拖拉機(jī)定位和航向角度信息的精確估計(jì)；
[0036]所述卡爾曼濾波器的模型中，狀態(tài)向量為拖拉機(jī)導(dǎo)航控制點(diǎn)位置的平面坐標(biāo)(X。，y。)、速度偏差εν、加速度偏差ea和航向角偏差ε ψ ;測(cè)量向量為GPS天線位置的平面坐標(biāo)(xg, Yg);狀態(tài)轉(zhuǎn)移方程是基于位置、速度和加速度的航位推算原理構(gòu)建的；測(cè)量方程是基于車體坐標(biāo)系和大地導(dǎo)航坐標(biāo)系的坐標(biāo)轉(zhuǎn)換公式構(gòu)建的；狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣根據(jù)步驟I)獲得的拖拉機(jī)的原始橫滾角度、俯仰角度、前進(jìn)速度、前進(jìn)方向加速度信息和由橫擺角速度累積得到的航向角度，進(jìn)行實(shí)時(shí)更新；測(cè)量噪聲方差矩陣根據(jù)OEM GPS板卡的定位質(zhì)量標(biāo)識(shí)gps_flag自適應(yīng)調(diào)節(jié)；
[0037]4)若步驟2)獲得的航向角度測(cè)量值與步驟3)經(jīng)過卡爾曼濾波后的航向角度精確估計(jì)值的差值超過設(shè)定閾值，則采用卡爾曼濾波后的航向角度精確估計(jì)值更新橫擺角速率的積分初值，以防止積分發(fā)散，同時(shí)重新初始化卡爾曼濾波器。
[0038]具體信息融合方法的處理步驟和公式推導(dǎo)如下:
[0039]1、OEM RTK GPS板卡數(shù)據(jù)采集
[0040]BD970GPS板卡通過TTL串口以20Hz的輸出頻率輸出NMEA-0183格式版本的格式數(shù)據(jù)，利用自主研發(fā)的嵌入式數(shù)據(jù)采集程序從GPGGA數(shù)據(jù)報(bào)中提取拖拉機(jī)GPS天線的定位數(shù)據(jù)，包括緯度、經(jīng)度和高度；從GPVTG數(shù)據(jù)報(bào)中提取拖拉機(jī)前進(jìn)速度信息。[0041 ] 2、微型姿態(tài)航向參考系統(tǒng)數(shù)據(jù)采集
[0042]MTi通過RS232串口以50Hz的輸出頻率輸出固定長(zhǎng)度的數(shù)據(jù)，包括3軸MEMS加速度計(jì)標(biāo)定數(shù)據(jù)、3軸MEMS陀螺儀標(biāo)定數(shù)據(jù)和內(nèi)置Kalman濾波后的3軸姿態(tài)角數(shù)據(jù)，利用自主研發(fā)的嵌入式數(shù)據(jù)采集程序從MTi數(shù)據(jù)報(bào)中提取拖拉機(jī)的橫擺角速度、前進(jìn)方向加速度、橫滾角度和俯仰角度數(shù)據(jù)。
[0043]3、GPS定位數(shù)據(jù)的坐標(biāo)轉(zhuǎn)換
[0044]3.l、WGS-84大地坐標(biāo)向大地導(dǎo)航坐標(biāo)系的轉(zhuǎn)換
[0045]為了使GPS板卡輸出的WGS-84大地坐標(biāo)定位數(shù)據(jù)能用于拖拉機(jī)的導(dǎo)航控制系統(tǒng)，需要將WGS-84地心大地坐標(biāo)轉(zhuǎn)換為對(duì)應(yīng)于WGS-84橢球的高斯平面坐標(biāo)，即需進(jìn)行高斯投影變換；
[0046]采用如下公式:
[0047]
【權(quán)利要求】
1.基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于所述方法包括以下步驟: 1)采用導(dǎo)航傳感器組合測(cè)量得到農(nóng)業(yè)機(jī)械在WGS-84坐標(biāo)系下的原始緯度、經(jīng)度、高度坐標(biāo)和前進(jìn)速度，并測(cè)量得到農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、橫擺角速度和前進(jìn)方向加速度； 2)將WGS-84坐標(biāo)系下的原始緯度、經(jīng)度和高度坐標(biāo)轉(zhuǎn)換到NEU大地導(dǎo)航坐標(biāo)系下得到初步導(dǎo)航定位位置值，將橫擺角速度經(jīng)積分后轉(zhuǎn)換為航向角度測(cè)量值； 3)采用改進(jìn)的卡爾曼濾波器對(duì)步驟I)獲得的橫滾角度、俯仰角度、前進(jìn)速度和前進(jìn)方向加速度，以及對(duì)步驟2)獲得的初步定位位置值和航向角度測(cè)量值進(jìn)行融合處理，獲得農(nóng)業(yè)機(jī)械定位和航向角度信息的精確估計(jì)。
2.根據(jù)權(quán)利要求1所述的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于:步驟I)所述導(dǎo)航傳感器組合包括OEM GPS板卡和微型姿態(tài)航向參考系統(tǒng)，其中農(nóng)業(yè)機(jī)械在WGS-84坐標(biāo)系下的原始緯度、經(jīng)度、高度坐標(biāo)和前進(jìn)速度采用OEM GPS板卡測(cè)量，農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、橫擺角速度和前進(jìn)方向加速度采用微型姿態(tài)航向參考系統(tǒng)測(cè)量。
3.根據(jù)權(quán)利要求1所述的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于:步驟2)所述將WGS-84坐標(biāo)系下的原始緯度、經(jīng)度和高度坐標(biāo)轉(zhuǎn)換到NEU大地導(dǎo)航坐標(biāo)系下得到初步導(dǎo)航定位位置值采用高斯投影變換或切平面法實(shí)現(xiàn)。
4.根據(jù)權(quán)利要求2所述的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于:步驟3)所述卡爾曼濾波器的模型中，狀態(tài)向量為農(nóng)業(yè)機(jī)械導(dǎo)航控制點(diǎn)位置的平面坐標(biāo)(Xc;，y。)、速度偏差％、加速度偏差ea和航向角偏差ε ψ ;測(cè)量向量為GPS天線位置的平面坐標(biāo)(xg，yg)。
5.根據(jù)權(quán)利要求2所述的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于:步驟3)所述卡爾曼濾波器的模型中，狀態(tài)轉(zhuǎn)移方程是基于位置、速度和加速度的航位推算原理構(gòu)建的；測(cè)量方程是基于車體坐標(biāo)系和大地導(dǎo)航坐標(biāo)系的坐標(biāo)轉(zhuǎn)換公式構(gòu)建的。
6.根據(jù)權(quán)利要求5所述的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于:步驟3)所述卡爾曼濾波器的模型中，狀態(tài)轉(zhuǎn)移矩陣和測(cè)量矩陣根據(jù)步驟I)獲得的農(nóng)業(yè)機(jī)械的原始橫滾角度、俯仰角度、前進(jìn)速度、前進(jìn)方向加速度信息和由橫擺角速度累積得到的航向角度，進(jìn)行實(shí)時(shí)更新；測(cè)量噪聲方差矩陣根據(jù)OEM GPS板卡的定位質(zhì)量標(biāo)識(shí)gps_flag自適應(yīng)調(diào)節(jié)。
7.根據(jù)權(quán)利要求1所述的基于GPS的農(nóng)業(yè)機(jī)械導(dǎo)航的多傳感器信息融合方法，其特征在于所述方法還包括: 若步驟2)獲得的航向角度測(cè)量值與步驟3)經(jīng)過卡爾曼濾波后的航向角度精確估計(jì)值的差值超過設(shè)定閾值，則采用卡爾曼濾波后的航向角度精確估計(jì)值更新橫擺角速率的積分初值，以防止積分發(fā)散，同時(shí)重新初始化卡爾曼濾波器。
【文檔編號(hào)】G01S19/47GK103885076SQ201410081311
【公開日】2014年6月25日 申請(qǐng)日期:2014年3月6日 優(yōu)先權(quán)日:2014年3月6日
【發(fā)明者】張智剛, 羅錫文, 胡煉, 單鵬輝, 張亞嬌 申請(qǐng)人:華南農(nóng)業(yè)大學(xué)