pure pursuit:无人车轨迹追踪算法
对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位置信息即可,也可以包含姿态信息,但是不需要与时间相关,这些路径点被称为全局路径点(Global Waypoint),路径(Path)和轨迹(Trajectory)的区别就在于,轨迹还包含了时间信息,轨迹点也是一种路径点,它在路径点的基础上加入了时间约束,通产我们将这些轨迹点称为局部路径点(Local Waypoints)。
如何让我们的无人车追踪这个轨迹?目前的主流方法分为两类:基于几何追踪的方法和基于模型预测的方法,在本节我们重点介绍一种广泛使用的基于几何追踪的方法——纯追踪法(Pure Pursuit)。
自行车模型和阿克曼转向几何
在论述纯追踪算法之前,首先我们回顾一下自行车模型,下图是一个几何学自行车模型:
自行车模型实际上是对阿克曼转向几何的一个简化,我们知道,自行车模型将4轮车辆简化成2轮的模型,并且假定车龄只在平面上行驶,采用自行车模型的一大好处就在于它简化了前轮转向角与后轴将遵循的曲率之间的几何关系,其关系如下式所示:
其中 δ表示前轮的转角,L为轴距(Wheelbase),R则为在给定的转向角下后轴遵循着的圆的半径。这个公式能够在较低速度的场景下对车辆运动做估计。
纯追踪算法
从自行车模型出发,纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角 , 使车辆可以沿着一条经过目标路点(goal point)的圆弧行驶,如下图所示:
图中 是我们下一个要追踪的路点,它位于我们已经规划好的全局路径上,现在需要控制车辆是的车辆的后轴经过该路点,表示车辆当前位置(即后轴位置)到目标路点的距离, 表示目前车身姿态和目标路点的夹角,那么更具正弦定理我们可以推导出如下转换式:
上式也可以表示为:
其中 κ是计算出来的圆弧的曲率,那么前轮的转角 δ的表达式为:
结合以上两式,我们可以得出纯追踪算法控制量的的最终表达式:
这里我们把时间考虑进来,在知道t时刻车身和目标路点的夹角 α(t)α(t) 和距离目标路点的前视距离 ldld 的情况下,由于车辆轴距 LL 固定,我们可以利用上式估计出应该作出的前轮转角 δδ ,为了更好的理解纯追踪控制器的原理,我们定义一个新的量:elel —— 车辆当前姿态和目标路点在横向上的误差,由此可得夹角正弦:
圆弧的弧度就可重写为:
考虑到本质是横向上的CTE,由上式可知纯追踪控制器其实是一个横向转角的P控制器,其P系数为 ,这个P控制器受到参数 ld(即前视距离)的影响很大,如何调整前视距离变成纯追踪算法的关键,通常来说,ld 被认为是车速的函数,在不同的车速下需要选择不同的前视距离。
一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数,即 那么前轮的转角公式就变成了:
那么纯追踪控制器的调整就变成了调整系数k,通常来说,会使用最大,最小前视距离来约束前视距离,越大的前视距离意味着轨迹的追踪越平滑,小的前视距离会使得追踪更加精确(当然也会带来控制的震荡),下面我们使用Python实现一个简单的纯追踪控制器。
基于Python的纯追踪算法实践
在这个实践中,我们纯追踪控制控制转向角度,使用一个简单的P控制器控制速度,首先我们定义参数数值如下:
import numpy as np
import math
import matplotlib.pyplot as plt
k = 0.1 # 前视距离系数
Lfc = 2.0 # 前视距离
Kp = 1.0 # 速度P控制器系数
dt = 0.1 # 时间间隔,单位:s
L = 2.9 # 车辆轴距,单位:m
在这里我们将最小前视距离设置为2,前视距离关于车速的系数k设置为0.1 ,速度P控制器的比例系数Kp设置为1.0,时间间隔为0.1 秒,车的轴距我们定为2.9米。
定义车辆状态类,在简单的自行车模型中,我们只考虑车辆的当前位置 (x,y) ,车辆的偏航角度yaw以及车辆的速度v,为了在软件上模拟,我们定义车辆的状态更新函数来模拟真实车辆的状态更新:
class VehicleState:
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a, delta):
state.x = state.x + state.v * math.cos(state.yaw) * dt
state.y = state.y + state.v * math.sin(state.yaw) * dt
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
state.v = state.v + a * dt
return state
在这个实践中,我们纵向控制使用一个简单的P控制器,横向控制(即转角控制)我们使用纯追踪控制器,这两个控制器定义如下:
def PControl(target, current):
a = Kp * (target - current)
return a
def pure_pursuit_control(state, cx, cy, pind):
ind = calc_target_index(state, cx, cy)
if pind >= ind:
ind = pind
if ind < len(cx):
tx = cx[ind]
ty = cy[ind]
else:
tx = cx[-1]
ty = cy[-1]
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
return delta, ind
定义函数用于搜索最临近的路点:
def calc_target_index(state, cx, cy):
# 搜索最临近的路点
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
L = 0.0
Lf = k * state.v + Lfc
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
return ind
主函数:
def main():
# 设置目标路点
cx = np.arange(0, 50, 1)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
target_speed = 10.0 / 3.6 # [m/s]
T = 100.0 # 最大模拟时间
# 设置车辆的出事状态
state = VehicleState(x=-0.0, y=-3.0, yaw=0.0, v=0.0)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while T >= time and lastIndex > target_ind:
ai = PControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai, di)
time = time + dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)
if __name__ == '__main__':
main()
运行效果:
我们看这个运行图,首先,红点表示我们实现规划好的路点,蓝线则表示我们的车辆实际运行的轨迹,前面的绿点表示我们的当前前视距离,在这段代码中,我们设置了最小前视距离为2m,同学们还可以进一步去实验,比如说将前视距离设置得更大些,那么我们的纯追踪控制器就会表现得更加“平滑”,更加平滑的结果就是在某些急剧的转角处会存在转向不足的情况。
小结
以纯追踪控制器为代表的几何路径跟踪器很容易理解和实现。 本文介绍的几何方法达到了基本的路径跟踪性能,但是当存在显著的速度改变时会遇到瓶颈,纯追踪方法使用前视距离考虑路径信息,这种方法在低速情况下几乎不受路径形状的影响。但是,选择最佳的前视距离的方法并不明确。将前视距离表示为速度的函数是一种常见的方法,但是,前视距离也可能是路径曲率的函数,甚至可能和纵向速度以外的CTE有关。所以纯追踪控制器的前视距离调整应该额外注意,很短的前视距离会造成车辆控制的不稳定甚至震荡,为了确保车辆稳定而设置较长的前视距离又会出现车辆在大转角处转向不足的问题。
————————————————
版权声明:本文为CSDN博主「AdamShan」的原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/adamshan/article/details/80555174
pure pursuit:无人车轨迹追踪算法相关推荐
- 无人驾驶汽车系统入门(十八)——使用pure pursuit实现无人车轨迹追踪
无人驾驶汽车系统入门(十八)--使用pure pursuit实现无人车轨迹追踪 对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位 ...
- 无人驾驶五 使用pure pursuit实现无人车轨迹追踪(python)
https://blog.csdn.net/adamshan/article/details/80555174 # coding=utf-8import numpy as np import math ...
- 无人车路径规划算法—(3)基于搜索的路径规划算法 (BFS/DFS/Dijkstra)
1.BFS(广度优先搜索) && DFS(深度优先搜索) 广度优先遍历图的方式为,一次性访问当前顶点的所有未访问状态相邻顶点,并依次对每个相邻顶点执行同样处理.因为要依次对每个相邻顶点 ...
- 无人车路径规划算法---(4)基于搜索的路径规划算法 II(贪心/Astar)
上篇博客中介绍了一些基本的图搜索算法,其中也重点介绍了基于势场来实现的Dijkstra算法.本篇博客将介绍关于Heuristic Function的图搜索算法 开源了一个结合Dijkstra,Gree ...
- 无人车系统(五):轨迹跟踪Pure Pursuit方法
今天介绍一种基于几何追踪的无人车轨迹跟踪方法--Pure Pursuit(纯跟踪)方法. 1. 阿克曼转向几何模型 在无人车系统(一):运动学模型及其线性化一文中,里面介绍无人车的运动学模型为阿克曼转 ...
- 自动驾驶笔记-轨迹跟踪之①纯跟踪算法(Pure Pursuit)
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 前言 一.阿克曼转向模型 1.1 模型理解 1.2 模型表达 二.纯跟踪算法(Pure Pursuit) 2.1 算法理解 ...
- 无人车采用纯跟踪算法跟随离线路径(ROS,C++实现)第一部分
一 前言 最近是由于实验室需求,做了一个简单的无人车轨迹跟踪算法,最主要要完成的功能还是希望无人车能够跟随离线轨迹动起来,因为自己也是在学习阶段,第一次接触这一块的知识,所以就写了这个博客,希望自己能 ...
- 核心案例|中原工学院无人机无人车协同规划与智能控制验证平台
项目名称:无人机无人车协同规划与智能控制验证平台 场 地:室内≥6m*6m*3m 关 键 词:多无人机.无人车协同规划.四旋翼无人机室内避障算法验证 核心案例|中原工学院无人机无人车协同 ...
- 无人车系统(十一):轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
最新文章
- 输入重定向,输出重定向,管道相关内容及实现方法
- 前端工程师笔试题(欢迎评论留言)
- day12装饰器进阶
- portainer 启动mysql_Docker管理工具Portainer
- PL/SQL Developer结合oracle精简客户端配置说明
- restTemplate重定向问题 cookie问题
- 编程大神进阶,Python技巧小贴士
- 随想录(搭建自己嵌入式项目的编译系统)
- python数组(矩阵)乘法(点乘、叉乘)
- 计算机思维中核心要素,思维导图核心三要素
- 难得一见的数据库事务异常 Deadlock found when trying to get lock解决办法dao.DeadlockLoserDataAccessException怎么办
- Python学习记录(一)
- 腾讯小程序php,小程序播放腾讯视频 - MyClassPHP-Colin主页 - OSCHINA - 中文开源技术交流社区...
- returned a response status of 405 Method Not Allowed
- long型转String(*)
- eclipse启动很慢调优
- 计算机专业生物信息方向博士,我是这样拿到宾大生物信息学全奖博士offer的
- 学习VM上运行dnf(整理)
- 纠结了五年,华为要动智能电视了? 1
- 2022年全国职业院校技能大赛试题3(中职组)
热门文章
- Image Segmentation with U-Net(吴恩达课程)
- html魔塔编辑器,migration.html
- ipv6dns服务器修改,ipv6服务器dns怎么设置
- 树莓派之外网动态域名访问
- 安卓dj专业打碟机软件_学DJ打碟 数码操作快捷键
- how2heap的fastbin_dup_consolidate(包含sleepyholder)
- [数据压缩]实验四 DPCM编码
- Django实现Linux服务端快速清理缓存
- 基础复习——Button——按钮——触发事件——监听器(单独公共)——点击事件与长按事件——禁用恢复按钮...
- html更改表单按钮文字,HTML进阶应用技巧(十)用好表单的按钮