CN112731932B - 一种移动机器人的路径跟踪方法 - Google Patents

Abstract

本发明公开了一种移动机器人的路径跟踪方法，所述方法具体包括如下步骤：S1、将规划路径分割成轨迹点，放入轨迹点集；S2、基于移动机器人在上一时刻的移动速度确定当前的跟踪距离L，基于当前的跟踪距离形成下一预瞄点及下一预瞄点所在的预瞄直线；S3、基于移动机器人当前位置与预瞄直线的垂直距离T_len及当前位姿与预瞄直线的夹角θ确定移动机器人当前的移动线速度；S4、计算出移动机器人虚拟转向轮的转向控制角，基于当前虚拟转向轮的转向控制角及移动线速度来确定左转向轮及右转向轮的速度。该方法能够在保证车辆稳定的前提下，有效地实现移动机器人预期路径的跟踪，具有响应速度快、跟踪误差小及鲁棒性强的特点，适应各种路径下的跟踪。

Description

一种移动机器人的路径跟踪方法

技术领域

本发明属于路径跟踪技术领域，更具体地，本发明涉及一种移动机器人的路径跟踪方法。

背景技术

近年来，移动机器人的功能日益强大，使得交通、医疗和工业等领域均得到受益，移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等功能于一体的综合系统。路径跟踪作为移动机器人的关键技术之一，通过控制移动机器人的转向系统，使得机器人按照期望的路径行驶，路径跟踪算法的性能直接决定了路径的追踪精度，影响移动机器人在行走过程中的精确性和安全可靠性。因此在移动机器人行驶过程中，需要尽可能的减小移动机器人的实际位姿与规划路径之间产生的姿态偏差及角度偏差。

为保证移动机器人实时跟踪设定的路径轨迹，国内外学者提出了如预瞄控制、传统PID控制、神经网络控制等方法，这些方法大多假设移动机器人在恒定速度下，通过控制角速度对机器人的位姿进行控制，但当速度变化较大时，控制系统的跟踪性能会变差，甚至会出现振荡。

发明内容

本发明提供了一种移动机器人的路径跟踪方法，旨在改善上述问题。

本发明是这样实现的，一种移动机器人的路径跟踪方法，所述方法具体包括如下步骤：

S1、将规划路径分割成轨迹点，放入轨迹点集；

S2、基于移动机器人在上一时刻的移动速度确定当前的跟踪距离L，基于当前的跟踪距离形成下一预瞄点及下一预瞄点所在的预瞄直线；

S3、基于移动机器人当前位置与预瞄直线的垂直距离T_len及当前位姿与预瞄直线的夹角θ确定移动机器人当前的移动线速度，移动机器人当前的移动线速度即为虚拟转向轮的线速度；

S4、基于Pure Pursuit算法计算出移动机器人虚拟转向轮的转向控制角，基于当前虚拟转向轮的转向控制角及移动线速度来确定左转向轮及右转向轮的速度。

进一步的，规划路径由直线路段和曲线路段组成，若当前路段为曲线路段，则基于如下方法来进行轨迹点的采集：

S11、计算分割长度len在曲线路段对应圆中的圆心角α，

S12、基于圆心角α在曲线路段上进行轨迹点的采集，各采样轨迹点的坐标具体如下：

其中，

表示第n(1.2.3…m)个采样轨迹点p_n的位置坐标，

表示起始采样轨迹点的位置坐标。

进一步的，跟踪距离L是基于移动机器人上一时刻的移动速度v动态的调整，其公式为：L＝kv，k为正整数，若跟踪距离L小于距离阈值，则跟踪距离L取值为距离阈值。

进一步的，若当前路段为直线路段，则下一预瞄点及以下一预瞄点所在预瞄直线的确定方法具体如下：

S21、获取移动机器人当前所在位置在直线路段上的投影点；

S22、以该投影点作为起点，在行驶方向上前进跟踪距离L达到的位置点即为下一预瞄点，该直线路段即为预瞄直线。

