唐宏偉,石書(shū)琪,陳源,彭志華

(邵陽(yáng)學(xué)院 多電源地區(qū)電網(wǎng)運(yùn)行與控制湖南省重點(diǎn)實(shí)驗(yàn)室，湖南 邵陽(yáng)，422000)

一種基于神經(jīng)網(wǎng)絡(luò)PID的機(jī)器人SLAM改進(jìn)算法

唐宏偉,石書(shū)琪,陳源,彭志華

(邵陽(yáng)學(xué)院 多電源地區(qū)電網(wǎng)運(yùn)行與控制湖南省重點(diǎn)實(shí)驗(yàn)室，湖南 邵陽(yáng)，422000)

提出了一種基于神經(jīng)網(wǎng)絡(luò)PID自適應(yīng)辨識(shí)噪聲的移動(dòng)機(jī)器人即時(shí)定位與地圖創(chuàng)建(SLAM)改進(jìn)算法。重點(diǎn)對(duì)自適應(yīng)辨識(shí)的EKF-SLAM改進(jìn)算法實(shí)現(xiàn)進(jìn)行了分析，神經(jīng)網(wǎng)絡(luò)PID控制器、中值濾波以及噪聲調(diào)整等組成噪聲在線辨識(shí)單元。在噪聲先驗(yàn)信息不足的情況下，通過(guò)自適應(yīng)在線辨識(shí)單元辨識(shí)未知系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲，并迭代修正噪聲協(xié)方差和平均值濾波新息協(xié)方差，實(shí)現(xiàn)機(jī)器人即時(shí)定位精度的在線提高。仿真結(jié)果表明,該算法可減小定位誤差、降低未知系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲對(duì)SLAM算法的影響。

神經(jīng)網(wǎng)絡(luò)PID；機(jī)器人；即時(shí)定位與地圖創(chuàng)建；EKF-SLAM

目前移動(dòng)機(jī)器人領(lǐng)域研究熱點(diǎn)之一是即時(shí)定位與地圖創(chuàng)建(Simultaneous Localization and Mapping，SLAM)問(wèn)題，特別是戶外未知環(huán)境下噪聲未知的情況下SLAM研究。擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)是解決SLAM問(wèn)題的經(jīng)典算法，卡爾曼濾波要求噪聲協(xié)方差信息精確，噪聲先驗(yàn)信息不正確會(huì)降低狀態(tài)估計(jì)精度[1-4]。文獻(xiàn)研究了提高觀測(cè)精度等的方法[5-7]，但當(dāng)機(jī)器人處于戶外未知環(huán)境中,系統(tǒng)未知噪聲是其中需要解決的重要問(wèn)題。有研究者提出SLAM在未知觀測(cè)噪聲的環(huán)境下,先假設(shè)觀測(cè)噪聲為允許的較大值，利用新息的理論值與實(shí)際值差異來(lái)調(diào)整觀測(cè)噪聲協(xié)方差[8-10]。有些研究者提出PID自適應(yīng)控制方法，具有神經(jīng)網(wǎng)絡(luò)的非線性映射能力、學(xué)習(xí)能力和自適應(yīng)性[11]。

本文提出一種基于神經(jīng)網(wǎng)絡(luò)PID的自適應(yīng)在線辨識(shí)的機(jī)器人SLAM算法，機(jī)器人SLAM在線辨識(shí)噪聲的同時(shí)進(jìn)行即時(shí)定位與地圖構(gòu)建，在未知噪聲協(xié)方差情況下,預(yù)設(shè)噪聲協(xié)方差為較大值,將新息的實(shí)際協(xié)方差與理論協(xié)方差輸入神經(jīng)網(wǎng)絡(luò)PID自適應(yīng)控制器,不斷修正噪聲協(xié)方差，減小定位誤差。

1 神經(jīng)網(wǎng)絡(luò)PID機(jī)器人SLAM改進(jìn)算法

1.1 神經(jīng)網(wǎng)絡(luò)PID控制

神經(jīng)網(wǎng)絡(luò)應(yīng)用于 PID控制是一種改進(jìn)和優(yōu)化算法[12]。

傳統(tǒng)PID控制器算式如下:

(1)

相應(yīng)的離散算式為:

(2)

比例、積分、微分系數(shù)分別用KP,KI,KD表示，e(k)為第k次采樣的輸入偏差值，u(k)為第k次采樣時(shí)的輸出值。

可以用單個(gè)神經(jīng)元構(gòu)造如圖 1所示的PID控制器。

圖1 單神經(jīng)元PID控制器Fig.1 Single neuron PID controller

控制器網(wǎng)絡(luò)的輸入為:

X1(k)=e(k)

(3)

(4)

X3(k)=Δe(k)=e(k)-e(k-1)

