郭鵬杰1，2，唐莉萍1，2

　?。?.東華大學(xué) 信息科學(xué)與技術(shù)學(xué)院，上海 201620；2.東華大學(xué) 數(shù)字化紡織服裝技術(shù)教育部工程研究中心，上海 201620）

　　摘要：討論了一種通過(guò)地磁進(jìn)行智能系統(tǒng)導(dǎo)航控制的方法，通過(guò)地磁傳感器獲得智能系統(tǒng)的行駛狀態(tài)，并對(duì)地磁導(dǎo)航角進(jìn)行誤差校正。綜合考慮車輛速度、轉(zhuǎn)向角大小等因素對(duì)智能系統(tǒng)行駛狀態(tài)的影響，提出一種適用于內(nèi)核為Cortex_M4的智能小車轉(zhuǎn)向控制策略。

　　關(guān)鍵詞：地磁導(dǎo)航；智能系統(tǒng)；行駛狀態(tài)；轉(zhuǎn)向控制

0引言

　　21世紀(jì)，智能交通系統(tǒng)的飛速發(fā)展引發(fā)了智能導(dǎo)航技術(shù)的不斷進(jìn)步。機(jī)器視覺(jué)由于其具有檢測(cè)范圍廣、信息容量大、類似于人類駕駛決策且成本低廉等諸多優(yōu)勢(shì)，在國(guó)內(nèi)外智能導(dǎo)航的研究中得到了廣泛應(yīng)用［1］。但是機(jī)器視覺(jué)是通過(guò)視覺(jué)識(shí)別道路和障礙物，進(jìn)而規(guī)劃路徑實(shí)現(xiàn)導(dǎo)航控制，控制結(jié)果容易受天氣狀況、光照等因素影響。全球定位系統(tǒng)(Global Positioning System，GPS)是另一種廣泛應(yīng)用的導(dǎo)航技術(shù)，它可以為全球用戶提供三維位置、速度等信息［2］。但是GPS導(dǎo)航具有一定的局限性，它在使用時(shí)要求用戶實(shí)時(shí)連接網(wǎng)絡(luò)或者存儲(chǔ)高精度的數(shù)字地圖，否則其導(dǎo)航精度和實(shí)時(shí)性難以保證［3］。

　　地磁場(chǎng)是地球的固有資源，它能為航天、航海、航空提供天然的坐標(biāo)系［3，4］。由于地磁場(chǎng)的矢量性，通過(guò)地磁傳感器感知測(cè)量即可確定系統(tǒng)當(dāng)前的運(yùn)動(dòng)狀態(tài)信息［5，6］。與其他導(dǎo)航方式相比，地磁導(dǎo)航具有無(wú)源、全天候、全地域、低能耗等優(yōu)勢(shì)，在工業(yè)部門、航空航天等諸多領(lǐng)域發(fā)揮了重要作用，成為近年來(lái)智能導(dǎo)航學(xué)術(shù)界關(guān)注的對(duì)象［7］。

　　本文在參考文獻(xiàn)［812］的基礎(chǔ)上，對(duì)基于地磁傳感器自動(dòng)導(dǎo)航的智能小車控制方法進(jìn)行探討，結(jié)合系統(tǒng)硬件框圖，對(duì)基于地磁導(dǎo)航的控制策略進(jìn)行研究，并采用PID控制導(dǎo)航。對(duì)通過(guò)地磁傳感器獲得的當(dāng)前航向角度，判斷其與目標(biāo)角度的夾角的偏差，給出轉(zhuǎn)向角度指令，調(diào)節(jié)舵機(jī)，實(shí)現(xiàn)小車自動(dòng)路徑行駛。最后在研究的理論算法基礎(chǔ)上，考慮硬件平臺(tái)限制，給出優(yōu)化方案，并通過(guò)實(shí)踐證明該優(yōu)化方案的可行性。

1系統(tǒng)設(shè)計(jì)總體方案

　　地磁導(dǎo)航小車在自動(dòng)路徑運(yùn)動(dòng)中不斷通過(guò)地磁傳感器獲得當(dāng)前行進(jìn)方向的角度信息，與預(yù)設(shè)路徑的航向角比較，通過(guò)所得結(jié)果的偏差角度調(diào)整舵機(jī)的偏轉(zhuǎn)方向，從而控制智能小車的行駛軌跡。系統(tǒng)的硬件主要模塊如圖1所示。其中地磁傳感器采用意法半導(dǎo)體公司的MEMS數(shù)字集成芯片LSM303DLHC。

