[发明专利]一种基于多传感器数据融合的小车实时姿态检测方法

权利要求书

1.一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，包括以下步骤，

1前置步骤一，得到卡尔曼滤波器：

具体为，

(101)设置包括直线、转向的轨道，并在特定位置放置多个标志物；

(102)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，测量小车与起始点的直线距离，得到固定点与起始点间的路程序列r_i、图像序列f_i、位移序列与位置及方向序列

(103)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(104)步；否则采集下一组数据，再次执行当前所在的第(103)步；

(104)初始化卡尔曼滤波器；

(105)根据测量数据，计算下一个位置的位移方位及所经历的路程及其误差协方差矩阵

(106)计算卡尔曼滤波器的增益更新值K_i；

(107)根据图像g_i中的特征物，计算当前小车位置，确定当前正处在哪一个固定点；

(108)根据第(102)步中采集的小车的方位、位移及经过的路程，向前推算误差协方差矩阵P_i，P_i＝(1-HX′_i)H代表观测矩阵，X′_i代表i时刻小车的预测状态值；

(109)若所有数据尚未全部计算完成，则转第(105)步；否则得到可用的卡尔曼滤波器；

其中，r_i代表小车在固定点与起始点间运行路程在i时刻的值；f_i代表相机在i时刻拍摄的激光线图片；s_i代表小车在i时刻的位移向量；p_i代表小车在i时刻的位置及方向向量；

2前置步骤二，训练神经网络：

(201)设置包括直线、转向的轨道，并在特定位置放置多个标志物；

(202)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，静态测量小车与起始点的直线距离，得到固定点与起始点间的路程序列位移序列与方位序列

(203)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(204)步；否则采集下一组数据，再次执行当前所在的第(203)步；

(204)利用卡尔曼滤波器，根据三轴加速度仪及三轴陀螺仪测量数据，计算处于固定点位置时，小车的方位、位移及距离；

(205)根据光电编码器采集的数据，计算小车的运动路程C；

(206)将第(204)、(205)步计算结果作为神经网络的输入，第(202)步采集的数据作为准确值，训练神经网络；

(207)多次重复上述过程，直到神经网络收敛稳定，得到可以融合多个传感器数据的神经网络；

3实时检测：

(301)通过三轴加速度仪和三轴陀螺仪采样当前小车的实时检测数据；

(302)将三轴陀螺仪及三轴加速度仪测量的结果输入卡尔曼滤波器，计算当前小车的方位、位移及距离；

(303)编码器采样并计算小车运动路程；

(304)将第(302)、(303)步实时检测到的数据作为神经网络输入，计算当前小车姿态；

(305)相机采集图像并提取特征物轮廓；

(306)根据特征物轮廓计算小车的实时姿态D；

(307)对步骤(304)和(306)得到的计算结果作差，若相差值不小于设定的阈值，重新计算卡尔曼滤波器的更新值，转至步骤(302)，否则继续测量，直到测量结束。