CN106020197A - 一种基于势能场的机器人路径跟踪算法 - Google Patents

Abstract

本发明提供本发明的一种基于势能场的机器人路径跟踪算法，包括以下步骤：(1)获得转弯半径；(2)根据转弯半径R和当前线速度V_x(t)，估算下一个时刻线速度V_x(t+1)；(3)根据R和V_x(t+1)计算角速度ω，获得合理的路径跟踪速度(Vx，ω)，随后对获得的当前路径与预设路径两者之间进行拟合获得最优的路径跟踪速度(Vx，ω_优)，由此确定最优跟踪路径轨迹。本发明通过对机器人实时虚拟路径的分析计算得到基础速度分解量(Vx，ω)，然后进一步通过势能场中机器人路径预测来得到最优路径，能够极大的提高计算效率，降低对计算机硬件的要求，并且不论系统本身的特性如何变化，或者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上。

Description

一种基于势能场的机器人路径跟踪算法

技术领域

本发明涉及一种路径跟踪算法，具体涉及一种基于势能场的机器人路径跟踪算法。

背景技术

路径跟踪算法是为了让移动机器人运行的轨迹局限在既定路径上可接受的误差范围之内。

目前常用的移动机器人是通过磁条或色条来规划机器人的轨迹，机器人通过磁感探头或者光电传感器阵列来检测机器人实际运行轨迹和预定轨迹之间的误差。传感器所检测到的误差作为反馈输入到运动控制单元，当误差为零时，机器人即在预定的轨迹上。这是线性控制理论的一个自然应用，控制器大多采用PID的方式实现。

另一种常见的机器人路径跟踪方法是基于栅格地图计算预设路径到二维平面上所有点的曼哈顿距离。再根据机器人当前速度，以及机器人自身加速度性能，计算出机器人所有可能的轨迹。对所有可能的轨迹在二维平面上通过曼哈顿距离加和进行打分，得分最低者(即偏离预设路径最小)被选为最优轨迹。例如，如果按照某个速度(Vx，ω)，在接下去的一段时间t内所产生的轨迹是{trji}，且{trji}和预设栅格地图上的路径完全吻合，则得分为0,即所得轨迹为最优的。

磁条式或色条式路径跟踪，其算法是线性控制理论的延伸，原理通俗易懂，控制器设计简单。但是这种方式需要预先铺设和规划机器人路径，一旦机器人工作路径需要变化，则需要人工干预，重新铺设路径。这样的方式费时费力，也不灵活。这种方式也只适用于单向路径的跟踪，对于路径出现交叉等情况，磁条或色条都无法有效的解决，必须要依赖外界辅助手段来处理，如RFID，QR code等。另外一个重大缺陷是对于PID控制器的调试，由于PID控制器是对系统控制性能的线性优化，其参数仅在系统特性，如载重、驱动能力、机械损耗等，不变的情况下有效。一旦系统发生变化，其参数也随之改变，如果控制器的参数不变，则控制性能将会下降，机器人对路径的跟踪能力也会下降。

普通的基于栅格地图的路径跟踪算法比较有效的解决了上述问题。尤其是对于工作路径多变的环境，利用栅格地图预设路径较磁条色条导航有着天然的成本优势。但是这种方式计算量大且搜索范围无法控制，只能对机器人所有可能达到的速度(Vx，ω)进行搜索，对于计算机硬件的要求偏高。且一大缺点是，当机器人位置偏角和当前预设路径偏离过大时，机器人仅通过(Vx，ω)的组合，无法修正偏角，最终会造成机器人脱离预设路径。

发明内容

为解决上述技术问题，本发明提供了一种基于势能场的机器人路径跟踪算法，能够极大的提高计算效率，降低对计算机硬件的要求，并且不论系统本身的特性如何变化，或者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上。

为达到上述目的，本发明的技术方案如下：一种基于势能场的机器人路径跟踪算法，其特征在于，包括以下步骤：