进一步的，若当前路段为曲线路段，则下一预瞄点及以下一预瞄点所在预瞄直线的确定方法具体如下：

S23、在轨迹点集中查找距移动机器人当前位置最近的轨迹点及轨迹点对应的下标index，以该轨迹点作为直线拟合的起始轨迹点；

S24、计算跟踪距离L及分割长度len的比值，该比值的取整值与起始轨迹点的下标值之和作为直线拟合的终止轨迹点的下标值；

S25、基于起始轨迹点与终止轨迹点之间的轨迹点进行直线拟合，该拟合直线即为预瞄直线；

S26、以起始轨迹点为起点，在行驶方向上前进跟踪距离L达到的位置点即为下一预瞄点。

进一步的，移动机器人当前的移动线速度的计算公式具体如下：

其中，θ为当前位姿与预瞄直线的夹角，T_len为移动机器人当前位置与预瞄直线的垂直距离，v_target为设定的移动机器人目标速度值。

本发明提出了一种移动机器人路径跟踪方法，根据小车所需执行的任务，规划出期望路径后，将路径分割成点集，根据小车行驶的速度，确定车辆需要追踪的直线，计算与追踪直线的偏差，确定线速度后，根据车体的位姿和预瞄点的位置偏差利用Pure Pursuit算法计算出移动机器人的转角控制量，从而实现车辆对规划路径的跟踪。该方法能够在保证车辆稳定的前提下，有效地实现移动机器人预期路径的跟踪，具有响应速度快、跟踪误差小及鲁棒性强的特点，适应各种路径下的跟踪.

附图说明

图1为本发明实施例提供的移动机器人的模型，(a)为四轮模型(b)为简化后的两轮模型；

图2为本发明实施例提供的Pure Puisuit算法几何图

图3为本发明实施例提供的移动机器人的路径跟踪方法流程图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

本发明提出了一种移动机器人路径跟踪方法，根据小车所需执行的任务，规划出期望路径后，将路径分割成点集，根据小车行驶的速度，确定车辆需要追踪的直线，计算与追踪直线的偏差，确定线速度后，根据车体的位姿和预瞄点的位置偏差利用Pure Pursuit算法计算出移动机器人的转角控制量，从而实现车辆对规划路径的跟踪。该方法能够在保证车辆稳定的前提下，有效地实现移动机器人预期路径的跟踪，具有响应速度快、跟踪误差小及鲁棒性强的特点，适应各种路径下的跟踪

图3为本发明实施例提供的移动机器人的路径跟踪方法流程图，该方法具体包括如下步骤：

S1、将规划路径分割成轨迹点，放入轨迹点集；

在本发明实施例中，每条规划路径需要事先保存在数据库中，每条规划路径保存了该条路径的起点、终点、路径属性(L为直线段，C为曲线，且每条曲线均包含了半径值、圆心坐标)，移动机器人在接收任务终点信息后，首先查找出自身位置所处的路径点，根据floyd搜索算法，搜索小车当前点至任务终点的最短路径，即为规划路径。

在本发明实施例中，若所述规划路径由直线路段和曲线路段组成，若当前路段为直线路段，则基于设定的分割长度len来进行轨迹点的采集，若当前路段为曲线路段，则基于如下方法来进行轨迹点的采集：

计算分割长度len在曲线路段对应圆中的圆心角α，其中，圆心角α的计算公式具体如下：

其中，R为曲线路段所在圆的半径值，len为设定的分割长度。

基于圆心角α在曲线路段上进行轨迹点的采集，各轨迹点的坐标具体如下：

其中，

表示第n(1.2.3…m)个采样轨迹点p_n的位置坐标，

表示起始采样轨迹点的位置坐标。

S2、基于移动机器人在上一时刻的移动速度确定当前的跟踪距离L，基于当前的跟踪距离形成下一预瞄点及下一预瞄点所在的预瞄直线；

在本发明实施例中，跟踪距离L是根据机器人的实时速度动态的调整，其公式为：L＝kv，(k＝3)，若跟踪距离L<0.3m，则跟踪距离L取值为0.3m。