(5)

網(wǎng)絡(luò)的輸出為

u(k)=w1X1(k)+w2X2(k)+w3X3(k)

(6)

控制器的加權(quán)系數(shù)用{wi}表示，相當(dāng)于PID控制器中的比例(KP)、積分(KI)、微分(KD)系數(shù)，可以進(jìn)行在線修正參數(shù){wi}使達(dá)到最優(yōu)值，從而改善控制系統(tǒng)性能[12-13]。

1.2 EKF-SLAM 算法

假設(shè)對(duì)于某一非線性系統(tǒng)模型:

xk+1=f(xk,uk,ωk)

(7)

zk+1=h(xk+1,νk+1)

(8)

xk表示系統(tǒng)狀態(tài)向量，zk+1表示系統(tǒng)觀測(cè)向量，uk表示輸入控制向量,f(.)表示系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)，h(.)為系統(tǒng)觀測(cè)函數(shù)，ωk表示系統(tǒng)過(guò)程噪聲，vk表示系統(tǒng)觀測(cè)噪聲，ωk和vk都是零均值高斯白噪聲且互不相關(guān)，即滿足式(9)和式(10)。

ωk～N(0,Qk)

(9)

vk～N(0,Rk)

(10)

1.預(yù)測(cè)過(guò)程，獲得系統(tǒng)狀態(tài)的加權(quán)平均值和協(xié)方差矩陣：

(11)

Pk+1,k=FPkFT+WkQkWkT

(12)

2.觀測(cè)過(guò)程

(1)觀測(cè)測(cè)量：根據(jù)系統(tǒng)模型，機(jī)器人在k+1時(shí)刻，傳感器獲取對(duì)環(huán)境特征的測(cè)量(包括距離和方位角)集合Zk+1:

zk+1={zi|i∈M}

(13)

環(huán)境特征總數(shù)目用M表示，環(huán)境中第i個(gè)特征的觀測(cè)值用Zi表示。

(3)數(shù)據(jù)關(guān)聯(lián)：從k+1時(shí)刻觀測(cè)數(shù)據(jù)中獲得觀測(cè)特征值，將其與k時(shí)刻的估計(jì)觀測(cè)值進(jìn)行數(shù)據(jù)關(guān)聯(lián)。當(dāng)觀測(cè)與預(yù)測(cè)觀測(cè)一致時(shí)，進(jìn)行系統(tǒng)狀態(tài)更新，否則實(shí)現(xiàn)地圖更新。

3.更新過(guò)程

(1)狀態(tài)更新

1)更新卡爾曼增益：

(15)

3)計(jì)算狀態(tài)后驗(yàn)估計(jì)的協(xié)方差矩陣：

Pk+1=(I-Kk+1Hk+1)TPk+1,k

(16)

系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)和觀測(cè)函數(shù)關(guān)于狀態(tài)向量的雅克比矩陣用F和Hk+1表示。

(2)狀態(tài)增廣與地圖更新

如果產(chǎn)生了新特征點(diǎn)集合，為實(shí)現(xiàn)全局地圖的更新需將新特征增廣至狀態(tài)向量中。設(shè)第i個(gè)新特征為zi=(ri,?i)，轉(zhuǎn)換至系統(tǒng)坐標(biāo)系表示的特征位置形式如下:

(17)

將(xi,yi)增廣至系統(tǒng)狀態(tài)向量中，并求其協(xié)方差矩陣：

(18)

(19)

根據(jù)誤差傳播定律求取新特征點(diǎn)之間的協(xié)方差陣Ppi,pi、特征與位姿之間的協(xié)方差陣Ppi,v、位姿與特征之間的協(xié)方差陣Pv,pi,以及特征與特征之間的協(xié)方差陣Ppi,p和Pp,pi。

2 基于神經(jīng)網(wǎng)絡(luò)PID的未知噪聲機(jī)器人SLAM改進(jìn)算法

2.1 神經(jīng)網(wǎng)絡(luò)PID自適應(yīng)在線辨識(shí)

未知系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲的在線辨識(shí)由神經(jīng)網(wǎng)絡(luò)PID控制器和中值濾波構(gòu)成，在線辨識(shí)噪聲方法如圖2所示。

圖2 神經(jīng)網(wǎng)絡(luò)PID自適應(yīng)在線辨識(shí)原理圖Fig.2 Schematic of the neural-network PID adaptive online identification

在EKF-SLAM中,k時(shí)刻的新息Imk為實(shí)際觀測(cè)Z(k+1)與觀測(cè)預(yù)測(cè)h(xk,vk)的差，新息Ink為實(shí)際觀測(cè)x(k+1)與觀測(cè)預(yù)測(cè)f(xk,vk)的差，即