(1)获得转弯半径：

1-1)创建路径信息数据库，路径信息数据库中包含有机器人将要行走的路径点坐标的集合；

1-2)从路径信息数据库中获取关于预设路径的坐标点集合{G[i]}、和当前所在的路径点索引i；

1-3)从路径信息数据库中获取三个相隔一段索引号n的路径点G[i]、G[i+n]、G[i+2n]，n＞0；

1-4)计算路径点G[i]、G[i+n]之间的距离Δd；

1-5)路径点G[i]、G[i+n]形成向量一，路径点G[i+n]、G[i+2n]形成向量二，计算向量一和向量二之间的夹角Δθ1；

1-6)计算车头朝向φ和向量G[i]、G[i+n]之间的夹角Δθ2；

1-7)计算转弯半径

当计算得的R＞R_max时，取R＝R_max；

当计算得的R＜R_min时，取R＝0；

当R_min＜R＜R_max时，

其中R_max、R_min均为算法预先设置的参数；

(2)根据转弯半径R和当前线速度V_x(t)，估算下一个时刻的线速度V_x(t+1)：

根据步骤(1)中获得的转弯半径R计算当前曲率半径允许的最大线速度其中V_max和R_max均为算法预先设置的参数；

如果则在下一个控制周期内继续加速，V_x(t+1)＝V_x(t)+ΔV；

ΔV＝a*Tc；

a—加速度；

Tc—控制周期，即本次计算和下一次计算之间的时间间隔；

否则，以当前曲率半径允许的最大速度为下一个控制周期内的线速度，即

(3)计算角速度ω：

(4)由获得的路径跟踪速度(V_x，ω)来确定跟踪路径轨迹。

本发明的一个较佳实施例中，进一步包括获得路径跟踪速度(V_x，ω)后，通过与预设路径的拟合来优化路径跟踪速度以获得最优路径跟踪速度(V_x，ω_优)，具体包括以下步骤：

3-1)创建基于路径信息的路径代价势能场图数据库；

3-2)选择参考角速度

当R＝R_max时，取

当R＝0时，其中Kr为增益系数；

当0＜R＜R_max时，

3-3)根据角速度参考值获得角速度搜索区间 其中ω_search为算法预先设置的参数；

3-4)轨迹仿真初始化：设置ω_s＝ω_min，Δω＝(ω_max-ω_min)/N，Δt，x＝0，y＝0；

其中：ω_s—角速度当前采样值；

Δω—角速度细分；

N—角速度采样数量；

Δt—时间颗粒度；

—机器人当前朝向；

x—轨迹横坐标；

y—轨迹纵坐标；

3-5)设置即时仿真速度(V_x，ω_s)

3-6)前向轨迹仿真：对即时仿真速度(V_x，ω_s)求积分 获得下一个仿真时间点上机器人所在的坐标(x，y)；其中，求积分以机器人当前位置为起点、沿车头朝向的初始方向φ、以Δt为时间颗粒度进行，由此获得前向仿真轨迹；

3-7)从数据库中获取实时路径代价势能场图；

3-8)累积轨迹的路径代价值：cost(V_x，ω_s)(t)＝cost(V_x，ω_s)(t-1)+C(x，y)；

C(x，y)为步骤3-6)中仿真获得的机器人当前所在坐标对应在实时路径代价势能场图中的路径代价值；

cost(V_x，ω_s)(t-1)为机器人在t-1，t-2……0每个时刻对应的坐标对应在实时路径代价势能场图中的路径代价值总和；

cost(V_x，ω_s)(t)为机器人当前轨迹的累积路径代价值；

3-9)判断当前轨迹的累积路径代价值cost(V_x，ω_s)：

i)如果cost(V_x，ω_s)≥0、且C(x，y)小于此前最小路径代价值，记录当前角速度为最优角速度，且进入步骤3-5)继续下一组速度(V_x，ω_s)的轨迹仿真，直至仿真角速度超过角速度搜索区间，即ω_s＝(ω_s+Δω)＞ω_max，如此循环，不断更新角速度为最优角速度ω_优；

