基于卡尔曼滤波的无人采矿设备状态预测研究

邬兴， 贺春林， 石义红

(西华师范大学　计算机学院， 四川　南充　637002)



基于卡尔曼滤波的无人采矿设备状态预测研究

邬兴， 贺春林， 石义红

(西华师范大学计算机学院， 四川南充637002)

为了对井下无人采矿设备的状态进行更准确的预测，及时对设备状态进行跟踪,提出了一种激光定位导航系统中的基于卡尔曼滤波的位置预测方法，并在一套试验系统上进行了可行性验证。该系统用可水平旋转、可俯仰的转台作为定位跟踪基站，用可平动和竖直运动的二维运动平台模拟运动目标，采用卡尔曼滤波技术对观测到的运动目标的坐标信息进行滤波和预测，预测值作为转台跟踪的设定值，使基站发出的激光束能平稳准确地跟踪运动目标。

卡尔曼滤波；滤波去噪；预测跟踪

0　引言

自动化和智能化是21世纪采矿业所面临的重要课题，将自动化和智能化应用于采矿行业中，可以起到改善作业环境、提高生产效率、降低生产成本以及保障现场工作人员的安全的作用[1]。本文的目的是为了对矿区无人设备的状态进行更准确的预测，及时对设备状态进行跟踪，从而对无人设备的运动实时地加以控制。图1是拟开发的一套车辆定位导航系统[2]，对象为无人驾驶智能采矿设备，可以理解为无人铲运机或无人矿车。本文在此系统上运用卡尔曼滤波算法实现对无人设备进行机动目标跟踪[3]，从而实现对设备将来的位姿进行精确的预报。

图1　系统简图

卡尔曼滤波[4]技术是一个“最优化自回归数据处理算法”，它对于解决很多问题是最优、效率最高的。它已经应用了很多年，且用途非常广泛[5]，包括雷达系统、组合导航[6]、机器人控制、传感器数据融合等。在计算机技术中也有较多应用，如原子钟驯服、高精度测量等。在车辆定位导航系统中也有重要的应用[7-8]。

1　理论分析及建模

考虑如下离散时间系统[9]：∀k≥1

Xk=φk,k-1Xk-1+Γk,k-1Wk-1

(1)

Zk=HkXk+Vk(2)

1.1运动模型的建立

现有的实验设备为一两轴运动台，可模拟小车在X轴、Z轴方向的振动。考虑系统状态量选取为航向角和小车在空间三维坐标系中X、Y、Z轴方向的坐标：[βk,SXk,SYk,SZk]T，但受运动平台的限制，βk和SYk都不会发生变化，均设置为0，只有SXk和SZk会随着运动平台的振动规律的影响而变化。若按式(1)、(2)来建立系统方程进行滤波，由于运动平台仅模拟小车的振动规律，k-1时刻到k时刻的状态量递推方程不易求得，状态转移矩阵也就不易获得。而且因为包含三角函数，需要选用扩展卡尔曼滤波代替标准卡尔曼滤波，对三角函数进行泰勒展开来近似，由此造成的误差会进一步增大，所以笔者考虑将状态转移矩阵设置为4阶单位矩阵，增加控制输入向量，由控制输入向量的变化来形成状态量的递推。则系统方程建立如下：∀k≥1

Xk=φk,k-1Xk-1+Γk,k-1Wk-1+uk-1(3)

Zk=HkXk+Vk(4)

其中φk,k-1=I4×4，Γk,k-1=I4×4，Wk-1假设为4维白噪声。控制输入向量uk-1的确定方法如下。

二维运动平台的振动规律设置为：X轴振幅为8 cm，Z轴振幅也为8 cm，每次均设置运动平台的运动初始位置为空间三维坐标系的0点和X轴振动的负最大值点以及Z轴振动的0点，但振动规律的周期和初相角随着运动平台的运动时刻发生变化。故笔者考虑由相邻两个采样点的X坐标和Z坐标来推算出每个采样点处的振动规律的周期和初相角。振动规律用简谐振动方程来模拟：

S=Rcos(ωt+φ)(5)

假设前一个采样点处推算出的周期为T1，初相角为fy1，后一个采样点推算出的周期为T2，初相角为fy2，采样间隔设为T。则可得控制输入向量为：

1.2位置预测算法研究

对运动平台的位姿的预测步骤如下：由相隔一个采样周期的两个相邻状态量(假设为上一时刻和当前时刻)确定X轴、Z轴的运动模型方程，对系统的当前时刻观测值做滤波，得当前时刻的最优系统状态量，再由预测方程预测一个采样周期后或者好几个采样周期后的系统状态量。状态转移矩阵、控制输入矩阵、观测方程确定之后，就能通过卡尔曼滤波获得滤波值，就可以基于滤波值来做预测，最优预测原理如下：所谓随机线性离散系统的最优预测，就是根据系统观测方程所提供的观测数据来对未来时刻的状态向量Xk在给定假设条件下进行最优估计的问题。正交投影定理是研究预测算法的基础。

(7)

将式(8)代入式(7)中：

(9)

所以，向前预测m步，综合的滤波预测公式为：

(11)

2　实际硬件设备上的应用

考虑用电机驱动的两自由度转台来跟踪也由电机驱动的二维运动平台，验证滤波预测方法是否可行。

通信连接如图2所示，全部采用CAN总线通信来实现信息的传递。

二维运动平台DSP芯片CAN通信：二维运动平台位置信息反馈。

转台ARM芯片CAN通信：从总线上接收激光测距传感器的打开、关闭指令，将测距传感器测距信息发送到总线上。