2地磁導(dǎo)航角度計(jì)算

　　圖5系統(tǒng)控制流程地磁傳感器只能提供當(dāng)前所在區(qū)域的磁場(chǎng)量，要獲取當(dāng)前小車的導(dǎo)航角，必須計(jì)算地球磁場(chǎng)和導(dǎo)航角之間的關(guān)系并進(jìn)行標(biāo)定。

　　地球的磁場(chǎng)總是由地磁南極指向地磁北極，在極點(diǎn)處磁場(chǎng)與當(dāng)?shù)厮矫娲怪保诔嗟捞幣c當(dāng)?shù)厮矫嫫叫?，如圖2所示。對(duì)于地球上絕大部分區(qū)域來(lái)說(shuō)，當(dāng)?shù)氐卮艌?chǎng)均可視為與水平面平行。由于地磁場(chǎng)是一個(gè)矢量，任意一點(diǎn)的地磁場(chǎng)都可以分解為兩個(gè)平行于當(dāng)?shù)厮矫娴姆至亢鸵粋€(gè)垂直于當(dāng)?shù)厮矫娴姆至?。如果保持地磁傳感器和?dāng)?shù)氐乃矫嫫叫?，那么就有圖3所示的地磁傳感器的3個(gè)軸和這3個(gè)分量對(duì)應(yīng)關(guān)系。

　　圖3磁場(chǎng)向量分解［5］圖3中，Hnorth和Hz分別為Hearth在水平面和垂直面的分量，Hx和Hy為Hnorth在相對(duì)坐標(biāo)系X軸和Y軸的分量。

　　在水平方向上磁場(chǎng)兩個(gè)分量的矢量和總是指向磁北。地磁傳感器中的航向角（Azimuth）是當(dāng)前方向和磁北的夾角，即：

　　當(dāng)?shù)卮艂鞲衅鞅３炙?，航向角只需要用傳感器水平方向兩軸（通常為X軸和Y軸）的數(shù)據(jù)就可以由式（1）計(jì)算得到。當(dāng)?shù)卮艂鞲衅魉叫D(zhuǎn)時(shí)，航向角在0°~360°之間變化。如果設(shè)備不是水平放置，那么設(shè)備與水平面存在一個(gè)夾角θ，如圖4所示。這個(gè)夾角將影響航向角的計(jì)算精度，需要借助加速度傳感器進(jìn)行傾角補(bǔ)償。先通過(guò)測(cè)量3個(gè)軸上重力加速度的分量，得到：

　　再將地磁傳感器測(cè)的得數(shù)據(jù)（XM，YM，ZM）通過(guò)γ和θ轉(zhuǎn)化為Hx、Hy：

　　Hy=YMcos(θ)+XMsin(θ)sin(φ)－ZMcos(φ)sin(θ)（4）

　　Hx=XMcos(φ)+ZMsin(φ)（5）

　　結(jié)合式（1）即可計(jì)算出航向角ψ。

　　圖4設(shè)備與水平面之間存在夾角［5］

3基于地磁導(dǎo)航的控制策略

　　3.1智能系統(tǒng)控制模型

　　智能導(dǎo)航控制的實(shí)質(zhì)是對(duì)小車前進(jìn)方向角度與設(shè)定目標(biāo)角度偏差的控制。主要控制算法是基于自適應(yīng)的PID控制。系統(tǒng)控制流程圖如圖5所示。

　　導(dǎo)航控制的具體過(guò)程如下：導(dǎo)航控制器根據(jù)接收到的智能車運(yùn)行航向角信息ψ與設(shè)定目標(biāo)角度進(jìn)行比較，計(jì)算出航向偏差Δφ。當(dāng)Δφ較大時(shí)，轉(zhuǎn)向控制器通過(guò)收到的角度信息和反饋的角速度信息計(jì)算出合適的轉(zhuǎn)向角θ，系統(tǒng)經(jīng)過(guò)大角度轉(zhuǎn)向、預(yù)判目標(biāo)、回轉(zhuǎn)等一系列操作后，以較小的偏差角度切入目標(biāo)航線；當(dāng)Δφ→0時(shí)，直接經(jīng)過(guò)PID控制算法生成角度控制指令u，然后通過(guò)導(dǎo)航控制器不斷調(diào)整，系統(tǒng)進(jìn)入直線行駛控制狀態(tài)。

　　3.2轉(zhuǎn)向控制器設(shè)計(jì)

