CN112241171A - 一种可绕障碍的轮式机器人直线轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种可绕障碍的轮式机器人直线轨迹跟踪方法，在全局地图使用人工规划路径后直线跟踪导航，能提高机器人移动的稳定性，且行走路径较为美观。为解决直线跟踪无法绕过障碍这一难题，本发明使用激光雷达实时监测障碍物，当检测到障碍物且存在大于2s，计算机器人当前位置和目标点位置的距离，若距离值大于谁当阈值，则采用DWA算法计算最优路径，实现绕障顺利达到目标点。本发明既可将人工规划路径的任务顺利完成，也可以在移动过程中实现对移动物体的避障，对原地图未出现的障碍及时绕行，具有智能化和实用化意义。

Description

一种可绕障碍的轮式机器人直线轨迹跟踪方法

技术领域

本发明涉及移动机器人导航领域，尤其是涉及一种可绕障碍的轮式机器人直线轨迹跟踪方法。

背景技术

随着人工智能相关科学技术的发展，移动机器人已经在各行各业中得到了广泛应用。在变电站复杂环境中，移动机器人可以将人从危险、繁重的工作环境中解放出来，自动巡检具有极大的优越性。其中机器人在完成任务时首先要知道自身当前位姿，通过激光雷达检测周围环境与已知地图实时匹配完成机器人定位；其次是要知道要去的终点，即当前任务的目标点位姿；最后机器人要分析计算如果从当前位姿移动到目标位姿。所以，机器人定位点与目标点之间的导航算法成为能否顺利完成任务的关键。

当前主要的导航方式有人工提前规划路径的轨迹跟踪方法和根据已知地图自动规划路径的导航方法。人工提前规划路径的轨迹跟踪方法的难点在于期望值随时间不断变化。现常用的方法有非线性状态反馈控制、滑模控制、自适应控制和智能控制方法。根据已知地图自动路径规划则有Dijkstra和A-Star等常用的全局路径规划法、DWA(DynamicWindow Approach)和TEB(Timed Elastic Band)等常用的局部路径规划法。

使用自动路径规划的方式导航，虽然能省去人工规划路径的局限性，在导航过程中也能实时避障、绕障，但是在变电站环境中，由于道路复杂且有些较为崎岖的道路不适应移动机器人经过，仅从激光雷达无法全面判断路况，需要人工辅助。使用人工规划路径后做轨迹跟踪的方式比较可靠，且行走轨迹较为美观可根据不用路况设置不用的行走方式，但是在机器人完成巡检任务时，只能根据规划好的路径绕行地图中已有的障碍物，无法做到实时绕障功能。

发明内容

本发明的目的在于提供一种可绕障的轮式机器人直线轨迹跟踪方法，该方法既可将人工规划路径的任务顺利完成，也可以在移动过程中实现对移动物体的避障，对原地图未出现的障碍及时绕行。

本发明的目的是这样实现的：

一种可绕障的轮式机器人直线轨迹跟踪方法，特征是：

步骤一：人工提前规划好各目标点间的路径，设起始点为B(x_begin，y_begin，θ_begin)，目标点为G(x_goal，y_goal，θ_goal)，机器人的实时位姿P(x_cur，y_cur，θ_cur)，其中x、y为机器人二维平面内的横坐标、纵坐标，θ为当前机器人方向朝向的角度；

根据机器人差速驱动方式的运动学模型得到以下公式：V_L为左轮速度，V_R为右轮速度，r为驱动轮半径，L为轴间距；

机器人的线速度v即前进速度为：

机器人的角速度ω为：

由上式可得：

进一步计算机器人的左轮和右轮的线速度与机器人的角速度、前进速度之间的关系为：

v_L＝v-ωL

v_R＝v+ωL

联立可得：

根据上式可得在控制机器人移动时，机器人中的跟踪模块能实时发出v和ω的速度指令实现控制左右电机以指定角速度和线速度移动；

由于变电站特殊环境，路径均设置为两点间的直线。首先控制角速度使机器人在原地旋转，将朝向θ调整至直线方向，使机器人满足数学公式为：

|arctan((y_goal-y_begin)/(x_2goal-x_begin))-θ_cur|＜θ_error；

步骤二：机器人方向调整至直线方向后，开始直线跟踪，使机器人沿着直线行走，但是由于车轮打滑、数据噪声这些因素，使得速度指令下发后不能严格地按直线行走，所以需要实时根据当前位姿控制角度和线速度使机器人朝着目标点移动，令(x_cur，y_cur，θ_cur)为机器人当前位姿，(x_r，y_r，θ_r)为机器人期望位姿，(x_e，y_e，θ_e)为机器人当前位姿与期望位姿的偏差；

计算角度误差时，引入sinθ_e/2，能够将θ_e的角度转化为(-1，1)内的单调函数，于是建立如下误差模型：

求导可得微分误差方程，针对误差方程构建如下线性表达式：

基于上述分析，需计算控制率使得误差微分方程趋于稳定，误差收敛到0，即可完成轨迹跟踪。

经计算可取如下控制率(以vr≠0的系统为例)

