一种基于神经网络PID的机器人SLAM改进算法

唐宏伟,石书琪,陈源,彭志华

(邵阳学院 多电源地区电网运行与控制湖南省重点实验室，湖南 邵阳，422000)

一种基于神经网络PID的机器人SLAM改进算法

唐宏伟,石书琪,陈源,彭志华

(邵阳学院 多电源地区电网运行与控制湖南省重点实验室，湖南 邵阳，422000)

提出了一种基于神经网络PID自适应辨识噪声的移动机器人即时定位与地图创建(SLAM)改进算法。重点对自适应辨识的EKF-SLAM改进算法实现进行了分析，神经网络PID控制器、中值滤波以及噪声调整等组成噪声在线辨识单元。在噪声先验信息不足的情况下，通过自适应在线辨识单元辨识未知系统过程噪声和观测噪声，并迭代修正噪声协方差和平均值滤波新息协方差，实现机器人即时定位精度的在线提高。仿真结果表明,该算法可减小定位误差、降低未知系统过程噪声和观测噪声对SLAM算法的影响。

神经网络PID；机器人；即时定位与地图创建；EKF-SLAM

目前移动机器人领域研究热点之一是即时定位与地图创建(Simultaneous Localization and Mapping，SLAM)问题，特别是户外未知环境下噪声未知的情况下SLAM研究。扩展卡尔曼滤波(Extended Kalman Filter，EKF)是解决SLAM问题的经典算法，卡尔曼滤波要求噪声协方差信息精确，噪声先验信息不正确会降低状态估计精度[1-4]。文献研究了提高观测精度等的方法[5-7]，但当机器人处于户外未知环境中,系统未知噪声是其中需要解决的重要问题。有研究者提出SLAM在未知观测噪声的环境下,先假设观测噪声为允许的较大值，利用新息的理论值与实际值差异来调整观测噪声协方差[8-10]。有些研究者提出PID自适应控制方法，具有神经网络的非线性映射能力、学习能力和自适应性[11]。

本文提出一种基于神经网络PID的自适应在线辨识的机器人SLAM算法，机器人SLAM在线辨识噪声的同时进行即时定位与地图构建，在未知噪声协方差情况下,预设噪声协方差为较大值,将新息的实际协方差与理论协方差输入神经网络PID自适应控制器,不断修正噪声协方差，减小定位误差。

1 神经网络PID机器人SLAM改进算法

1.1 神经网络PID控制

神经网络应用于 PID控制是一种改进和优化算法[12]。

传统PID控制器算式如下:

(1)

相应的离散算式为:

(2)

比例、积分、微分系数分别用KP,KI,KD表示，e(k)为第k次采样的输入偏差值，u(k)为第k次采样时的输出值。

可以用单个神经元构造如图 1所示的PID控制器。

图1 单神经元PID控制器Fig.1 Single neuron PID controller

控制器网络的输入为:

X1(k)=e(k)

(3)

(4)

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

(5)

网络的输出为

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

(6)

控制器的加权系数用{wi}表示，相当于PID控制器中的比例(KP)、积分(KI)、微分(KD)系数，可以进行在线修正参数{wi}使达到最优值，从而改善控制系统性能[12-13]。

1.2 EKF-SLAM 算法

假设对于某一非线性系统模型:

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

(7)

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

(8)

xk表示系统状态向量，zk+1表示系统观测向量，uk表示输入控制向量,f(.)表示系统状态转移函数，h(.)为系统观测函数，ωk表示系统过程噪声，vk表示系统观测噪声，ωk和vk都是零均值高斯白噪声且互不相关，即满足式(9)和式(10)。

ωk～N(0,Qk)

(9)

vk～N(0,Rk)

(10)

1.预测过程，获得系统状态的加权平均值和协方差矩阵：

(11)

Pk+1,k=FPkFT+WkQkWkT

(12)

2.观测过程

(1)观测测量：根据系统模型，机器人在k+1时刻，传感器获取对环境特征的测量(包括距离和方位角)集合Zk+1:

zk+1={zi|i∈M}

(13)

环境特征总数目用M表示，环境中第i个特征的观测值用Zi表示。

(3)数据关联：从k+1时刻观测数据中获得观测特征值，将其与k时刻的估计观测值进行数据关联。当观测与预测观测一致时，进行系统状态更新，否则实现地图更新。

3.更新过程

(1)状态更新

1)更新卡尔曼增益：

(15)

3)计算状态后验估计的协方差矩阵：

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

(16)

系统状态转移函数和观测函数关于状态向量的雅克比矩阵用F和Hk+1表示。

(2)状态增广与地图更新

如果产生了新特征点集合，为实现全局地图的更新需将新特征增广至状态向量中。设第i个新特征为zi=(ri,∅i)，转换至系统坐标系表示的特征位置形式如下:

(17)

将(xi,yi)增广至系统状态向量中，并求其协方差矩阵：

