一种超声波辅助融合定位的无人车导航纠偏方法技术

本发明专利技术公开了一种超声波辅助融合定位的无人车导航纠偏方法，包括S1、以无人车自身坐标系建立车身坐标系；S2、在无人车上安装信息采集装置和超声波传感器，采集并计算无人车的位姿信息以及所在空间的环境信息；S3、对无人车的位姿信息进行扩展卡尔曼滤波融合，得到融合后的位姿；S4、将超声波传感器的量测数据结合融合后的位姿，比较导航过程中左右两侧前后超声波与墙壁距离关系以及无人车融合后的位姿，并根据无人车与左右墙体之间的相对位置关系判断车体是否偏移；S5、判断车体偏移后停止导航并下发转向角度w，将无人车矫正到与左右障碍平面平行的关系再继续导航。本发明专利技术可有效降低传感器测量过程随机噪声，极大减少无人车与行进途中本体左右的墙体发生碰撞的概率。与行进途中本体左右的墙体发生碰撞的概率。与行进途中本体左右的墙体发生碰撞的概率。

【技术实现步骤摘要】
一种超声波辅助融合定位的无人车导航纠偏方法


[0001]本专利技术属于无人车导航
，具体涉及一种超声波辅助融合定位的无人车导航纠偏方法。

技术介绍

[0002]无人车在军事、工业、民生等各个领域都已经有了广泛应用，在全球已经形成一个庞大的产业体系，无人车技术的发展在一定程度上体现了一个国家的智能化技术的发展现状。
[0003]导航定位是无人车必须具备的功能，是无人车完成各项任务的前提和基础。从目前用于无人车的导航定位技术来看，多是以多种传感器融合的方式实现，若在导航的同时需要与墙平行执行检测任务，这样的要求导航还不能精确控制。
[0004]因此亟需研发一种导航纠偏方法，提高无人车工作性能，降低无人车在运行过程中发生碰撞的概率，提高无人车自身及其用户的的安全。

技术实现思路

