【技术实现步骤摘要】
基于多传感器融合SLAM和磁导的组合导航方法及存储介质
[0001]本专利技术涉及智能巡检机器人
，具体涉及一种基于多传感器融合SLAM和磁导的组合导航方法及存储介质。
技术介绍
[0002]随着现代技术的不断发展，很多行业形成了向信息化和智能化的发展趋势，越来越多的地方引入智能巡检机器人来代替人工巡检设备是否运行正常，可以减少人工成本，且机器人可以一直循环巡检，提高工作效率，减少出问题的风险；
[0003]目前巡检机器人主要是使用激光雷达或者深度相机技术进行建图和实时定位导航，但是激光雷达定位配准存在其弱点，如长走廊退化，点云数据稀疏，易受玻璃等物体干扰等导致匹配错误，深度相机则有计算量大，易受光照影响，地图精度低等缺点，因此单一的技术方案都无法满足在狭长管道，或环境变化巨大的场景的使用。
[0004]如地铁，高铁的列车底盘巡检，其检查坑道一般很狭长，周边环境特征相似，坑道两侧挨着坑道，坑道之间区域供检修人员和检修设备通过，但因为列车检修时间不定，导致坑道两侧的列车环境和人员设备等经常变化且幅度较大，这对传统激光和视觉SLAM都有很大的难度。
[0005]总得来说，现在智能巡检机器人的应用场景越来越多，有一些特殊场景，如狭长相似管道，周边环境变化巨大，巡检区域存在较大高度差等场景，传统的导航方式难以进行正常的建图和定位导航，进而限制了巡检机器人的使用。
[0006]相关术语解释：
[0007]SLAM：simultaneous localization and mapp...
【技术保护点】
【技术特征摘要】 【专利技术属性】
1.一种基于多传感器融合SLAM和磁导的组合导航方法，包括坑道内和坑道外组合导航的方式，机器人会在相应位置进行地图切换和导航方式的切换，其特征在于，包括以下步骤，坑道内包括首先是创建先验地图，建图算法结合了里程计数据，IMU惯导数据，深度相机和激光雷达来实现，巡检时机器人实时定位导航，使用的是中线导航和先验地图组合定位导航的方式；坑道外首先是根据巡检任务确认巡检路径，然后铺设磁条和RFID，并对RFID进行编号；导航时机器人底部的磁感应装置根据磁条偏离装置中心距离来实时计算控制速度；在检测到RFID编号和目标RFID编号一致时，表示到达目的地。2.根据权利要求1所述的基于多传感器融合SLAM和磁导的组合导航方法，其特征在于：所述先验地图的创建，建图算法结合了里程计数据，IMU惯导数据，深度相机和激光雷达来实现包括以下步骤，S1.1、对里程计的数据进行修正，输入数据有：编码器采集的里程计数据，IMU惯导数据和深度相机实时计算的视觉里程计的数据，将此三种数据进行融合估计计算，得到更精确的里程计数据；S1.2、将深度相机和激光雷达的数据进行融合计算，得到更准确的点云数据；事先要将深度相机和激光雷达的位置坐标进行标定，然后根据标定坐标，将激光雷达数据进行过滤，转换到深度相机坐标系下；然后将二者的数据进行平滑操作，得到融合后的点云数据；S1.3、进行由粗到精的帧间匹配建图，使用步骤S1.1得到的更精确的里程计数据对机器人的实时位姿进行预估，得到机器人当前粗位姿；然后再根据当前帧点云数据和当前地图数据进行ICP匹配计算，对粗位姿进行修正，得到更精确的机器人位姿。3.根据权利要求2所述的基于多传感器融合SLAM和磁导的组合导航方法，其特征在于：所述巡检时机器人实时定位导航，使用的是中线导航和先验地图组合定位导航的方式，包括以下步骤，S2.1、采用中线导航的方式，确保机器人一直沿着坑道的中间行走，而不会碰到坑道两侧墙壁；实时检测前置深度相机的数据，找出两侧墙壁，并计算机器人此时距两侧墙壁的距离和与墙壁平行线的夹角，根据距离和夹角控制机器人严格沿着中间行走；S2.2、基于事先创建的先验地图，采用蒙特卡洛定位策略，使用似然域模型和波束模型相结合的粒子滤波算法来计算机器人实时后验位姿；定位计算所使用的里程计数据和点云数据，都使用步骤S1.1和步骤S1.2优化后的数据；S2.3、采用步骤S2.1的中线导航控制机器人前进和后退和步骤2.2计算机器人的实时位姿，来判断机器人是否到达目标点，并控制机器人停车。4.根据权利要求2所述的基于多传感器融合SLAM和磁导的组合导航方法，其特征在于：所述S1.1、对里程计的数据进行修正，输入数据有：编码器采集的里程计数据，IMU惯导数据和深度相机实时计算的视觉里程计的数据，将此三种数据进行融合估计计算，得到更精确的里程计数据，具体包括：Step111.首先是多传感器数据的采集，作为机器人建图和定位的前端输入；odom和IMU数据通过编码器直接读取得到，视觉odom需要根据前后两帧视觉点云进行ICP点云配准得到其偏移量；X
′
＝R
3x3
X+T
3x1
，其中X
′
表示当前帧点云，X表示前一帧点云，R
3x3
表示点云变换
旋转矩阵，T
3x1
表示点云变换平移矩阵，将旋转矩阵和平移矩阵进行平面约束，得到视觉里程计odom；其中表示里程计的x坐标，y坐标和角度测量值，(ρ
imu
)表示IMU的测量值，表示视觉里程计的x坐标，y坐标和角度计算值，表示融合后的里程计测量值；Step112.对odom，IMU和视觉odom进行EKF融合计算非线性运动和观测模型如下：x
k
＝f(x
k
‑1,v
k
)+w
k
ꢀꢀ
(1)y
k
＝g(x
k
)+n
k
ꢀꢀ
(2)x
k
为状态量，y
k
为观测量，v
k
为控制量，f()与g()都是非线性函数，假设x
k
的置信度函数为高斯分布：假设：w
k
和n
k
都是白噪声，f()，g()可微，w
k
～N(0,Q
k
)n
k
～N(0,R
k
)
ꢀꢀ
(4)其中为0时刻的先验预测状态量，v
1：k
为1到k时刻的控制量，y
0：k
为0到k时刻的测量量，为k时刻的后验状态估计值，为k时刻的协方差估计值，N为高斯分布数学表示，表示期望值为协方差为的高斯白噪声，Q
k
为状态方程白噪声协方差,R
k
为观测方程白噪声协方差，整个表达式表示w
k
为期望值为0，协方差为Q
k
的高斯白噪声，n
k
为期望值为0，协方差为R
k
的高斯白噪声；针对运动和观测模型进行线性化，将f()与g()进行一阶泰勒展开；运动方程泰勒展开：观测方程泰勒展开：其中：w
′
k
为一阶泰勒展开的运动白噪声，F
k
‑1为运动函数f()的雅可比矩阵，为k时刻的先验状态量，为k
‑
1时刻的后验状态估计值，为k时刻的先验观测量，为k时刻的先验状态量，G
k
为观测方程g()的雅可比矩阵，n
′
k
为一阶泰勒展开的观测白噪声；采用EKF算法大体分为预测与更新两步：
预测：预测：更新更新更新其中为k时刻的先验协方差，为k
‑
1时刻的后验估计协方差，为运动函数在k
‑
1时刻的雅可比矩阵的转置矩阵，Q
′
k
为状态量真实值与预测值之间的误差协方差，K
k
为k时刻的卡尔曼增益矩阵，为观测方程在k时刻的雅可比矩阵的转置矩阵，R
′
k
为k时刻测量量真实值与预测值的误差协方差，为k时刻的后验估计协方差；预测是基于当前后验量根据运动方程来更新均值与方差，更新则是根据观测信息来修正均值与方差，由此来估计准确的位姿数据，EKF经典递归更新方程，通过更新方程，由计算出使用Odom数据作为观测量，用IMU作为测量量，在机器人Odom坐标系下进行一次EKF融合，保证融合数据的连续性，然后再采用视觉odom数据作为观测量，以IMU与Odom融合数据为测量量在全局map坐标系下进行二次EKF融合，由此获得全局下的融合位姿5.根据权利要求2所述的基于多传感器融合SLAM和磁导的组合导航方法，其特征在于：所述S1.2、将深度相机和激光雷达的数据进行融合计算，得到更准确的点云数据；事先要将深度相机和激光雷达的位置坐标进行标定，然后根据标定坐标，将激光雷达数据进行过滤，转换到深度相机坐标系下；然后将二者的数据进行平滑操作，得到融合后的点云数据，具体包括：Step121、根据机器人上激光雷达和深度相机的布局，使用标定算法进行标定，得到激光雷达和深度相机的准确坐标变换T1＝{x,y,z,roll,pitch,yaw}，其中x表示x轴距离，y表示y轴距离，z表示z轴距离，roll表示绕x轴旋转角度，pitch表示绕y轴旋转角度，yaw表示绕z轴旋转角度；Step122、在实时建图和定位过程中，对激光雷达的数据进行过滤，过滤的规则是范围不超过深度相机的探测范围，得到滤波之后的激光雷达数据；∪
lidar
＝<P1|P2|
…
|P
n
>,其中P
i
＝{x
i
,y
i
,z
i
}，表示具体某个点云，且x
i
<Max
x
，y
i
<Max
y
，z
i
<Max
z
，其中U
lidar
表示激光雷达数据集合，P
i
表示具体点云，x
i
表示点云x轴坐标，y
i
表示点云y轴坐标，z
i
表示点云z轴坐标，Max
x
表示深度相机x轴最大探测距离，Max
y
表示深度相机y轴最大探测距离，Max
z
表示深度相机z轴最大探测距离；
Step123、将激光雷达数据根据传感器之间的坐标变换，将激光雷达数据转换到深度相机坐标系下；U
′
lidar
技术研发人员：褚衍超，章海兵，丁亚琦，奚笑冬，汪中原，
申请(专利权)人：科大智能科技股份有限公司科大智能电气技术有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。