ii)判断最优角速度ω_优的路径代价值C(x，y)_min：

如果C(x，y)_min≥0，下发速度指令(V_x、ω_优)；

否则认为无法规划合理的速度，将速度命令设为(0，0)。

本发明的一个较佳实施例中，进一步包括步骤1-3)中n取1或者2。

本发明的有益效果是:

本发明的一种基于势能场的机器人路径跟踪算法，通过对机器人实时虚拟路径的分析计算得到基础速度分解量(Vx，ω)，然后进一步通过势能场中机器人路径预测来得到最优速度分解量(V_x、ω_优)，获得与预设路径最为贴近的跟踪路径。算法能够极大的提高计算效率，降低对计算机硬件的要求，并且不论系统本身的特性如何变化，或者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上。

附图说明

为了更清楚地说明本发明实施例技术中的技术方案，下面将对实施例技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是典型的差分驱动轮机器人底盘及其即时速度分解示意图；

图2是本发明优选实施例的总流程图；

图3是本发明优选实施例获得转弯半径的流程图；

图4是本发明优选实施例获得当前线速度和参考角速度的流程图；

图5是本发明优选实施例获得当前路径与预设路径两者之间拟合程度的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

图1所示为典型的差分驱动轮机器人底盘，黑色的两个后轮为主动轮，空心白色的前轮为从动轮，点划线是底盘任意行进经过的路径。由运动学基础理论，任意一个时刻，其速度都是一个线速度Vx和角速度ω的组合(Vx，ω)，且其角速度和线速度之间存在如下换算关系：Vx＝R*ω，其中R是底盘转轴中心相对于车辆实际旋转圆心O的转弯半径，Vx的方向为即时路径的切线方向。

相反的，如果给定一段路径{G[i]}，让机器人去跟踪，则为了能够让机器人尽快到达目的地，机器人底盘无论何时都应该保持最大合理线速度。由此，在藉由对即时路径分析得出的实时转弯半径R，可以推算出合理的角速度ω＝Vx/R,再根据对前向轨迹实时的仿真在路径代价地图中的代价值，可以得到与预设路径最为贴近的一组路径跟踪速度(Vx,ω_优)。

基于此，如图2、3、4、5所示，本实施例中公开了一种基于势能场的机器人路径跟踪算法，可以得到与预设路径最为贴近的一组路径跟踪速度(Vx,ω),包括以下步骤：

(1)获得转弯半径：

1-1)创建路径信息数据库，路径信息数据库中包含有机器人将要行走的路径点坐标的集合；

1-2)从路径信息数据库中获取关于预设路径的坐标点集合{G[i]}、和当前所在的路径点索引i；

1-3)从路径信息数据库中获取三个相隔一段索引号n的路径点G[i]、G[i+n]、G[i+2n]，n＞0；n的取值根据具体应用而定，本实施例中优选n取1或者2，能够体现路径真实的曲率。

1-4)计算路径点G[i]、G[i+n]之间的距离Δd；

1-5)路径点G[i]、G[i+n]形成向量一，路径点G[i+n]、G[i+2n]形成向量二，计算向量一和向量二之间的夹角Δθ1；

1-6)计算车头朝向φ和向量G[i]、G[i+n]之间的夹角Δθ2；

1-7)计算转弯半径

当计算得的R＞R_max时，取R＝R_max；

当计算得的R＜R_min时，取R＝0；

当R_min＜R＜R_max时，

其中R_max、R_min均为算法预先设置的参数；

(2)根据转弯半径R和当前线速度V_x(t)，估算下一个时刻的线速度V_x(t+1)：

根据步骤(1)中获得的转弯半径R计算当前曲率半径允许的最大线速度其中V_max和R_max均为算法预先设置的参数；