转台DSP芯片CAN通信：接收转台运动指令信息、接收测距传感器测距信息，将测距信息和转台偏转、俯仰角度进行编码并发出。

电脑CAN卡：接收二维运动平台位置信息、坐标编码信息，发出转台运动指令信息(笔者的卡尔曼滤波预测程序使用C语言编写，在电脑上运行)。

图2　通信连接图

实验设计：由于仅在水平方向实现了反馈装置的安装，故验证性实验考虑运动平台仅在水平方向上做平动的运动状态。开始时手动调整转台，使激光测距仪发射的激光通过反射镜打到二维运动平台的接收靶上，以此时二维运动平台位置为坐标系原点，建立空间三维坐标系，并且以此时转台的俯仰角、偏转角均为0点，转台到二维运动平台距离为距离初始值。二维运动平台开始振动后，向PC机发送含有实时X、Z坐标值的CAN消息，可计算出坐标值和距离、俯仰角、偏转角，且由相机及附带的图像处理板对偏转角做修正，由实时坐标值和距离、俯仰角、偏转角做滤波和预测，给出转台下一步需要调整的偏转角和俯仰角。当转台完成调整后，激光再次打到接收靶上时，发回新的测距值及俯仰角、偏转角，然后再得到新的实时运动平台坐标值，使整个跟踪随动过程继续下去。模拟实验图如图3所示，用到的二维运动平台和基站转台如图4、图5所示。

图3　模拟实验图

图4　二维运动平台

图5　基站转台

实验时所编写的程序的流程图及实验所得的X轴方向的跟踪效果图如图6和图7所示，可以看出随着滤波方差矩阵的收敛，跟踪逐渐稳定。

图6　程序流程图

图7　X轴方向跟踪效果图

3　结论

本文在简化的采矿设备运动模型的条件下，将卡尔曼滤波理论应用于井下无人采矿设备的定位与导航,利用硬件设备搭建了一套实验定位导航系统，探究了这套系统在井下使用的可行性。最终在实验室环境下实现了利用扩展卡尔曼滤波方法预测运动设备的位姿并使基站发射的激光束跟踪运动设备，下一步即是移植到井下看能否使用成功。由于转台上实验了不同的激光定位跟踪系统，使得转台的俯仰方向的机械结构需要再次设计，由于时间原因，所以只做了水平方向激光定位跟踪的实验,实验结果如图7。由于在实际情况下，井下作业的采矿设备的运动速度并不快，典型的铲运机的平均运动速度约为1 m/s，且可以通过各种方法使系统过程噪声和测量噪声尽量减小，由图7可看出，在这样的条件下，跟踪效果较好，设计的算法可预报运动设备的位姿，实现定位与导航。下一步工作分为两方面：一是由于CAN通信受线缆的限制，在井下应用起来并不十分方便，考虑将采矿设备的运动信息包含到激光束中，由激光发射器和接收器组成激光通信系统，替代CAN通信；二是考虑将算法移植到基站转台的ARM芯片或DSP芯片中，提高算法的实时性，从而提高定位精度。

[1]战凯，顾洪枢，周俊武等，地下遥控铲运机遥控技术和精确定位技术研究[J]．有色金属，2009，61(1)：107-112.

[2]富立, 范耀祖. 车辆定位导航系统——智能交通系统[M]. 北京:中国铁道出版社, 2004.

[3]周宏仁，敬忠良，王培德.机动目标跟踪[M].北京：国防工业出版社，1991.

[4]Kalman B R E. A new approach to linear filtering and predition problems[J]. Journal of Basic Engineering, 2012,82(1):167 - 179.

[5]Maybeck P S. Stochastic models, estimation and control[ M] . NewYork: Academic Press, 1982: 129.

[6]张国良，曾静.组合导航原理与技术[M].西安：西安交通大学出版社，2008.

[7]赵亦林著，谭国真译.车辆定位与导航系统[M].北京：电子工艺出版社，1999.

[8]Ge L, Han S, Rizos C. Multipath mitigation of continuous GPS measurements using an adaptive filter[J].GPS Solution,2000,4(2):19-30.

[9]付梦印，邓志红，闫莉萍. Kalman滤波理论及其在导航系统中的应用[M].北京：科学出版社，2003.

[Abstract]In order to make predictions more accurate about the position of the unmanned mining equipment and track the states of it in real time, a prediction method of the position of autonomous mining vehicle based on Kalman filter in the laser positioning and navigation system is proposed. To verify the feasibility, we make up a simple positioning system in our laboratory and carry out verification experiment within this system. The system uses a turntable which can horizontally rotate and tilt as the location tracking base station, two-dimensional motion platform which can play translational and vertical movement as the moving target (actual moving target is one kind of the unmanned mining vehicles). It applies Kalman filter technology to do filtering and prediction with the coordinate information of the moving target, uses the predicted value as the set value of the tracking parameter of the turntable, so as to make the Laser emitted from the base station steadily and accurately track the moving target.

[Key words]Kalman filter； filtering and denoising； prediction and tracking

[责任编辑刘景平]

Study of the State Prediction of Unmanned Mining Equipment Based on the Kalman Filter

WU Xing， HE Chun-lin， SHI Yi-hong

(School of Computer Science, China West Normal University, Nanchong, Sichuan 637002, China)

TP301

A

1672-9021(2016)02-0088-06

邬兴(1990-)，女，四川广元人，西华师范大学计算机学院硕士研究生，主要研究方向：卡尔曼滤波器设计、基于计算机的网络应用；贺春林(1971-)，男，四川广安人，西华师范大学计算机学院教授，硕士生导师，主要研究方向：基于计算机的网络应用。

2016-03-02