基于地磁導(dǎo)航的智能小車方向控制方法

郭鵬杰，唐莉萍

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

基于地磁導(dǎo)航的智能小車方向控制方法

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

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

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

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

0 引言

21世紀(jì)，智能交通系統(tǒng)的飛速發(fā)展引發(fā)了智能導(dǎo)航技術(shù)的不斷進(jìn)步。機(jī)器視覺由于其具有檢測范圍廣、信息容量大、類似于人類駕駛決策且成本低廉等諸多優(yōu)勢，在國內(nèi)外智能導(dǎo)航的研究中得到了廣泛應(yīng)用[1]。但是機(jī)器視覺是通過視覺識(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]。

地磁場是地球的固有資源，它能為航天、航海、航空提供天然的坐標(biāo)系[3，4]。由于地磁場的矢量性，通過地磁傳感器感知測量即可確定系統(tǒng)當(dāng)前的運(yùn)動(dòng)狀態(tài)信息[5，6]。與其他導(dǎo)航方式相比，地磁導(dǎo)航具有無源、全天候、全地域、低能耗等優(yōu)勢，在工業(yè)部門、航空航天等諸多領(lǐng)域發(fā)揮了重要作用，成為近年來智能導(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ì)通過地磁傳感器獲得的當(dāng)前航向角度，判斷其與目標(biāo)角度的夾角的偏差，給出轉(zhuǎn)向角度指令，調(diào)節(jié)舵機(jī)，實(shí)現(xiàn)小車自動(dòng)路徑行駛。最后在研究的理論算法基礎(chǔ)上，考慮硬件平臺(tái)限制，給出優(yōu)化方案，并通過實(shí)踐證明該優(yōu)化方案的可行性。

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

圖1 地磁導(dǎo)航小車控制硬件框圖

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

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

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

圖2 地球磁場示意

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

圖3 磁場向量分解[5]

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

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

(1)

當(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ǔ)償。先通過測量3個(gè)軸上重力加速度的分量，得到：

(2)

(3)

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

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

(4)

Hx=XMcos(φ)+ZMsin(φ)

(5)

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

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

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

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

(6)

圖6 系統(tǒng)理想運(yùn)動(dòng)模型

(7)

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

(8)

由于轉(zhuǎn)向系統(tǒng)中方向偏角是不斷積累的過程，故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)航過程中，可以將目標(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ù)，再通過實(shí)驗(yàn)不斷調(diào)整優(yōu)化，最終建立PID參數(shù)經(jīng)驗(yàn)表。

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

圖7 智能車導(dǎo)航控制流程

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

5 結(jié)論

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

[1] 郭才發(fā),胡正東,張士峰,等.地磁導(dǎo)航綜述[J].宇航學(xué)報(bào),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.

[5] 陳偉韜. 基于MEMS傳感器的電子羅盤系統(tǒng)的設(shè)計(jì)[D].廣州：廣東工業(yè)大學(xué)，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ù)瞄最優(yōu)曲率模型的大曲率轉(zhuǎn)向控制方法[J].中國機(jī)械工程,2012, 23(17): 2111-2116.

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

[10] 高振海.汽車方向預(yù)瞄式自適應(yīng)PD控制算法[J].機(jī)械工程學(xué)報(bào),2004, 40(5): 101-105.

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

[12] 李愛娟,李舜酩,沈峘,等.智能車大角度彎道轉(zhuǎn)向曲率連續(xù)的軌跡規(guī)劃方法[J]. 中國機(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

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

2015-10-26)

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

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