如果则在下一个控制周期内继续加速，V_x(t+1)＝V_x(t)+ΔV；

ΔV＝a*Tc；

a—加速度；

Tc—控制周期，即本次计算和下一次计算之间的时间间隔；

否则，以当前曲率半径允许的最大速度为下一个控制周期内的线速度，即

(3)计算角速度ω：

(4)获得合理的路径跟踪速度(V_x，ω)。

获得路径跟踪速度(V_x，ω)后，通过与预设路径的拟合来优化路径跟踪速度以获得最优路径跟踪速度(V_x，ω_优)，具体包括以下步骤：

3-1)创建基于路径信息的路径代价势能场图数据库；

3-2)选择参考角速度

当R＝R_max时，取

当R＝0时，其中Kr为增益系数；

当0＜R＜R_max时，

3-3)根据角速度参考值获得角速度搜索区间 其中ω_search为算法预先设置的参数；

3-4)轨迹仿真初始化：设置ω_s＝ω_min，Δω＝(ω_max-ω_min)/N，Δt，x＝0，y＝0；

其中：ω_s—角速度当前采样值；

Δω—角速度细分；

N—角速度采样数量；

Δt—时间颗粒度；

—机器人当前朝向；

x—轨迹横坐标；

y—轨迹纵坐标；

3-5)设置即时仿真速度(V_x，ω_s)

3-6)前向轨迹仿真：对即时仿真速度(V_x，ω_s)求积分 获得下一个仿真时间点上机器人所在的坐标(x，y)；其中，求积分以机器人当前位置为起点、沿车头朝向的初始方向φ、以Δt为时间颗粒度进行，由此获得前向仿真轨迹；

3-7)从数据库中获取实时路径代价势能场图；

3-8)累积轨迹的路径代价值：cost(V_x，ω_s)(t)＝cost(V_x，ω_s)(t-1)+C(x，y)；

C(x，y)为步骤3-6)中仿真获得的机器人当前所在坐标对应在实时路径代价势能场图中的路径代价值；

cost(V_x，ω_s)(t-1)为机器人在t-1，t-2……0每个时刻对应的坐标对应在实时路径代价势能场图中的路径代价值总和；

cost(V_x，ω_s)(t)为机器人当前轨迹的累积路径代价值；

3-9)判断当前轨迹的累积路径代价值cost(V_x，ω_s)：

i)如果cost(V_x，ω_s)≥0、且C(x，y)小于此前最小路径代价值，记录当前角速度为最优角速度，且进入步骤3-5)继续下一组速度(V_x，ω_s)的轨迹仿真，直至仿真角速度超过角速度搜索区间，即ω_s＝(ω_s+Δω)＞ω_max，如此循环，不断更新角速度为最优角速度ω_优；

ii)判断最优角速度ω_优的路径代价值C(x，y)_min：

如果C(x，y)_min≥0，下发速度指令(V_x、ω_优)；

否则认为无法规划合理的速度，将速度命令设为(0，0)。

本发明通过对获得的当前路径与预设路径两者之间进行拟合，使得不论系统本身的特性如何变化，或者机器人相对预设路径的初始偏角多大，算法本身都会保证机器人收敛到预设路径上，同时能够极大的提高计算效率，降低对计算机硬件的要求。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (3)

1.一种基于势能场的机器人路径跟踪算法，其特征在于，包括以下步骤：

(1)获得转弯半径：

1-1)创建路径信息数据库，路径信息数据库中包含有机器人将要行走的路径点坐标的集合；

1-2)从路径信息数据库中获取关于预设路径的坐标点集合{G[i]}、和当前所在的路径点索引i；

1-3)从路径信息数据库中获取三个相隔一段索引号n的路径点G[i]、G[i+n]、G[i+2n]，n＞0；

1-4)计算路径点G[i]、G[i+n]之间的距离Δd；

