无人车系统(五):轨迹跟踪Pure Pursuit方法
今天介绍一种基于几何追踪的无人车轨迹跟踪方法——Pure Pursuit(纯跟踪)方法。
1. 阿克曼转向几何模型
在无人车系统(一):运动学模型及其线性化一文中,里面介绍无人车的运动学模型为阿克曼转向几何模型,并最终可简化为如下图所示的单车模型。
精确描述单车模型中几何关系的核心公式如下:
δ = a r c t a n ( L R ) (1) \delta=arctan(\frac{L}{R}) \tag{1} δ=arctan(RL)(1)
其中, δ \delta δ为前轮转角, L L L为轴距(wheel base), R R R为给定转向角运动时,无人车形成的轨迹圆的半径。
2. Pure pursuit控制器
我们利用下图【引自无人驾驶汽车系统入门(十八)——使用pure pursuit实现无人车轨迹追踪】来介绍Pure pursuit控制器的设计。
Pure pursuit方法的依据是使如上图所示的单车模型以合适的前轮转向( δ \delta δ)运动,并恰好使无人车后轴中心经过当前的路点。 这样一来,我们就可以根据当前的路点以及单车几何模型计算当前的期望前轮转向角( δ \delta δ)。
根据上图,我们有以下几何关系:
sin α = l d 2 R ⇒ R = l d 2 sin α (2) \sin \alpha=\frac{l_d}{2R} \Rightarrow R=\frac{l_d}{2\sin \alpha} \tag{2} sinα=2Rld⇒R=2sinαld(2)
将式(2)代入式(1)可得:
δ = a r c t a n ( 2 L sin α l d ) (3) \delta=arctan(\frac{2L \sin \alpha}{l_d}) \tag{3} δ=arctan(ld2Lsinα)(3)
其中, α \alpha α为路点与车后轴连成的向量的角度与车航向角的差值,当路点在车的左边时, α > 0 \alpha>0 α>0,反之则 α < 0 \alpha<0 α<0; l d l_d ld为车后轴离路点的距离,又被称为前视距离。
为了让式(3)在形式上更加统一,我们可以利用无人车相对当前路点的横向偏差 e y e_y ey与前视距离 l d l_d ld表示(如果不能立马了悟,还请看无人车系统(四):轨迹跟踪PID控制第一张图):
sin α = e y l d (4) \sin \alpha = \frac{e_y}{l_d} \tag{4} sinα=ldey(4)
将式(4)代入式(3)得:
δ ( k ) = a r c t a n ( 2 L e y ( k ) l d 2 ( k ) ) (5) \delta(k) = arctan(\frac{2Le_y(k)}{l_d^2(k)}) \tag{5} δ(k)=arctan(ld2(k)2Ley(k))(5)
在pure pursuit方法中,前视距离表示成无人车纵向线速度的形式,即 l d = λ v x + c l_d=\lambda v_x+c ld=λvx+c, c c c为一常值,最终的控制器如下:
δ ( k ) = a r c t a n ( 2 L e y ( k ) ( k v x ( k ) + c ) 2 ) (6) \delta(k) = arctan(\frac{2Le_y(k)}{(kv_x(k)+c)^2}) \tag{6} δ(k)=arctan((kvx(k)+c)22Ley(k))(6)
分析式(6),利用小角度近似,我们有:
δ ( k ) ≈ 2 L l d 2 e y (7) \delta(k)\approx \frac{2L}{l_d^2} e_y \tag{7} δ(k)≈ld22Ley(7)
我们把 2 L l d 2 \frac{2L}{l_d^2} ld22L看作比例控制器的参数, e y e_y ey作为系统误差,那么这就相当于一个以横向跟踪误差CTE作为系统误差的比例控制器。
与无人车系统(四):轨迹跟踪PID控制不同的是,在pure pursuit 方法中, l d l_d ld是关于速度的线性函数 l d = λ v X l_d=\lambda v_X ld=λvX,其中参数 λ \lambda λ与 c c c是需要调整。
无人车模型的两个控制输入:无人车前轮转向角( δ \delta δ),有时候也用方向盘转角来表示;无人车线速度 v v v。前者是影响无人车横向运动的主要控制输入,因此,设计的轨迹跟踪控制器,都是为了得到合适的 δ \delta δ。
3. python 示例
"""
Pure Pursuit
"""import numpy as np
import math
import copy
import matplotlib
import matplotlib.pyplot as plt
%matplotlib inline# set up matplotlib
is_ipython = 'inline' in matplotlib.get_backend()
if is_ipython:from IPython import displayplt.ion()
plt.figure(figsize=(18, 3))class UGV_model:def __init__(self, x0, y0, theta0, L, v0, T): # L:wheel baseself.x = x0 # Xself.y = y0 # Yself.theta = theta0 # headdingself.l = L # wheel baseself.v = v0 # speedself.dt = T # decision time periodicdef update(self, vt, deltat): # update ugv's statedx = self.v*np.cos(self.theta)dy = self.v*np.sin(self.theta)dtheta = self.v*np.tan(deltat)/self.lself.x += dx*self.dtself.y += dy*self.dtself.theta += dtheta*self.dtdef plot_duration(self):plt.scatter(self.x, self.y, color='r') plt.axis([self.x-9, self.x+9, -3, 3])
# plt.axis([self.x-9, self.x+9, -10, 10])if is_ipython:display.clear_output(wait=True)display.display(plt.gcf()) from scipy.spatial import KDTree# set reference trajectory
refer_path = np.zeros((1000, 2))
refer_path[:,0] = np.linspace(0, 1000, 1000)
# refer_path[:,1] = 5*np.sin(refer_path[:,0]/5.0)refer_tree = KDTree(refer_path)plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=5.0)
L = 2.0
v = 2.0
k = 1.0 # 前视距离ld = k*v+2.0
ugv = UGV_model(0, 1.0, 0, L, v, 0.1)
ind = 0
ld = k*v+2.0
for i in range(1000):robot_state = np.zeros(2)robot_state[0] = ugv.xrobot_state[1] = ugv.yfor i in range(ind, len(refer_path)):dist = np.linalg.norm(robot_state-refer_path[i])if dist >= ld:ind = ibreakdist = np.linalg.norm(robot_state-refer_path[ind])dx, dy = refer_path[ind] - robot_statealpha = math.atan2(dy, dx)delta = math.atan2(2.0*L*np.sin(alpha-ugv.theta)/ld, 1) # pure pursuit controllerugv.update(2.0, delta)ugv.plot_duration()
我随便选了一个参数: λ = 1.0 , c = 2.0 \lambda=1.0, c=2.0 λ=1.0,c=2.0
跟踪直线效果
跟踪直线效果
4. 当前视距离 l d = 1.0 ∗ v x + 2.0 l_d=1.0*v_x+2.0 ld=1.0∗vx+2.0时,PID的效果
为了进一步说有pure pursuit方法其实等价于无人车系统(四):轨迹跟踪PID控制中介绍的纯比例控制器。我们采用与pure pursuit同样的前视距离,做一组实验。实验结果如下:
纯比例P, k p = 1.0 k_p=1.0 kp=1.0
PD控制, k p = 1.0 , k d = 20.0 k_p=1.0,k_d=20.0 kp=1.0,kd=20.0
- PID控制, k p = 1.0 , k i = 0.001 , k d = 20.0 k_p=1.0,k_i=0.001, k_d=20.0 kp=1.0,ki=0.001,kd=20.0
对比纯P控制的效果与pure pursuit非常接近,加入微分与积分后,控制效果反而变差。(也可能是我PID参数没有调好,这套参数与无人车系统(四):轨迹跟踪PID控制)中的一样。
总结
本篇主要对用于无车轨迹跟踪控制的pure pursuit方法进行介绍。我们从原理出发,并给出pure pursuit的控制器计算公式,最后给出python示例。结果显示pure pursuit的跟踪效果还不错。文中也说明了,pure pursuit与P控制形式一样,只不过在无人车系统(四):轨迹跟踪PID控制中,我们只是选择离无人车最近的路点作为当前的目标点,并没有根据前视距离选择适当的路点。
无人车系统(五):轨迹跟踪Pure Pursuit方法相关推荐
- 无人车系统(十一):轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
- 无人车系统(二):横向动力学模型
运动学模型只是在几何意义上描述了无人车的运动.实际情况中,无人车的运动受外界的因素(如:路面.坡度.空气动力)影响较大.在高速动行时,这些外界扰动变得更加明显.为了构建更加精准的无人车模型,有必要考虑 ...
- 无人车系统(一):运动学模型及其线性化
相对无人机与机械臂来说,无人车系统的运动学模型非常简洁.尽管简洁,无人车的运动学模型也是非线性的.应用于具体控制算法时,有必要对原始运动学模型进行变形或线性化.本篇主要介绍无人车的运动学模型,并对原始 ...
- 第6章 - 多无人车系统的协同控制 --> 多无人车协同控制
第6章 - 多无人车系统的协同控制 --> 多无人车系统建模 回到目录 第6章 - 多无人车系统的协同控制 --> 实验验证 文章目录 6.4 多无人车协同控制 6.4.1 一致性控制 定 ...
- 第6章 - 多无人车系统的协同控制 --> 无人车运动原理
第5章 - 二阶多智能体系统的协同控制 --> 连续时间系统编队控制[程序代码] 回到目录 第6章 - 多无人车系统的协同控制 --> 无人车模型分析 文章目录 6.1 无人车运动原理 6 ...
- 第6章 - 多无人车系统的协同控制 --> 无人车模型分析
第6章 - 多无人车系统的协同控制 --> 无人车运动原理 回到目录 第6章 - 多无人车系统的协同控制 --> 多无人车系统建模 文章目录 6.2 无人车模型分析 6.2.1 动力学模型 ...
- 第6章 - 多无人车系统的协同控制 --> 多无人车系统建模
第6章 - 多无人车系统的协同控制 --> 无人车模型分析 回到目录 第6章 - 多无人车系统的协同控制 --> 多无人车协同控制 文章目录 6.3 多无人车系统建模 6.3.1 模型转换 ...
- 第一篇综述-无人车系统架构(三)
第一篇综述-无人车简介(一) 第一篇综述-无人车简介(二) 目录 硬件架构 软件架构 前面简述了无人车的定义及分级.发展历程.社会影响及所面临的技术挑战.本章将讲述无人车的系统架构,包括硬件架构和软件 ...
- 腾讯自动驾驶新动作!与现代合作开发无人车系统
乾明 发自 凹非寺 量子位 报道 | 公众号 QbitAI 关于自动驾驶,腾讯又有新动作. 根据韩国<每日经济新闻>(Maeil Business Newspaper)报道,腾讯控股与现 ...
最新文章
- JAX-RS 方式的 RESTful Web Service 开发
- 【OpenCV归纳】4 关于HighGUI
- 如何创建一个Eureka Client?
- 堆、栈及静态数据区详解 转
- map集合的常用方法和遍历
- JAVA设计模式之【外观模式】
- 延迟和带宽:时延简介、最后一英里、核心网带宽、网络边缘
- java reduce 分组_使用JAVA8 stream中三个参数的reduce方法对List进行分组统计
- python自动化办公教程-Python自动化办公之操作Excel文件
- CTO@全体成员,未经允许和评审不让用反射,Java反射到底慢在哪?
- matlab中class,[转载]Matlab中的类(Class)
- 《持续集成实践指南》第1章 DevOps实践简介
- 电脑配置之CPU性能参数[一]
- 牛津博士讲大数据和量化金融
- 2005年商业科技盘点:最被低估10大技术+最被高估10大技术
- 基于springboot的支持http接口+dubbo接口的TestNG自动化测试框架
- JS通过 身份证号码 获取 出生年月日 和 性别
- WAS以及weblogic日志说明
- 设计模式---观察者模式(Observer)和委托事件模型(DEM)
- Electron理论知识 1-GN语法及使用介绍