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

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

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

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

0引言

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

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

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

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

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

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

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

　　地球的磁場總是由地磁南極指向地磁北極，在極點處磁場與當(dāng)?shù)厮矫娲怪?，在赤道處與當(dāng)?shù)厮矫嫫叫校鐖D2所示。對于地球上絕大部分區(qū)域來說，當(dāng)?shù)氐卮艌鼍梢暈榕c水平面平行。由于地磁場是一個矢量，任意一點的地磁場都可以分解為兩個平行于當(dāng)?shù)厮矫娴姆至亢鸵粋€垂直于當(dāng)?shù)厮矫娴姆至?。如果保持地磁傳感器和?dāng)?shù)氐乃矫嫫叫?，那么就有圖3所示的地磁傳感器的3個軸和這3個分量對應(yīng)關(guān)系。

　　圖3磁場向量分解［5］圖3中，Hnorth和Hz分別為Hearth在水平面和垂直面的分量，Hx和Hy為Hnorth在相對坐標系X軸和Y軸的分量。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　　同時，在不考慮車輛的滯后情況下，其運動簡單地服從Acklman幾何關(guān)系。

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

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

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

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

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

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

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

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

　　δ=αθ（10）

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

4實驗結(jié)果與改進

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

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

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

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

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

5結(jié)論

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

參考文獻

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

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

　　［3］ 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è)計［D］.廣州：廣東工業(yè)大學(xué)，