CN113029138B - 一种基于多传感器数据融合的小车实时姿态检测方法 - Google Patents

一种基于多传感器数据融合的小车实时姿态检测方法 Download PDF

Info

Publication number
CN113029138B
CN113029138B CN202110360552.3A CN202110360552A CN113029138B CN 113029138 B CN113029138 B CN 113029138B CN 202110360552 A CN202110360552 A CN 202110360552A CN 113029138 B CN113029138 B CN 113029138B
Authority
CN
China
Prior art keywords
trolley
time
real
data
attitude
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110360552.3A
Other languages
English (en)
Other versions
CN113029138A (zh
Inventor
赵俊龙
张雅欣
张康茹
魏子哲
张晓斌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Yangzhou University
Original Assignee
Yangzhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Yangzhou University filed Critical Yangzhou University
Priority to CN202110360552.3A priority Critical patent/CN113029138B/zh
Publication of CN113029138A publication Critical patent/CN113029138A/zh
Application granted granted Critical
Publication of CN113029138B publication Critical patent/CN113029138B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Biomedical Technology (AREA)
  • Software Systems (AREA)
  • Evolutionary Biology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Mathematical Physics (AREA)
  • Health & Medical Sciences (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Navigation (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Analysis (AREA)
  • Gyroscopes (AREA)

Abstract

本发明公开了隧道检测技术领域内的一种基于多传感器数据融合的小车实时姿态检测方法，包括以下步骤，（1）通过三轴加速度仪和三轴陀螺仪采样当前小车的实时检测数据；（2）采用卡尔曼滤波器对实时检测数据滤波；（3）计算当前小车的运动状态；（4）编码器采样并计算小车运动路程；（5）将上述数据作为神经网络输入，计算当前小车姿态；（6）相机采集图像并提取特征物轮廓；（7）根据特征物轮廓计算小车的实时姿态D；（8）根据D的数据，重新计算卡尔曼滤波器得更新值，并微调第（5）步的输出结果；使用本发明可提高检测精度。

Description

一种基于多传感器数据融合的小车实时姿态检测方法
技术领域
本发明属于隧道检测技术领域，特别涉及一种基于多传感器数据融合的小车实时姿态检测方法。
背景技术
随着城市化进程及城市的大规模发展，我国正加紧地铁建设以解决日益严重的交通堵塞问题。由于隧道规模的快速扩大，隧道检测工作量随之骤增，加上中国幅域辽阔，从东到西、从南到北的地质构造、自然环境差异巨大，周围环境的变化以及列车运行时的震动等各种因素的影响，地铁隧道会出现各种病害，在地铁施工和运营维护过程中，隧道检测车的姿态及里程检测是一项极其重要的工作。随着姿态检测车不断运动，实时姿态检测的误差值不断累积，造成计算结果在很短的时间内迅速发散。因此，对误差的校正是测量系统的关键。
现有技术中，实时姿态检测方法融合陀螺仪和加速度计传感器采集的数据来减小误差，以此来提高精度，但是随着时间的推移，误差值会不断累积，造成结果偏差较大，对最终的隧道测量产生影响。
发明内容
本发明的目的在于，克服现有技术中的不足之处，提供一种基于多传感器数据融合的小车实时姿态检测方法，解决了现有技术中测量误差大的技术难题，使用本发明检测误差小，检测更加稳定，检测精度更高。
本发明的目的是这样实现的：一种基于多传感器数据融合的小车实时姿态检测方法，包括以下步骤，
1前置步骤一，得到卡尔曼滤波器：
具体为，
(101)设置包括直线、转向的轨道，并在特定位置放置多个标志物；
(102)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，测量小车与起始点的直线距离，得到固定点与起始点间的路程序列<ri>、图像序列<fi>、位移序列
Figure GDA0003720172480000021
与位置及方向序列
Figure GDA0003720172480000022
(103)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(104)步；否则采集下一组数据，再次执行当前所在的第(103)步；
(104)初始化卡尔曼滤波器；
(105)根据测量数据，计算下一个位置的位移
Figure GDA0003720172480000023
方位
Figure GDA0003720172480000024
及所经历的路程
Figure GDA0003720172480000025
及其误差协方差矩阵
Figure GDA0003720172480000026
(106)计算卡尔曼滤波器的增益更新值Ki；
(107)根据图像gi中的特征物，计算当前小车位置，确定当前正处在哪一个固定点；
(108)根据第(102)步中采集的小车的方位、位移及经过的路程，向前推算误差协方差矩阵Pi；
(109)若所有数据尚未全部计算完成，则转第(105)步；否则得到可用的卡尔曼滤波器；
其中，<ri>代表小车在固定点与起始点间运行路程在i时刻的值； <fi>代表相机在i时刻拍摄的激光线图片；
Figure GDA0003720172480000031
代表小车在i时刻的位移向量；
Figure GDA0003720172480000032
代表小车在i时刻的位置及方向向量；
2前置步骤二，训练神经网络：
(201)设置包括直线、转向的轨道，并在特定位置放置多个标志物；
(202)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，静态测量小车与起始点的直线距离，得到固定点与起始点间的路程序列
Figure GDA0003720172480000033
位移序列
Figure GDA0003720172480000034
与方位序列
Figure GDA0003720172480000035
(203)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(204)步；否则采集下一组数据，再次执行当前所在的第(203)步；
(204)利用卡尔曼滤波器，根据三轴加速度仪及三轴陀螺仪测量数据，计算处于固定点位置时，小车的方位、位移及距离；
(205)根据光电编码器采集的数据，计算小车的运动路程；
(206)将第(204)、(205)步计算结果作为神经网络的输入，第(202) 步采集的数据作为准确值，训练神经网络；
(207)多次重复上述过程，直到神经网络收敛稳定，得到可以融合多个传感器数据的神经网络；
3实时检测：
(301)通过三轴加速度仪和三轴陀螺仪采样当前小车的实时检测数据；
(302)将三轴陀螺仪及三轴加速度仪测量的结果输入卡尔曼滤波器，计算当前小车的方位、位移及距离；
(303)编码器采样并计算小车运动路程；
(304)将第(302)、(303)步实时检测到的数据作为神经网络输入，计算当前小车姿态；
(305)相机采集图像并提取特征物轮廓；
(306)根据特征物轮廓计算小车的实时姿态；
(307)对步骤(304)和(306)得到的计算结果作差，若相差值不小于设定的阈值后，重新计算卡尔曼滤波器的更新值，转至步骤(302)，否则继续测量，直到测量结束。
作为本发明的进一步改进，所述步骤(103)中，获取各个测量值的步骤具体为，
(103a)编码器采集当前数据xi；
(103b)将数据xi、时间间隔Δti以及编码器的采集的前一次的最终数据
Figure GDA0003720172480000041
路程
Figure GDA0003720172480000042
送入编码器的私有神经网络，计算小车行走的路程
Figure GDA0003720172480000043
(103c)摄像头采集当前图像gi；
(103d)从图像gi中提取特征物的轮廓，并二值化后得到bi，根据公式 bi＝Hib0，计算得到Hi；
(103e)将Hi输入到相机的私有神经网络，得到小车的位置
Figure GDA0003720172480000044
及姿态
Figure GDA0003720172480000051
(103f)将小车所处位置
Figure GDA0003720172480000052
”及姿态
Figure GDA0003720172480000053
输入到相机的私有神经网络，计算得到小车的位置
Figure GDA0003720172480000054
及姿态
Figure GDA0003720172480000055
(103g)从三轴加速度仪采集加速度数据
Figure GDA0003720172480000056
(103h)从三轴传感器采集当前姿态数据
Figure GDA0003720172480000057
(103i)将前一次计算得到加速度数据
Figure GDA0003720172480000058
姿态数据
Figure GDA0003720172480000059
计算得到的速度vi-1、位置
Figure GDA00037201724800000510
路程
Figure GDA00037201724800000511
及当前的
Figure GDA00037201724800000512
时间间隔Δti输入两个传感器共用的神经网络，计算得到小车的位置
Figure GDA00037201724800000513
及姿态
Figure GDA00037201724800000514
及路程
Figure GDA00037201724800000515
(103j)将步骤(103b)、(103f)和(103i)得到的数据进行算术平均，得到当前步骤的小车的位置
Figure GDA00037201724800000516
及姿态
Figure GDA00037201724800000517
及路程
Figure GDA00037201724800000518
(103k)将步骤(103j)的计算结果更新到各个传感器；
(1031)计算当前小车的速度
Figure GDA00037201724800000519
bi为i时刻相机采集到的特征物图像经二值化后得到特征物轮廓，Hi为i时刻计算得到的转换矩阵，
Figure GDA00037201724800000520
为i时刻相机神经网络计算出的小车的位置向量，
Figure GDA00037201724800000521
为相机神经网络计算出的i时刻小车的姿态向量，
Figure GDA00037201724800000522
为i-1时刻算术平均后得到的小车的加速度向量，
Figure GDA00037201724800000523
为i-1时刻算术平均后得到的小车的姿态向量，
Figure GDA0003720172480000061
为i-1时刻算术平均后得到的小车的位置向量，
Figure GDA0003720172480000062
为i-1 时刻算术平均后得到的小车的行驶路程值，
Figure GDA0003720172480000063
为i时刻传感器神经网络输出的小车的位置向量，
Figure GDA0003720172480000064
为i时刻传感器神经网络输出的小车的姿态向量，
Figure GDA0003720172480000065
为i时刻传感器神经网络输出的小车的行驶路程值，
Figure GDA0003720172480000066
为i时刻算术平均后得到的小车的位置向量，
Figure GDA0003720172480000067
为i时刻算术平均后得到的小车的姿态向量，
Figure GDA0003720172480000068
为i时刻算术平均后得到的小车的行驶路程值。
作为本发明的进一步改进，所述步骤(104)中，初始化卡尔曼滤波器的步骤具体为，
定义预测方程：
Figure GDA0003720172480000069
P′i＝APi-1AT+Q (3)；
定义校正方程：
Xi＝X′i+Ki(Zi-HX′i) (4)；
其中，Xi代表i时刻小车的状态值，包括加速度数据
Figure GDA00037201724800000610
姿态数据
Figure GDA00037201724800000611
位置
Figure GDA00037201724800000612
以及路程
Figure GDA00037201724800000613
Figure GDA00037201724800000614
代表i时刻小车的预测状态值；
Figure GDA00037201724800000615
代表i时刻的优化预测状态；Zi代表i时刻的观测值；A代表状态转移方程；B代表输入控制矩阵；Q代表预测噪声协方差矩阵；H代表观测矩阵；P代表误差矩阵；ui代表i时刻外界对系统的作用，P′i为误差协方差矩阵预测值，AT为状态转移方程的转置，Pi-1为i-1时刻的误差协方差矩阵值，Ki为i时刻的卡尔曼增益值，X′i为i时刻小车的状态预测值。
作为本发明的进一步改进，所述步骤(105)中，计算误差协方差矩阵步骤具体为，
误差协方差矩阵公式：
Figure GDA0003720172480000071
其中，
Figure GDA0003720172480000072
代表i时刻的状态信息，
Figure GDA0003720172480000073
E[]表示数学期望，
Figure GDA0003720172480000074
为
Figure GDA0003720172480000075
的转置。
作为本发明的进一步改进，所述步骤(106)中，计算卡尔曼滤波器的增益更新值具体为，
Ki＝PiHT(HP′iHT+R)-1 (7)；
其中，Ki代表i时刻的卡尔曼增益；H代表观测矩阵，HT为H的转置， Pi为i时刻的误差协方差矩阵值；R代表观测噪声协方差矩阵。
为了进一步实现小车当前位置的检测，所述步骤(107)中，根据图像中的特征物，计算当前小车位置步骤具体为，
(107a)设一像点p(u，v)，其空间对应点为P(x，y，z)，用下式建立P 点二维像坐标与三维空间坐标的对应关系，
Figure GDA0003720172480000081
(107b)通过四组样本点对，求出转换矩阵T’，用(u，v)表示像空间一点，对应物空间坐标为(Y，Z)，则可以用平方回归多项式表示像空间与物空间的对应关系，
Figure GDA0003720172480000082
(107c)对于已知物空间的标定点(Yk，Zk)，Y(u，v)、Z(u，v)的误差函数分别为，
Figure GDA0003720172480000083
误差函数Ey、Ez达到最小，则令
Figure GDA0003720172480000084
(11)，
求解方程组(11)，求出回归系数aij和bij，建立物像对应关系；
其中，n为多项式的次数，m为物空间的标定点数。
作为本发明的进一步改进，所述步骤(108)中，Pi＝(1-HX′i)(12)。
作为本发明的进一步改进，所述步骤(204)中，计算处于固定点位置时小车的方位、位移及距离步骤具体为，
采用步骤(109)得到的可用的卡尔曼滤波器，对Xi进行滤波，代入传感器共用的神经网络，得到小车的方位、位移和距离。
为了进一步降低测量误差，所述步骤(307)中，重新计算卡尔曼滤波器的更新值结果步骤具体为，
(307a)若神经网络输出的小车实时姿态值和根据特征物轮廓计算出的小车的实时姿态值相差达到一定阈值，即
Figure GDA0003720172480000091
(13)；
(307b)采用公式(7)重新计算卡尔曼滤波器的增益更新值Ki；
其中，
Figure GDA0003720172480000092
代表神经网络输出的小车实时姿态值，
Figure GDA0003720172480000093
代表根据特征物轮廓计算出的小车的实时姿态值，η代表阈值。
本发明与现有技术相比，具有的技术效果为，通过三轴加速度仪和三轴陀螺仪采样当前小车的实时检测数据后通过卡尔曼滤波器对其进行滤波，同时通过编码器采样并计算小车运动路程，将上述数据作为神经网络的输入，计算小车当前姿态，使用相机采集图像并提取特征物轮廓后计算出小车的实时状态，重新计算卡尔曼滤波器的更新值，提高实时姿态计算的准确性；使用本发明降低实时检测误差，检测更加稳定，检测精度更高；可应用于隧道检测时小车的实时姿态检测的工作中。
附图说明
图1为本发明的流程框图。
具体实施方式
下面结合附图对本发明进行进一步说明。
一种基于多传感器数据融合的小车实时姿态检测方法，包括以下步骤， 1前置步骤一，得到卡尔曼滤波器：
具体为，
(101)设置包括直线、转向的轨道，并在特定位置放置多个标志物；
(102)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，测量小车与起始点的直线距离，得到固定点与起始点间的路程序列<ri>、图像序列<fi>、位移序列
Figure GDA0003720172480000101
与位置及方向序列
Figure GDA0003720172480000102
(103)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(104)步；否则采集下一组数据，再次执行当前所在的第(103)步；
(104)初始化卡尔曼滤波器；
(105)根据测量数据，计算下一个位置的位移
Figure GDA0003720172480000103
方位
Figure GDA0003720172480000104
及所经历的路程
Figure GDA0003720172480000105
及其误差协方差矩阵
Figure GDA0003720172480000106
(106)计算卡尔曼滤波器的增益更新值Ki；
(107)根据图像gi中的特征物，计算当前小车位置，确定当前正处在哪一个固定点；
(108)根据第(102)步中采集的小车的方位、位移及经过的路程，向前推算误差协方差矩阵Pi；
(109)若所有数据尚未全部计算完成，则转第(105)步；否则得到可用的卡尔曼滤波器；
其中，<ri>代表小车在固定点与起始点间运行路程在i时刻的值；<fi>代表相机在i时刻拍摄的激光线图片；
Figure GDA0003720172480000111
代表小车在i时刻的位移向量；
Figure GDA0003720172480000112
代表小车在i时刻的位置及方向向量；
2前置步骤二，训练神经网络：
(201)设置包括直线、转向的轨道，并在特定位置放置多个标志物；
(202)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，静态测量小车与起始点的直线距离，得到固定点与起始点间的路程序列
Figure GDA0003720172480000113
位移序列
Figure GDA0003720172480000114
与方位序列
Figure GDA0003720172480000115
(203)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(204)步；否则采集下一组数据，再次执行当前所在的第(203)步；
(204)利用卡尔曼滤波器，根据三轴加速度仪及三轴陀螺仪测量数据，计算处于固定点位置时，小车的方位、位移及距离；
(205)根据光电编码器采集的数据，计算小车的运动路程；
(206)将第(204)、(205)步计算结果作为神经网络的输入，第(202) 步采集的数据作为准确值，训练神经网络；
(207)多次重复上述过程，直到神经网络收敛稳定，得到可以融合多个传感器数据的神经网络；
3实时检测：
(301)通过三轴加速度仪和三轴陀螺仪采样当前小车的实时检测数据；
(302)将三轴陀螺仪及三轴加速度仪测量的结果输入卡尔曼滤波器，计算当前小车的运动状态A，运动状态包括方位、位移及距离；
(303)编码器采样并计算小车运动路程C；
(304)将第(302)、(303)步实时检测到的数据作为神经网络输入，计算当前小车姿态；
(305)相机采集图像并提取特征物轮廓；
(306)根据特征物轮廓计算小车的实时姿态D；
(307)对步骤(304)和(306)得到的计算结果作差，若相差值不小于设定的阈值，重新计算卡尔曼滤波器的更新值，转至步骤(302)，否则继续测量，直到测量结束。
其中，步骤(103)中，获取各个测量值的步骤具体为，
(103a)编码器采集当前数据xi；
(103b)将数据xi、时间间隔Δti以及编码器的采集的前一次的最终数据
Figure GDA0003720172480000121
路程
Figure GDA0003720172480000122
送入编码器的私有神经网络，计算小车行走的路程
Figure GDA0003720172480000123
(103c)摄像头采集当前图像gi；
(103d)从图像gi中提取特征物的轮廓，并二值化后得到bi，根据公式 bi＝Hib0，计算得到Hi；
(103e)将Hi输入到相机的私有神经网络，得到小车的位置
Figure GDA0003720172480000124
及姿态
Figure GDA0003720172480000125
(103f)将小车所处位置
Figure GDA0003720172480000126
”及姿态
Figure GDA0003720172480000127
输入到相机的私有神经网络，计算得到小车的位置
Figure GDA0003720172480000131
及姿态
Figure GDA0003720172480000132
(103g)从三轴加速度仪采集加速度数据
Figure GDA0003720172480000133
(103h)从三轴传感器采集当前姿态数据
Figure GDA0003720172480000134
(103i)将前一次计算得到加速度数据
Figure GDA0003720172480000135
姿态数据
Figure GDA0003720172480000136
计算得到的速度vi-1、位置
Figure GDA0003720172480000137
路程
Figure GDA0003720172480000138
及当前的
Figure GDA0003720172480000139
时间间隔Δti输入两个传感器共用的神经网络，计算得到小车的位置
Figure GDA00037201724800001310
及姿态
Figure GDA00037201724800001311
及路程
Figure GDA00037201724800001312
(103j)将步骤(103b)、(103f)和(103i)得到的数据进行算术平均，得到当前步骤的小车的位置
Figure GDA00037201724800001313
及姿态
Figure GDA00037201724800001314
及路程
Figure GDA00037201724800001315
(103k)将步骤(103j)的计算结果更新到各个传感器；
(1031)计算当前小车的速度
Figure GDA00037201724800001316
bi为i时刻相机采集到的特征物图像经二值化后得到特征物轮廓，Hi为i时刻计算得到的转换矩阵，
Figure GDA00037201724800001317
为i时刻相机神经网络计算出的小车的位置向量，
Figure GDA00037201724800001318
为相机神经网络计算出的i时刻小车的姿态向量，
Figure GDA00037201724800001319
为i-1时刻算术平均后得到的小车的加速度向量，
Figure GDA00037201724800001320
为i-1时刻算术平均后得到的小车的姿态向量，
Figure GDA00037201724800001321
为i-1时刻算术平均后得到的小车的位置向量，
Figure GDA00037201724800001322
为i-1 时刻算术平均后得到的小车的行驶路程值，
Figure GDA00037201724800001323
为i时刻传感器神经网络输出的小车的位置向量，
Figure GDA0003720172480000141
为i时刻传感器神经网络输出的小车的姿态向量，
Figure GDA0003720172480000142
为i时刻传感器神经网络输出的小车的行驶路程值，
Figure GDA0003720172480000143
为i时刻算术平均后得到的小车的位置向量，
Figure GDA0003720172480000144
为i时刻算术平均后得到的小车的姿态向量，
Figure GDA0003720172480000145
为i时刻算术平均后得到的小车的行驶路程值。
步骤(104)中，初始化卡尔曼滤波器的步骤具体为，
定义预测方程：
Figure GDA0003720172480000146
P′i＝APi-1AT+Q (3)；
定义校正方程：
Xi＝X′i+Ki(Zi-HX′i) (4)；
其中，Xi代表i时刻小车的状态值，包括加速度数据
Figure GDA0003720172480000147
姿态数据
Figure GDA0003720172480000148
位置
Figure GDA0003720172480000149
以及路程
Figure GDA00037201724800001410
Figure GDA00037201724800001411
代表i时刻小车的预测状态值；
Figure GDA00037201724800001412
代表i时刻的优化预测状态；Zi代表i时刻的观测值；A代表状态转移方程；B代表输入控制矩阵；Q代表预测噪声协方差矩阵；H代表观测矩阵；P代表误差矩阵；ui代表i时刻外界对系统的作用，P′i为误差协方差矩阵预测值，AT为状态转移方程的转置，Pi-1为i-1时刻的误差协方差矩阵值，Ki为i时刻的卡尔曼增益值，X′i为i时刻小车的状态预测值。
步骤(105)中，计算误差协方差矩阵步骤具体为，
误差协方差矩阵公式：
Figure GDA0003720172480000151
其中，
Figure GDA0003720172480000152
代表i时刻的状态信息，
Figure GDA0003720172480000153
E[]表示数学期望，
Figure GDA0003720172480000154
为
Figure GDA0003720172480000155
的转置。
作为本发明的进一步改进，所述步骤(106)中，计算卡尔曼滤波器的增益更新值具体为，
Ki＝PiHT(HP′iHT+R)-1 (7)；
其中，Ki代表i时刻的卡尔曼增益；H代表观测矩阵，HT为H的转置， Pi为i时刻的误差协方差矩阵值；R代表观测噪声协方差矩阵。
为了进一步实现小车当前位置的检测，所述步骤(107)中，根据图像中的特征物，计算当前小车位置步骤具体为，
(107a)设一像点p(u，v)，其空间对应点为P(x，y，z)，用下式建立P 点二维像坐标与三维空间坐标的对应关系，
Figure GDA0003720172480000156
(107b)通过四组样本点对，求出转换矩阵T’，用(u，v)表示像空间一点，对应物空间坐标为(Y，Z)，则可以用平方回归多项式表示像空间与物空间的对应关系，
Figure GDA0003720172480000161
(107c)对于已知物空间的标定点(Yk，Zk)，Y(u，v)、Z(u，v)的误差函数分别为，
Figure GDA0003720172480000162
误差函数Ey、Ez达到最小，则令
Figure GDA0003720172480000163
(11)，
求解方程组(11)，求出回归系数aij和bij，建立物像对应关系；
其中，n为多项式的次数，m为物空间的标定点数。
步骤(108)中，Pi＝(1-HX′i) (12)。
步骤(204)中，计算处于固定点位置时小车的方位、位移及距离步骤具体为，
采用步骤(109)得到的可用的卡尔曼滤波器，对Xi进行滤波，代入传感器共用的神经网络，得到小车的方位、位移和距离。
步骤(307)中，重新计算卡尔曼滤波器的更新值结果步骤具体为，
(307a)若神经网络输出的小车实时姿态值和根据特征物轮廓计算出的小车的实时姿态值相差达到一定阈值，即
Figure GDA0003720172480000164
(13)；
(307b)采用公式(7)重新计算卡尔曼滤波器的增益更新值Ki；
其中，
Figure GDA0003720172480000165
代表神经网络输出的小车实时姿态值，
Figure GDA0003720172480000166
代表根据特征物轮廓计算出的小车的实时姿态值，η代表阈值。
本发明与现有技术相比，具有的技术效果为，通过三轴加速度仪和三轴陀螺仪采样当前小车的实时检测数据后通过卡尔曼滤波器对其进行滤波，同时通过编码器采样并计算小车运动路程，将上述数据作为神经网络的输入，计算小车当前姿态，使用相机采集图像并提取特征物轮廓后计算出小车的实时状态，重新计算卡尔曼滤波器的更新值，提高实时姿态计算的准确性；使用本发明降低实时检测误差，检测更加稳定，检测精度更高；可应用于隧道检测时小车的实时姿态检测的工作中。
本发明并不局限于上述实施例，激光线条数、激光器安装方式、测量间隔、速度倍数可按实际需求设计，在本发明公开的技术方案的基础上，本邻域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。

Claims (8)

1.一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，包括以下步骤，
1前置步骤一，得到卡尔曼滤波器：
具体为，
(101)设置包括直线、转向的轨道，并在特定位置放置多个标志物；
(102)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，测量小车与起始点的直线距离，得到固定点与起始点间的路程序列<ri>、图像序列<fi>、位移序列
Figure FDA0003739145470000011
与位置及方向序列
Figure FDA0003739145470000012
(103)根据摄像机采集的数据，确定小车是否在预设的固定点上，若在固定点，计算当前小车实时状态，然后转第(104)步；否则采集下一组数据，再次执行当前所在的第(103)步；
(104)初始化卡尔曼滤波器；
(105)根据测量数据，计算下一个位置的位移
Figure FDA0003739145470000013
方位
Figure FDA0003739145470000014
及所经历的路程
Figure FDA0003739145470000015
及其误差协方差矩阵
Figure FDA0003739145470000016
(106)计算卡尔曼滤波器的增益更新值Ki；
(107)根据图像gi中的特征物，计算当前小车位置，确定当前正处在哪一个固定点；
(108)根据第(102)步中采集的小车的方位、位移及经过的路程，向前推算误差协方差矩阵Pi，Pi＝(1-HX′i)H代表观测矩阵，X′i代表i时刻小车的预测状态值；
(109)若所有数据尚未全部计算完成，则转第(105)步；否则得到可用的卡尔曼滤波器；
其中，ri代表小车在固定点与起始点间运行路程在i时刻的值；fi代表相机在i时刻拍摄的激光线图片；si代表小车在i时刻的位移向量；pi代表小车在i时刻的位置及方向向量；
2前置步骤二，训练神经网络：
(201)设置包括直线、转向的轨道，并在特定位置放置多个标志物；
(202)推动小车沿着轨道行走，标志物进入车载摄像机指定位置时，静态测量小车与起始点的直线距离，得到固定点与起始点间的路程序列
Figure FDA0003739145470000021
位移序列
Figure FDA0003739145470000022
与方位序列
Figure FDA0003739145470000023
(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)，否则继续测量，直到测量结束。
2.根据权利要求1所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，所述步骤(103)中，获取各个测量值的步骤具体为，
(103a)编码器采集当前数据xi；
(103b)将数据xi、时间间隔Δti以及编码器的采集的前一次的最终数据
Figure FDA0003739145470000031
路程
Figure FDA0003739145470000032
送入编码器的私有神经网络，计算小车行走的路程
Figure FDA0003739145470000033
(103c)摄像头采集当前图像gi；
(103d)从图像gi中提取特征物的轮廓，并二值化后得到bi，根据公式bi＝Hib0，计算得到Hi；
(103e)将Hi输入到相机的私有神经网络，得到小车的位置
Figure FDA0003739145470000041
及姿态
Figure FDA0003739145470000042
(103f)将小车所处位置
Figure FDA0003739145470000043
”及姿态
Figure FDA0003739145470000044
输入到相机的私有神经网络，计算得到小车的位置
Figure FDA0003739145470000045
及姿态
Figure FDA0003739145470000046
(103g)从三轴加速度仪采集加速度数据
Figure FDA0003739145470000047
(103h)从三轴传感器采集当前姿态数据
Figure FDA0003739145470000048
(103i)将前一次计算得到加速度数据
Figure FDA0003739145470000049
姿态数据
Figure FDA00037391454700000410
计算得到的速度vi-1、位置
Figure FDA00037391454700000411
路程
Figure FDA00037391454700000412
及当前的
Figure FDA00037391454700000413
时间间隔Δti输入两个传感器共用的神经网络，计算得到小车的位置
Figure FDA00037391454700000414
及姿态
Figure FDA00037391454700000415
及路程
Figure FDA00037391454700000416
(103j)将步骤(103b)、(103f)和(103i)得到的数据进行算术平均，得到当前步骤的小车的位置
Figure FDA00037391454700000417
及姿态
Figure FDA00037391454700000418
及路程
Figure FDA00037391454700000419
(103k)将步骤(103j)的计算结果更新到各个传感器；
(1031)计算当前小车的速度
Figure FDA00037391454700000420
bi为i时刻相机采集到的特征物图像经二值化后得到特征物轮廓，Hi为i时刻计算得到的转换矩阵，
Figure FDA00037391454700000421
为i时刻相机神经网络计算出的小车的位置向量，
Figure FDA0003739145470000051
为相机神经网络计算出的i时刻小车的姿态向量，
Figure FDA0003739145470000052
为i-1时刻算术平均后得到的小车的加速度向量，
Figure FDA0003739145470000053
为i-1时刻算术平均后得到的小车的姿态向量，
Figure FDA0003739145470000054
为i-1时刻算术平均后得到的小车的位置向量，
Figure FDA0003739145470000055
为i-1时刻算术平均后得到的小车的行驶路程值，
Figure FDA0003739145470000056
为i时刻传感器神经网络输出的小车的位置向量，
Figure FDA0003739145470000057
为i时刻传感器神经网络输出的小车的姿态向量，
Figure FDA0003739145470000058
为i时刻传感器神经网络输出的小车的行驶路程值，
Figure FDA0003739145470000059
为i时刻算术平均后得到的小车的位置向量，
Figure FDA00037391454700000510
为i时刻算术平均后得到的小车的姿态向量，
Figure FDA00037391454700000511
为i时刻算术平均后得到的小车的行驶路程值。
3.根据权利要求2所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，所述步骤(104)中，初始化卡尔曼滤波器的步骤具体为，
定义预测方程：
Figure FDA00037391454700000512
Pi′＝APi-1AT+Q (3)；
定义校正方程：
Xi＝X′i+Ki(Zi-HX′i) (4)；
其中，Xi代表i时刻小车的状态值，包括加速度数据
Figure FDA00037391454700000513
姿态数据
Figure FDA00037391454700000514
位置
Figure FDA0003739145470000061
以及路程
Figure FDA0003739145470000062
代表i时刻小车的预测状态值；
Figure FDA0003739145470000063
代表i时刻的优化预测状态；Zi代表i时刻的观测值；A代表状态转移方程；B代表输入控制矩阵；Q代表预测噪声协方差矩阵；H代表观测矩阵；P代表误差矩阵；ui代表i时刻外界对系统的作用，Pi′为误差协方差矩阵预测值，AT为状态转移方程的转置，Pi-1为i-1时刻的误差协方差矩阵值，Ki为i时刻的卡尔曼增益值，X′i为i时刻小车的状态预测值。
4.根据权利要求2所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，
所述步骤(105)中，计算误差协方差矩阵步骤具体为，
误差协方差矩阵公式：
Figure FDA0003739145470000064
其中，
Figure FDA0003739145470000065
代表i时刻的状态信息，
Figure FDA0003739145470000066
E[]表示数学期望，
Figure FDA0003739145470000067
为
Figure FDA0003739145470000068
的转置。
5.根据权利要求2所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，所述步骤(106)中，计算卡尔曼滤波器的增益更新值具体为，
Ki＝PiHT(HPi′HT+R)-1 (7)；
其中，Ki代表i时刻的卡尔曼增益；H代表观测矩阵，HT为H的转置，Pi为i时刻的误差协方差矩阵值；R代表观测噪声协方差矩阵。
6.根据权利要求2所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，所述步骤(107)中，根据图像中的特征物，计算当前小车位置步骤具体为，
(107a)设一像点p(u，v)，其空间对应点为P(x，y，z)，用下式建立P点二维像坐标与三维空间坐标的对应关系，
Figure FDA0003739145470000071
(107b)通过四组样本点对，求出转换矩阵T’，用(u，v)表示像空间一点，对应物空间坐标为(Y，Z)，则可以用平方回归多项式表示像空间与物空间的对应关系，
Figure FDA0003739145470000072
(107c)对于已知物空间的标定点(Yk,Zk)，Y(u，v)、Z(u，v)的误差函数分别为，
Figure FDA0003739145470000073
误差函数Ey、Ez达到最小，则令
Figure FDA0003739145470000074
求解方程组(11)，求出回归系数aij和bij，建立物像对应关系；
其中，n为多项式的次数，m为物空间的标定点数。
7.根据权利要求2所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，所述步骤(204)中，计算处于固定点位置时小车的方位、位移及距离步骤具体为，
采用步骤(109)得到的可用的卡尔曼滤波器，对Xi进行滤波，代入传感器共用的神经网络，得到小车的方位、位移和距离。
8.根据权利要求5所述的一种基于多传感器数据融合的小车实时姿态检测方法，其特征在于，采用公式(7)重新计算卡尔曼滤波器的更新值结果步骤具体为，
(307a)若神经网络输出的小车实时姿态值和根据特征物轮廓计算出的小车的实时姿态值相差达到一定阈值，即
Figure FDA0003739145470000081
(307b)重新计算卡尔曼滤波器的增益更新值Ki；
其中，
Figure FDA0003739145470000082
代表神经网络输出的小车实时姿态值，
Figure FDA0003739145470000083
代表根据特征物轮廓计算出的小车的实时姿态值，η代表阈值。
CN202110360552.3A 2021-04-02 2021-04-02 一种基于多传感器数据融合的小车实时姿态检测方法 Active CN113029138B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110360552.3A CN113029138B (zh) 2021-04-02 2021-04-02 一种基于多传感器数据融合的小车实时姿态检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110360552.3A CN113029138B (zh) 2021-04-02 2021-04-02 一种基于多传感器数据融合的小车实时姿态检测方法

Publications (2)

Publication Number Publication Date
CN113029138A CN113029138A (zh) 2021-06-25
CN113029138B true CN113029138B (zh) 2022-09-06

Family

ID=76454126

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110360552.3A Active CN113029138B (zh) 2021-04-02 2021-04-02 一种基于多传感器数据融合的小车实时姿态检测方法

Country Status (1)

Country Link
CN (1) CN113029138B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113566828A (zh) * 2021-07-09 2021-10-29 哈尔滨工业大学 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3726884B2 (ja) * 2001-04-25 2005-12-14 学校法人日本大学 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
CN109883418A (zh) * 2019-01-17 2019-06-14 中国科学院遥感与数字地球研究所 一种室内定位方法及装置
CN110398251B (zh) * 2019-08-16 2021-02-09 北京邮电大学 一种基于多传感器融合的无轨导航agv定位系统及其定位方法
CN111947644B (zh) * 2020-08-10 2022-04-12 北京洛必德科技有限公司 一种室外移动机器人定位方法、系统及其电子设备

Also Published As

Publication number Publication date
CN113029138A (zh) 2021-06-25

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN109991636B (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN113819914A (zh) 一种地图构建方法及装置
CN113945206A (zh) 一种基于多传感器融合的定位方法及装置
CN109282808B (zh) 用于桥梁三维巡航检测的无人机与多传感器融合定位方法
CN113781582A (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN110160542A (zh) 车道线的定位方法和装置、存储介质、电子装置
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN109579838A (zh) Agv小车的定位方法及定位系统
CN112004183B (zh) 一种基于卷积神经网络融合IMU和WiFi信息的机器人自主定位方法
CN115690338A (zh) 地图构建方法、装置、设备及存储介质
CN107402005A (zh) 一种基于惯性/里程计/rfid的高精度组合导航方法
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114485643B (zh) 一种煤矿井下移动机器人环境感知与高精度定位方法
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN113029138B (zh) 一种基于多传感器数据融合的小车实时姿态检测方法
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
CN114440895B (zh) 基于因子图的气压辅助Wi-Fi/PDR室内定位方法
CN116429116A (zh) 一种机器人定位方法及设备
CN115540850A (zh) 一种激光雷达与加速度传感器结合的无人车建图方法
CN117075158A (zh) 基于激光雷达的无人变形运动平台的位姿估计方法及系统
Deusch et al. Improving localization in digital maps with grid maps

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant