本发明涉及导航定位技术领域，具体涉及一种多传感器数据融合的方法及agv导航定位装置。
背景技术
agv是(automatedguidedvehicle)的缩写，意即“自动导引运输车”，是指装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。广泛的应用于物流仓储输送环节中。
传统的工业agv利用单一传感器作为agv导引决策技术方案比较普遍，这就造成了导航装置信息数据来源的可信度和有效性大大降低，进而影响到系统整体的引导决策能力。
技术实现要素：
本发明的目的是提供一种多传感器数据融合的agv导航定位装置，以解决现有技术利用单一传感器的agv存在的测量误差大的问题；同时提供一种多传感器数据融合的方法。
为实现上述目的，本发明多传感器数据融合的agv导航定位装置采用如下技术方案：多传感器数据融合的agv导航定位装置，包括agv车体及布置在agv车体上的传感器，传感器包括设置在agv车体左右两侧的第一红外传感器、第二红外传感器以及设置在agv车体前部的超声波传感器，超声波传感器有3～5个；采用扩展卡尔曼滤波算法对多传感器数据信息进行融合，具体地，1)设定agv车体运动状态方程及测量方程，分别为：x＝f(x，u)+w，z＝h(x，v)；其中，u＝[u1,u2,u3…um]为m维输入变量；w＝[w1,w2,w3…wn]为n维的系统零均值的白色高斯激励噪声序列；z＝[z1,z2,z3…zj]为j维系统输出变量，代表传感器的测量值；v＝[v1,v2,v3…vs]为s维零均值的白色高斯测量噪声，表征测量方程与真实值之间的误差；2)先对非线性系统进行线性化，后离散化，推导扩展卡尔曼滤波方程；3)系统的卡尔曼滤波方程可以表征为：
4)采用基于扩展卡尔曼滤波算法对系统方程初始化，
其中，
系统的测量方程中测量噪声协方差矩阵为：
其中，r1，r2，…rn是系统中各个传感器测量误差的协方差的值；
5)设系统k时刻的状态估计值和与之相应的协方差矩阵分别为
6)对k+1时刻的测量方程估计值和与之对应的协方差矩阵进行一步预测，如下：
其中，
7)根据k+1时刻的观测值，对系统状态进行更新，k+1时刻系统状态最优估计为：
kk+1＝pk+1|k(hk+1)t[hk+1pk+1|k(hk+1)t+rk+1]t
其中，kk+1为k+1时刻的kalman增益；与此同时，系统更新k+1时刻的协方差：
进一步地，所述超声波传感器有5个，分别设置在agv车体右前方、前右方、正前方、前左方、左前方。
进一步地，步骤2)中，线性化方程：
xk＝fxk+wk
其中，
离散化状态方程：
进一步地，测量方程的线性化方程与离散化方程一致，
其中，
本发明多传感器数据融合的方法采用如下技术方案，包括以下步骤，1)设定agv车体运动状态方程及测量方程，分别为：x＝f(x，u)+w，z＝h(x，v)；其中，u＝[u1,u2,u3…um]为m维输入变量；w＝[w1,w2,w3…wn]为n维的系统零均值的白色高斯激励噪声序列；z＝[z1,z2,z3…zj]为j维系统输出变量，代表传感器的测量值；v＝[v1,v2,v3…vs]为s维零均值的白色高斯测量噪声，表征测量方程与真实值之间的误差；2)先对非线性系统进行线性化，后离散化，推导扩展卡尔曼滤波方程；3)系统的卡尔曼滤波方程可以表征为：
4)采用基于扩展卡尔曼滤波算法对系统方程初始化，
其中，
系统的测量方程中测量噪声协方差矩阵为：
其中，r1，r2，…rn是系统中各个传感器测量误差的协方差的值；
5)设系统k时刻的状态估计值和与之相应的协方差矩阵分别为
6)对k+1时刻的测量方程估计值和与之对应的协方差矩阵进行一步预测，如下：
其中，
7)根据k+1时刻的观测值，对系统状态进行更新，k+1时刻系统状态最优估计为：
kk+1＝pk+1|k(hk+1)t[hk+1pk+1|k(hk+1)t+rk+1]t
其中，kk+1为k+1时刻的kalman增益；与此同时，系统更新k+1时刻的协方差：
进一步地，步骤2)中，线性化方程：
xk＝fxk+wk
其中，
离散化状态方程：
进一步地，测量方程的线性化方程与离散化方程一致，
其中，
本发明的有益效果：与现有技术中单一传感器系统相比，本发明采用多传感器融合方法进行导航定位，具有以下优势：首先，由于使用多传感器，若某个传感器无法正常工作或单独处理的结果错误时，则可利用其他传感器的冗余信息对失真信息进行纠正，提高系统的容错能力和数据可信度，降低数据获取成本。其次，本发明中采用扩展卡尔曼滤波算法对系统状态测量数据进行实时跟踪预测，提高了定位精度，同时也降低了系统硬件成本。本发明的方案提高了导航定位的数据可靠性和系统运行精度。
附图说明
图1是agv车体上传感器布局示意图；
图2是agv在系统坐标系中的运动模型；
图3是agv导航系统多传感器数据融合系统框图；
图4是基于扩展卡尔曼滤波的多传感器数据融合工作方式流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
本发明多传感器数据融合的agv导航定位装置的实施例：如图1-图4所示，包括agv车体及布置在agv车体上的7组传感器，传感器包括设置在agv车体左右两侧的第一红外传感器s1、第二红外传感器s7以及设置在agv车体前部的超声波传感器。超声波传感器有5个，分别设置在agv车体右前方、前右方、正前方、前左方、左前方。超声传感器分别标记为s2～s5。
工程实践中所遇到的物理系统数学模型往往是非线性系统，而扩展卡尔曼滤波方程的推导有两条途径，一是先进行非线性连续系统的离散化后线性化，另外是先对非线性连续系统进行线性化，后离散化。先线性化后离散化比先离散再线性化的滤波方程计算更加简便，本发明中采用先对非线性系统线性化、后离散化的方式。
本发明采用扩展卡尔曼滤波算法对多传感器数据信息进行融合，具体地，1)设定agv车体运动状态方程及测量方程，分别为：x＝f(x，u)+w，z＝h(x，v)；其中，u＝[u1,u2,u3…um]为m维输入变量；w＝[w1,w2,w3…wn]为n维的系统零均值的白色高斯激励噪声序列；z＝[z1,z2,z3…zj]为j维系统输出变量，代表传感器的对地标的测量值；v＝[v1,v2,v3…vs]为s维零均值的白色高斯测量噪声，表征测量方程与真实值之间的误差。
如图2所示，agv在系统坐标系中的运动模型，xk、yk、θk分别表示agv在k时刻系统的横坐标、纵坐标和位姿角度。在系统状态向量中我们假设系统单位状态向量为x＝[δdx,δdy,δθ]t,则系统在k+1时刻的状态向量可以表示为xk+1,因此本系统的状态方程可以表征为：
zk＝hxk+vk，
zk为agv系统中布置的传感器对路标测量的观测值。
(2)、线性化、离散化处理：
通过一阶泰勒级数展开对它做近似线性化处理
xk＝fxk+wk
其中：
再对其进行离散化，离散化状态方程为：
同样的，通过一阶线性泰勒级数展开对测量方程做近似线性化处理可得：
其中：
测量方程的离散化方程与线性化方程一致，即为：
(3)、综合以上可知，系统的卡尔曼滤波方程可以表征为：
(4)、经过上述过程，得到经过线性化和离散化的系统状态方程和测量方程，采用基于扩展卡尔曼滤波算法对系统方程进行初始化、预测及时间更新。
首先对系统进行初始化，即：
其中，
其中qk即为系统噪声wk在k时刻的协方差矩阵。
系统的测量方程中测量噪声协方差矩阵为：
其中，r1，r2，…rn是系统中各个传感器测量误差的协方差的值。
设本系统k时刻的状态估计值和与之相应的协方差矩阵分别为
下一步，对k+1时刻的测量方程估计值和与之对应的协方差矩阵进行一步预测，如下：
其中，
下一步，根据k+1时刻的测量值，对系统状态进行更新，k+1时刻系统状态最优估计为：
其中，kk+1为k+1时刻的kalman增益，kalman增益使系统估计的协方差误差矩阵达到最小的最佳增益阵。与此同时，系统更新k+1时刻的协方差：
通过上述步骤，不断的进行kalman滤波的迭代更新，获取agv系统位姿的最优状态估计值，消除系统中产生的误差，提高系统预测精度。
本发明agv定位装置的原理及效果，简述如下。agv车体感知所处的周围环境信息，经过车体上布置的传感器进行信号采集，由agv车上传感器采集到的信号进行温度补偿(针对超声波传感器)、信号放大、电平转换等预处理后，采用本发明中使用的扩展卡尔曼滤波算法对多传感器数据信息进行融合，最终得出融合结果，来预测agv在该时刻的导航定位信息。与单一传感器系统相比，本发明采用多传感器融合方法进行导航定位，具有以下优势：当使用多传感器时，若某个传感器无法正常工作或单独处理的结果错误时，则可利用其他传感器的冗余信息对失真信息进行纠正，提高系统的容错能力和数据可信度，降低数据获取成本；其次，本发明中采用扩展卡尔曼滤波算法对系统状态测量数据进行实时跟踪预测，提高了定位精度，同时也降低了系统硬件成本。本发明提高了组合导航定位的数据可靠性和系统运行精度。
本发明不局限于上述最佳实施方式，任何人在本发明的启示下都可得出其他形式的产品或装置，但不论在其形状或结构上作任何变化，凡是具有与本申请相同或相近似的技术方案，均落在本发明的保护范围之内。