[發(fā)明專利]一種導(dǎo)航路徑幾何約束輔助的農(nóng)業(yè)機(jī)械自主導(dǎo)航方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202110070855.1
• 申請(qǐng)日: 2021-01-19
• 公開（公告）號(hào): CN112904393A
• 公開（公告）日: 2021-06-04
• 發(fā)明（設(shè)計(jì)）人: 崔冰波;吳永勝;魏新華;廖偉鋒;吉峰;孫宇
• 申請(qǐng)（專利權(quán)）人: 江蘇大學(xué)
• 主分類號(hào): G01S19/43
• 分類號(hào): G01S19/43;G01S19/39;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發(fā)明提供了一種導(dǎo)航路徑幾何約束輔助的農(nóng)業(yè)機(jī)械自主導(dǎo)航方法，屬于智能農(nóng)業(yè)機(jī)械領(lǐng)域。本發(fā)明利用自動(dòng)駕駛系統(tǒng)和自動(dòng)導(dǎo)航系統(tǒng)的歷史數(shù)據(jù)，結(jié)合農(nóng)機(jī)導(dǎo)航路徑的幾何形狀約束和速度方向約束，構(gòu)造擴(kuò)增狀態(tài)向量和擴(kuò)增量測(cè)向量；利用自動(dòng)駕駛系統(tǒng)路徑跟蹤結(jié)果，設(shè)置量測(cè)噪聲方差的初始值，在線更新量測(cè)方差矩陣基于狀態(tài)轉(zhuǎn)移矩陣系統(tǒng)噪聲方差矩陣量測(cè)矩陣及得到擴(kuò)增狀態(tài)估計(jì)值的前6個(gè)元素作為k時(shí)刻自主導(dǎo)航系統(tǒng)的狀態(tài)輸出。本發(fā)明能提高農(nóng)機(jī)自主導(dǎo)航系統(tǒng)的精度、可靠性和自適應(yīng)性。

技術(shù)領(lǐng)域

本發(fā)明涉及智能農(nóng)業(yè)機(jī)械領(lǐng)域，具體涉及一種導(dǎo)航路徑幾何約束輔助的農(nóng)業(yè)機(jī)械自主導(dǎo)航方法。

背景技術(shù)

農(nóng)業(yè)機(jī)械自動(dòng)駕駛基于導(dǎo)航定位感知系統(tǒng)、路徑規(guī)劃軟件、轉(zhuǎn)角傳感器和轉(zhuǎn)向執(zhí)行機(jī)構(gòu)實(shí)現(xiàn)農(nóng)機(jī)的導(dǎo)航控制決策與路徑跟蹤。導(dǎo)航定位感知系統(tǒng)的精度和可靠性對(duì)自動(dòng)駕駛系統(tǒng)性能影響較大，現(xiàn)有的自動(dòng)駕駛系統(tǒng)多基于高精度衛(wèi)星定位、視覺導(dǎo)航、慣性導(dǎo)航及其多源數(shù)據(jù)融合方法實(shí)現(xiàn)農(nóng)機(jī)位姿的實(shí)時(shí)更新。單一的導(dǎo)航系統(tǒng)常常難以滿足實(shí)際生產(chǎn)的需要，衛(wèi)星定位易受傳輸空間電磁波干擾和環(huán)境遮擋的影響，視覺傳感器受光照環(huán)境影響顯著，慣性導(dǎo)航解算誤差隨時(shí)間累積，因此多源數(shù)據(jù)融合的導(dǎo)航方法能取長(zhǎng)補(bǔ)短，實(shí)現(xiàn)高精度高可靠的導(dǎo)航參數(shù)更新，受到廣泛的關(guān)注。常規(guī)的組合導(dǎo)航系統(tǒng)其性能受傳感器精度和數(shù)據(jù)融合方法影響較大，近幾年先進(jìn)的數(shù)據(jù)融合方法獲得快速發(fā)展，但高精度的幾何空間定位系統(tǒng)價(jià)格仍居高不下，且先進(jìn)數(shù)據(jù)融合方法對(duì)系統(tǒng)硬件要求也較高，造成研制高精度、高更新率的組合導(dǎo)航系統(tǒng)成本較高，且該狀況短時(shí)間內(nèi)難以改變，阻礙了農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)的大面積推廣應(yīng)用。

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

針對(duì)現(xiàn)有技術(shù)中存在不足，本發(fā)明提供了一種導(dǎo)航路徑幾何約束輔助的農(nóng)業(yè)機(jī)械自主導(dǎo)航方法，改善農(nóng)機(jī)自主導(dǎo)航系統(tǒng)的精度與可靠性，提高自主導(dǎo)航系統(tǒng)數(shù)據(jù)融合方法的自適應(yīng)性。

本發(fā)明是通過以下技術(shù)手段實(shí)現(xiàn)上述技術(shù)目的的。

一種導(dǎo)航路徑幾何約束輔助的農(nóng)業(yè)機(jī)械自主導(dǎo)航方法，利用自動(dòng)駕駛系統(tǒng)和自動(dòng)導(dǎo)航系統(tǒng)的歷史數(shù)據(jù)，結(jié)合農(nóng)機(jī)導(dǎo)航路徑的幾何形狀約束和速度方向約束，構(gòu)造擴(kuò)增狀態(tài)向量和擴(kuò)增量測(cè)向量；利用自動(dòng)駕駛系統(tǒng)路徑跟蹤結(jié)果，實(shí)現(xiàn)量測(cè)噪聲方差初始值的設(shè)置，在線更新量測(cè)方差矩陣基于狀態(tài)轉(zhuǎn)移矩陣系統(tǒng)噪聲方差矩陣量測(cè)矩陣及得到擴(kuò)增狀態(tài)估計(jì)值的前6個(gè)元素即為k時(shí)刻自主導(dǎo)航系統(tǒng)的狀態(tài)輸出。

進(jìn)一步的技術(shù)方案，所述擴(kuò)增量測(cè)向量為其中z_k為量測(cè)向量，為約束量測(cè)向量，且(x_k，y_k)、(x_k-1，y_k-1)分別為k、k-1時(shí)刻農(nóng)機(jī)在高斯平面坐標(biāo)系下的位置，分別為k、k-1時(shí)刻的農(nóng)機(jī)速度。

更進(jìn)一步的技術(shù)方案，所述k、k-1時(shí)刻農(nóng)機(jī)在高斯平面坐標(biāo)系下的位置和農(nóng)機(jī)速度滿足如下關(guān)系式：

更進(jìn)一步的技術(shù)方案，所述量測(cè)矩陣和量測(cè)方差矩陣分別為：

其中，d表示k時(shí)刻之前的濾波時(shí)間單位；分別表示x軸向位置、y軸向位置、速度、航向角、轉(zhuǎn)向角、橫向偏差和橫向偏差一階差分對(duì)應(yīng)的量測(cè)噪聲方差；分別表示速度v_k在x、y軸向上量測(cè)分量。

進(jìn)一步的技術(shù)方案，所述擴(kuò)增量測(cè)向量為d表示k時(shí)刻之前的濾波時(shí)間單位。

進(jìn)一步的技術(shù)方案，所述狀態(tài)轉(zhuǎn)移矩陣和系統(tǒng)噪聲方差矩陣分別為：

其中，Φ_k為狀態(tài)量x_k的狀態(tài)轉(zhuǎn)移矩陣，I為單位矩陣，Q_k為量測(cè)噪聲。