基于大型稀疏線性方程組拓?fù)涞耐侠瓩C(jī)精確定位系統(tǒng)

王發(fā)興，趙衛(wèi)濱，蔣 晶

(南京郵電大學(xué) 通達(dá)學(xué)院，江蘇 揚(yáng)州 225127)

0 引言

拖拉機(jī)自動導(dǎo)航系統(tǒng)的設(shè)計(jì)重點(diǎn)在于使拖拉機(jī)利用導(dǎo)航定位系統(tǒng)完成自動定位的同時可以按照預(yù)定義的路徑實(shí)現(xiàn)自行走作業(yè)。其中，拖拉機(jī)實(shí)時位置的判斷以及行駛方向的控制算法是設(shè)計(jì)的重點(diǎn)，通過位置判斷和路徑規(guī)劃，最終完成農(nóng)業(yè)作業(yè)。隨著農(nóng)機(jī)現(xiàn)代化快速發(fā)展，拖拉機(jī)自動精確導(dǎo)航受到越來越多的關(guān)注，但導(dǎo)航系統(tǒng)面臨著存儲和處理能力與數(shù)據(jù)、應(yīng)用功能的快速增長之間的巨大矛盾，受到機(jī)載PC處理器的限制，數(shù)據(jù)處理系統(tǒng)的能力越來越趕不上應(yīng)用的需求。因此，研究新的數(shù)據(jù)處理算法，實(shí)現(xiàn)實(shí)時導(dǎo)航數(shù)據(jù)，高速而準(zhǔn)確的處理，對于現(xiàn)代農(nóng)機(jī)精準(zhǔn)導(dǎo)航的研究具有重要的意義。

1 基于數(shù)據(jù)稀疏處理和方程拓?fù)涞耐侠瓩C(jī)導(dǎo)航感知系統(tǒng)

拖拉機(jī)的導(dǎo)航感知系統(tǒng)主要是利用傳感器和GPS導(dǎo)航定位原理，通過預(yù)設(shè)導(dǎo)航線和誤差校正，實(shí)現(xiàn)拖拉機(jī)的自主導(dǎo)航。目前，導(dǎo)航感知系統(tǒng)中常用的定位方法和工具主要有GPS導(dǎo)航和傳感器、機(jī)器視覺、慣性導(dǎo)航、激光導(dǎo)航，以及多種導(dǎo)航的組合。本次主要采用GPS導(dǎo)航和傳感器，通過在農(nóng)田中預(yù)設(shè)導(dǎo)航線，利用大地坐標(biāo)和導(dǎo)航定位點(diǎn)的坐標(biāo)轉(zhuǎn)換，實(shí)現(xiàn)拖拉機(jī)導(dǎo)航的自主感知，其導(dǎo)航流程如圖1所示。

圖1 拖拉機(jī)精確導(dǎo)航流程圖

精準(zhǔn)導(dǎo)航采用預(yù)設(shè)導(dǎo)航線的方法，利用路徑擬合原理實(shí)現(xiàn)自主導(dǎo)航，在實(shí)時導(dǎo)航過程中，通過導(dǎo)航控制器實(shí)時調(diào)整定位誤差，以提高導(dǎo)航的精度。

2 基于大型稀疏線性方程組拓?fù)涞耐侠瓩C(jī)精確定位模型

我國目前采用的地圖是高斯-克呂格投影，該方法是由德國數(shù)學(xué)家高斯提出并由德國大地測量學(xué)家克呂格補(bǔ)充。在拖拉機(jī)定位時，假設(shè)拖拉機(jī)所在處經(jīng)緯度的弧度為(P,Q)，(x,y)分別表示橫坐標(biāo)和縱坐標(biāo)，可以利用高斯投影變換完成坐標(biāo)轉(zhuǎn)換，即

(1)

式(1)中，有

(2)

其中

(3)

由于地球?yàn)闄E圓形，式(3)中a表示長半徑；b表示短半徑；e表示地球的第二子午偏心率；N表示地球某一點(diǎn)的卯酉圈曲率半徑，其表達(dá)式為

(4)

X0的表達(dá)式為

X0=A0P-
cosP[A1sinP+A2(sinP)3+A3(sinP)5](5)

其中，A0、A1、A2、A3表示常數(shù)。利用GPS導(dǎo)航的坐標(biāo)轉(zhuǎn)換，可以實(shí)現(xiàn)對拖拉機(jī)的路徑跟蹤，以便使其實(shí)現(xiàn)直線行走和曲線行走，本次研究以直線行走為研究對象。在實(shí)際導(dǎo)航時，可以設(shè)置一條導(dǎo)航線，假設(shè)該導(dǎo)航線為AB線，導(dǎo)航拖拉機(jī)的行駛方向?yàn)镋F，拖拉機(jī)的實(shí)際位置標(biāo)記為S，S'表示S到直線AB的投影，NM表示過S的平行線。其中，S到AB的距離為路徑追蹤偏差，∠FSM表示路徑追蹤偏差角，如圖2所示。

假設(shè)基線AB的端點(diǎn)的坐標(biāo)分別為A(x1,y1)，B(x2,y2)，其直線方程為

Ax+By+C=0

(6)

通過推導(dǎo)可得

(7)

路徑追蹤的橫向偏差可以寫成

(8)

圖2 拖拉機(jī)追蹤路徑偏差示意圖