v＝v_rcosθ_e+k₁x_e

其中k₁，k₂，k₃为控制参数，k₁，k₂，k₃＞0；

步骤三：在机器人直线跟踪移动时，通过激光雷达实时检测前方固定角度内的障碍物，如图2所示雷达扫面示意图，雷达一圈扫描点数为N，N为正整数，如图取正前方30°内的点实时监测，若满足下式2s，则系统判断前方有障碍阻挡；

其中d为设定的避障距离阈值，l是当前雷达检测到在(165°，195°)距离，n为满足(l<d)的点个数，之间的2s的时间限制是为了排除行人或者动物这些移动物体短暂经过机器人前导致误判断；

步骤四：如果确定前方出现障碍物，将导航任务交给DWA算法规划局部绕障路径。以差速前驱机器人为例：

1、设机器人可偏角的范围为(-π/2，π/2)，间隔Δ_ω取弧度值，最终有A个取值；

2、设机器人速度为(0，v_max)间隔Δ_vm/s取速度值，最终有B个取值；

3、仿真时长为ts；

4、构造DWA运动轨迹，即预测未来ts时间内的角度和速度组合下可能生成的所有运动轨迹，每条轨迹单独储存，则可以计算出A*B条运动轨迹；

5、一次对每一条轨迹做检测，如果与障碍物发生碰撞，则该轨迹删除

6、构造最终评价函数，在上述无碰撞的轨迹中，对机器人的速度、航向进行分析，选取最终的一组最优速度和角度组合作为机器人未来2s的移动轨迹，使机器人顺利绕过障碍物；

步骤五：机器人通过直线跟踪或者DWA路径规划运动满足如下数学公式时：

认为机器人x、y位置已经到，此时发送相应的ω控制指令，使其原地旋转满足如下数学公式：

|θ_cur-θ_goal|＜θ_error

认为此次从B(x_begin，y_begin，θ_begin)至目标点G(x_goal，y_goal，θ_goal)导航到位。

本发明为一种可绕障的轮式机器人直线轨迹跟踪方法，在全局地图使用人工规划路径后直线跟踪导航，能提高机器人移动的稳定性，且行走路径较为美观。为解决直线跟踪无法绕过障碍这一难题，本发明使用激光雷达实时监测障碍物，引入DWA算法仿真最优轨迹实现绕障顺利达到目标点。本发明既可将人工规划路径的任务顺利完成，也可以在移动过程中实现对移动物体的避让，对原地图未出现的障碍及时绕行，具有智能化和实用化意义。

附图说明

图1是本发明的系统流程示意图；

图2是本发明的雷达扫面示意图；

图3是本发明的直线跟踪示意图；

图4是本发明的检测障碍示意图；

图5是本发明的绕行障碍示意图；

图6是本发明的到达目标点示意图。

具体实施方式：

下面结合实施例并对照附图对本发明作进一步详细说明。

本实施例提供一种可绕障的轮式机器人直线轨迹跟踪方法。本实施例使用机器人CSG-PIR-TS200进行算法演示，机器人搭载sick151二位激光雷达、运动控制模块、信息采集模块、轮式里程计、电源管理模块、工控机(处理器为i5-4300U)。

本实施例所用的栅格地图由谷歌开源算法cartographer生成，所测试地点为办公室和楼道。选择A作为起始点，B点作为目标点。规划直线路径，测试在路径图中有无障碍物观察机器人是否可以避障并且成功绕障。

经实验对比，本发明在路径没有障碍物时能进行直线跟踪至目标点；在路径中出现行人等移动无提示可暂停行走，待行人经过后继续直线跟踪至目标点；在路径中出现固定障碍物阻挡，能规划新的路径绕过局部障碍物至目标点。以下是具体操作步骤：

步骤一：将搭载二维激光雷达的机器人置于办公室中，正式开始定位前需加载已经建立好的办公室和楼道二维占据栅格地图，通过激光数据与地图信息实时匹配确定机器人当前位姿(x_cur，y_cur，θ_cur)，基于机器人运动学模型，实时下发v和ω速度指令控制机器人的线速度和角速度选取C(x_begin，y_begin，θ_begin)为起始点，D(x_goal，y_goal，θ_goal)为目标点，如图3所示。令θ_error＝0.03通过下发合适的角速度使机器人原地旋转至满足如下公式：

|arctan((y_goal-y_begin)/(x_2goal-x_begin))-θc_ur|＜θ_error；

步骤二：对AB直线跟踪导航，由于直线行走斜率一定，故选择以下2组控制律。

目标点较远时，距离大于0.5m：

v＝0.5cosθ_e+0.2x_e

目标点较近时，距离小于0.5m：

v＝0.2cosθ_e+0.1x_e

当其中v和ω同样受机器人最大速度限制；

步骤三：在机器人直线跟踪移动时，通过激光雷达实时检测前方固定角度内点云距离，如图4所示，设置避障距离阈值为1m，当激光雷达正前方30°点云距离大于1m点的个数大于N/36时，机器人认为前方有障碍物阻挡，将当前速度置为0，静止2s后如果仍然满足前方障碍物，且目标点距离大于车身长度和避障距离之和，则需要DWA规划路径绕障；