Imk=Z(k+1)-h(xk,vk)

(20)

Ink=x(k+1)-f(xk,uk)

(21)

新息在穩(wěn)態(tài)濾波下具有平穩(wěn)遍歷特性，可用n個(gè)移動(dòng)窗口內(nèi)Imk和Ink樣本協(xié)方差的平均值來(lái)近似其協(xié)方差Crk,即

(22)

經(jīng)過(guò)神經(jīng)網(wǎng)絡(luò)PID控制器獲得兩個(gè)協(xié)方差修正值ΔQK(系統(tǒng)過(guò)程噪聲協(xié)方差修正值)和ΔRk(觀測(cè)噪聲協(xié)方差修正值)，噪聲調(diào)整后經(jīng)過(guò)中值濾波環(huán)節(jié)即可得到漸近穩(wěn)定的噪聲協(xié)方差QK和Rk[1]。

2.2 基于PID自適應(yīng)控制的SLAM改進(jìn)算法

在SLAM系統(tǒng)預(yù)設(shè)較大噪聲協(xié)方差，利用在線調(diào)整來(lái)逼近充分的噪聲信息，PID自適應(yīng)控制器的輸入為新息的理論協(xié)方差與實(shí)際協(xié)方差,文獻(xiàn)[1]中實(shí)現(xiàn)了自適應(yīng)辨識(shí)觀測(cè)噪聲，本文對(duì)系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲一起辨識(shí)，得到系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲的協(xié)方差修正值。修正噪聲協(xié)方差后,把它用于 EKF-SLAM更新步驟，得到本次迭代的定位結(jié)果。

本文所提出的EKF-SLAM改進(jìn)算法結(jié)構(gòu)如圖3所示。

圖3 SLAM改進(jìn)算法結(jié)構(gòu)Fig.3 Structure of SLAM improved algorithm

基于PID自適應(yīng)控制的EKF-SLAM改進(jìn)算法主要包括如下6步：

Step2 通過(guò)傳感器獲取環(huán)境特征i的觀測(cè),得到觀測(cè)值z(mì)ik。

Step4 根據(jù)式(20)～(21)計(jì)算新息Imk、Ink,經(jīng)過(guò)神經(jīng)網(wǎng)絡(luò)PID控制器得到新息協(xié)方差矩陣Rk、Qk。根據(jù)式(22)計(jì)算Crk。

Step5 經(jīng)過(guò)神經(jīng)網(wǎng)絡(luò)PID控制器計(jì)算ΔRk和ΔQk，并將噪聲協(xié)方差進(jìn)行修正。

3 仿真結(jié)果

在Matlab仿真環(huán)境下，對(duì)EKF-SLAM算法和本文所提SLAM改進(jìn)算法進(jìn)行了仿真實(shí)驗(yàn)。圖4和圖5是利用EKF-SLAM算法和本文提算法估計(jì)的3σ區(qū)域仿真結(jié)果圖，圖4和圖5中的橢圓表示為特征點(diǎn)估計(jì)的3σ區(qū)域，圖中(a)、(b)分別對(duì)應(yīng)60和120個(gè)特征點(diǎn)(路標(biāo))時(shí)的仿真結(jié)果。

(a)60特征點(diǎn)

(b)120特征點(diǎn)

(a)60特征點(diǎn)

(b)120特征點(diǎn)

從仿真結(jié)果圖中可以看出，機(jī)器人從右邊的起點(diǎn)開(kāi)始移動(dòng)到達(dá)終點(diǎn)位置，同時(shí)根據(jù)特征點(diǎn)(路標(biāo))的位置進(jìn)行了即時(shí)定位與地圖創(chuàng)建，效果良好。本文提出的SLAM改進(jìn)算法估計(jì)的3σ的紅色橢圓區(qū)域要明顯小于EKF-SLAM算法估計(jì)的區(qū)域，實(shí)驗(yàn)結(jié)果表明自適應(yīng)辨識(shí)噪聲SLAM改進(jìn)算法的定位精度好于EKF-SLAM算法，也可看出特征點(diǎn)越多兩種算法估計(jì)的3σ橢圓區(qū)域區(qū)別越小，也進(jìn)一步說(shuō)明在野外實(shí)際環(huán)境已知特征點(diǎn)并不太多的情況下，改進(jìn)算法具有更大的優(yōu)勢(shì)。仿真結(jié)果表明,本文所提算法可靠的實(shí)現(xiàn)了即時(shí)定位和地圖創(chuàng)建，同時(shí)降低了未知系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲對(duì)SLAM算法的影響，減小了定位誤差。

4 結(jié) 語(yǔ)

