机器人纯跟踪算法 Base Pure Pursuit

机器人纯跟踪算法 Base Pure Pursuit

  • 前言
  • 原理介绍
  • 算法
    • 如何找到匹配点
      • 路径
    • 前瞻距离
    • 匹配点
    • 圆与直线交点求解
    • 只取路径间的解
    • 匹配点的判断
  • 机器人控制
  • 效果:
    • 引用

前言

纯跟踪算法是一种路径跟踪算法,他通过改变角速度指令将机器人当前位置移动到前面的某个路径点,并不断的跟随路径前进。适用于很多机器人运动学模型,包括阿克曼和全向轮。
本文简单分析求解运动学之后的纯跟踪算法

原理介绍


如果把机器人当做驴,上图形象的解释了纯跟踪算法的原理,纯跟踪算法需要一直更新“胡萝卜”的位置,让机器人朝着路径的方向前进。

算法

纯跟踪算法的步骤是

  1. 定位当前机器人的位置。
  2. 找到路径中匹配的目标点。
  3. 改变机器人移动的角度。

如何找到匹配点

为了解决这个问题,我们需要先定义路径,我们是在什么样的路径里找到匹配点?

路径

描述一个路径一般是用一些离散的点,如:`(x,y) 。一般而言机器人的移动轨迹是连续的,我们也应该使在路径中找到的匹配点连续, 在算法里我们将这些离散的路径点相连接,变成连续的路径。

前瞻距离

和上图中人拿着杆子吸引机器人前进一样,我们的算法中也需要一个一定长度的杆子,在此我们定义“杆子”为前瞻距离。
前瞻距离的选取应该与机器人移动的速度相关,太大或太小都不行。如下图就是太长和太短的效果。
机器人纯跟踪算法 Base Pure Pursuit_第1张图片

匹配点

当我们定义完路径和前瞻距离,接下来就应该解决匹配点。这个问题本质就是直线与圆的交点。
具体而言为路径中连续的两点连接成一直线,而前瞻距离绕机器人当前位置一圈为圆。求这两几何的交点。
机器人纯跟踪算法 Base Pure Pursuit_第2张图片

圆与直线交点求解

机器人纯跟踪算法 Base Pure Pursuit_第3张图片
这里可以用公式法或者向量法求解交点,公式法可能会遇到斜率为0的情况,需要分类讨论,在这里我介绍向量法,只需要考虑一种情况。
已知,路径点分别为(x1,y1),(x2,y2) 当前机器人位置为(currentX,currentY),前瞻距离为l
为了简化计算,我们将当前所有的路径点减去机器人的坐标,算出来的交点再加回来即可。
则路径两点变为(x1-currentX,y1-currentY)(x2-currentXmy2-currentY)
圆的方程为: x 2 + y 2 = l 2 x^2+y^2 = l^2 x2+y2=l2
直线的方程为: ( y 1 − y 2 ) x − ( x 1 − x 2 ) y + x 1 y 2 − x 2 y 1 = 0 (y_1-y_2)x-(x_1-x_2)y+x_1y_2-x_2y_1=0 (y1y2)x(x1x2)y+x1y2x2y1=0 (化为一般式不用考虑斜率为0的情况)
直线到点的距离公式为:
直线到点的距离公式
我们可求得此路径两点所成的直线到圆心(0,0)的距离d,
d>l则没有交点
d=l则有且仅有一个交点
d>l则有两个交点
下面用向量法求当有交点的时候如何得到交点。
机器人纯跟踪算法 Base Pure Pursuit_第4张图片
记AB为当前两路径点,其方向向量为 a ⃗ = ( x a , y a ) \vec{a} = (x_a,y_a) a =(xa,ya)
则与AB垂直的OC 方向向量为 b ⃗ = ( − y a , x a ) \vec{b}=(-y_a,x_a) b =(ya,xa)
可求得 O C ⃗ = d ∗ b ⃗ \vec{OC} = d* \vec{b} OC =db
机器人纯跟踪算法 Base Pure Pursuit_第5张图片
∣ C E ⃗ ∣ = l 2 − d 2 \left| \vec{CE} \right| = \sqrt[ ]{l^2 - d^2} CE =l2d2
CE方向向量为: A B ⃗ ∣ A B ⃗ ∣ \frac{\vec{AB}}{ \left| \vec{AB} \right| } AB AB
则: C E ⃗ = l 2 − d 2 ∣ A B ⃗ ∣ A B ⃗ \vec{CE} = \frac{\sqrt[ ]{l^2 - d^2} }{ \left| \vec{AB} \right| } \vec{AB} CE =AB l2d2 AB
O E ⃗ = O C ⃗ + C E ⃗ \vec{OE} = \vec{OC} + \vec{CE} OE =OC +CE
同理可得 O F ⃗ \vec{OF} OF
得到的向量加上机器人的位置坐标即可得交点。

只取路径间的解

机器人纯跟踪算法 Base Pure Pursuit_第6张图片
这种情况虽然直线与圆有两个交点,即有两个匹配点,但在算法中,我们只取在路径间线段的点。

  • 情况一:无交点
    机器人纯跟踪算法 Base Pure Pursuit_第7张图片

  • 情况二:无交点
    机器人纯跟踪算法 Base Pure Pursuit_第8张图片

  • 情况三:一交点
    机器人纯跟踪算法 Base Pure Pursuit_第9张图片

  • 情况四:两交点
    机器人纯跟踪算法 Base Pure Pursuit_第10张图片

匹配点的判断

机器人纯跟踪算法 Base Pure Pursuit_第11张图片
这种情况下,两条线段我们都能寻得解,那哪个点跟合适?
很明显,我们需要参考机器人当前的位置,如果 机器人到路径点1的距离小于匹配点到路径点1的距离,则我们跟应该去寻找下一个点。
所有在我们的遍历循环中,找到匹配点之后,应该再加入当前位置和匹配点、路径点的判断,通过这个判断才能知道是否是真的匹配点。

机器人控制

不同的机器人运动学在这一步可能会有所不同,我在此举的是舵轮,需要输入速度方向和底盘角度,在此我输入的是速度方向和底盘角度
x c u r r r e n t − x m a t c h {x_{currrent}} -x_{match} xcurrrentxmatch,大小为定值1。

效果:

黄色为前瞻距离,红色小球为匹配点

舵轮纯跟踪 ROS2 Control Gazebo 仿真

引用

[1]: base pure pursuit
[2]: mathworks pure puresuit controller
[3]: autoware
[4]: pure pursuit papaer

你可能感兴趣的:(算法,机器学习,人工智能)