CN111352415B - 一种煤矿蛇形探测机器人定位方法 - Google Patents
一种煤矿蛇形探测机器人定位方法 Download PDFInfo
- Publication number
- CN111352415B CN111352415B CN201910964967.4A CN201910964967A CN111352415B CN 111352415 B CN111352415 B CN 111352415B CN 201910964967 A CN201910964967 A CN 201910964967A CN 111352415 B CN111352415 B CN 111352415B
- Authority
- CN
- China
- Prior art keywords
- coal mine
- snake
- detection robot
- shaped detection
- time
- 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
Links
- 239000003245 coal Substances 0.000 title claims abstract description 338
- 238000001514 detection method Methods 0.000 title claims abstract description 322
- 238000000034 method Methods 0.000 title claims abstract description 110
- 230000008859 change Effects 0.000 claims abstract description 18
- 230000008569 process Effects 0.000 claims description 55
- 238000013528 artificial neural network Methods 0.000 claims description 45
- 230000006870 function Effects 0.000 claims description 38
- 238000003062 neural network model Methods 0.000 claims description 38
- 239000011159 matrix material Substances 0.000 claims description 36
- WYTGDNHDOZPMIW-RCBQFDQVSA-N alstonine Natural products C1=CC2=C3C=CC=CC3=NC2=C2N1C[C@H]1[C@H](C)OC=C(C(=O)OC)[C@H]1C2 WYTGDNHDOZPMIW-RCBQFDQVSA-N 0.000 claims description 35
- 238000006073 displacement reaction Methods 0.000 claims description 32
- 238000001914 filtration Methods 0.000 claims description 29
- 238000012545 processing Methods 0.000 claims description 29
- 238000012549 training Methods 0.000 claims description 19
- 230000005284 excitation Effects 0.000 claims description 16
- 238000010606 normalization Methods 0.000 claims description 12
- 238000013178 mathematical model Methods 0.000 claims description 9
- 238000005259 measurement Methods 0.000 claims description 7
- 239000000126 substance Substances 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 230000009471 action Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 241000270295 Serpentes Species 0.000 claims description 2
- 230000007547 defect Effects 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 16
- 230000000694 effects Effects 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 238000002474 experimental method Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 2
- 230000009023 proprioceptive sensation Effects 0.000 description 2
- 230000001373 regressive effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种煤矿蛇形探测机器人定位方法，包括步骤：一、定位模型的建立；二、初始位姿的检测；三、当前位姿的确定。本发明所提出的煤矿蛇形探测机器人定位方法简单，克服了传统的煤矿蛇形探测机器人在定位时由于考虑到巷道地面环境参数的变化而导致机器人姿态、速度和位置误差，从而降低定位精度的不足；能解决在未知煤矿井下巷道环境中的蛇形探测机器人相对定位，定位精度高，实时性好，自主性强，便于在封闭式复杂环境下推广使用。
Description
技术领域
本发明涉及机器人定位技术领域，尤其是涉及煤矿蛇形探测机器人定位方法。
背景技术
移动机器人的定位是实现其自主能力的前提，是机器人完成预定任务如路径规划、导航的基本保障。定位是指机器人在二维或三维工作空间内确定其自身相对于全局坐标系的位置和姿态。目前，机器人的定位技术可分为相对定位、绝对定位和组合定位三种方法。
相对定位的原理是在给定机器人初始位姿的基础上，通过测量机器人相对于初始位置的距离和姿态确定机器人当前的位姿；该方法包括航迹推算定位和测程法两种。航迹推算的优点是实现相对定位不需要依赖外界环境信息，只需通过本体感受传感器如陀螺仪、编码器等来测量机器人相对初始位姿的位移和姿态，具有灵活性和良好的自主性，不足之处是由于该方法在推算过程中隐含了累积误差，因此，随时间距离增加，航迹误差在不断累积，故该方法不适合长距离定位；测程法是根据相对参考特征的测量信息来算出机器人的实时位置和姿态，该方法的优点是定位误差不随时间累积，不足是实现机器人的定位需要依赖参考特征信息，而参考特征信息同样存在误差累积现象。
绝对定位的原理是根据全局坐标系和参考点以及机器人相对参考点之间的位姿关系，确定在全局坐标系内机器人的位置和姿态，该方法依赖于工作空间环境信息，如GPS信息、导航路标或信标、地图匹配等，绝对定位的优点是定位精度高，定位误差不随时间累积。不足是需要的硬件成本和维护成本高，当机器人无法提前获取环境参考信息时，该方法失效。
组合定位就是将上述两种定位方法相结合，取长补短，通常采取信息融合的方法将二者结合以提高移动机器人定位精度；目前，采用多种传感器如惯性传感器等相结合、基于视觉和图像定位、即时定位与地图构建(Simultaneous localization and mapping，SLAM)等组合定位方法被广泛应用在移动机器人定位中。
机器人定位系统的准确性和稳定性决定了机器人实现其它行为动作的能力，灾后煤矿井下环境为封闭式非结构化环境，其特点在于不能接收外界无线电信号，无法实现类似GPS的全局卫星定位，加之井下内部环境激励干扰大，可用环境信息少。由于定位方法的不同，定位传感器被分为相对定位传感器和绝对定位传感器。相对定位传感器又称为本体感受传感器，常用的包括陀螺仪、编码器、里程计及惯性导航传感器等，其中，陀螺仪是机器人定位的关键部件，用于测量机器人的姿态角；绝对定位传感器又称为外部传感器，常用的包括测距传感器和视觉传感器等。由于传感器都存在检测误差，因此，要对传感器数据进行滤波剔除误差，以提高机器人的定位精度。传统的传感器数据滤波算法主要包括卡尔曼滤波算法、小波去噪算法、基于误差模型的处理方法等，但是，现有技术中依然存在这定位的自主性差、定位精度低的缺陷和不足。
发明内容
本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种煤矿蛇形探测机器人定位方法，其实现简单，能解决在未知煤矿井下巷道环境中的蛇形探测机器人相对定位，定位精度高，实时性好，自主性强，便于在封闭式复杂环境下推广使用。
为解决上述技术问题，本发明采用的技术方案是：一种煤矿蛇形探测机器人定位方法，该方法包括以下步骤：
步骤一、定位模型的建立：采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，建立煤矿蛇形探测机器人在转向时和直线行走时的定位模型；
步骤二、初始位姿的检测：采用安装在煤矿蛇形探测机器人的驱动电机上的磁性霍尔编码器检测煤矿蛇形探测机器人的线位移和线速度；采用安装在煤矿蛇形探测机器人上的陀螺仪检测煤矿蛇形探测机器人的航向角；
步骤三、当前位姿的确定，具体过程为：
步骤301、判断步骤二中检测的煤矿蛇形探测机器人的航向角的大小，当步骤二中检测的煤矿蛇形探测机器人的航向角不为0时，判断为煤矿蛇形探测机器人在转向，执行步骤302～步骤304；当步骤二中检测的煤矿蛇形探测机器人的航向角为0时，判断为煤矿蛇形探测机器人在直线行走，执行步骤305；
步骤302、采用卡尔曼滤波算法对步骤二中检测的煤矿蛇形探测机器人的航向角进行滤波处理，去除高斯白噪声；
步骤303、采用预先构建的LSTM深度神经网络模型对步骤301处理后的煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值；
步骤304、根据步骤一中确定的煤矿蛇形探测机器人在转向时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移和步骤303中处理得到的煤矿蛇形探测机器人的当前航向角预测值，确定出煤矿蛇形探测机器人的当前位姿；
步骤305、根据步骤一中确定的煤矿蛇形探测机器人在直线行走时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移，确定出煤矿蛇形探测机器人的当前位姿。
上述的一种煤矿蛇形探测机器人定位方法，步骤一中所述采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，建立煤矿蛇形探测机器人在转向时和直线行走时的定位模型的具体过程为：
步骤101、将煤矿蛇形探测机器人的定位模型描述为在控制输入U(k-1)的作用下，煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)的变化；
步骤102、建立煤矿蛇形探测机器人在转向时的定位模型，具体过程为：
步骤1021、当煤矿蛇形探测机器人转向时，将控制输入U(k-1)表示为U(k-1)＝[Δd(k-1) Δθ(k-1)T]，其中，Δd(k-1)为煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移，Δθ(k-1)为从k-1时刻到k时刻煤矿蛇形探测机器人航向的改变角度；将煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)表示为X(k)＝[x(k) y(k) θ(k)]T，其中，x(k)为在k时刻煤矿蛇形探测机器人的位置横坐标，y(k)为在k时刻煤矿蛇形探测机器人的位置纵坐标，θ(k)为在k时刻煤矿蛇形探测机器人的航向角且θ(k)＝θ(k-1)+Δθ(k-1)；
步骤1022、建立煤矿蛇形探测机器人定位系统的动态方程为X(k)＝f(X(k-1),U(k-1))+ω(k-1)，其中，X(k-1)为煤矿蛇形探测机器人在k-1时刻的全局位姿状态且X(k-1)＝[x(k-1) y(k-1) θ(k-1)]T，x(k-1)为在k-1时刻煤矿蛇形探测机器人的位置横坐标，y(k-1)为在k-1时刻煤矿蛇形探测机器人的位置纵坐标，θ(k-1)为在k-1时刻煤矿蛇形探测机器人的航向角；ω(k-1)为k-1时刻的过程噪声；
步骤1023、将煤矿蛇形探测机器人在转向时的转向轨迹假设为圆弧，从k-1时刻到k时刻煤矿蛇形探测机器人转向轨迹的圆弧半径为
建立煤矿蛇形探测机器人在转向时的定位模型为：
其中，
ωx(k-1)为k-1时刻煤矿蛇形探测机器人的位置横坐标的过程噪声；ωy(k-1)为k-1时刻煤矿蛇形探测机器人的位置纵坐标的过程噪声；ωθ(k-1)为k-1时刻煤矿蛇形探测机器人的航向角的过程噪声；
步骤103、建立煤矿蛇形探测机器人在直线行走时的定位模型，具体过程为：
步骤1031、当煤矿蛇形探测机器人直线行走时，将控制输入U(k-1)表示为U(k-1)＝Δd(k-1)，将煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)表示为X(k)＝[x(k) y(k) θ(k)]T；
步骤1032、建立煤矿蛇形探测机器人定位系统的动态方程为X(k)＝f(X(k-1),U(k-1))+ω(k-1)；
步骤1033、建立煤矿蛇形探测机器人在直线行走时的定位模型为：
上述的一种煤矿蛇形探测机器人定位方法，所述ωx(k-1)、ωy(k-1)和ωθ(k-1)均为均值为零的高斯白噪声。
上述的一种煤矿蛇形探测机器人定位方法，步骤302中所述采用卡尔曼滤波算法对步骤二中检测的煤矿蛇形探测机器人的航向角进行滤波处理，去除高斯白噪声的具体过程为：
步骤3021、建立数学模型
其中，Xk为k时刻的状态变量且Xk＝θ(k)，Xk-1为k-1时刻的状态向量且Xk-1＝θ(k-1)，Uk-1为k-1时刻的控制输入量且
rk-1为从k-2时刻到k-1时刻煤矿蛇形探测机器人转向轨迹的圆弧半径且
Δd(k-2)为煤矿蛇形探测机器人从k-2时刻到k-1时刻移动的线位移，Δθ(k-2)为从k-2时刻到k-1时刻煤矿蛇形探测机器人航向的改变角度；Zk为k时刻的观测变量，vk为k时刻的观测噪声；
步骤3022、采用卡尔曼滤波算法的预测方程
和预测误差方差矩阵方程
对数学模型进行时间更新处理；其中，
为利用k-1时刻的状态向量值预测k时刻的状态向量值，Ak,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的非奇异状态一步转移矩阵，
为k-1时刻状态向量最优预测值，Bk,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的控制输入量矩阵，Uk为k时刻的控制输入量且
Δd(k)为煤矿蛇形探测机器人从k时刻到k+1时刻移动的线位移，Pk/k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的预测误差方差矩阵，Pk-1/k-1为
对应的误差方差矩阵，Γk,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的过程噪声输入矩阵，Qk-1为k-1时刻过程噪声的方差矩阵；
步骤3023、采用卡尔曼滤波算法的卡尔曼增益方程
卡尔曼滤波方程
和卡尔曼滤波误差方差矩阵方程Pk/k＝[I-KgkHk]Pk/k-1对数学模型进行观测更新处理；其中，Kgk为利用k-1时刻的状态向量值预测k时刻的状态向量值时的卡尔曼增益值，Hk为k时刻的测量矩阵，Rk为k时刻的观测噪声的方差矩阵，
为k时刻状态向量最优预测值，Pk/k为
对应的误差方差矩阵，I为单位矩阵；
步骤3024、重复执行步骤3022～步骤3023，直到达到了预设的迭代次数。
上述的一种煤矿蛇形探测机器人定位方法，所述预设的迭代次数为10000次。
上述的一种煤矿蛇形探测机器人定位方法，步骤303中所述预先构建LSTM深度神经网络模型的具体过程为：
步骤3031、确定LSTM深度神经网络模型的结构，具体过程为：
步骤30311、确定LSTM深度神经网络的输入为当前时刻的前四个时刻的陀螺仪输出值，LSTM深度神经网络的输入层节点数n为4个；
步骤30312、确定LSTM深度神经网络的输出为当前时刻的陀螺仪预测输出值，LSTM深度神经网络的输出层节点数l为1个；
步骤30313、根据公式
确定LSTM深度神经网络的隐含层节点个数m，其中，α′的取值为1～10的常数；
步骤3032、构建训练样本：煤矿蛇形探测机器人在长度为L的模拟煤矿巷道中行走并走完模拟煤矿巷道，行走过程中，陀螺仪按照采样周期T检测煤矿蛇形探测机器人的航向角，设定煤矿蛇形探测机器人行走的线速度为v，根据公式
计算得到煤矿蛇形探测机器人走完模拟煤矿巷道所需的时间t，再根据公式
计算得到陀螺仪采集到的航向角的总个数N，并将N个陀螺仪采集到的航向角作为训练集样本；
步骤3033、样本归一化：对训练集样本中的输入数据和输出数据根据公式
进行数据归一化处理，其中，X为待归一化的数据，Xmax为待归一化的数据的最大值，Xmin为待归一化的数据的最小值，Y为数据归一化处理得到的归一化数据，Ymax为归一化数据的最大值，Ymin为归一化数据的最小值；
步骤3034、采用训练集样本对LSTM深度神经网络模型进行训练，确定LSTM深度神经网络模型的最佳参数，包括激励函数、学习率、误差目标函数和迭代总次数，以经卡尔曼滤波去噪后当前时刻陀螺仪的输出值为期望值，当达到迭代总次数时停止训练，得到训练好的LSTM深度神经网络模型；
步骤303中所述采用预先构建的LSTM深度神经网络模型对步骤301处理后的煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值的具体过程为：将步骤301处理后的煤矿蛇形探测机器人的航向角数据中当前时刻的前四个时刻的航向角数据输入预先构建的LSTM深度神经网络模型中，得到LSTM深度神经网络模型的输出，再对LSTM深度神经网络模型的输出数据进行反归一化处理，得到煤矿蛇形探测机器人的当前航向角预测值。
上述的一种煤矿蛇形探测机器人定位方法，步骤30313中所述α′的取值为5.76，m的取值为8；
步骤3022中所述模拟煤矿巷道的形状为半椭圆形，L的取值为291.2cm，v的取值为25.19cm/s，
T的取值为20ms，
上述的一种煤矿蛇形探测机器人定位方法，步骤3024中所述激励函数为sigmoid函数和tanh函数，sigmoid函数作为LSTM深度神经网络模型各控制门的激励函数，tanh函数作为LSTM深度神经网络模型记忆单元的输入、输出激励函数；所述学习率为0.01，所述误差目标函数为MSE函数，所述迭代总次数为100次。
上述的一种煤矿蛇形探测机器人定位方法，步骤304中所述根据步骤一中确定的煤矿蛇形探测机器人在转向时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移和步骤303中处理得到的煤矿蛇形探测机器人的当前航向角预测值，确定出煤矿蛇形探测机器人的当前位姿的具体过程为：
步骤3041、将煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移Δd(k-1)表示为
其中，ΔM(k-1)为从k-1时刻到k时刻的时间段内磁性霍尔编码器的码盘输出的脉冲数，N为磁性霍尔编码器的码盘线数，L为煤矿蛇形探测机器人的轮子的周长；
步骤3042、将在k-1时刻煤矿蛇形探测机器人的航向角表示为θ(k-1)，将从k-1时刻到k时刻煤矿蛇形探测机器人航向的改变角度表示为Δθ(k-1)；
步骤3043、将Δd(k-1)、θ(k-1)和Δθ(k-1)代入煤矿蛇形探测机器人在转向时的定位模型中，得到煤矿蛇形探测机器人在转向时的当前位姿。
上述的一种煤矿蛇形探测机器人定位方法，步骤305中所述根据步骤一中确定的煤矿蛇形探测机器人在直线行走时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移，确定出煤矿蛇形探测机器人的当前位姿的具体过程为：
步骤3051、将煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移Δd(k-1)表示为
其中，ΔM(k-1)为从k-1时刻到k时刻的时间段内磁性霍尔编码器的码盘输出的脉冲数，N为磁性霍尔编码器的码盘线数，L为煤矿蛇形探测机器人的轮子的周长；
步骤3052、将在k-1时刻煤矿蛇形探测机器人的航向角表示为θ(k-1)；
步骤3053、将Δd(k-1)和θ(k-1)代入煤矿蛇形探测机器人在直线行走时的定位模型中，得到煤矿蛇形探测机器人在直线行走时的当前位姿。
本发明与现有技术相比具有以下优点：
1、煤矿蛇形探测机器人的定位与煤矿巷道地面环境关系紧密，由于煤矿巷道地面的不平坦会引起煤矿蛇形探测机器人运动时振动、冲击等干扰，导致机器人姿态、速度和位置误差，从而降低了定位精度；因此，在转向时，采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，代替对复杂地面环境参数的估计，建立形式简单的煤矿蛇形探测机器人在转向时的定位模型，能够有效提高定位精度。
2、本发明利用煤矿蛇形探测机器人自身装配的磁性霍尔编码器检测煤矿蛇形探测机器人的线位移和线速度，并利用煤矿蛇形探测机器人自身装配的陀螺仪检测煤矿蛇形探测机器人的航向角，编码器因测量精度高，误差小，常被认为是无误差的传感器，陀螺仪为微机电型(Micro Electro Mechanical System，MEMS)陀螺仪，适用于方向定位，煤矿蛇形探测机器人的航向角预测值由陀螺仪检测、卡尔曼滤波和LSTM深度神经网络处理完成，提高了煤矿蛇形探测机器人的定位精度。
3、陀螺仪的误差主要表现为高斯白噪声，在频域内，高斯白噪声基本上涵盖了整个频段，为此，我们提出采用卡尔曼滤波算法对煤矿蛇形探测机器人的航向角进行滤波处理，能够有效控制陀螺仪内部噪声，主要是高斯白噪声对蛇形探测机器人定位精度的影响。
4、针对陀螺仪输出信号为时间序列的特点，本发明提出了采用LSTM深度神经网络对煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值的方法，相比较传统的时间序列预测模型包括AR(Auto Regressive model)模型、MA(Moving Average model)模型、ARMA(Auto Regressive Moving Average model)模型及BP神经网络等智能预测方法，LSTM深度神经网络用于陀螺仪预测建模中具有以下优点：①继承了BP神经网络能充分逼近复杂的非线性映射，具有自学习和自适应能力、良好的泛函能力和容错能力等特点；②克服了BP神经网络在预测时容易出现局部极小值，导致预测效果不佳的不足；③充分考虑到时间序列的时序性，随着时间的增加，LSTM深度神经网络不会像RNN神经网络容易出现梯度爆炸或梯度弥散问题，影响最终预测结果精度；④采用LSTM深度神经网络对陀螺仪输出数据进行预测时，使得陀螺仪输出的历史数据一直对当前时刻的输出数据有影响，对时间序列数据而言，LSTM深度神经网络对其的预测效果更为理想；因此，该方法能够有效提高煤矿蛇形探测机器人定位的精度。
5、本发明采取相对定位的方法来实现煤矿蛇形探测机器人在巷道中的短距离定位，具有一定的自主性；在建立基于转向的蛇形探测机器人定位模型的基础上，将卡尔曼滤波算法和深度学习算法相结合实现煤矿蛇形探测机器人的相对定位，提高了机器人煤矿蛇形探测机器人在转向时的定位精度。
综上所述，本发明提出的煤矿蛇形探测机器人定位方法，实现简单，能解决在未知煤矿井下巷道环境中的蛇形探测机器人相对定位，定位精度高，实时性好，自主性强，便于在封闭式复杂环境下推广使用。
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
图1为本发明的方法流程框图。
图2为本发明煤矿蛇形探测机器人在转向时的位姿变化过程图。
图3为本发明Kalman滤波实验图。
图4为本发明测LSTM深度神经网络与现有技术中的RBF神经网络预测结果对比图。
图5为本发明LSTM深度神经网络预测误差曲线图。
图6为现有技术中的RBF神经网络预测误差曲线图。
图7为本发明煤矿蛇形探测机器人的航向角变化显示界面图。
图8本发明煤矿蛇形探测机器人行驶中实验数据显示界面图。
图9为本发明模拟煤矿巷道的轨迹图。
图10为本发明煤矿蛇形探测机器人的位置曲线仿真对比图。
图11为经本发明的LSTM深度神经网络学习后的位置误差曲线图。
图12为经现有技术中的RBF学习后的位置误差曲线图。
具体实施方式
如图1所示，本发明的煤矿蛇形探测机器人定位方法，包括以下步骤：
步骤一、定位模型的建立：采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，建立煤矿蛇形探测机器人在转向时和直线行走时的定位模型；
本实施例中，步骤一中所述采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，建立煤矿蛇形探测机器人在转向时和直线行走时的定位模型的具体过程为：
步骤101、将煤矿蛇形探测机器人的定位模型描述为在控制输入U(k-1)的作用下，煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)的变化；
步骤102、建立煤矿蛇形探测机器人在转向时的定位模型，具体过程为：
步骤1021、当煤矿蛇形探测机器人转向时，将控制输入U(k-1)表示为U(k-1)＝[Δd(k-1) Δθ(k-1)T]，其中，Δd(k-1)为煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移，Δθ(k-1)为从k-1时刻到k时刻煤矿蛇形探测机器人航向的改变角度；将煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)表示为X(k)＝[x(k) y(k) θ(k)]T，其中，x(k)为在k时刻煤矿蛇形探测机器人的位置横坐标，y(k)为在k时刻煤矿蛇形探测机器人的位置纵坐标，θ(k)为在k时刻煤矿蛇形探测机器人的航向角且θ(k)＝θ(k-1)+Δθ(k-1)；
步骤1022、建立煤矿蛇形探测机器人定位系统的动态方程为X(k)＝f(X(k-1),U(k-1))+ω(k-1)，其中，X(k-1)为煤矿蛇形探测机器人在k-1时刻的全局位姿状态且X(k-1)＝[x(k-1) y(k-1) θ(k-1)]T，x(k-1)为在k-1时刻煤矿蛇形探测机器人的位置横坐标，y(k-1)为在k-1时刻煤矿蛇形探测机器人的位置纵坐标，θ(k-1)为在k-1时刻煤矿蛇形探测机器人的航向角；ω(k-1)为k-1时刻的过程噪声；
步骤1023、将煤矿蛇形探测机器人在转向时的转向轨迹假设为圆弧，从k-1时刻到k时刻煤矿蛇形探测机器人转向轨迹的圆弧半径为
建立煤矿蛇形探测机器人在转向时的定位模型为：
其中，
ωx(k-1)为k-1时刻煤矿蛇形探测机器人的位置横坐标的过程噪声；ωy(k-1)为k-1时刻煤矿蛇形探测机器人的位置纵坐标的过程噪声；ωθ(k-1)为k-1时刻煤矿蛇形探测机器人的航向角的过程噪声；
具体实施时，煤矿蛇形探测机器人在转向时的位姿变化过程如图2所示，图2中，rk为从k-1时刻到k时刻煤矿蛇形探测机器人转向轨迹的圆弧半径且
为煤矿蛇形探测机器人转向轨迹的曲率且
步骤103、建立煤矿蛇形探测机器人在直线行走时的定位模型，具体过程为：
步骤1031、当煤矿蛇形探测机器人直线行走时，将控制输入U(k-1)表示为U(k-1)＝Δd(k-1)，将煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)表示为X(k)＝[x(k) y(k) θ(k)]T；
由于煤矿蛇形探测机器人直线行走时，航向角不发生变化，Δθ(k-1)＝0，因此将控制输入U(k-1)表示为U(k-1)＝Δd(k-1)。
步骤1032、建立煤矿蛇形探测机器人定位系统的动态方程为X(k)＝f(X(k-1),U(k-1))+ω(k-1)；
步骤1033、建立煤矿蛇形探测机器人在直线行走时的定位模型为：
本实施例中，所述ωx(k-1)、ωy(k-1)和ωθ(k-1)均为均值为零的高斯白噪声。
步骤二、初始位姿的检测：采用安装在煤矿蛇形探测机器人的驱动电机上的磁性霍尔编码器检测煤矿蛇形探测机器人的线位移和线速度；采用安装在煤矿蛇形探测机器人上的陀螺仪检测煤矿蛇形探测机器人的航向角；
具体实施时，陀螺仪的型号为MPU6050；
步骤三、当前位姿的确定，具体过程为：
步骤301、判断步骤二中检测的煤矿蛇形探测机器人的航向角的大小，当步骤二中检测的煤矿蛇形探测机器人的航向角不为0时，判断为煤矿蛇形探测机器人在转向，执行步骤302～步骤304；当步骤二中检测的煤矿蛇形探测机器人的航向角为0时，判断为煤矿蛇形探测机器人在直线行走，执行步骤305；
步骤302、采用卡尔曼滤波算法对步骤二中检测的煤矿蛇形探测机器人的航向角进行滤波处理，去除高斯白噪声；
本实施例中，步骤302中所述采用卡尔曼滤波算法对步骤二中检测的煤矿蛇形探测机器人的航向角进行滤波处理，去除高斯白噪声的具体过程为：
步骤3021、建立数学模型
其中，Xk为k时刻的状态变量且Xk＝θ(k)，Xk-1为k-1时刻的状态向量且Xk-1＝θ(k-1)，Uk-1为k-1时刻的控制输入量且
rk-1为从k-2时刻到k-1时刻煤矿蛇形探测机器人转向轨迹的圆弧半径且
Δd(k-2)为煤矿蛇形探测机器人从k-2时刻到k-1时刻移动的线位移，Δθ(k-2)为从k-2时刻到k-1时刻煤矿蛇形探测机器人航向的改变角度；Zk为k时刻的观测变量，vk为k时刻的观测噪声；
步骤3022、采用卡尔曼滤波算法的预测方程
和预测误差方差矩阵方程
对数学模型进行时间更新处理；其中，
为利用k-1时刻的状态向量值预测k时刻的状态向量值，Ak,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的非奇异状态一步转移矩阵，
为k-1时刻状态向量最优预测值，Bk,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的控制输入量矩阵，Uk为k时刻的控制输入量且
Δd(k)为煤矿蛇形探测机器人从k时刻到k+1时刻移动的线位移，Pk/k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的预测误差方差矩阵，Pk-1/k-1为
对应的误差方差矩阵，Γk,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的过程噪声输入矩阵，Qk-1为k-1时刻过程噪声的方差矩阵；
步骤3023、采用卡尔曼滤波算法的卡尔曼增益方程
卡尔曼滤波方程
和卡尔曼滤波误差方差矩阵方程Pk/k＝[I-KgkHk]Pk/k-1对数学模型进行观测更新处理；其中，Kgk为利用k-1时刻的状态向量值预测k时刻的状态向量值时的卡尔曼增益值，Hk为k时刻的测量矩阵，Rk为k时刻的观测噪声的方差矩阵，
为k时刻状态向量最优预测值，Pk/k为
对应的误差方差矩阵，I为单位矩阵；
步骤3024、重复执行步骤3022～步骤3023，直到达到了预设的迭代次数。
本实施例中，所述预设的迭代次数为10000次。
本实施例中，取10000个样本进行卡尔曼滤波(Kalman滤波)实验，得到的Kalman滤波实验图如图3所示，图3中，横坐标代表采样点，纵坐标表示煤矿蛇形探测机器人的航向角，蓝线为带高斯白噪声的航向角测量值，红线为经卡尔曼滤波后的效果。从图3能够看出，经卡尔曼滤波后的航向角测量值在很大程度上消除了高斯白噪声的影响；由此可见，陀螺仪输出数据中存在的高斯白噪声通过卡尔曼滤波得到了有效的抑制。
步骤303、采用预先构建的LSTM深度神经网络模型对步骤301处理后的煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值；
本实施例中，步骤303中所述预先构建LSTM深度神经网络模型的具体过程为：
步骤3031、确定LSTM深度神经网络模型的结构，具体过程为：
步骤30311、确定LSTM深度神经网络的输入为当前时刻的前四个时刻的陀螺仪输出值，LSTM深度神经网络的输入层节点数n为4个；
步骤30312、确定LSTM深度神经网络的输出为当前时刻的陀螺仪预测输出值，LSTM深度神经网络的输出层节点数l为1个；
步骤30313、根据公式
确定LSTM深度神经网络的隐含层节点个数m，其中，α′的取值为1～10的常数；
本实施例中，步骤30313中所述α′的取值为5.76，m的取值为8；即LSTM深度神经网络模型的结构为4—8—1；
步骤3032、构建训练样本：煤矿蛇形探测机器人在长度为L的模拟煤矿巷道中行走并走完模拟煤矿巷道，行走过程中，陀螺仪按照采样周期T检测煤矿蛇形探测机器人的航向角，设定煤矿蛇形探测机器人行走的线速度为v，根据公式
计算得到煤矿蛇形探测机器人走完模拟煤矿巷道所需的时间t，再根据公式
计算得到陀螺仪采集到的航向角的总个数N，并将N个陀螺仪采集到的航向角作为训练集样本；
本实施例中，步骤3032中所述模拟煤矿巷道的形状为半椭圆形，L的取值为291.2cm(即半椭圆形模拟煤矿巷道的周长为291.2cm)，v的取值为25.19cm/s，
T的取值为20ms，
步骤3033、样本归一化：对训练集样本中的输入数据和输出数据根据公式
进行数据归一化处理，其中，X为待归一化的数据(训练集样本中的输入数据和输出数据)，Xmax为待归一化的数据的最大值，Xmin为待归一化的数据的最小值，Y为数据归一化处理得到的归一化数据，Ymax为归一化数据的最大值，Ymin为归一化数据的最小值；
步骤3034、采用训练集样本对LSTM深度神经网络模型进行训练，确定LSTM深度神经网络模型的最佳参数，包括激励函数、学习率、误差目标函数和迭代总次数，以经卡尔曼滤波去噪后当前时刻陀螺仪的输出值为期望值，当达到迭代总次数时停止训练，得到训练好的LSTM深度神经网络模型；
本实施例中，步骤3034中所述激励函数为sigmoid函数和tanh函数，sigmoid函数作为LSTM深度神经网络模型各控制门的激励函数，tanh函数作为LSTM深度神经网络模型记忆单元的输入、输出激励函数；所述学习率为0.01，所述误差目标函数为MSE函数，所述迭代总次数为100次。
步骤303中所述采用预先构建的LSTM深度神经网络模型对步骤301处理后的煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值的具体过程为：将步骤301处理后的煤矿蛇形探测机器人的航向角数据中当前时刻的前四个时刻的航向角数据输入预先构建的LSTM深度神经网络模型中，得到LSTM深度神经网络模型的输出，再对LSTM深度神经网络模型的输出数据进行反归一化处理，得到煤矿蛇形探测机器人的当前航向角预测值。
为了检验LSTM深度神经网络对煤矿蛇形探测机器人的当前航向角预测值的预测效果，将采用LSTM深度神经网络模型和现有技术中的RBF(Radial Basis Function)神经网络进行预测的预测效果进行对比，得到的结果对比图如图4所示，图4中，横坐标代表预测值序列，纵坐标表示煤矿蛇形探测机器人的航向角；从图4能够看出，相比较RBF神经网络，LSTM深度神经网络能够获得更好的预测效果。得到的LSTM深度神经网络的预测误差曲线图如图5所示，得到的现有技术中的RBF神经网络的预测误差曲线图如图6所示，其中，横坐标代表预测值序列，纵坐标表示网络的实际输出与期望输出的误差矩阵的范数，当LSTM做预测时，最大误差为0.0028，最小误差为2.173×10-5；当RBF做预测时，最大误差为5.423，最小误差为0.014；从图5和图6能够看出，LSTM深度神经网络相比RBF神经网络有着更高的预测精度。
步骤304、根据步骤一中确定的煤矿蛇形探测机器人在转向时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移和步骤303中处理得到的煤矿蛇形探测机器人的当前航向角预测值，确定出煤矿蛇形探测机器人的当前位姿；
本实施例中，步骤304中所述根据步骤一中确定的煤矿蛇形探测机器人在转向时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移和步骤303中处理得到的煤矿蛇形探测机器人的当前航向角预测值，确定出煤矿蛇形探测机器人的当前位姿的具体过程为：
步骤3041、将煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移Δd(k-1)表示为
其中，ΔM(k-1)为从k-1时刻到k时刻的时间段内磁性霍尔编码器的码盘输出的脉冲数，N为磁性霍尔编码器的码盘线数，L为煤矿蛇形探测机器人的轮子的周长；
步骤3042、将在k-1时刻煤矿蛇形探测机器人的航向角表示为θ(k-1)，将从k-1时刻到k时刻煤矿蛇形探测机器人航向的改变角度表示为Δθ(k-1)；
步骤3043、将Δd(k-1)、θ(k-1)和Δθ(k-1)代入煤矿蛇形探测机器人在转向时的定位模型中，得到煤矿蛇形探测机器人在转向时的当前位姿。
步骤305、根据步骤一中确定的煤矿蛇形探测机器人在直线行走时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移，确定出煤矿蛇形探测机器人的当前位姿。
本实施例中，步骤305中所述根据步骤一中确定的煤矿蛇形探测机器人在直线行走时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移，确定出煤矿蛇形探测机器人的当前位姿的具体过程为：
步骤3051、将煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移Δd(k-1)表示为
其中，ΔM(k-1)为从k-1时刻到k时刻的时间段内磁性霍尔编码器的码盘输出的脉冲数，N为磁性霍尔编码器的码盘线数，L为煤矿蛇形探测机器人的轮子的周长；
步骤3052、将在k-1时刻煤矿蛇形探测机器人的航向角表示为θ(k-1)；
步骤3053、将
和θ(k-1)代入煤矿蛇形探测机器人在直线行走时的定位模型中，得到煤矿蛇形探测机器人在直线行走时的当前位姿。
为了进一步验证本发明能够产生的技术效果，将煤矿蛇形探测机器人放入模拟煤矿巷道中做实验，图7为上位机显示的煤矿蛇形探测机器人在巷道中行驶时姿态角的变化曲线界面，其中，蓝色的曲线表示煤矿蛇形探测机器人航向角的变化情况；图8为煤矿蛇形探测机器人行驶中的实验数据显示界面，进一步得出以下仿真结果：图9为模拟煤矿巷道的轨迹，该轨迹为一半椭圆，长轴为80cm，短轴为60cm，周长为291.2cm，图中横坐标代表巷道轨迹的X轴坐标，纵坐标表示巷道轨迹的Y轴坐标。图10为煤矿蛇形探测机器人实际位置和推算位置的对比试验，横坐标代表机器人位置的X轴坐标，纵坐标表示机器人位置的Y轴坐标；其中，黑线表示通过编码器记录的机器人实际行走路径；蓝线表示利用陀螺仪通过卡尔曼滤波算法进行机器人位置推算的路径，这里，煤矿蛇形探测机器人的位置通过测得的加速度在导航坐标系中对时间的二次积分得到；红线表示通过LSTM深度神经网络学习后煤矿蛇形探测机器人的路径；绿线表示经过RBF神经网络学习后机器人的路径。从图能够看出，经LSTM深度神经网络学习后的路径更接近于煤矿蛇形探测机器人的实际路径；图11为经LSTM深度神经网络学习后的位置误差，其中，横坐标代表采样点，纵坐标表示机器人位置的相对误差。最大的位置误差为1.872×10-9cm，最小误差为3.299×10-12cm；图12为经RBF神经网络学习后的位置误差，其中，横坐标代表采样点，纵坐标表示机器人位置的相对误差。最大的位置误差为14.119cm，最小误差为1.612×10-5cm；能够看出，LSTM深度神经网络相比RBF神经网络有着更高的学习精度。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。
Claims (8)
1.一种煤矿蛇形探测机器人定位方法，其特征在于，该方法包括以下步骤：
步骤一、定位模型的建立：采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，建立煤矿蛇形探测机器人在转向时和直线行走时的定位模型；
步骤二、初始位姿的检测：采用安装在煤矿蛇形探测机器人的驱动电机上的磁性霍尔编码器检测煤矿蛇形探测机器人的线位移和线速度；采用安装在煤矿蛇形探测机器人上的陀螺仪检测煤矿蛇形探测机器人的航向角；
步骤三、当前位姿的确定，具体过程为：
步骤301、判断步骤二中检测的煤矿蛇形探测机器人的航向角的大小，当步骤二中检测的煤矿蛇形探测机器人的航向角不为0时，判断为煤矿蛇形探测机器人在转向，执行步骤302～步骤304；当步骤二中检测的煤矿蛇形探测机器人的航向角为0时，判断为煤矿蛇形探测机器人在直线行走，执行步骤305；
步骤302、采用卡尔曼滤波算法对步骤二中检测的煤矿蛇形探测机器人的航向角进行滤波处理，去除高斯白噪声；
步骤303、采用预先构建的LSTM深度神经网络模型对步骤301处理后的煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值；
步骤304、根据步骤一中确定的煤矿蛇形探测机器人在转向时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移和步骤303中处理得到的煤矿蛇形探测机器人的当前航向角预测值，确定出煤矿蛇形探测机器人的当前位姿；
步骤305、根据步骤一中确定的煤矿蛇形探测机器人在直线行走时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移，确定出煤矿蛇形探测机器人的当前位姿；
步骤一中所述采用对煤矿蛇形探测机器人运动轨迹曲率与航向角进行推算的方法，建立煤矿蛇形探测机器人在转向时和直线行走时的定位模型的具体过程为：
步骤101、将煤矿蛇形探测机器人的定位模型描述为在控制输入U(k-1)的作用下，煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)的变化；
步骤102、建立煤矿蛇形探测机器人在转向时的定位模型，具体过程为：
步骤1021、当煤矿蛇形探测机器人转向时，将控制输入U(k-1)表示为U(k-1)＝[Δd(k-1) Δθ(k-1)]T，其中，Δd(k-1)为煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移，Δθ(k-1)为从k-1时刻到k时刻煤矿蛇形探测机器人航向的改变角度；将煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)表示为X(k)＝[x(k) y(k) θ(k)]T，其中，x(k)为在k时刻煤矿蛇形探测机器人的位置横坐标，y(k)为在k时刻煤矿蛇形探测机器人的位置纵坐标，θ(k)为在k时刻煤矿蛇形探测机器人的航向角且θ(k)＝θ(k-1)+Δθ(k-1)；
步骤1022、建立煤矿蛇形探测机器人定位系统的动态方程为X(k)＝f(X(k-1),U(k-1))+ω(k-1)，其中，X(k-1)为煤矿蛇形探测机器人在k-1时刻的全局位姿状态且X(k-1)＝[x(k-1) y(k-1) θ(k-1)]T，x(k-1)为在k-1时刻煤矿蛇形探测机器人的位置横坐标，y(k-1)为在k-1时刻煤矿蛇形探测机器人的位置纵坐标，θ(k-1)为在k-1时刻煤矿蛇形探测机器人的航向角；ω(k-1)为k-1时刻的过程噪声；
步骤1023、将煤矿蛇形探测机器人在转向时的转向轨迹假设为圆弧，从k-1时刻到k时刻煤矿蛇形探测机器人转向轨迹的圆弧半径为
建立煤矿蛇形探测机器人在转向时的定位模型为：
其中，
ωx(k-1)为k-1时刻煤矿蛇形探测机器人的位置横坐标的过程噪声；ωy(k-1)为k-1时刻煤矿蛇形探测机器人的位置纵坐标的过程噪声；ωθ(k-1)为k-1时刻煤矿蛇形探测机器人的航向角的过程噪声；
步骤103、建立煤矿蛇形探测机器人在直线行走时的定位模型，具体过程为：
步骤1031、当煤矿蛇形探测机器人直线行走时，将控制输入U(k-1)表示为U(k-1)＝Δd(k-1)，将煤矿蛇形探测机器人在k时刻的全局位姿状态X(k)表示为X(k)＝[x(k) y(k) θ(k)]T；
步骤1032、建立煤矿蛇形探测机器人定位系统的动态方程为X(k)＝f(X(k-1),U(k-1))+ω(k-1)；
步骤1033、建立煤矿蛇形探测机器人在直线行走时的定位模型为：
步骤303中所述预先构建LSTM深度神经网络模型的具体过程为：
步骤3031、确定LSTM深度神经网络模型的结构，具体过程为：
步骤30311、确定LSTM深度神经网络的输入为当前时刻的前四个时刻的陀螺仪输出值，LSTM深度神经网络的输入层节点数n为4个；
步骤30312、确定LSTM深度神经网络的输出为当前时刻的陀螺仪预测输出值，LSTM深度神经网络的输出层节点数l为1个；
步骤30313、根据公式
确定LSTM深度神经网络的隐含层节点个数m，其中，α′的取值为1～10的常数；
步骤3032、构建训练样本：煤矿蛇形探测机器人在长度为L的模拟煤矿巷道中行走并走完模拟煤矿巷道，行走过程中，陀螺仪按照采样周期T检测煤矿蛇形探测机器人的航向角，设定煤矿蛇形探测机器人行走的线速度为v，根据公式
计算得到煤矿蛇形探测机器人走完模拟煤矿巷道所需的时间t，再根据公式
计算得到陀螺仪采集到的航向角的总个数N，并将N个陀螺仪采集到的航向角作为训练集样本；
步骤3033、样本归一化：对训练集样本中的输入数据和输出数据根据公式
进行数据归一化处理，其中，X为待归一化的数据，Xmax为待归一化的数据的最大值，Xmin为待归一化的数据的最小值，Y为数据归一化处理得到的归一化数据，Ymax为归一化数据的最大值，Ymin为归一化数据的最小值；
步骤3034、采用训练集样本对LSTM深度神经网络模型进行训练，确定LSTM深度神经网络模型的最佳参数，包括激励函数、学习率、误差目标函数和迭代总次数，以经卡尔曼滤波去噪后当前时刻陀螺仪的输出值为期望值，当达到迭代总次数时停止训练，得到训练好的LSTM深度神经网络模型；
步骤303中所述采用预先构建的LSTM深度神经网络模型对步骤301处理后的煤矿蛇形探测机器人的航向角进行处理，得到煤矿蛇形探测机器人的当前航向角预测值的具体过程为：将步骤301处理后的煤矿蛇形探测机器人的航向角数据中当前时刻的前四个时刻的航向角数据输入预先构建的LSTM深度神经网络模型中，得到LSTM深度神经网络模型的输出，再对LSTM深度神经网络模型的输出数据进行反归一化处理，得到煤矿蛇形探测机器人的当前航向角预测值。
2.按照权利要求1所述的一种煤矿蛇形探测机器人定位方法，其特征在于：所述ωx(k-1)、ωy(k-1)和ωθ(k-1)均为均值为零的高斯白噪声。
3.按照权利要求1所述的一种煤矿蛇形探测机器人定位方法，其特征在于：步骤302中所述采用卡尔曼滤波算法对步骤二中检测的煤矿蛇形探测机器人的航向角进行滤波处理，去除高斯白噪声的具体过程为：
步骤3021、建立数学模型
其中，Xk为k时刻的状态变量且Xk＝θ(k)，Xk-1为k-1时刻的状态向量且Xk-1＝θ(k-1)，Uk-1为k-1时刻的控制输入量且
rk-1为从k-2时刻到k-1时刻煤矿蛇形探测机器人转向轨迹的圆弧半径且
Δd(k-2)为煤矿蛇形探测机器人从k-2时刻到k-1时刻移动的线位移，Δθ(k-2)为从k-2时刻到k-1时刻煤矿蛇形探测机器人航向的改变角度；Zk为k时刻的观测变量，vk为k时刻的观测噪声；
步骤3022、采用卡尔曼滤波算法的预测方程
和预测误差方差矩阵方程
对数学模型进行时间更新处理；其中，
为利用k-1时刻的状态向量值预测k时刻的状态向量值，Ak,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的非奇异状态一步转移矩阵，
为k-1时刻状态向量最优预测值，Bk,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的控制输入量矩阵，Uk为k时刻的控制输入量且
Δd(k)为煤矿蛇形探测机器人从k时刻到k+1时刻移动的线位移，Pk/k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的预测误差方差矩阵，Pk-1/k-1为
对应的误差方差矩阵，Γk,k-1为利用k-1时刻的状态向量值预测k时刻的状态向量值时的过程噪声输入矩阵，Qk-1为k-1时刻过程噪声的方差矩阵；
步骤3023、采用卡尔曼滤波算法的卡尔曼增益方程
卡尔曼滤波方程
和卡尔曼滤波误差方差矩阵方程Pk/k＝[I-KgkHk]Pk/k-1对数学模型进行观测更新处理；其中，Kgk为利用k-1时刻的状态向量值预测k时刻的状态向量值时的卡尔曼增益值，Hk为k时刻的测量矩阵，Rk为k时刻的观测噪声的方差矩阵，
为k时刻状态向量最优预测值，Pk/k为
对应的误差方差矩阵，I为单位矩阵；
步骤3024、重复执行步骤3022～步骤3023，直到达到了预设的迭代次数。
4.按照权利要求3所述的一种煤矿蛇形探测机器人定位方法，其特征在于：所述预设的迭代次数为10000次。
5.按照权利要求1所述的一种煤矿蛇形探测机器人定位方法，其特征在于：步骤30313中所述α′的取值为5.76，m的取值为8；
步骤3022中所述模拟煤矿巷道的形状为半椭圆形，L的取值为291.2cm，v的取值为25.19cm/s，
T的取值为20ms，
6.按照权利要求1所述的一种煤矿蛇形探测机器人定位方法，其特征在于：步骤3024中所述激励函数为sigmoid函数和tanh函数，sigmoid函数作为LSTM深度神经网络模型各控制门的激励函数，tanh函数作为LSTM深度神经网络模型记忆单元的输入、输出激励函数；所述学习率为0.01，所述误差目标函数为MSE函数，所述迭代总次数为100次。
7.按照权利要求1或3所述的一种煤矿蛇形探测机器人定位方法，其特征在于：步骤304中所述根据步骤一中确定的煤矿蛇形探测机器人在转向时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移和步骤303中处理得到的煤矿蛇形探测机器人的当前航向角预测值，确定出煤矿蛇形探测机器人的当前位姿的具体过程为：
步骤3041、将煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移Δd(k-1)表示为
其中，ΔM(k-1)为从k-1时刻到k时刻的时间段内磁性霍尔编码器的码盘输出的脉冲数，N为磁性霍尔编码器的码盘线数，L为煤矿蛇形探测机器人的轮子的周长；
步骤3042、将在k-1时刻煤矿蛇形探测机器人的航向角表示为θ(k-1)，将从k-1时刻到k时刻煤矿蛇形探测机器人航向的改变角度表示为Δθ(k-1)；
步骤3043、将Δd(k-1)、θ(k-1)和Δθ(k-1)代入煤矿蛇形探测机器人在转向时的定位模型中，得到煤矿蛇形探测机器人在转向时的当前位姿。
8.按照权利要求1或3所述的一种煤矿蛇形探测机器人定位方法，其特征在于：步骤305中所述根据步骤一中确定的煤矿蛇形探测机器人在直线行走时的定位模型，并根据步骤二中检测的煤矿蛇形探测机器人的线位移，确定出煤矿蛇形探测机器人的当前位姿的具体过程为：
步骤3051、将煤矿蛇形探测机器人从k-1时刻到k时刻移动的线位移Δd(k-1)表示为
其中，ΔM(k-1)为从k-1时刻到k时刻的时间段内磁性霍尔编码器的码盘输出的脉冲数，N为磁性霍尔编码器的码盘线数，L为煤矿蛇形探测机器人的轮子的周长；
步骤3052、将在k-1时刻煤矿蛇形探测机器人的航向角表示为θ(k-1)；
步骤3053、将Δd(k-1)和θ(k-1)代入煤矿蛇形探测机器人在直线行走时的定位模型中，得到煤矿蛇形探测机器人在直线行走时的当前位姿。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910964967.4A CN111352415B (zh) | 2019-10-11 | 2019-10-11 | 一种煤矿蛇形探测机器人定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910964967.4A CN111352415B (zh) | 2019-10-11 | 2019-10-11 | 一种煤矿蛇形探测机器人定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111352415A CN111352415A (zh) | 2020-06-30 |
| CN111352415B true CN111352415B (zh) | 2020-12-29 |
Family
ID=71192787
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910964967.4A Active CN111352415B (zh) | 2019-10-11 | 2019-10-11 | 一种煤矿蛇形探测机器人定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111352415B (zh) |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB0027238D0 (en) * | 2000-11-08 | 2000-12-27 | Secr Defence | Adaptive filter |
| CN100573387C (zh) * | 2007-12-10 | 2009-12-23 | 华中科技大学 | 机器人自主定位系统 |
| CN102288176B (zh) * | 2011-07-07 | 2013-01-30 | 中国矿业大学(北京) | 基于信息融合的煤矿救灾机器人导航系统及方法 |
| CN106647265A (zh) * | 2016-12-12 | 2017-05-10 | 辽宁工程技术大学 | 一种矿井救灾探测机器人的智能控制方法 |
| CN206605503U (zh) * | 2017-03-20 | 2017-11-03 | 王琳 | 一种煤矿井下探测机器人 |
| US10705525B2 (en) * | 2017-04-07 | 2020-07-07 | Nvidia Corporation | Performing autonomous path navigation using deep neural networks |
| CN107139168B (zh) * | 2017-06-29 | 2023-09-01 | 西安科技大学 | 一种煤矿救援蛇形机器人及其煤矿救援方法 |
| CN109118787B (zh) * | 2018-08-20 | 2020-08-04 | 浙江工业大学 | 一种基于深度神经网络的车辆速度预测方法 |
| US10269125B1 (en) * | 2018-10-05 | 2019-04-23 | StradVision, Inc. | Method for tracking object by using convolutional neural network including tracking network and computing device using the same |
| CN110189304B (zh) * | 2019-05-07 | 2022-08-12 | 南京理工大学 | 基于人工智能的光学遥感图像目标在线快速检测方法 |
-
2019
- 2019-10-11 CN CN201910964967.4A patent/CN111352415B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111352415A (zh) | 2020-06-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105785999B (zh) | 无人艇航向运动控制方法 | |
| CN106017486B (zh) | 一种面向无人车导航的基于轨迹拐点滤波的地图定位方法 | |
| JP6855524B2 (ja) | 低速特徴からのメトリック表現の教師なし学習 | |
| CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
| Zhang et al. | IMU data processing for inertial aided navigation: A recurrent neural network based approach | |
| CN111098335B (zh) | 一种双轮差速驱动机器人里程计标定方法及装置 | |
| Mu et al. | End-to-end navigation for autonomous underwater vehicle with hybrid recurrent neural networks | |
| CN110118560A (zh) | 一种基于lstm和多传感器融合的室内定位方法 | |
| CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN110488818A (zh) | 一种基于激光雷达的机器人定位方法、装置和机器人 | |
| US11874666B2 (en) | Self-location estimation method | |
| Zong et al. | Research on obstacle avoidance method for mobile robot based on multisensor information fusion | |
| Zhou et al. | DeepVIP: Deep learning-based vehicle indoor positioning using smartphones | |
| Escoriza et al. | Data-driven Kalman-based velocity estimation for autonomous racing | |
| Phunopas et al. | Motion Improvement of Four-Wheeled Omnidirectional Mobile Robots for Indoor Terrain. | |
| Reina et al. | Odometry correction using visual slip angle estimation for planetary exploration rovers | |
| CN111352415B (zh) | 一种煤矿蛇形探测机器人定位方法 | |
| Balzer et al. | Epe and speed adaptive extended kalman filter for vehicle position and attitude estimation with low cost gnss and imu sensors | |
| KR101907611B1 (ko) | 자율 주행체의 위치 정보를 획득하는 방법 | |
| Tong et al. | Smartphone-based vehicle tracking without GPS: Experience and improvements | |
| CN110703611B (zh) | 基于递归模糊神经网络的微陀螺传感器终端滑模控制系统 | |
| Choi et al. | Robust localization of unmanned surface vehicle using DDQN-AM | |
| Ashokaraj et al. | Feature based robot navigation: using fuzzy logic and interval analysis | |
| Yan et al. | Integration of Vehicle Dynamic Model and System Identified Model for Navigation in Autonomous Mobile Robots | |
| Yang et al. | Path-tracking controller design and implementation of a vision-based wheeled mobile robot |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220411 Address after: 710075 room 32106, building 2, 86 Gaoxin Road, high tech Zone, Xi'an City, Shaanxi Province Patentee after: XI'AN YUBIN INFORMATION TECHNOLOGY Co.,Ltd. Address before: 710054 No. 58, middle section, Yanta Road, Shaanxi, Xi'an Patentee before: XI'AN University OF SCIENCE AND TECHNOLOGY |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220510 Address after: 719300 room 506, highway bureau, No. 1161, Zhenxing Road, high tech Industrial Park, Yulin City, Shaanxi Province (southwest of the intersection of Yulin Avenue and Changxing Road) Patentee after: Shaanxi Feier Zhichuang Intelligent Equipment Co.,Ltd. Address before: 710075 room 32106, building 2, 86 Gaoxin Road, high tech Zone, Xi'an City, Shaanxi Province Patentee before: XI'AN YUBIN INFORMATION TECHNOLOGY Co.,Ltd. |