戶外系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲先驗(yàn)信息未知情況下，提出了一種基于神經(jīng)網(wǎng)絡(luò)PID控制的未知噪聲自適應(yīng)在線辨識(shí)機(jī)器人SLAM算法。該算法利用不斷逼近充分噪聲先驗(yàn)信息的估計(jì)值代替噪聲先驗(yàn)信息，通過(guò)仿真驗(yàn)證表明自適應(yīng)辨識(shí)SLAM改進(jìn)算法實(shí)現(xiàn)了在未知噪聲條件下進(jìn)行機(jī)器人同步定位與地圖構(gòu)建，并與EKF-SLAM算法的仿真結(jié)果進(jìn)行比較，證明了本文所提算法在處理未知噪聲條件情況時(shí)有更好的效果。

[1]弋英民,劉丁,黃瑩.未知觀測(cè)噪聲時(shí)機(jī)器人同步定位與地圖構(gòu)建[J].控制理論與應(yīng)用，2015,32(3):320-325.

[2]Lee Chee Sing,Clark Daniel E,Salvi Joaquim.SLAM with dynamic targets via single-cluster PHD filtering [J].IEEE Journal of Selected Topics in Signal Processing,2013,7(3):543-552.

[3]Kim A,Eustice R M.Real-time visual SLAM for autonomous un-derwater hull inspection using visual saliency [J].IEEE Transactions on Robotics,2013,29(3):719-733.

[4]Zou D P,Tan P.Collaborative visual SLAM in dynamic environ-ments [J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2013,35(2):354-366.

[5]Bosse M,Newman P,Leonard J,et al.Simultaneous localiza-tion and map building in large-scale cyclic environments using the At-las framework [J].International Journal of Robotics Research,2004,23(12):1113-1140.

[6]Mehra R K.On the identification of varinaces and adaptive Kalman filtering [J].IEEE Transactions on Automatic Control,1970,15(2):175-184.

[7]Marcos S,Marsal A,Benidir M.The propagator met hod for source bearing estimation [J].Signal Processing,1995,42(2):121-138.

[8]Mehra R K.On the identification of variances and adaptive Kalman filtering [J].IEEE Transactions on Automatic Control,1970,15(2):175-184.

[9]Reynolds R G.Robust estimation of covariance matrices [J].IEEE Transactions on Automatic Control,1990,35(9):1047-1051.

[10]Chatterjee Amitava,Matsuno Fumitoshi.A neuro-fuzzy assisted extended kalman filter-based approach for simultaneous localization and mapping(SLAM)problems [J].IEEE Transactions on Fuzzy Systems,2007,15(5):984-997.

[11]Astrom K J,Hang C C,Hagglund T,et al.Automatic tuning and adaptation for PID controllers-survey [J].Control Engineering Practice,1993,28(1):699-714.

[12]曾軍,方厚輝.神經(jīng)網(wǎng)絡(luò)PID控制及其Matlab仿真[J].現(xiàn)代電子技術(shù)，2004,(2):51-52.

[13]劉迪.基于神經(jīng)網(wǎng)絡(luò)的PID控制算法[D].哈爾濱：黑龍江大學(xué).2008.

[14]周旭.基于改進(jìn)粒子濾波的SLAM算法研究[D].北京：北京理工大學(xué).2014.

Improved algorithm of robot simultaneous localization and mapping based on neural network PID

TANG Hongwei,SHI Shuqi,CHEN Yuan，PENG Zhihua

(Shaoyang University，Hunan Provincial Key Laboratory of Grids Operation and Control on Multi-Power
Sources Area，Shaoyang 422000，China)

An adaptive noise identification algorithm based on neural network PID for robot SLAM is proposed.Adaptive identification of EKF-SLAM improved algorithm is focus on analyzed,an online noise identification unit consisting of neural network PID controller,noise adjustor and median filtering is developed.Under the condition of the noise lack of prior knowledge,through the adaptive online identification unit to identify the unknown system process noise and observation noise.In addition,the innovation covariance is used to iteratively revise it and match the noise covariance,the estimation accuracy of the robot position is improved online.The simulation results show that the proposed algorithm can reduce the positioning error,and reduce the impact of the unknown system process noise and observation noise on SLAM algorithm.

neural network PID;robot;simultaneous localization and mapping(SLAM);EKF-SLAM

1672-7010(2017)01-0082-06

2016-09-28

湖南省教育廳科學(xué)研究項(xiàng)目(14C1015；16A191)；湖南省科技計(jì)劃項(xiàng)目(2016TP1023)

唐宏偉(1982-)，男，湖南武岡人，講師，博士生，從事移動(dòng)機(jī)器人定位與導(dǎo)航研究；E-mail:thwei2008@126.com，

TP24 < class="emphasis_bold">文獻(xiàn)標(biāo)志碼：A

A