在本发明实施例中，若当前路段为直线路段，则下一预瞄点及以下一预瞄点所在预瞄直线的确定方法具体如下：

获取移动机器人当前所在位置在直线路段上的投影点；

以该投影点作为起点，在行驶方向上前进跟踪距离L达到的位置点即为下一预瞄点，该直线路段即为预瞄直线。

在本发明是实施例中，若当前路段为曲线路段，则下一预瞄点及以下一预瞄点所在预瞄直线的确定方法具体如下：

在轨迹点集中查找距移动机器人当前位置最近的轨迹点及轨迹点对应的下标index，以该轨迹点作为直线拟合的起始轨迹点，轨迹点的下标表示轨迹点在轨迹点集中的位置排序；

计算跟踪距离L及分割长度len的比值，该比值的取整值与起始轨迹点的下标值之和(L/len+index)作为直线拟合的终止轨迹点的下标值；

基于起始轨迹点与终止轨迹点之间的轨迹点进行直线拟合，该拟合直线即为预瞄直线，

以起始轨迹点为起点，在行驶方向上前进跟踪距离L达到的位置点即为下一预瞄点。

S3、基于移动机器人当前位置与预瞄直线的垂直距离T_len及当前位姿与预瞄直线的夹角θ确定移动机器人当前的移动速度，即为当前虚拟转向轮的线速度；

在本发明实施例中，移动机器人当前的移动速度基于如下公式进行计算：

其中，θ为当前位姿与预瞄直线的夹角，T_len为移动机器人当前位置与预瞄直线的垂直距离，v_target为设定的移动机器人目标速度值。

S4、基于Pure Pursuit算法计算出移动机器人虚拟转向轮的转向控制角，基于机器人当前虚拟转向轮的转向控制角及移动速度来确定左转向轮及右转向轮的速度。

建立移动机器人的运动学模型：移动机器人底部安装4个轮子，前面两个轮子为转向轮，后面的两个轮子为从动轮，如图1(a)所示。为了减小计算量，将车体中的4个轮子简化为分别位于轴线中点的前后两个虚拟轮子，前轮的为虚拟转向轮，后轮的是虚拟从动轮，如图1(b)所示，因此，可将计算转向轮的速度输出转化为计算虚拟转向轮的线速度及角度的输出。

结合图2进行虚拟转向控制角的计算说明，移动机器人虚拟从动轮中心点(Cx,Cy)为切点，车体纵轴为切线画一条圆弧，该圆弧经过规划路径上的预瞄点(Gx,Gy)，预瞄点与虚拟从动轮中心点的连线l_d为跟踪距离，φ为移动机器人虚拟转向轮的转角，PurePursuit路径跟踪算法就是根据几何特性，计算机器人行驶轨迹驶过预瞄点的虚拟转向轮偏角控制量；

Pure Pursuit算法根据正选定理可得如下公式：

因此圆弧的曲率半径

根据简化的移动机器人模型可得，虚拟转向轮的偏角公式为φ＝tan^-1(κL)，故而得到了Pure Puisuit算法的控制量表达式：

其中L为图1所示的前后轮间的长度，因此，通过Pure Pursit算法采用的比例增益的控制测量，计算得到的控制量实时改变虚拟转向轮的角度，提高了移动机器人在直线或弯道环境中行驶的平稳性。基于虚拟转向轮的转向角转化为左右转向轮的速度是根据机器人的几何模型所得，转化的公式为：

其中，v_r、v_l分别为右转向轮、左转向轮的线速度，Δt为移动机器人的控制时间间隔，W_l为移动机器人的左右轮的轮距宽度，v为虚拟转向轮当前的线速度，即上文中的移动机器人当前的移动速度，φ为虚拟转向轮的转向控制角。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (3)

1.一种移动机器人的路径跟踪方法，其特征在于，所述方法具体包括如下步骤：

S1、将规划路径分割成轨迹点，放入轨迹点集；