(18)

(19)

根据误差传播定律求取新特征点之间的协方差阵Ppi,pi、特征与位姿之间的协方差阵Ppi,v、位姿与特征之间的协方差阵Pv,pi,以及特征与特征之间的协方差阵Ppi,p和Pp,pi。

2 基于神经网络PID的未知噪声机器人SLAM改进算法

2.1 神经网络PID自适应在线辨识

未知系统过程噪声和观测噪声的在线辨识由神经网络PID控制器和中值滤波构成，在线辨识噪声方法如图2所示。

图2 神经网络PID自适应在线辨识原理图Fig.2 Schematic of the neural-network PID adaptive online identification

在EKF-SLAM中,k时刻的新息Imk为实际观测Z(k+1)与观测预测h(xk,vk)的差，新息Ink为实际观测x(k+1)与观测预测f(xk,vk)的差，即

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

(20)

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

(21)

新息在稳态滤波下具有平稳遍历特性，可用n个移动窗口内Imk和Ink样本协方差的平均值来近似其协方差Crk,即

(22)

经过神经网络PID控制器获得两个协方差修正值ΔQK(系统过程噪声协方差修正值)和ΔRk(观测噪声协方差修正值)，噪声调整后经过中值滤波环节即可得到渐近稳定的噪声协方差QK和Rk[1]。

2.2 基于PID自适应控制的SLAM改进算法

在SLAM系统预设较大噪声协方差，利用在线调整来逼近充分的噪声信息，PID自适应控制器的输入为新息的理论协方差与实际协方差,文献[1]中实现了自适应辨识观测噪声，本文对系统过程噪声和观测噪声一起辨识，得到系统过程噪声和观测噪声的协方差修正值。修正噪声协方差后,把它用于 EKF-SLAM更新步骤，得到本次迭代的定位结果。

本文所提出的EKF-SLAM改进算法结构如图3所示。

图3 SLAM改进算法结构Fig.3 Structure of SLAM improved algorithm

基于PID自适应控制的EKF-SLAM改进算法主要包括如下6步：

Step2 通过传感器获取环境特征i的观测,得到观测值zik。

Step4 根据式(20)～(21)计算新息Imk、Ink,经过神经网络PID控制器得到新息协方差矩阵Rk、Qk。根据式(22)计算Crk。

Step5 经过神经网络PID控制器计算ΔRk和ΔQk，并将噪声协方差进行修正。

3 仿真结果

在Matlab仿真环境下，对EKF-SLAM算法和本文所提SLAM改进算法进行了仿真实验。图4和图5是利用EKF-SLAM算法和本文提算法估计的3σ区域仿真结果图，图4和图5中的椭圆表示为特征点估计的3σ区域，图中(a)、(b)分别对应60和120个特征点(路标)时的仿真结果。

(a)60特征点

(b)120特征点

(a)60特征点

(b)120特征点

从仿真结果图中可以看出，机器人从右边的起点开始移动到达终点位置，同时根据特征点(路标)的位置进行了即时定位与地图创建，效果良好。本文提出的SLAM改进算法估计的3σ的红色椭圆区域要明显小于EKF-SLAM算法估计的区域，实验结果表明自适应辨识噪声SLAM改进算法的定位精度好于EKF-SLAM算法，也可看出特征点越多两种算法估计的3σ椭圆区域区别越小，也进一步说明在野外实际环境已知特征点并不太多的情况下，改进算法具有更大的优势。仿真结果表明,本文所提算法可靠的实现了即时定位和地图创建，同时降低了未知系统过程噪声和观测噪声对SLAM算法的影响，减小了定位误差。

4 结 语

户外系统过程噪声和观测噪声先验信息未知情况下，提出了一种基于神经网络PID控制的未知噪声自适应在线辨识机器人SLAM算法。该算法利用不断逼近充分噪声先验信息的估计值代替噪声先验信息，通过仿真验证表明自适应辨识SLAM改进算法实现了在未知噪声条件下进行机器人同步定位与地图构建，并与EKF-SLAM算法的仿真结果进行比较，证明了本文所提算法在处理未知噪声条件情况时有更好的效果。

[1]弋英民,刘丁,黄莹.未知观测噪声时机器人同步定位与地图构建[J].控制理论与应用，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]曾军,方厚辉.神经网络PID控制及其Matlab仿真[J].现代电子技术，2004,(2):51-52.

[13]刘迪.基于神经网络的PID控制算法[D].哈尔滨：黑龙江大学.2008.

[14]周旭.基于改进粒子滤波的SLAM算法研究[D].北京：北京理工大学.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

湖南省教育厅科学研究项目(14C1015；16A191)；湖南省科技计划项目(2016TP1023)

唐宏伟(1982-)，男，湖南武冈人，讲师，博士生，从事移动机器人定位与导航研究；E-mail:thwei2008@126.com，

TP24 < class="emphasis_bold">文献标志码：A

A