1-5)路径点G[i]、G[i+n]形成向量一，路径点G[i+n]、G[i+2n]形成向量二，计算向量一和向量二之间的夹角Δθ1；

1-6)计算车头朝向φ和向量G[i]、G[i+n]之间的夹角Δθ2；

1-7)计算转弯半径

当计算得的R＞R_max时，取R＝R_max；

当计算得的R＜R_min时，取R＝0；

当R_min＜R＜R_max时，

其中R_max、R_min均为算法预先设置的参数；

(2)根据转弯半径R和当前线速度V_x(t)，估算下一个时刻的线速度V_x(t+1)：

根据步骤(1)中获得的转弯半径R计算当前曲率半径允许的最大线速度其中V_max和R_max均为算法预先设置的参数；

如果则在下一个控制周期内继续加速，V_x(t+1)＝V_x(t)+ΔV；

ΔV＝a*Tc；

a—加速度；

Tc—控制周期，即本次计算和下一次计算之间的时间间隔；

否则，以当前曲率半径允许的最大速度为下一个控制周期内的线速度，即

(3)计算角速度ω：

(4)由获得的路径跟踪速度(V_x，ω)来确定跟踪路径轨迹。

2.根据权利要求1所述的一种基于势能场的机器人路径跟踪算法，其特征在于：获得路径跟踪速度(V_x，ω)后，通过与预设路径的拟合来优化路径跟踪速度以获得最优路径跟踪速度(V_x，ω_优)，具体包括以下步骤：

3-1)创建基于路径信息的路径代价势能场图数据库；

3-2)选择参考角速度

当R＝R_max时，取

当R＝0时，其中Kr为增益系数；

当0＜R＜R_max时，

3-3)根据角速度参考值获得角速度搜索区间 其中ω_search为算法预先设置的参数；

3-4)轨迹仿真初始化：设置ω_s＝ω_min，Δω＝(ω_max-ω_min)/N，Δt，x＝0，y＝0；

其中：ω_s—角速度当前采样值；

Δω—角速度细分；

N—角速度采样数量；

Δt—时间颗粒度；

—机器人当前朝向；

x—轨迹横坐标；

y—轨迹纵坐标；

3-5)设置即时仿真速度

3-6)前向轨迹仿真：对即时仿真速度求积分 获得下一个仿真时间点上机器人所在的坐标(x，y)；其中，求积分以机器人当前位置为起点、沿车头朝向的初始方向φ、以Δt为时间颗粒度进行，由此获得前向仿真轨迹；

3-7)从数据库中获取实时路径代价势能场图；

3-8)累积轨迹的路径代价值：cost(V_x，ω_s)(t)＝cost(V_x，ω_s)(t-1)+C(x，y)；

C(x，y)为步骤3-6)中仿真获得的机器人当前所在坐标对应在实时路径代价势能场图中的路径代价值；

cost(V_x，ω_s)(t-1)为机器人在t-1，t-2……0每个时刻对应的坐标对应在实时路径代价势能场图中的路径代价值总和；

cost(V_x，ω_s)(t)为机器人当前轨迹的累积路径代价值；

3-9)判断当前轨迹的累积路径代价值cost(V_x，ω_s)：

i)如果cost(V_x，ω_s)≥0、且C(x，y)小于此前最小路径代价值，记录当前角速度为最优角速度，且进入步骤3-5)继续下一组速度的轨迹仿真，直至仿真角速度超过角速度搜索区间，即ω_s＝(ω_s+Δω)＞ω_max，如此循环，不断更新角速度为最优角速度ω_优；

ii)判断最优角速度ω_优的路径代价值C(x，y)_min：

如果C(x，y)_min≥0，下发速度指令(V_x、ω_优)；

否则认为无法规划合理的速度，将速度命令设为(0，0)。

3.根据权利要求1或2所述的一种基于势能场的机器人路径跟踪算法，其特征在于：步骤1-3)中n取1或者2。