S2、基于移动机器人在上一时刻的移动速度确定当前的跟踪距离L，基于当前的跟踪距离形成下一预瞄点及下一预瞄点所在的预瞄直线；

S3、基于移动机器人当前位置与预瞄直线的垂直距离T_len及当前位姿与预瞄直线的夹角θ确定移动机器人当前的移动线速度；

S4、计算出移动机器人虚拟转向轮的转向控制角，基于当前虚拟转向轮的转向控制角及移动线速度来确定左转向轮及右转向轮的速度；

建立移动机器人的运动学模型：移动机器人底部安装4个轮子，前面两个轮子为转向轮，后面的两个轮子为从动轮；

以移动机器人虚拟从动轮中心点(C_x,C_y)为切点，车体纵轴为切线画一条圆弧，该圆弧经过规划路径上的预瞄点(G_x,G_y)，预瞄点与虚拟从动轮中心点的连线l_d为跟踪距离，φ为虚拟转向轮的转角，基于Pure Pursuit路径跟踪算法计算机器人行驶轨迹驶过预瞄点的虚拟转向轮偏角控制量；

Pure Pursuit算法根据正选定理可得如下公式：

R为曲线路段所在圆的半径值，圆弧的曲率半径

根据简化的移动机器人模型可得，虚拟转向轮的转角公式为φ＝tan^-1(κL)，故而得到了Pure Puisuit算法的控制量表达式：

L为前后轮间的长度，基于虚拟转向轮的转向角转化为左右转向轮的速度是根据机器人的几何模型所得，转化的公式为：

其中，v_r、v_l分别为右转向轮、左转向轮的线速度，Δt为移动机器人的控制时间间隔，W_l为移动机器人的左右轮的轮距宽度，v为虚拟转向轮当前的线速度，即移动机器人当前的移动速度，φ为虚拟转向轮的转角；

规划路径由直线路段和曲线路段组成，若当前路段为曲线路段，则基于如下方法来进行轨迹点的采集：

S11、计算分割长度len在曲线路段对应圆中的圆心角α，

S12、基于圆心角α在曲线路段上进行轨迹点的采集，各采样轨迹点的坐标具体如下：

其中，

表示第n个采样轨迹点p_n的位置坐标，

表示起始采样轨迹点的位置坐标；

若当前路段为曲线路段，则下一预瞄点及以下一预瞄点所在预瞄直线的确定方法具体如下：

S23、在轨迹点集中查找距移动机器人当前位置最近的轨迹点及轨迹点对应的下标index，以该轨迹点作为直线拟合的起始轨迹点；

S24、计算跟踪距离L及分割长度len的比值，该比值的取整值与起始轨迹点的下标值之和作为直线拟合的终止轨迹点的下标值；

S25、基于起始轨迹点与终止轨迹点之间的轨迹点进行直线拟合，形成拟合直线，该拟合直线即为预瞄直线；

S26、以起始轨迹点为起点，在行驶方向上前进跟踪距离L达到的位置点即为下一预瞄点；

移动机器人当前的移动线速度的计算公式具体如下：

其中，θ为当前位姿与预瞄直线的夹角，T_len为移动机器人当前位置与预瞄直线的垂直距离，v_target为设定的移动机器人目标速度值。

2.如权利要求1所述移动机器人的路径跟踪方法，其特征在于，跟踪距离L是基于移动机器人上一时刻的移动速度v动态的调整，其公式为：L＝kv，k为正整数，若跟踪距离L小于距离阈值，则跟踪距离L取值为距离阈值。

3.如权利要求1所述移动机器人的路径跟踪方法，其特征在于，若当前路段为直线路段，则下一预瞄点及以下一预瞄点所在预瞄直线的确定方法具体如下：

S21、获取移动机器人当前所在位置在直线路段上的投影点；

S22、以该投影点作为起点，在行驶方向上前进跟踪距离L达到的位置点即为下一预瞄点，该直线路段即为预瞄直线。

Images