郭鵬杰，唐莉萍

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

基于地磁導航的智能小車方向控制方法

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

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

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

地磁導航；智能系統(tǒng)；行駛狀態(tài)；轉向控制

0 引言

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

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

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

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

圖1 地磁導航小車控制硬件框圖

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

2 地磁導航角度計算

地磁傳感器只能提供當前所在區(qū)域的磁場量，要獲取當前小車的導航角，必須計算地球磁場和導航角之間的關系并進行標定。

圖2 地球磁場示意

地球的磁場總是由地磁南極指向地磁北極，在極點處磁場與當地水平面垂直，在赤道處與當地水平面平行，如圖2所示。對于地球上絕大部分區(qū)域來說，當地地磁場均可視為與水平面平行。由于地磁場是一個矢量，任意一點的地磁場都可以分解為兩個平行于當地水平面的分量和一個垂直于當地水平面的分量。如果保持地磁傳感器和當地的水平面平行，那么就有圖3所示的地磁傳感器的3個軸和這3個分量對應關系。

圖3 磁場向量分解[5]

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

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

(1)

當地磁傳感器保持水平，航向角只需要用傳感器水平方向兩軸(通常為X軸和Y軸)的數據就可以由式(1)計算得到。當地磁傳感器水平旋轉時，航向角在0°～360°之間變化。如果設備不是水平放置，那么設備與水平面存在一個夾角θ，如圖4所示。這個夾角將影響航向角的計算精度，需要借助加速度傳感器進行傾角補償。先通過測量3個軸上重力加速度的分量，得到：

(2)

(3)

再將地磁傳感器測的得數據(XM，YM，ZM)通過γ和θ轉化為Hx、Hy：

Hy=YMcos(θ)+XMsin(θ)sin(φ)-ZMcos(φ)sin(θ)

(4)

Hx=XMcos(φ)+ZMsin(φ)

(5)

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

圖4 設備與水平面之間存在夾角[5]

圖5 系統(tǒng)控制流程

3 基于地磁導航的控制策略

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

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

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

3.2 轉向控制器設計

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

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

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

(6)

圖6 系統(tǒng)理想運動模型

(7)

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

(8)

由于轉向系統(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為比例系數，Ki為積分系數。

3.3 導航控制器設計

導航控制器采用PID控制算法，設定值為目標角度，反饋量為當前航向角ψ；當系統(tǒng)直線行駛或者與目標角度的偏差較小時，系統(tǒng)進入直行導航階段。為了防止系統(tǒng)調節(jié)時出現(xiàn)振蕩現(xiàn)象，當地磁傳感器所得到航向角與目標角度的偏差在±2°以內時，系統(tǒng)不進行調整，運動方向不發(fā)生改變。當偏差達到±3°時，系統(tǒng)采用PID控制方法調整智能車方向。

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

δ=αθ

(10)

其中，δ代表轉向角指令，α為比例系數。

4 實驗結果與改進

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

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

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

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

圖7 智能車導航控制流程

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

5 結論

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

[1] 郭才發(fā),胡正東,張士峰,等.地磁導航綜述[J].宇航學報,2009, 30(4): 1314-1319.

[2] 尤亞男.智能車輛導航技術[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] 李興城,張慧心,張雙彪.地磁導航技術的發(fā)展現(xiàn)狀[J].飛航導彈,2013, 10(10): 80-83.

[5] 陳偉韜. 基于MEMS傳感器的電子羅盤系統(tǒng)的設計[D].廣州：廣東工業(yè)大學，2013.

[6] HAVERINEN J, KEMPPAINEN A. A geomagnetic field based positioning technique for underground mines[C].2011 IEEE International Symposium on Robotic and Sensors Environments (ROSE), 2011: 7-12.

[7] CAMPBELL W H. Earth magnetism: a guided tour through magnetic fields[M]. Academic Press, 2001.

[8] 沈垣,凌銳,李舜酩.基于預瞄最優(yōu)曲率模型的大曲率轉向控制方法[J].中國機械工程,2012, 23(17): 2111-2116.

[9] 郭孔輝,軋浩.橫擺角速度反饋汽車轉向控制的理論研究[J].中國機械工程,2000, 11(1): 61-64.

[10] 高振海.汽車方向預瞄式自適應PD控制算法[J].機械工程學報,2004, 40(5): 101-105.

[11] 高雷,胡靜濤,李逃昌,等.以角速度為轉向動作反饋的拖拉機自動導航控制系統(tǒng)[J]. 農機化研究,2015(3):7.

[12] 李愛娟,李舜酩,沈峘,等.智能車大角度彎道轉向曲率連續(xù)的軌跡規(guī)劃方法[J]. 中國機械工程, 2014, 25(2): 273-278.

Direction control method of intelligent vehicle based on geomagnetic navigation

Guo Pengjie1,2,Tang Liping1,2

(1.College of Information Science and Technology, Donghua University, Shanghai 201620,China;2.Engineering Research Center of Digitized Textile & Fashion Technology, Ministry of Education, Donghua University, Shanghai 201620, China)

An intelligent system controlled by geomagnetic navigation is discussed in this paper. The system driving state is obtained through a geomagnetic sensor, which is used to correct geomagnetic navigation angle error. Then the vehicle speed and steering angle size conditions on the state intelligent system are taken into account，beside the practical application , resulting in that a steering control strategy based on Cortex_M4 kernel is proposed .

geomagnetic navigation；intelligent system；driving state；steering control

TN93

A

1674-7720(2016)04-0085-03

郭鵬杰，唐莉萍．基于地磁導航的智能小車方向控制方法[J] .微型機與應用，2016，35(4)：85-87，90.

2015-10-26)

郭鵬杰(1990-)，男，碩士研究生，主要研究方向：嵌入式系統(tǒng)與應用。

唐莉萍(1957-)，女，碩士，副教授，主要研究方向：圖形與圖像處理、模式識別、嵌入式系統(tǒng)。