專利名稱：農(nóng)用機(jī)械的導(dǎo)航方法、導(dǎo)航裝置和農(nóng)用機(jī)械的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)械導(dǎo)航技術(shù)領(lǐng)域，特別涉及一種農(nóng)用機(jī)械的導(dǎo)航方法、導(dǎo)航裝置和 農(nóng)用機(jī)械。
背景技術(shù)：
農(nóng)用機(jī)械的導(dǎo)航可定義為根據(jù)農(nóng)用機(jī)械的實(shí)際位置與 期望路徑的誤差確定車輛 轉(zhuǎn)向角的大小，使其按照預(yù)定的路徑行使。農(nóng)用機(jī)械的導(dǎo)航系統(tǒng)可以包括定位、路徑規(guī)劃、 導(dǎo)航控制、機(jī)械模型和通信等模塊。其中的定位主要是根據(jù)相應(yīng)的傳感器檢測(cè)農(nóng)用機(jī)械的位置、姿態(tài)、速度、加速度等 信息，具體可以使用機(jī)械視覺、激光定位、全球定位系統(tǒng)(GlobalPositioning System,以下 簡(jiǎn)稱GPS)、慣性導(dǎo)航定位或電子羅盤定位的方法。同時(shí)也可以采用多種定位方法的融合， 充分利用多個(gè)傳感器資源實(shí)現(xiàn)檢測(cè)信息的合理支配和應(yīng)用。路徑規(guī)劃是指確定農(nóng)用機(jī)械的 路徑信息，包括路線的彎曲半徑、長(zhǎng)度等幾何信息以及農(nóng)用機(jī)械的航向角、前輪轉(zhuǎn)向角、地 面的傾斜度等信息。導(dǎo)航控制模塊用于控制農(nóng)用機(jī)械按照規(guī)劃的路線行駛。機(jī)械模型是為 研究對(duì)象建立的數(shù)學(xué)模型，能夠進(jìn)行系統(tǒng)仿真。通信模塊用于完成上述各個(gè)模塊之間的數(shù) 據(jù)交換。發(fā)明人在實(shí)現(xiàn)本發(fā)明的過程中發(fā)現(xiàn)，現(xiàn)有的農(nóng)用機(jī)械對(duì)導(dǎo)航系統(tǒng)存在跟蹤精度低 的缺點(diǎn)，容易引起較大的跟蹤誤差，無法實(shí)現(xiàn)精確導(dǎo)航。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種農(nóng)用機(jī)械的導(dǎo)航方法、導(dǎo)航裝置和農(nóng)用機(jī)械，以實(shí)現(xiàn)對(duì) 農(nóng)用機(jī)械的精確導(dǎo)航。為實(shí)現(xiàn)上述目的，本發(fā)明提供了一種農(nóng)用機(jī)械的導(dǎo)航方法，包括通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信 息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算 以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生 成路徑跟蹤最優(yōu)控制器；根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；根據(jù)所述路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取路徑調(diào)整信息并發(fā)送給自動(dòng) 轉(zhuǎn)向系統(tǒng)。本發(fā)明還提供了一種農(nóng)用機(jī)械的導(dǎo)航裝置，包括定位信息獲取模塊，用于通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、 速度和航向信息的定位信息；預(yù)期路徑計(jì)算模塊，用于根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；最優(yōu)控制器生成模塊，用于根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí) 間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑跟蹤最優(yōu)控制器；路徑誤差獲取模塊，用于根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取 路徑誤差；發(fā)送模塊，用于根據(jù)所述路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取調(diào)整信息并發(fā) 送給自動(dòng)轉(zhuǎn)向系統(tǒng)。本發(fā)明還提供了一種農(nóng)用機(jī)械，包括農(nóng)用機(jī)械本體和人工轉(zhuǎn)向系統(tǒng)，還包括導(dǎo)航裝置，用于通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航 向信 息的定位信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題 進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題 終端時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑跟蹤最優(yōu)控制器；根據(jù)所述農(nóng)用機(jī)械的定位信息和 所述預(yù)期路徑信息獲取路徑誤差；根據(jù)所述路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取路徑 調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)；自動(dòng)轉(zhuǎn)向系統(tǒng)，與導(dǎo)航裝置連接，用于根據(jù)導(dǎo)航裝置的調(diào)整信息對(duì)農(nóng)用機(jī)械進(jìn)行 控制。本發(fā)明上述實(shí)施例提供的農(nóng)用機(jī)械的導(dǎo)航方法、導(dǎo)航裝置和農(nóng)用機(jī)械，通過一個(gè) 以上的傳感器獲取農(nóng)用機(jī)械的定位信息，并根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制 理論中的最小時(shí)間問題計(jì)算獲取了農(nóng)用機(jī)械的預(yù)期路徑信息；根據(jù)上述信息可獲取農(nóng)用 機(jī)械的路徑誤差，最后根據(jù)上述的路徑誤差通過路徑跟蹤最優(yōu)控制器計(jì)算獲取路徑調(diào)整信 息，并將其發(fā)送給農(nóng)用機(jī)械上設(shè)置的自動(dòng)轉(zhuǎn)向系統(tǒng)，可實(shí)現(xiàn)對(duì)農(nóng)用機(jī)械的精確導(dǎo)航。