步骤四：确定在直线跟踪时检测到障碍物，DWA算法开始实时下发速度指令，根据机器人实际情况，设置仿真时间为2s，最大速度v_max为0.5m/s,如图所示行走路径至目标点实时下法角速度和线速度指令，直至绕过障碍物，如图5所示；

步骤五：令dis_error＝0.05m，θ_error＝0.03，机器人通过直线跟踪或者DWA路径规划运动满足如下数学公式时：

认为机器人x、y位置已经到位，此时发送相应的ω控制指令，使其原地旋转满足如下数学公式：

|θ_cur-θ_goal|＜θ_error

此时机器人导航任务完成，如图6所示从起始点C移动至目标点D。

Claims (2)

1.一种可绕障的轮式机器人直线轨迹跟踪方法，其特征在于：

步骤一：人工提前规划好各目标点间的路径，设起始点为B(x_begin，y_begin，θ_begin)，目标点为G(x_goal，y_goal，θ_goal)，机器人的实时位姿P(x_cur，y_cur，θ_cur)，其中x、y为机器人二维平面内的横坐标、纵坐标，θ为当前机器人方向朝向的角度；

根据机器人差速驱动方式的运动学模型得到以下公式：V_L为左轮速度，V_R为右轮速度，r为驱动轮半径，L为轴间距；

机器人的线速度v即前进速度为：

机器人的角速度ω为：

由上式可得：

进一步计算机器人的左轮和右轮的线速度与机器人的角速度、前进速度之间的关系为：

v_L＝v-ωL

v_R＝v+ωL

联立可得：

根据上式可得在控制机器人移动时，机器人中的跟踪模块能实时发出v和ω的速度指令实现控制左右电机以指定角速度和线速度移动；

由于变电站特殊环境，路径均设置为两点间的直线。首先控制角速度使机器人在原地旋转，将朝向θ调整至直线方向，使机器人满足数学公式为：

|arctan((y_goal-y_begin)/(x_2goal-x_begin))-θ_cur|＜θ_error；

步骤二：机器人方向调整至直线方向后，开始直线跟踪，使机器人沿着直线行走，但是由于车轮打滑、数据噪声这些因素，使得速度指令下发后不能严格地按直线行走，所以需要实时根据当前位姿控制角度和线速度使机器人朝着目标点移动，令(x_cur，y_cur，θ_cur)为机器人当前位姿，(x_r，y_r，θ_r)为机器人期望位姿，(x_e，y_e，θ_e)为机器人当前位姿与期望位姿的偏差；

计算角度误差时，引入sinθ_e/2，能够将θ_e的角度转化为(-1，1)内的单调函数，于是建立如下误差模型：

求导可得微分误差方程，针对误差方程构建如下线性表达式：

基于上述分析，需计算控制率使得误差微分方程趋于稳定，误差收敛到0，即可完成轨迹跟踪。

经计算可取如下控制率(以vr≠0的系统为例)

v＝v_rcosθ_e+k₁x_e

其中k₁，k₂，k₃为控制参数，k₁，k₂，k₃＞0；

步骤三：在机器人直线跟踪移动时，通过激光雷达实时检测前方固定角度内的障碍物，如图2所示雷达扫面示意图，雷达一圈扫描点数为N，N为正整数，如图取正前方30°内的点实时监测，若满足下式2s，则系统判断前方有障碍阻挡；

其中d为设定的障碍物最小距离阈值，l是当前雷达检测到在(165°，195°)距离，n为满足(l＜d)的点个数，之间的2s的时间限制是为了排除行人或者动物这些移动物体短暂经过机器人前导致误判断；

步骤四：如果确定前方出现障碍物，将导航任务交给DWA算法规划局部绕障路径；

步骤五：机器人通过直线跟踪或者DWA路径规划运动满足如下数学公式时：

认为机器人x、y位置已经到，此时发送相应的ω控制指令，使其原地旋转满足如下数学公式：

|θ_cur-θ_goal|＜θ_error

认为此次从B(x_begin，y_begin，θ_begin)至目标点G(x_goal，y_goal，θ_goal)导航到位。

2.根据权利要求1所述的可绕障的轮式机器人直线轨迹跟踪方法，其特征在于：在步骤四中：

(1)、设机器人可偏角的范围为(-π/2，π/2)，间隔Δ_ω取弧度值，最终有A个取值；

(2)、设机器人速度为(0，v_max)间隔Δ_vm/s取速度值，最终有B个取值；

(3)、仿真时长为ts；

(4)、构造DWA运动轨迹，即预测未来ts时间内的角度和速度组合下可能生成的所有运动轨迹，每条轨迹单独储存，则可以计算出A*B条运动轨迹；

(5)、一次对每一条轨迹做检测，如果与障碍物发生碰撞，则该轨迹删除

(6)、构造最终评价函数，在上述无碰撞的轨迹中，对机器人的速度、航向进行分析，选取最终的一组最优速度和角度组合作为机器人未来2s的移动轨迹，使机器人顺利绕过障碍物。

Images