利用GPS導(dǎo)航坐標(biāo)轉(zhuǎn)換，可以實(shí)時測量橫縱坐標(biāo)(x,y)，然后求出路徑追蹤的導(dǎo)航偏差，進(jìn)而實(shí)現(xiàn)自主導(dǎo)航功能；但是，在坐標(biāo)轉(zhuǎn)換時會存在大量的數(shù)據(jù)，數(shù)據(jù)以矩陣的形式存儲，如果不做稀疏處理，其計(jì)算量非常大。因此，利用線性方程組對數(shù)據(jù)進(jìn)行拓?fù)?，得到?shù)據(jù)處理的稀疏矩陣，再利用DGPMHSS迭代方法完成數(shù)據(jù)的處理。首先對導(dǎo)航坐標(biāo)轉(zhuǎn)換數(shù)據(jù)進(jìn)行稀疏處理可以得到線性方程組，即

(9)

(10)

(11)

利用這種方法可以大大提升數(shù)據(jù)的處理速度，進(jìn)而提高拖拉機(jī)的導(dǎo)航實(shí)時性、可靠性和導(dǎo)航效率。

3 拖拉機(jī)精確導(dǎo)航數(shù)值計(jì)算和定位精度實(shí)驗(yàn)研究

在實(shí)際農(nóng)田作業(yè)時，由于拖拉機(jī)本身控制系統(tǒng)的延遲性較大，因此在實(shí)驗(yàn)時需要在普通的控制系統(tǒng)中加入PC處理器，以提高系統(tǒng)的數(shù)據(jù)處理速度，并運(yùn)用第2節(jié)的處理算法，加速數(shù)據(jù)的收斂。實(shí)驗(yàn)系統(tǒng)的基本模型如圖3所示。

圖3 實(shí)驗(yàn)系統(tǒng)模型

該系統(tǒng)模型中利用PC處理器對GPS接收到的海量數(shù)據(jù)進(jìn)行處理，采用CAN總線將GPS數(shù)據(jù)接收設(shè)備和機(jī)器鏈接，在短距離通信時使用Bluetooth無線通信的形式，在長距離時需要借助于GPRS來實(shí)現(xiàn)數(shù)據(jù)傳輸。

圖4表示拖拉機(jī)實(shí)際的導(dǎo)航實(shí)驗(yàn)。本次實(shí)驗(yàn)選擇在地勢較為平坦的寬闊農(nóng)田中進(jìn)行，在拖拉機(jī)上安裝了導(dǎo)航設(shè)備和PC處理器，并利用無線通信傳輸數(shù)據(jù)，以便可以在遠(yuǎn)程端實(shí)時的輸出結(jié)果曲線。

圖5表示數(shù)據(jù)稀疏化和DGPMHSS迭代前實(shí)時曲線。由圖5可以看出：在指定的時間范圍內(nèi)，利用本實(shí)驗(yàn)系統(tǒng)可以有效地輸出導(dǎo)航的數(shù)據(jù)轉(zhuǎn)換后的誤差曲線。為了進(jìn)行對比，輸出了數(shù)據(jù)稀疏化和DGPMHSS迭代后實(shí)驗(yàn)實(shí)時曲線。

圖6表示數(shù)據(jù)稀疏化和DGPMHSS迭代后實(shí)時曲線。

圖4 拖拉機(jī)導(dǎo)航實(shí)驗(yàn)

圖5 數(shù)據(jù)稀疏化和DGPMHSS迭代前實(shí)時曲線

由圖6可以看出：通過數(shù)據(jù)處理后，冗余數(shù)據(jù)大大減少，從而可以有效地提高數(shù)據(jù)的處理速度。將實(shí)驗(yàn)數(shù)據(jù)進(jìn)行統(tǒng)計(jì)，得到了如表1所示的統(tǒng)計(jì)結(jié)果。

圖6 處理后數(shù)據(jù)實(shí)時曲線

對比項(xiàng)目迭代步數(shù)CPU執(zhí)行時間/s計(jì)算精度/%稀疏和迭代計(jì)算處理前203050.2589.23稀疏和迭代計(jì)算處理后121821.2295.28

由表1可以看出：采用稀疏和迭代計(jì)算處理后可以明顯地提高計(jì)算的效率，提升了計(jì)算的精度。為了進(jìn)一步驗(yàn)證算法的可靠性，對導(dǎo)航的實(shí)時誤差曲線進(jìn)行了輸出，如圖7所示。

圖7 拖拉機(jī)導(dǎo)航實(shí)時誤差曲線

由圖7可以看出：拖拉機(jī)在實(shí)際自主行走過程中有一定的誤差，但是通過自動調(diào)整后，誤差越來越小，最后小于15cm，滿足精確導(dǎo)航的設(shè)計(jì)要求，從而驗(yàn)證了算法的可靠性。

4 結(jié)論

為了解決拖拉機(jī)精確導(dǎo)航過程中大型矩陣的數(shù)據(jù)處理問題，基于預(yù)設(shè)導(dǎo)航和路徑追蹤導(dǎo)航偏差原理，設(shè)計(jì)了導(dǎo)航數(shù)據(jù)坐標(biāo)轉(zhuǎn)換的稀疏化線性矩陣的求解方法，并利用DGPMHSS迭代方法完成數(shù)據(jù)的處理，從而實(shí)現(xiàn)了拖拉機(jī)精準(zhǔn)導(dǎo)航的快速計(jì)算功能。為了驗(yàn)證該方案的可行性，設(shè)計(jì)了拖拉機(jī)導(dǎo)航實(shí)驗(yàn)，結(jié)果表明：采用稀疏和迭代計(jì)算處理后可以明顯地提高計(jì)算的效率，提升計(jì)算的精度。對導(dǎo)航偏差進(jìn)行了統(tǒng)計(jì)，由統(tǒng)計(jì)結(jié)果發(fā)現(xiàn)：拖拉機(jī)通過導(dǎo)航自動調(diào)整后，其誤差最大僅為15cm，滿足精確導(dǎo)航的設(shè)計(jì)要求，為現(xiàn)代農(nóng)機(jī)精準(zhǔn)化導(dǎo)航的研究提供了技術(shù)參考。