[0005]本专利技术所要解决的技术问题是针对上述现有技术的不足，提供一种超声波辅助融合定位的无人车导航纠偏方法，能够精确控制无人车与墙平行执行检测任务，提高无人车工作性能，降低无人车在运行过程中发生碰撞的概率，提高无人车自身及其用户的安全。
[0006]为实现上述技术目的，本专利技术采取的技术方案为：
[0007]一种超声波辅助融合定位的无人车导航纠偏方法，包括
[0008]S1、以无人车自身坐标系建立车身坐标系，车中心为原点，正前方为X方向，正左方为Y方向，给定无人车的X方向速度Vx；
[0009]S2、在无人车上安装信息采集装置和超声波传感器，采集并计算无人车的位姿信息以及所在空间的环境信息；
[0010]S3、对无人车的位姿信息进行扩展卡尔曼滤波融合，实现位姿矫正，得到融合后的位姿；
[0011]S4、将超声波传感器的量测数据结合融合后的位姿，比较导航过程中左右两侧前后超声波与墙壁距离关系以及无人车融合后的位姿，并根据无人车与左右墙体之间的相对位置关系判断车体是否偏移；
[0012]S5、判断车体偏移后停止导航并下发转向角度w，按照下发的转向角度w将无人车矫正到与左右障碍平面平行的关系再继续导航。
[0013]为优化上述技术方案，采取的具体措施还包括：
[0014]上述的步骤S2所述信息采集装置包括二维激光雷达、惯性测量单元和增量式编码器；
[0015]所述二维激光雷达，用于测量无人车所在空间内与周围墙体的距离信息；
[0016]所述惯性测量单元，用于测量无人车自身线加速度和角速度；
[0017]所述增量式编码器，用于将无人车的位移转换成周期性的电信号输出计数脉冲得到角速度和直线速度。
[0018]上述的步骤S2中，在无人车左右两侧各安装两个超声波传感器。
[0019]上述的步骤S2中采集并计算无人车的位姿信息以及所在空间的环境信息的具体步骤包括：
[0020]对所述增量式编码器测量得到的角速度和直线速度进行积分得到无人车的里程信息；
[0021]对所述惯性测量单元测量得到的角速度进行计算得到无人车的姿态角；
[0022]对所述二维激光雷达测量得到的无人车所在空间与周围墙体的距离信息进行线段特征提取，在栅格地图中进行直线拟合得到无人车所在空间的环境信息，即空间二维点云地图。
[0023]对所述二维激光雷达测量得到的无人车所在空间二维点云地图进行扫描匹配得到无人车的位移量。
[0024]上述的增量式编码器对左右轮速进行积分得到无人车的里程信息，计算公式为，
[0025]Vx＝(Vr+Vl)/2
[0026]Vth＝(Vr
‑
Vl)/d
[0027]delta_x＝Vx*cos(th)*dt
[0028]delta_y＝Vx*sin(th)*dt
[0029]delta_th＝Vth*dt
[0030]其中，Vr为增量式编码器所测得的右轮速度，Vl为增量式编码器所测得的左轮速度，d为无人车左右轮距，delta_x为无人车正前方X方向上的位移量，delta_y为无人车正左方Y方向上的位移量，delta_th为无人车姿态角，Vx为无人车前进方向上的速度，Vth为无人车逆时针旋转的角速度，th为无人车逆时针旋转角度，dt为两次计算的时间差。
[0031]上述的步骤S3包括：
[0032]将增量式编码器所计算的里程信息与惯性测量单元所计算的无人车姿态角做扩展卡尔曼滤波融合，初始化状态方程及协方差矩阵，根据控制参数进行状态更新，计算预测状态的均值，计算预测误差的协方差矩阵及卡尔曼增益矩阵，用测量值修正预测状态，计算更新后误差协方差矩阵，迭代计算预测状态的均值，得到融合后的无人车姿态角；
[0033]将增量式编码器所计算的里程信息与空间二维点云地图扫描匹配得到无人车的位移量做扩展卡尔曼滤波融合，初始化状态方程及协方差矩阵，根据控制参数进行状态更新，计算预测状态的均值，计算预测误差的协方差矩阵及卡尔曼增益矩阵，用测量值修正预测状态，计算更新后误差协方差矩阵，迭代计算预测状态的均值，得到融合后的无人车位置；
[0034]将融合后的无人车姿态角和无人车位置，即无人车位姿用于导航定位，提高无人车导航过程中的定位精度，使后续导航纠偏效果更准确。
[0035]上述的步骤S4中判断无人车与左右障碍平面的相对位置关系的具体步骤为：
[0036]比较导航过程中左右两侧前后超声波与墙壁距离关系以及无人车融合后的位姿；
[0037]若左右两侧前后超声波传感器距离相等，则判断此时无人车与左右障碍平面平行不需要纠偏；
[0038]若左右两侧前后超声波传感器距离不相等，无人车融合后的姿态角在1s时间内的波动小于0.2rad，则认为无人车当前车体发生了偏移，暂停导航发布需要调整的角度给底盘，订阅调整后的位姿信息启动导航；
[0039]若左右两侧前后超声波传感器距离不相等并且无人车融合后的姿态角在1s时间内的波动大于0.2rad，判断车体当前正在转弯不需要纠偏。
[0040]上述的步骤S5中下发的转向角度w具体为：
[0041]以所述无人车车体前进方向为正方向，以所述无人车车体逆时针旋转为正方向；
[0042]对于无人车右方墙体：
[0043]无人车转向角度w＝
‑
arcsin(|x1
‑
x2|/d1)；
[0044]式中，x1为所述无人车车身右前侧超声波传感器距离右侧墙面的距离；
[0045]x2为所述无人车车身右后侧超声波传感器距离右侧墙面的距离；
[0046]d1为右侧前后两超声波传感器的安装距离；
[0047]对于无人车左方墙体：
[0048]无人车转向角度w＝arcsin(|x3
‑
x4|/d2)；
[0049]式中，x3为所述无人车车身左前侧超声波传感器距离左侧墙面的距离；
[0050]x4为所述无人车车身左后侧超声波传感器距离左侧墙面的距离；
[0本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种超声波辅助融合定位的无人车导航纠偏方法，其特征在于，包括S1、以无人车自身坐标系建立车身坐标系，车中心为原点，正前方为X方向，正左方为Y方向，给定无人车的X方向速度Vx；S2、在无人车上安装信息采集装置和超声波传感器，采集并计算无人车的位姿信息以及所在空间的环境信息；S3、对无人车的位姿信息进行扩展卡尔曼滤波融合，实现位姿矫正，得到融合后的位姿；S4、将超声波传感器的量测数据结合融合后的位姿，比较导航过程中左右两侧前后超声波与墙壁距离关系以及无人车融合后的位姿，并根据无人车与左右墙体之间的相对位置关系判断车体是否偏移；S5、判断车体偏移后停止导航并下发转向角度w，按照下发的转向角度w将无人车矫正到与左右障碍平面平行的关系再继续导航。2.根据权利要求1所述的一种超声波辅助融合定位的无人车导航纠偏方法，其特征在于，步骤S2所述信息采集装置包括二维激光雷达、惯性测量单元和增量式编码器；所述二维激光雷达，用于测量无人车所在空间内与周围墙体的距离信息；所述惯性测量单元，用于测量无人车自身线加速度和角速度；所述增量式编码器，用于将无人车的位移转换成周期性的电信号输出计数脉冲得到角速度和直线速度。3.根据权利要求2所述的一种超声波辅助融合定位的无人车导航纠偏方法，其特征在于，步骤S2中，在无人车左右两侧各安装两个超声波传感器。4.根据权利要求3所述的一种超声波辅助融合定位的无人车导航纠偏方法，其特征在于，所述步骤S2中采集并计算无人车的位姿信息以及所在空间的环境信息的具体步骤包括：对所述增量式编码器测量得到的角速度和直线速度进行积分得到无人车的里程信息；对所述惯性测量单元测量得到的角速度进行计算得到无人车的姿态角；对所述二维激光雷达测量得到的无人车所在空间与周围墙体的距离信息进行线段特征提取，在栅格地图中进行直线拟合得到无人车所在空间的环境信息，即空间二维点云地图；对所述二维激光雷达测量得到的无人车所在空间二维点云地图进行扫描匹配得到无人车的位移量。5.根据权利要求4所述的一种超声波辅助融合定位的无人车导航纠偏方法，其特征在于，所述增量式编码器对左右轮速进行积分得到无人车的里程信息，计算公式为，Vx＝(Vr+Vl)/2Vth＝(Vr
‑
Vl)/ddelta_x＝Vx*cos(th)*dtdelta_y＝Vx*sin(th)*dtdelta_th＝Vth*dt其中，Vr为增量式编码器所测得的右轮速度，Vl为增量式编码器所测得的左轮速度，d为无人车左右轮距，delta_x为无人车正前方X方向上的位移量，delta_y为无人车正左方Y
方向上的位移量，delta_th为无人车姿态角，Vx为无人车前进方向上的速度，Vth...

【专利技术属性】
技术研发人员：刘义亭，张磊，路红，随连杰，李佩娟，郁汉琪，贾通，赵贤林，高芳征，
申请(专利权)人：南京工程学院，
类型：发明
国别省市：