基于惯导和激光雷达的巡逻机器人行走避障方法及系统
技术领域
本发明涉及导航领域，具体涉及一种基于惯导和激光雷达的巡逻机器人行走避障方法及系统。
背景技术
目前通过智能车载硬件中的GPS模块计算小车行驶距离的方法主要有两种：
一种方法是主要基于GPS模块经度、维度坐标点，并按照地球上两点之间的弧长公式进行计算。该方法缺点是：①当GPS模块采样频率较低时，计算出的距离误差较大；②当GPS信号不好时，如通过隧道、高楼、林荫等环境，导致GPS坐标点漂移、丢失，此时根据弧长公式计算出的行驶距离与实际行驶距离偏差较大；
另一种方法是通过GPS速度近似计算行驶距离。该方法缺点是：①当 GPS采样频率较低时，相邻采样点之间的速度变化不规律，导致根据GPS 速度计算出来的行驶距离不准确；②GPS速度本身会产生异常值，根据异常的GPS速度计算出来的行驶距离偏差很大；
传统的避障方法，如红外避障、超声避障、双目视觉导航系统等，都存在缺点及弊端。红外避障效果与所安装的红外传感器数量成正比，这对于成本控制、外观设计等有一定的限制，而且红外侦测的距离有限，对于较远的障碍物不能及时发现和提前规避；超声避障无法侦测其障碍物大小形状，并且只有在障碍物距离较近时才能侦测到，可能不发及时躲避；双目视觉导航同样存在侦测距离近的问题，且夜间或光线较弱的情况下会受到影响。
发明内容
本发明所要解决的技术问题是针对现有技术中存在上述缺陷，提供一种通过惯导信息精确计算巡逻机器人行驶距离，并用激光雷达辅助避障的一种精确的应用场景广泛的导航方法。
根据本发明，提供了一种基于惯导和激光雷达的巡逻机器人行走避障方法，包括：
第一步骤：针对巡逻机器人的起始点和目的地，利用学习方式记录设定的巡逻路线轨迹，为后续巡逻建立数字地图；
第二步骤：利用建立的地图进行导航使巡逻机器人沿设定的巡逻路线轨迹巡逻。
优选地，在第二步骤，当巡逻机器人在途中遇到障碍物时，由激光雷达扫描得到巡逻机器人前方障碍物的点云数据，对数据进行聚类以得到前方障碍物聚类；根据障碍物的远近以及障碍物之间的空隙决定机器人转弯角度以及是否停止。
优选地，在第二步骤，当巡逻机器人在途中遇到障碍物时，巡逻机器人的控制单元利用卫星导航设备输出的方位角和速度信号和激光雷达扫描得到的障碍物的距离和角度数据，绕过障碍物后再用角度修正算法控制电机返回到原定路线。
优选地，在第二步骤，巡逻机器人的控制单元利用卫星导航设备输出的方位角和速度信号和激光雷达扫描得到的障碍物的距离和角度数据，计算巡逻车当前位置与数字路线上最近点的偏离值，再计算出回到数字路线上的控制量，若巡逻车在数字路线上，则计算沿数字路线行走的控制量。
优选地，在第二步骤，巡逻机器人开始行走以后巡逻机器人的惯导单元每隔固定时间输出里程、速度、航向角信息，在检测到位置坐标时，寻找与位置坐标距离最近的地图坐标，以及该地图坐标后续的预定个地图坐标，拟合当前点到地图点的直线，计算直线与正北方向夹角，将此夹角输入给电机，用于方向控制。
优选地，在第二步骤执行：
计算障碍物聚类的中心点O'距离坐标原点O的距离d，与x轴的夹角φ，用
计算d(φ)在任意角度θ产生的斥力：
A或C是聚类边界点：
定义在角度θ方向的总合力KRF(θ)为各个聚类产生的斥力最大值，其中Dm为设定的最大评估距离；
设定巡逻机器人到达的局部目标点A(d)，θobj为目标点方位角，dobj为目标点距离原点距离；目标点在角度θ产生的引力为：
KCF(θ)＝cos(θ-θobj)，；
设定巡逻机器人在角度θ可通行的代价函数：
KP(θ)＝KCF(θ)/KRF(θ)，
选择
优选地，在KPC＝0时，使巡逻机器人停车。
优选地，激光雷达是16线激光雷达。
本发明利用惯导初始信息，借助作为运载体的巡逻机器人的惯性测量单元的加速度和姿态变化，并通过时间积分测量值来估算巡逻机器人行驶距离作为预设路线并用诸如16线激光雷达之类的激光雷达实现道路避障的一种组合导航方法，该组合导航将两种或两种以上不同的导航设备以适当的方式组合在一起，利用其性能上的互补性，获得了比单独使用任一系统是更高导航性能的导航系统。
附图说明
结合附图，并通过参考下面的详细描述，将会更容易地对本发明有更完整的理解并且更容易地理解其伴随的优点和特征，其中：
图1示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法的流程图。
图2示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法的地图导航示意图。
图3示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法的电机控制示意图。
图4示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法的避障算法。
图5示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法的避障区域示意图。
图6示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障系统的巡逻线路示例。
图7示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障系统的系统框图。
需要说明的是，附图用于说明本发明，而非限制本发明。注意，表示结构的附图可能并非按比例绘制。并且，附图中，相同或者类似的元件标有相同或者类似的标号。
具体实施方式
为了使本发明的内容更加清楚和易懂，下面结合具体实施例和附图对本发明的内容进行详细描述。
图1示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法的流程图。优选地，采用的激光雷达是16线激光雷达。
如图2所示，根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障方法包括：
第一步骤S1：针对巡逻机器人的起始点和目的地，利用学习方式记录设定的巡逻路线轨迹，为后续巡逻建立数字地图；
具体地，例如，如图2所示，在第一步骤，记录有向距离
由于GPS因遮挡等因素导致位置偏差大，以及惯导系统的累积误差，可通过GPS和惯导联合卡尔曼方程推算相对位置，以减小误差。
对地图进行平滑处理(分段平滑)：分段平滑进一步减小经纬度不准确导致地图采集误差。
第二步骤S2：利用建立的地图进行导航使巡逻机器人沿设定的巡逻路线轨迹巡逻。
在第二步骤，当巡逻机器人在途中遇到障碍物时，由激光雷达扫描得到巡逻机器人前方障碍物的点云数据，对数据进行聚类以得到前方障碍物聚类；根据障碍物的远近以及障碍物之间的空隙决定机器人转弯角度。
而且在第二步骤，当巡逻机器人在途中遇到障碍物时，巡逻机器人的控制单元利用卫星导航设备输出的方位角和速度信号和激光雷达扫描得到的障碍物的距离和角度数据，绕过障碍物后再用角度修正算法控制电机返回到原定路线。
例如，在第二步骤，巡逻机器人的控制单元利用卫星导航设备输出的方位角和速度信号和激光雷达扫描得到的障碍物的距离和角度数据，计算巡逻车当前位置与数字路线上最近点的偏离值，再计算出回到数字路线上的控制量，若巡逻车在数字路线上，则计算沿数字路线行走的控制量。
巡逻机器人开始行走以后巡逻机器人的惯导单元每隔固定时间输出里程、速度、航向角信息。在某一时刻T检测到位置坐标
如图4所示，由激光雷达扫描得到巡逻机器人前方障碍物的点云数据，对数据进行聚类以得到前方障碍物聚类；根据障碍物的远近以及障碍物之间的空隙决定机器人转弯角度以及是否停止。
障碍物聚类产生的斥力场计算：
计算障碍物聚类的中心点O'距离坐标原点(巡逻机器人当前位置)O 的距离d，与x轴(巡逻机器人当前位置在巡逻路线轨迹处的切线)的夹角φ，用
计算d(φ)在任意角度θ产生的斥力定义如下：
A或C是聚类边界点：
在某一角度θ各个障碍物聚类均产生斥力，定义在角度θ方向的总合力 KRF(θ)为各个聚类产生的斥力最大值。
这里Dm为设定的最大评估距离，超过Dm的障碍物按最小斥力。
斥力受两方面影响：
障碍物距离：障碍物距离原点O距离越近，斥力越大；同时影响夹角范围越大；
障碍物聚类尺度：尺度越大，斥力影响夹角越大，即Δα越大。
例如，考虑无人车速度不快，简化制动安全距离Dsf＝0.8m，与行驶速度无关；最大评估距离Dm＝10m。
目标点产生的引力场：
设定巡逻机器人到达的局部目标点A(d)，θobj为目标点方位角，dobj为目标点距离原点距离；目标点在角度θ产生的引力为：
KCF(θ)＝cos(θ-θobj)，当θ接近θobj时变化平稳，当θ远离θobj时KCF(θ)迅速减小。
无人车在某一角度可通行的代价函数：
KP(θ)＝KCF(θ)/KRF(θ)，
选择
无人车运动决策：
KPC＝0，停车；
KPC＞0，选择θCur＝argmax(KPC)做为无人车当前转向角度；
速度：
这里KPCmax＝Dm-Dsr，KPCmax对应引力最大、斥力最小时情景。
无人车避障区域划分，如图5所示：
在停车区域，无人车停止，状态＝3，为停止状态；
在转向区域，无人车按照势场法确定转向方向与速度，状态＝2，为避障状态；
在转向区域以外，无人车按照势场法确定转向方向与速度，状态＝1，为正常行驶状态。
图6示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障系统的巡逻线路示例。
1、起始点：
1)巡逻车停在起点；
2)横向偏差小于0.2m、纵向偏差小于0.5m；
3)方位角任意；
2、直线行驶：
1)方位角导引前进；
2)根据速度计算东向、北向位移；
3)计算横向位移，修正横向位移；
4)计算前向通畅道路的宽度和长度，给出绕障角度数据(无障，值0)；
3、遇到障碍物：
1)根据障碍物所处位置，判定并给出转向控制数据，实施转向前进；
2)根据速度计算东向、北向位移；
3)计算横向位移，修正横向位移；
4)计算前向通畅道路的宽度和长度，给出绕障角度数据；
4、绕障碍物：
1)根据障碍物所处位置，判定并给出转向控制数据，实施转向前进；
2)根据速度计算东向、北向位移；
3)计算前向通畅道路的宽度和长度，给出绕障角度数据；
5、过障碍物：
1)根据障碍物所处位置判定障碍物已过时，根据数字路线最近点数据，计算给出转向控制数据，实施转向前进；
2)根据速度计算东向、北向位移；
3)根据东向、北向位移偏差调整前进速度，直到回到数字路线上；
4)计算前向通畅道路的宽度和长度，给出绕障角度数据；
6、沿预设路线前进：
1)根据速度计算东向、北向位移，当位移已回到数字路线上时，启动方位角导航前进；
2)根据速度计算东向、北向位移；
3)计算横向位移，修正横向位移；
4)计算前向通畅道路的宽度和长度，给出绕障角度数据；
7、转弯处修正：
1)当直线位移到达转角处的-2m时，启动激光雷达修正转弯点；
2)根据速度计算东向、北向位移；
3)计算横向位移，修正横向位移；
4)计算前向通畅道路的宽度和长度，给出绕障角度数据；
8、转向前进：
1)当激光雷达修正转弯点结束，启动下一段角度导引，实施转向；
2)根据速度计算新一段的东向、北向位移；
3)计算横向位移，修正横向位移；
4)计算前向通畅道路的宽度和长度，给出绕障角度数据；
9、终点停止：
1)当角度导引正常；
2)根据速度计算的东向、北向位移已回起点；
3)前向通畅道路的宽度和长度合适；
4)停止；
10、重新开始：
1)当多次巡逻未结束时；
2)启动角度导引，调整至巡逻起始状态；
3)角度导引前进；
4)根据速度计算新一段的东向、北向位移；
5)计算横向位移，修正横向位移；
6)计算前向通畅道路的宽度和长度，给出绕障角度数据；
图7示意性地示出了根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障系统的系统框图。
如图7所示，根据本发明优选实施例的基于惯导和激光雷达的巡逻机器人行走避障系统包括：激光雷达1、工控机2、卫星导航单元3和控制单元4，可以采用这种系统来执行上述巡逻机器人行走避障方法。
与现有技术相比，本发明解决了巡逻机器人在行进过程中电机带动履带转速不精确问题，大大减小了巡逻机器人实际姿态与预定路线的误差，并且辅助南北位移的计算；在避障绕障方面，巡逻机器人躲避障碍物后能精确的返回原设定路线，在实际使用中微调控制参数便可实现复杂路线的巡逻，这在应用中具有较高灵活性、实用性。
需要说明的是，除非特别指出，否则说明书中的术语“第一”、“第二”、“第三”等描述仅仅用于区分说明书中的各个组件、元素、步骤等，而不是用于表示各个组件、元素、步骤之间的逻辑关系或者顺序关系等。
可以理解的是，虽然本发明已以较佳实施例披露如上，然而上述实施例并非用以限定本发明。对于任何熟悉本领域的技术人员而言，在不脱离本发明技术方案范围情况下，都可利用上述揭示的技术内容对本发明技术方案作出许多可能的变动和修饰，或修改为等同变化的等效实施例。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。