　　轉(zhuǎn)向控制器采用PID控制方法，設(shè)定量和控制量都是角度值，反饋量為智能系統(tǒng)的轉(zhuǎn)向角速度ωw。

　　本地磁導(dǎo)航系統(tǒng)以方向角為預(yù)瞄條件，其中車輛系統(tǒng)的二自由度運(yùn)動(dòng)模型如圖6所示。

　　圖6中，θ為智能系統(tǒng)相對(duì)于相對(duì)坐標(biāo)系x軸的方向角；φ為舵機(jī)轉(zhuǎn)角；R為智能系統(tǒng)的轉(zhuǎn)彎半徑；l為車輛軸距；v和ω分別為智能系統(tǒng)在點(diǎn)（x，y）處的速度和前輪轉(zhuǎn)向角速度。由圖6所示的車輛運(yùn)動(dòng)幾何關(guān)系可以得到車輛的運(yùn)動(dòng)模型如下：

　　為了使軌道誤差最小，根據(jù)智能系統(tǒng)當(dāng)前狀態(tài)下的理想側(cè)向速率(t)與理想側(cè)向加速度(t)，由系統(tǒng)理想運(yùn)動(dòng)模型，此時(shí)智能車所需理想側(cè)向加速度為：

　　同時(shí)，在不考慮車輛的滯后情況下，其運(yùn)動(dòng)簡(jiǎn)單地服從Acklman幾何關(guān)系。

　　式（8）表明智能車的曲率1/R與方向盤轉(zhuǎn)角δ成正比，即智能車轉(zhuǎn)彎半徑R與方向盤轉(zhuǎn)角δ成反比。汽車的側(cè)向加速度同時(shí)與車速v和舵機(jī)轉(zhuǎn)角φ有關(guān)。智能車通常行駛在低速狀態(tài)下，系統(tǒng)通過(guò)調(diào)整轉(zhuǎn)向角度即可獲得良好的轉(zhuǎn)彎半徑?？紤]到切入目標(biāo)角度和轉(zhuǎn)彎半徑的約束，必須首先輸出一個(gè)較大的轉(zhuǎn)向角度，然后達(dá)到此角度后舵機(jī)進(jìn)行回轉(zhuǎn)，以達(dá)到切入目標(biāo)角度時(shí)角度偏差最小的目的。

　　由于轉(zhuǎn)向系統(tǒng)中方向偏角是不斷積累的過(guò)程，故PID控制器采用比例控制和積分控制就能實(shí)現(xiàn)控制目的。這里采用位置式PID算法。

　　uf(k)=uf(k－1)+Kp［δe(k)－δe(k－1)］

　　+KiTδe(k－1)（9）

　　其中，uf為控制信號(hào)量，T為系統(tǒng)采樣周期，δe(k)為角度偏差，Kp為比例系數(shù)，Ki為積分系數(shù)。

　　3.3導(dǎo)航控制器設(shè)計(jì)

　　導(dǎo)航控制器采用PID控制算法，設(shè)定值為目標(biāo)角度，反饋量為當(dāng)前航向角ψ；當(dāng)系統(tǒng)直線行駛或者與目標(biāo)角度的偏差較小時(shí)，系統(tǒng)進(jìn)入直行導(dǎo)航階段。為了防止系統(tǒng)調(diào)節(jié)時(shí)出現(xiàn)振蕩現(xiàn)象，當(dāng)?shù)卮艂鞲衅魉玫胶较蚪桥c目標(biāo)角度的偏差在±2°以內(nèi)時(shí)，系統(tǒng)不進(jìn)行調(diào)整，運(yùn)動(dòng)方向不發(fā)生改變。當(dāng)偏差達(dá)到±3°時(shí)，系統(tǒng)采用PID控制方法調(diào)整智能車方向。

　　為了提高系統(tǒng)響應(yīng)速度，算法采用比例控制方法，控制率為：

　　δ=αθ（10）

　　其中，δ代表轉(zhuǎn)向角指令，α為比例系數(shù)。

4實(shí)驗(yàn)結(jié)果與改進(jìn)