圖1為本發(fā)明農(nóng)用機(jī)械的導(dǎo)航方法實(shí)施例的流程示意圖；圖2為本發(fā)明農(nóng)用機(jī)械的運(yùn)動(dòng)示意圖；圖3為本發(fā)明農(nóng)用機(jī)械的導(dǎo)航裝置實(shí)施例的結(jié)構(gòu)示意圖；圖4為本發(fā)明農(nóng)用機(jī)械的導(dǎo)航裝置具體實(shí)施例的結(jié)構(gòu)示意圖；圖5為本發(fā)明農(nóng)用機(jī)械實(shí)施例的結(jié)構(gòu)示意圖；圖6為本發(fā)明實(shí)施例中轉(zhuǎn)向系統(tǒng)具體實(shí)施例示意圖。
具體實(shí)施例方式下面通過附圖和實(shí)施例，對(duì)本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)描述。本發(fā)明實(shí)施例提供了一種農(nóng)用機(jī)械的導(dǎo)航方法，圖1為本發(fā)明農(nóng)用機(jī)械的導(dǎo)航方 法實(shí)施例的流程示意圖，如圖1所示，包括如下步驟步驟101、通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信 息的定位信息；本步驟中一個(gè)或多個(gè)傳感器獲取農(nóng)用機(jī)械的位姿信息，包括位置、航向、 前輪轉(zhuǎn)向角及速度等數(shù)據(jù)，上述的傳感器可以包括全球定位系統(tǒng)(Global Positioning System,以下簡(jiǎn)稱GPS)、慣性測(cè)量單元(Inertia Measurement Unit,以下簡(jiǎn)稱IMU)以及 電子羅盤中的一個(gè)或多個(gè)，上述傳感器通過標(biāo)準(zhǔn)串口與工控機(jī)連接，并將獲取的數(shù)據(jù)存儲(chǔ)到工控機(jī)中，然后進(jìn)行信號(hào)轉(zhuǎn)換和處理即可得到農(nóng)用機(jī)械的位姿數(shù)據(jù)，例如包括數(shù)據(jù)由經(jīng) 緯度向大地坐標(biāo)的轉(zhuǎn)換，位姿數(shù)據(jù)的坐標(biāo)統(tǒng)一化等處理；上述的位姿數(shù)據(jù)是相對(duì)獨(dú)立的，需 要進(jìn)行數(shù)據(jù)融合以獲得最終的定位信息，本步驟中可以根據(jù)基于雙模糊控制器的自適應(yīng)卡 爾曼濾波算法對(duì)上述獲取的位姿數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，并獲取農(nóng)用機(jī)械的位置坐標(biāo)、速度和 航向信息的定位信息。例如可以取系統(tǒng)的狀態(tài)向量為X = [X，y，θ，α，ν]τ，其中X，y為 融合后的位置坐標(biāo)，θ為融合后的航向角，α為農(nóng)用機(jī)械的前輪轉(zhuǎn)向角，ν為農(nóng)用機(jī)械的行 駛速度；觀測(cè)向量為=Z = [xDE yDE xGPS yGPS ΘΒΕ θ c α ]τ，其中xDK，yDK和θ ^分別為由IMU 獲取的信息經(jīng)航位推算后得到的位置坐標(biāo)和航向角，Xgps, Ygps為GPS獲取的位置信息經(jīng)坐標(biāo) 轉(zhuǎn)換后的位置坐標(biāo)，θ。為電子羅盤獲取的航向角；
步驟102、根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題 進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；本步驟是根據(jù)兩輪運(yùn)動(dòng)學(xué)模型建立農(nóng)用機(jī)械的 狀態(tài)方程，并根據(jù)最優(yōu)控制器中的最小時(shí)間問題獲取農(nóng)用機(jī)械的預(yù)期路徑信息；步驟103、根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí)間無限調(diào)節(jié)器設(shè) 計(jì)準(zhǔn)則進(jìn)行路徑跟蹤最優(yōu)控制器設(shè)計(jì)；步驟104、根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；本 步驟是在上述步驟101和步驟102的基礎(chǔ)上，通過計(jì)算獲取路徑誤差；步驟105、根據(jù)所述路徑誤差信息以及步驟103設(shè)計(jì)的路徑跟蹤最優(yōu)控制器獲取 調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)，本步驟是在獲取到路徑誤差之后，即可計(jì)算得出相應(yīng)路 徑調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)，由自動(dòng)轉(zhuǎn)向系統(tǒng)控制農(nóng)用機(jī)械進(jìn)行路徑調(diào)整。本步驟 中的自動(dòng)轉(zhuǎn)向系統(tǒng)可以是在原人工轉(zhuǎn)向系統(tǒng)的基礎(chǔ)上并列設(shè)置，并且可以選擇使用其中的 自動(dòng)轉(zhuǎn)向系統(tǒng)或者是人工轉(zhuǎn)向系統(tǒng)。本發(fā)明上述實(shí)施例提供的農(nóng)用機(jī)械的導(dǎo)航方法，能夠根據(jù)獲取的農(nóng)用機(jī)械的定位 信息、預(yù)期路徑信息獲取路徑誤差，且上述的預(yù)期路徑信息為根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué) 模型以及最優(yōu)控制理論的最小時(shí)間問題計(jì)算方法獲??；并根據(jù)該路徑誤差及路徑跟蹤最優(yōu) 控制器獲得路徑調(diào)整信息，進(jìn)而將其發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)實(shí)現(xiàn)對(duì)農(nóng)用機(jī)械的調(diào)整。以下是本發(fā)明一個(gè)具體實(shí)施例，在GPS、IMU和電子羅盤獲取到位置、航向角等數(shù) 據(jù)信息后，利用自適應(yīng)卡爾曼濾波器對(duì)上述的數(shù)據(jù)信息進(jìn)行融合，首先建立狀態(tài)方程模型， 此時(shí)假設(shè)農(nóng)用機(jī)械在平面上行駛并且不考慮車輪與地面的作用，認(rèn)為左右部分對(duì)稱無傾 斜、俯仰、側(cè)滑等運(yùn)動(dòng)，則農(nóng)用機(jī)械的運(yùn)動(dòng)可以看作是一種平面的剛體的平動(dòng)與旋轉(zhuǎn)運(yùn)動(dòng)的 二輪模型，如圖2所述示，其中θ為航向角，α為前輪轉(zhuǎn)向角，1為軸距，ν為車速，沿x、y 軸可以分解為Vx和vy，vf為前輪速度。則其運(yùn)動(dòng)學(xué)模型為
V
^ = — tan a I= v cos a ；> =ν sin a并取系統(tǒng)的狀態(tài)變量為X = [x,y, θ , α，ν]τ，其中x，y為融合后的位置坐標(biāo)，θ 為融合后的航向角，α為農(nóng)用機(jī)械的前輪轉(zhuǎn)角的角度，ν為農(nóng)用機(jī)械的行駛速度。因此可得 系統(tǒng)的狀態(tài)方程為 其中，τ α和τ ν為時(shí)間常數(shù)，Wa和Wv分別為農(nóng)用機(jī)械前輪轉(zhuǎn)角和速度的噪聲。將 狀態(tài)方程離散化，得到系統(tǒng)離散的狀態(tài)方程為Xk = f[Xk_i; k-U+ff^上式中，k為離散時(shí)各向量的索引數(shù)，Wlri = W，0，0，wa，wv]T。根據(jù)卡爾曼(Kalman)濾波原理可以得到 _4] Xk=上式中 其中，T為濾波器的遞推時(shí)間間隔。在上述步驟基礎(chǔ)上，可以建立量測(cè)方程，取量測(cè)向量為 可建立量測(cè)方程Zk = HXk+Vk上式中H為觀測(cè)矩陣，可表示為 其中，ν為傳感器的觀測(cè)噪聲，近似為(0，σ 2)的高斯白噪聲。再次利用Kalman濾波算法進(jìn)行處理，該算法可包括如下的流程狀態(tài)一步預(yù)測(cè)方程Xkk_,
一步預(yù)測(cè)誤差方差陣 濾波增益矩陣 狀態(tài)估計(jì)方程xk =xk^+kkzk 估計(jì)誤差方差陣 在上述對(duì)數(shù)據(jù)信息進(jìn)行融合后，即可采用模糊邏輯控制器對(duì)卡爾曼濾波器的增益 矩陣K、測(cè)量誤差協(xié)方差R以及觀測(cè)誤差協(xié)方差Q進(jìn)行實(shí)時(shí)修正，將卡爾曼濾波其調(diào)整到最 優(yōu)狀態(tài)。通過兩個(gè)模糊控制器分別確定濾波增益、濾波噪聲的調(diào)整系統(tǒng)μ ^和系統(tǒng)噪聲的 調(diào)整系數(shù)μ ,，具體包括如下的步驟首先，建立濾波器增益Kk和測(cè)量噪聲R(t)系數(shù)模糊控制器；濾波器增益Kk和測(cè)量噪聲R(t)都受到傳感器信號(hào)質(zhì)量的影響，它們同GPS、航位 推算兩種定位方式獲得的位置差值A(chǔ)y和GPS的位置精度因子間存在一定的模糊關(guān)系。該 模糊控制器的輸入為GPS、航位推算兩種定位方式獲得的位置差A(yù)y和GPS的位置精度因 子，輸出為濾波增益、測(cè)量噪聲的調(diào)整系數(shù)μ — 其次，建立基于新息向量Zk變化的Q(t)系數(shù)模糊控制器；Q(t)是系統(tǒng)的過程噪聲的方程強(qiáng)度陣，受到系統(tǒng)過程噪聲的影響，該值不易直接 測(cè)得，常通過對(duì)各預(yù)測(cè)信號(hào)的系統(tǒng)噪聲估計(jì)得到。然而這種估計(jì)是建立在新息是零均值白 噪聲的基礎(chǔ)上的。由于新息是觀測(cè)值與預(yù)測(cè)值的差，故當(dāng)濾波器處于最優(yōu)狀態(tài)時(shí)，新息的值 是一組零均值的白噪聲。而在田間作業(yè)過程中，由于受到各種干擾的影響，新息的值會(huì)夾雜 不同程度的有色噪聲，新息的值越大，說明此時(shí)系統(tǒng)的觀測(cè)值和預(yù)測(cè)值差異越大，即系統(tǒng)的 過程噪聲越大，導(dǎo)致Q(t)的值也越大，因此Q(t)的值與新息向量Zk的大小存在一定的模 糊關(guān)系。建立基于新息變化的模糊控制器，對(duì)Q(t)的值進(jìn)行實(shí)時(shí)在線調(diào)整，另卡爾曼濾波 器始終處于最優(yōu)狀態(tài)。該模糊控制器的輸入為橫向偏差與航向角的新息值Z(X)，Z(y)和 2(9)，輸出為000，0(7)和Q(9)。則過程噪聲Q(t)的調(diào)整系數(shù)矩陣為Pq為
上述實(shí)施例中采用具有雙模糊控制器的卡爾曼濾波算法對(duì)個(gè)傳感器獲取的數(shù)據(jù) 進(jìn)行了融合，獲取了包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息。對(duì)于農(nóng)用機(jī)械的導(dǎo)航系統(tǒng)來說，除了要獲取上述的農(nóng)用機(jī)械的位置信息，還需要 獲取農(nóng)用機(jī)械理想的路徑，即預(yù)期路徑信息，本實(shí)施例中首先由農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模 型示意圖獲取農(nóng)用機(jī)械的狀態(tài)方程，即 其中狀態(tài)量為X = [x，y，θ , a]T;u為要求解的控制量。在本實(shí)施例中采用最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行預(yù)期路徑規(guī)劃，其性能指 標(biāo)如下 上式中XQ= [x0,y0, θ。，α Jt為農(nóng)用機(jī)械要到達(dá)的目標(biāo)點(diǎn)；α _為轉(zhuǎn)向系統(tǒng)的最 大轉(zhuǎn)向角；r為懲罰因子；tf為終端時(shí)間。根據(jù)目標(biāo)點(diǎn)信息，解上述最小時(shí)間問題即可獲取農(nóng)用機(jī)械的預(yù)期路徑信息，包括 位置信息，航向信息和轉(zhuǎn)向信息。將拖拉機(jī)當(dāng)前點(diǎn)Q的狀態(tài)量和目標(biāo)點(diǎn)P的狀態(tài)量的差表示為 對(duì)上述狀態(tài)方程進(jìn)行變分得到如下的狀態(tài)方程 狀態(tài)量為δχ=[δχ，δγ，δθ，δα]τ， Su = Sd ,其中 上式中的氏是規(guī)劃的路徑中包含的信息，因此最優(yōu)控制器的參數(shù)隨路徑上的參 考點(diǎn)變化而變化。在本實(shí)施例中采用最優(yōu)控制理論中的終端無限時(shí)間調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則設(shè)計(jì)導(dǎo)航跟蹤最優(yōu)控制器，上述設(shè)計(jì)準(zhǔn)則如下 其中，矩陣Q為對(duì)角陣，R為常數(shù)，反映前輪轉(zhuǎn)向角和拖拉機(jī)速度的權(quán)重?？傻贸鯮iccati代數(shù)方程為PA+ATP-PBR_1BTP+Q = O解上述矩陣方程即可求得路徑跟蹤最優(yōu)控制器 圖3為本發(fā)明農(nóng)用機(jī)械的導(dǎo)航裝置實(shí)施例的結(jié)構(gòu)示意圖，如圖3所示，農(nóng)用機(jī)械的 導(dǎo)航裝置包括定位信息獲取模塊11、預(yù)期路徑計(jì)算模塊12、最優(yōu)控制器生成模塊13、路徑 誤差獲取模塊14和發(fā)送模塊15，其中定位信息獲取模塊11用于通過一個(gè)以上的傳感器獲 取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息；預(yù)期路徑計(jì)算模塊12用于根據(jù) 農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算以獲取農(nóng)用機(jī) 械的預(yù)期路徑信息；最優(yōu)控制器生成模塊13用于根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟 蹤問題終端時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑跟蹤最優(yōu)控制器；路徑誤差獲取模塊14用 于根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；發(fā)送模塊15用于根 據(jù)所述路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)。本實(shí)施例提供的農(nóng)用機(jī)械的導(dǎo)航裝置，通過農(nóng)用機(jī)械的定位信息和預(yù)期路徑信息 獲取路徑誤差，并根據(jù)路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取路徑調(diào)整信息并發(fā)送給自 動(dòng)轉(zhuǎn)向系統(tǒng)。其中預(yù)期路徑信息為預(yù)期路徑計(jì)算模塊根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及 最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算獲得；路徑跟蹤最優(yōu)控制器根據(jù)最優(yōu)控制理論中 的終端無限時(shí)間調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則進(jìn)行設(shè)計(jì)獲得。上述實(shí)施例中的定位信息獲取模塊包括一個(gè)以上的傳感器及計(jì)算單元，所述傳感 器用于獲取農(nóng)用機(jī)械的位姿數(shù)據(jù)；所述計(jì)算單元用于根據(jù)基于雙模糊控制器的自適應(yīng)卡爾 曼濾波算法對(duì)上述獲取的位姿數(shù)據(jù)進(jìn)行數(shù)據(jù)融合以獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和 航向信息的定位信息。圖4為本發(fā)明農(nóng)用機(jī)械的導(dǎo)航裝置具體實(shí)施例的結(jié)構(gòu)示意圖，如圖4所示，其中 GPS、IMU以及電子羅盤獲取農(nóng)用機(jī)械的位姿數(shù)據(jù)，經(jīng)過計(jì)算單元進(jìn)行上述數(shù)據(jù)信息的融合 處理獲取農(nóng)用機(jī)械的定位信息，并且預(yù)期路徑獲取模塊進(jìn)行計(jì)算獲取預(yù)期的路徑信息，通 過對(duì)上述的信息進(jìn)行分析，經(jīng)過路徑跟塊進(jìn)行計(jì)算獲取預(yù)期的路徑信息，通過對(duì)上述的信 息進(jìn)行分析，經(jīng)過路徑跟蹤最優(yōu)控制器可以獲取對(duì)農(nóng)用機(jī)械的調(diào)整信息，可將該調(diào)整信息 發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)實(shí)現(xiàn)調(diào)整，角位移傳感器能夠?qū)C(jī)械裝置的角度變化為電壓信號(hào)。圖5為本發(fā)明農(nóng)用機(jī)械實(shí)施例的結(jié)構(gòu)示意圖，如圖5所示，包括農(nóng)用機(jī)械本體21、 人工轉(zhuǎn)向系統(tǒng)22、導(dǎo)航裝置23和自動(dòng)轉(zhuǎn)向系統(tǒng)24，其中導(dǎo)航裝置23用于通過一個(gè)以上的 傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息；根據(jù)農(nóng)用機(jī)械的兩輪 運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信 息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑 跟蹤最優(yōu)控制器；根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；根據(jù) 所述路徑誤差信息獲取調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)；自動(dòng)轉(zhuǎn)向系統(tǒng)24與導(dǎo)航裝置23連接，用于根據(jù)導(dǎo)航裝置的調(diào)整信息對(duì)農(nóng)用機(jī)械進(jìn)行控制。本實(shí)施例提供的農(nóng)用機(jī)械，其中的導(dǎo)航裝置根據(jù)獲取的農(nóng)用機(jī)械的定位信息以及 計(jì)算獲取的農(nóng)用機(jī)械的預(yù)期路徑信息獲取路徑誤差，并根據(jù)路徑誤差獲取調(diào)整信息，由自 動(dòng)轉(zhuǎn)向系統(tǒng)對(duì)農(nóng)用機(jī)械進(jìn)行控制，能夠?qū)崿F(xiàn)對(duì)農(nóng)用機(jī)械的精確導(dǎo)航。上述的自動(dòng)轉(zhuǎn)向系統(tǒng)包括油泵、電磁換向閥、驅(qū)動(dòng)模塊和步進(jìn)電機(jī)，其中用于為自 動(dòng)轉(zhuǎn)向系統(tǒng)提供轉(zhuǎn)向動(dòng)力；電磁換向閥用于進(jìn)行人工轉(zhuǎn)向系統(tǒng)和自動(dòng)轉(zhuǎn)向系統(tǒng)的切換，選 擇自動(dòng)控制系統(tǒng)作為農(nóng)用機(jī)械的控制系統(tǒng)；驅(qū)動(dòng)模塊用于根據(jù)接收到的調(diào)整信息生成控制 信號(hào)；步進(jìn)電機(jī)與所述驅(qū)動(dòng)模塊連接，用于根據(jù)控制信號(hào)調(diào)整液壓轉(zhuǎn)向器轉(zhuǎn)向和轉(zhuǎn)向速度。
圖6為本發(fā)明實(shí)施例中轉(zhuǎn)向系統(tǒng)具體實(shí)施例示意圖，其中包括并列設(shè)置的自動(dòng)轉(zhuǎn) 向系統(tǒng)1與人工轉(zhuǎn)向系統(tǒng)2，從而可以選擇使用其中的一個(gè)轉(zhuǎn)向系統(tǒng)進(jìn)行轉(zhuǎn)向。具體的在 實(shí)施過程中，可以由電磁換向閥3根據(jù)控制器的指令進(jìn)行人工裝箱模式或自動(dòng)換向模式的 切換，具體的可以是一個(gè)二位三通電磁換向閥實(shí)現(xiàn)，該二位三通電磁換向閥與油泵4配合 使用，油泵4是轉(zhuǎn)向系統(tǒng)的動(dòng)力源，為轉(zhuǎn)向控制系統(tǒng)供給具有壓力的液壓油，油泵的技術(shù)參 數(shù)可以設(shè)置為額定壓力lOMpa，排量14ml/r，最大工作壓力12. 5Mpa，安全閥調(diào)整范圍5 12. 5Mpa，額定轉(zhuǎn)速2000r/min，轉(zhuǎn)速范圍800 3000r/min，輸出流量為12、14或16L/min ； 油泵的主要技術(shù)參數(shù)可以設(shè)為工作電壓12V，功率30W，最高壓力35Mpa，流量50L/min ；在 具體實(shí)施過程中，可以在電磁換向閥斷電時(shí)，閥芯在彈簧力的作用下處于右位，油泵的液壓 油流向原人工轉(zhuǎn)向器，此時(shí)人工轉(zhuǎn)向系統(tǒng)起作用；當(dāng)加裝的轉(zhuǎn)向器處于中位時(shí)，它與油源的 連接電磁換向閥斷開，與轉(zhuǎn)向油缸的連接被自身特定結(jié)構(gòu)斷開，處于隔離狀態(tài)，不起轉(zhuǎn)向作 用。當(dāng)上述的電磁換向閥通電時(shí)，電磁換向閥的閥芯處于左位，液壓油流向加裝的自動(dòng)轉(zhuǎn) 向系統(tǒng)，此時(shí)由步進(jìn)電機(jī)驅(qū)動(dòng)自動(dòng)轉(zhuǎn)向系統(tǒng)實(shí)現(xiàn)自動(dòng)轉(zhuǎn)向功能，同理，原轉(zhuǎn)向器處于隔離狀 態(tài)，不起轉(zhuǎn)向作用。調(diào)整步進(jìn)電機(jī)的轉(zhuǎn)速實(shí)現(xiàn)轉(zhuǎn)向輪轉(zhuǎn)向速度控制，控制步進(jìn)電機(jī)的轉(zhuǎn)向?qū)?現(xiàn)轉(zhuǎn)向輪的轉(zhuǎn)向控制。自動(dòng)轉(zhuǎn)向系統(tǒng)的液壓轉(zhuǎn)向器5由步進(jìn)電機(jī)6驅(qū)動(dòng)，根據(jù)步進(jìn)電機(jī)的轉(zhuǎn)向、轉(zhuǎn)角大 小，定向、定量的將液壓油輸入轉(zhuǎn)向油缸7左腔或右腔，推動(dòng)導(dǎo)向輪駛向動(dòng)力轉(zhuǎn)向，另一腔 的油則經(jīng)過轉(zhuǎn)向器回油箱8。自動(dòng)轉(zhuǎn)向系統(tǒng)可選用與人工轉(zhuǎn)向系統(tǒng)相同的液壓轉(zhuǎn)向器9， 以便于系統(tǒng)匹配，即能夠滿足原液壓轉(zhuǎn)向系統(tǒng)壓力、流量和溫度等性能的要求，例如可選用 具有如下技術(shù)參數(shù)的液壓轉(zhuǎn)向器轉(zhuǎn)動(dòng)扭矩4. O 5. ON · m,流量7. 51/min,排量IOOml/ r，額定工作壓力16Mpa，最大瞬時(shí)背壓6. 3Mpa，最大連續(xù)背壓2. 5Mpa，系統(tǒng)工作溫度-20 80 "C。全液壓轉(zhuǎn)向器地轉(zhuǎn)向扭矩的大小直接決定了所需步進(jìn)電機(jī)地功率，如果液壓轉(zhuǎn)向 器的扭矩過大，受車載電源電壓的限制能夠滿足功率要求的步進(jìn)電機(jī)很難選擇，全液壓轉(zhuǎn)
向器的額定轉(zhuǎn)速η可以如下的公式計(jì)算獲得"=|·，其中q為液壓轉(zhuǎn)向器的流量，V為液壓 轉(zhuǎn)向器的排量，根據(jù)上述參數(shù)的液壓轉(zhuǎn)向器，則液壓轉(zhuǎn)向器的額定轉(zhuǎn)速《 = 2 = g = 75rpm。在
ν 0.1
液壓轉(zhuǎn)向器以額定轉(zhuǎn)速轉(zhuǎn)動(dòng)時(shí)步進(jìn)電機(jī)的功率ρ由如下公式計(jì)算P =Χ 150% = 2^rx75x5 X150% = 98.125W L 」 6077s"m 60x0.75x0.8其中液壓轉(zhuǎn)向器的額定轉(zhuǎn)速η為75rpm，轉(zhuǎn)動(dòng)扭矩M為5Ν·πι，步進(jìn)電機(jī)的效率ns為75%，機(jī)械傳動(dòng)效率1^為80%，因步進(jìn)電機(jī)過載能力差，選擇功率余量為50%。步進(jìn)電 機(jī)的工作電壓選擇12伏的直流電壓，功率為99W時(shí)，步進(jìn)電機(jī)的工作電流I為 P 99 在上述實(shí)施例中，步進(jìn)電機(jī)的電流需要達(dá)到8. 25A。本發(fā)明實(shí)施例中還可以進(jìn)一步 減少轉(zhuǎn)向器中復(fù)位彈簧片的數(shù)量，將其復(fù)位彈簧片減至兩片，將液壓轉(zhuǎn)向器的轉(zhuǎn)動(dòng)扭矩降 到0. 8N · m以下，能夠降低了轉(zhuǎn)向所需要的扭矩。改裝后步進(jìn)電機(jī)的功率為 計(jì)算可得其電流I為1.4A，從而能夠更好的選擇合適的步進(jìn)電機(jī)，具體的可選 擇具有如下技術(shù)參數(shù)的步進(jìn)電機(jī)步距角1. 8°，相數(shù)4，電壓4. 4V，電流2A，最大靜轉(zhuǎn)矩 2. IN · m，轉(zhuǎn)動(dòng)慣量 2Kg · cm2，阻抗 2. 2士 lOohm，重量 2. 2Kg。角位移傳感器能夠?qū)C(jī)械裝置的角度變化為電壓信號(hào)，在本系統(tǒng)中利用角位移轉(zhuǎn) 向器測(cè)量轉(zhuǎn)向輪的轉(zhuǎn)向角度，并且角位置傳感器的精度直接決定著控制系統(tǒng)的轉(zhuǎn)向精度， 是農(nóng)用機(jī)械自動(dòng)駕駛控制系統(tǒng)控制精度的基礎(chǔ)，需要選擇具有較高精度的角位移傳感器， 具體的可以參考選擇具有如下技術(shù)參數(shù)的角位移傳感器電壓12VDC，機(jī)械轉(zhuǎn)角360連續(xù)可 轉(zhuǎn)，線性量程0 120，輸出信號(hào)0 5V，分辨率為連續(xù)，精度0. 1，相對(duì)濕度95士3% RH，使 用溫度-30 80°C，抗電等級(jí)500V，防護(hù)等級(jí)IP64。本發(fā)明上述實(shí)施例提供的農(nóng)用機(jī)械的導(dǎo)航方法、導(dǎo)航裝置和農(nóng)業(yè)機(jī)械，能夠通過 獲取農(nóng)用機(jī)械的定位信息和預(yù)期路徑，并根據(jù)上述定位信息和預(yù)期路徑信息計(jì)算得出調(diào)整 信息，對(duì)農(nóng)用機(jī)械的運(yùn)行進(jìn)行導(dǎo)航，并能夠提高導(dǎo)航的精確度。最后應(yīng)說明的是以上實(shí)施例僅用以說明本發(fā)明的技術(shù)方案而非對(duì)其進(jìn)行限制， 盡管參照較佳實(shí)施例對(duì)本發(fā)明進(jìn)行了詳細(xì)的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解其依 然可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換，而這些修改或者等同替換亦不能使修 改后的技術(shù)方案脫離本發(fā)明技術(shù)方案的精神和范圍。
權(quán)利要求
一種農(nóng)用機(jī)械的導(dǎo)航方法，其特征在于，包括通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑跟蹤最優(yōu)控制器；根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；根據(jù)所述路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取路徑調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)。
2.根據(jù)權(quán)利要求1所述的農(nóng)用機(jī)械的導(dǎo)航方法，其特征在于，所述通過一個(gè)以上的傳 感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息包括從一個(gè)以上的傳感器中獲取農(nóng)用機(jī)械的位姿數(shù)據(jù)；根據(jù)基于雙模糊控制器的自適應(yīng)卡爾曼濾波算法對(duì)上述獲取的位姿數(shù)據(jù)進(jìn)行數(shù)據(jù)融 合以獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息。
3.根據(jù)權(quán)利要求1所述的農(nóng)用機(jī)械的導(dǎo)航方法，其特征在于，所述最優(yōu)控制器隨機(jī)械 在路徑中位置的變化而變化。
4.根據(jù)權(quán)利要求1所述的農(nóng)用機(jī)械的導(dǎo)航方法，其特征在于，還包括自動(dòng)轉(zhuǎn)向系統(tǒng)的驅(qū)動(dòng)模塊根據(jù)調(diào)整信息生成調(diào)整信號(hào)，并將所述調(diào)整信號(hào)發(fā)送給液壓 轉(zhuǎn)向器，由所述液壓轉(zhuǎn)向器驅(qū)動(dòng)轉(zhuǎn)向輪進(jìn)行轉(zhuǎn)向。
5.一種農(nóng)用機(jī)械的導(dǎo)航裝置，其特征在于，包括定位信息獲取模塊，用于通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度 和航向信息的定位信息；預(yù)期路徑計(jì)算模塊，用于根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小 時(shí)間問題進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；最優(yōu)控制器生成模塊，用于根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí)間無 限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑跟蹤最優(yōu)控制器；路徑誤差獲取模塊，用于根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑 誤差；發(fā)送模塊，用于根據(jù)所述路徑誤差信息及所述路徑跟蹤最優(yōu)控制器獲取調(diào)整信息并發(fā) 送給自動(dòng)轉(zhuǎn)向系統(tǒng)。
6.根據(jù)權(quán)利要求5所述的農(nóng)用機(jī)械的導(dǎo)航裝置，其特征在于，所述定位信息獲取模塊 包括一個(gè)以上的傳感器及計(jì)算單元，所述傳感器用于獲取農(nóng)用機(jī)械的位姿數(shù)據(jù)；所述計(jì)算 單元用于根據(jù)基于雙模糊控制器的自適應(yīng)卡爾曼濾波算法對(duì)上述獲取的位姿數(shù)據(jù)進(jìn)行數(shù) 據(jù)融合以獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息。
7.一種農(nóng)用機(jī)械，包括農(nóng)用機(jī)械本體和人工轉(zhuǎn)向系統(tǒng)，其特征在于，還包括導(dǎo)航裝置，用于通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信 息的定位信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行 計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端 時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則生成路徑跟蹤最優(yōu)控制器；根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；根據(jù)所述路徑誤差信息及路徑跟蹤最優(yōu)控制器獲取路徑調(diào)整 信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)；自動(dòng)轉(zhuǎn)向系統(tǒng)，與導(dǎo)航裝置連接，用于根據(jù)導(dǎo)航裝置的調(diào)整信息對(duì)農(nóng)用機(jī)械進(jìn)行控制。
8.根據(jù)權(quán)利要求7所述的農(nóng)用機(jī)械，其特征在于，所述自動(dòng)轉(zhuǎn)向系統(tǒng)包括 油泵，用于為自動(dòng)轉(zhuǎn)向系統(tǒng)提供轉(zhuǎn)向動(dòng)力；電磁換向閥，用于進(jìn)行人工轉(zhuǎn)向系統(tǒng)和自動(dòng)轉(zhuǎn)向系統(tǒng)的切換，選擇自動(dòng)控制系統(tǒng)作為 農(nóng)用機(jī)械的控制系統(tǒng)；驅(qū)動(dòng)模塊，用于根據(jù)接收到的導(dǎo)航裝置發(fā)送的調(diào)整信息生成控制信號(hào)； 步進(jìn)電機(jī)，與所述驅(qū)動(dòng)模塊連接，用于根據(jù)控制信號(hào)調(diào)整液壓轉(zhuǎn)向器轉(zhuǎn)向和轉(zhuǎn)向速度， 控制轉(zhuǎn)向輪進(jìn)行轉(zhuǎn)向。
9.根據(jù)權(quán)利要求8所述的農(nóng)用機(jī)械，其特征在于，還包括角位移傳感器，用于測(cè)量轉(zhuǎn)向輪的轉(zhuǎn)向角度，并將測(cè)量結(jié)果發(fā)送給導(dǎo)航裝置。
全文摘要
本發(fā)明提供了一種農(nóng)用機(jī)械的導(dǎo)航方法、導(dǎo)航裝置和農(nóng)用機(jī)械，其中農(nóng)用機(jī)械的導(dǎo)航方法包括通過一個(gè)以上的傳感器獲取包括農(nóng)用機(jī)械的位置坐標(biāo)、速度和航向信息的定位信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及最優(yōu)控制理論中的最小時(shí)間問題進(jìn)行計(jì)算以獲取農(nóng)用機(jī)械的預(yù)期路徑信息；根據(jù)農(nóng)用機(jī)械的兩輪運(yùn)動(dòng)學(xué)模型以及跟蹤問題終端時(shí)間無限調(diào)節(jié)器設(shè)計(jì)準(zhǔn)則設(shè)計(jì)了路徑跟蹤最優(yōu)控制器；根據(jù)所述農(nóng)用機(jī)械的定位信息和所述預(yù)期路徑信息獲取路徑誤差；根據(jù)所述路徑誤差信息以及路徑跟蹤最優(yōu)控制器獲取路徑調(diào)整信息并發(fā)送給自動(dòng)轉(zhuǎn)向系統(tǒng)。本發(fā)明實(shí)施例還提供了農(nóng)用機(jī)械的導(dǎo)航裝置以及具有上述導(dǎo)航裝置的農(nóng)用機(jī)械。上述方法、裝置和設(shè)備能夠提高導(dǎo)航的精確度。
文檔編號(hào)G05D1/00GK101866181SQ20091008246
公開日2010年10月20日 申請(qǐng)日期2009年4月16日 優(yōu)先權(quán)日2009年4月16日
發(fā)明者劉剛, 宋曉波, 宋正河, 張漫, 朱忠祥, 毛恩榮, 王尚俊, 謝斌 申請(qǐng)人:中國(guó)農(nóng)業(yè)大學(xué)