　　本文設(shè)計(jì)的智能小車其核心是Cortex_M4，嵌入式設(shè)備由于硬件限制，其運(yùn)算能力不及PC，如果按照常規(guī)的控制算法進(jìn)行自動(dòng)導(dǎo)航，系統(tǒng)必然出現(xiàn)較大延時(shí)；如果要提高系統(tǒng)響應(yīng)速度，則會(huì)影響系統(tǒng)轉(zhuǎn)向角的控制精度。因此需要對(duì)第3節(jié)討論的方法進(jìn)行改進(jìn)，以期在時(shí)間和精度之間得到一個(gè)較優(yōu)的平衡。在智能小車自動(dòng)導(dǎo)航過(guò)程中，可以將目標(biāo)角度進(jìn)行區(qū)域劃分，針對(duì)不同的車速和角度區(qū)域，根據(jù)經(jīng)驗(yàn)值設(shè)置不同的轉(zhuǎn)角指令，將智能車的轉(zhuǎn)彎曲線軌跡盡可能擬合為理想轉(zhuǎn)彎軌跡，以達(dá)到良好的轉(zhuǎn)彎效果。

　　在圖4所示的理想運(yùn)動(dòng)系統(tǒng)模型中，Δφ是系統(tǒng)導(dǎo)航控制的關(guān)鍵因素。在自動(dòng)導(dǎo)航實(shí)驗(yàn)中， 取Δφ≤2°角度作為導(dǎo)航控制器判斷的閾值時(shí)，系統(tǒng)具有良好的響應(yīng)速度，但是系統(tǒng)偏角響應(yīng)處理很快，系統(tǒng)振蕩周期較短，以至于系統(tǒng)出現(xiàn)明顯的左右擺動(dòng)。取Δφ>5°時(shí)，系統(tǒng)有明顯的響應(yīng)延時(shí)，造成明顯的路徑偏移。根據(jù)實(shí)驗(yàn)結(jié)果取Δφ=±3°作為導(dǎo)航控制器和轉(zhuǎn)向控制器的閾值。

　　PID參數(shù)的選擇是影響系統(tǒng)性能的重要因素，對(duì)PID參數(shù)的整定比較繁瑣，需要進(jìn)行復(fù)雜的積分、微分計(jì)算。這些運(yùn)算如果在嵌入式硬件平臺(tái)上實(shí)現(xiàn)，由于計(jì)算工作需要占用大量的時(shí)間，系統(tǒng)的實(shí)時(shí)控制能力將下降。為此，對(duì)PID參數(shù)可以采用查表法進(jìn)行選擇。首先建立一個(gè)模擬系統(tǒng)，計(jì)算出系統(tǒng)在不同角度與不同車速下的PID參數(shù)，再通過(guò)實(shí)驗(yàn)不斷調(diào)整優(yōu)化，最終建立PID參數(shù)經(jīng)驗(yàn)表。

　　優(yōu)化過(guò)后的系統(tǒng)導(dǎo)航控制過(guò)程如圖7所示。

　　導(dǎo)航控制流程如下：MCU通過(guò)地磁傳感器獲得數(shù)據(jù)并計(jì)算出當(dāng)前行駛角度v以及與目標(biāo)角度的偏角差值Δφ，通過(guò)查找經(jīng)驗(yàn)表獲得適合當(dāng)前車速與偏角Δφ的PID控制系數(shù)，然后調(diào)節(jié)PWM的占空比調(diào)節(jié)舵機(jī)轉(zhuǎn)向角度大小。

5結(jié)論

　　實(shí)踐證明：通過(guò)經(jīng)驗(yàn)表代替系統(tǒng)復(fù)雜的積分微分運(yùn)算，在幾乎不影響系統(tǒng)的運(yùn)行穩(wěn)定性的情況下，提高了系統(tǒng)的響應(yīng)速度，此優(yōu)化方案具有可行性。

參考文獻(xiàn)

　?。?］ 郭才發(fā),胡正東,張士峰,等.地磁導(dǎo)航綜述［J］.宇航學(xué)報(bào),2009, 30(4): 1314-1319.

　?。?］ 尤亞男.智能車輛導(dǎo)航技術(shù)［J］.金田,2013(8): 346，323.

　?。?］ STORMS W, SHOCKLEY J, RAQUET J. Magnetic field navigation in an indoor environment［C］.Ubiquitous Positioning Indoor Navigation and Location Based Service (UPINLBS), 2010， IEEE, 2010: 1-10.

　　［4］ 李興城,張慧心,張雙彪.地磁導(dǎo)航技術(shù)的發(fā)展現(xiàn)狀［J］.飛航導(dǎo)彈,2013, 10(10): 80-83.

　?。?］ 陳偉韜. 基于MEMS傳感器的電子羅盤系統(tǒng)的設(shè)計(jì)［D］.廣州：廣東工業(yè)大學(xué)，