前言

对于AGV小车，利用路径规划算法在规划好路径以后，全局路径由一系列路径点构成，这些路径点只要包含空间位置信息即可，也可以包含姿态信息。在上一篇文章中，我们在webots中创建了舵轮小车，在本文中，我们将利用纯追踪算法(Pure Pursuit)来对舵轮小车进行路径点追踪。

一、自行车模型

上图为几何学自行车模型，假设我们的自行车模型仅在平面上行驶。我们将四轮模型简化为两轮模型，我们可以计算出前轮转向角度δ与轴距L之间的关系：

R为在给定的转向角δ下后轴遵循着的圆的半径。

二、纯追踪算法

纯跟踪算法以车后轴为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶，如上图所示。上图中(g_x,g_y )是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆的后轴经过该路点，表示车辆当前位置（即后轴位置）到目标路点的距离，表示目前车身姿态和目标路点的夹角，那么更具正弦定理我们可以推导出如下转换式：

将上式变形可得：

K为圆弧的曲率，前轮的转向角δ：

将以上两个式子结合，并且把时间考虑进来可得：

t时刻车身和目标路点的夹角 α(t)和距离目标路点的前视距离ld的情况下，由于车辆轴距L固定，我们可以利用上式估计出应该作出的前轮转角δ，为了更好的理解纯追踪控制器的原理，我们定义el 为车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：

此时，圆弧弧度为：

上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为l_d^2，这个P控制器受到参数 ld（即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说，ld 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。

三、Webots中对舵轮使用纯追踪算法

在Webots中新建一个控制器，代码如下：
代码如下（示例）：

import time
import numpy as np
from controller import *
import mathrobot = Robot()timestep = int(robot.getBasicTimeStep())lf_dir = robot.getMotor('lf_dir_motor')
lf_val = robot.getMotor('lf_run_motor')lb_dir = robot.getMotor('lb_dir_motor')
lb_val = robot.getMotor('lb_run_motor')rf_dir = robot.getMotor('rf_dir_motor')
rf_val = robot.getMotor('rf_run_motor')rb_dir = robot.getMotor('rb_dir_motor')
rb_val = robot.getMotor('rb_run_motor')gps = robot.getGPS("gps")
gps.enable(timestep)pen = robot.getPen("pen")k = 0.00001
Lfc = 0.1
Kp = 1.0
dt = 0.032
L = 0.5# tr_ = 180/3.1415926def rotation(angle):lf_dir.setPosition(angle)lb_dir.setPosition(angle)rf_dir.setPosition(angle)rb_dir.setPosition(angle)def translation():lf_val.setPosition(float('inf'))lf_val.setVelocity(leftSpeed)lb_val.setPosition(float('inf'))lb_val.setVelocity(leftSpeed)rf_val.setPosition(float('inf'))rf_val.setVelocity(rightSpeed)rb_val.setPosition(float('inf'))rb_val.setVelocity(rightSpeed)class VehicleState:def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):self.x = xself.y = yself.yaw = yawself.v = vdef update(state, a, delta):state.x = state.x + state.v * math.cos(state.yaw) * dtstate.y = state.y + state.v * math.sin(state.yaw) * dtstate.yaw = state.yaw + state.v / L * math.tan(delta) * dt   # 航向角=后轴瞬时速度*tan前轮转角/轴距state.v = state.v + a * dtreturn statedef PControl(target, current):a = Kp * (target - current)return adef 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.0Lf = 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 += 1return inddef pure_pursuit_control(state, cx, cy, pind):ind = calc_target_index(state, cx, cy)if pind >= ind:ind = pindif ind < len(cx):tx = cx[ind]ty = cy[ind]else:  tx = cx[-1]ty = cy[-1]ind = len(cx) - 1alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw  # 计算当前点到目标点的方向角差if state.v < 0:  alpha = math.pi - alphaLf = k * state.v + Lfc  delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)  # 计算转向角return delta, indcx = np.arange(0, 50, 1)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]target_speed = 2.0 / 3.6T = 1000.0state = VehicleState(x=-0.0, y=-0.0, yaw=0.0, v=2.0)
lastIndex = len(cx) - 1
time = 0.0
# print(state.x)
# print("Hello World")x1= [state.x]
y1 = [state.y]
yaw1 = [state.yaw]
v1 = [state.v]
t1 = [0.0]target_ind = calc_target_index(state, cx, cy)while robot.step(timestep) != -1:ai = PControl(target_speed, state.v)di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)print("di:", di)state = update(state, ai, di)print("yaw:", state.yaw)time = time + dtx1.append(state.x)y1.append(state.y)yaw1.append(state.yaw)v1.append(state.v)t1.append(time)values = gps.getValues()e_node = [-100000000000000, 60000000000000]x = values[0]y = values[1]if x > e_node[0] and y < e_node[1]:leftSpeed = 20.0rightSpeed = 20.0else:leftSpeed = 0.0rightSpeed = 0.0rotation(di)translation()pen.write(True)print("MY_ROBOT is at position: %g %g %g" % (values[0], values[1], values[2]))

更多内容请关注微信公众号:深度学习与路径规划

参考文献：https://blog.csdn.net/AdamShan/article/details/80555174

Webots舵轮使用纯追踪算法相关推荐

pure pursuit：无人车轨迹追踪算法
对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位置信息即可,也可以包含姿态信息,但是不需要与时间相关,这些路径点被称为全局路径点 ...
自动驾驶路径跟踪控制——纯追踪控制
文章目录 1.自行车模型(汽车二自由度模型) 注意点 Point1 Point2 2.纯追踪控制注意点 Point1 Point2 Point3 3.相关代码参考文献声明全局路径由一系 ...
自动驾驶笔记-轨迹跟踪之①纯跟踪算法（Pure Pursuit）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.阿克曼转向模型 1.1 模型理解 1.2 模型表达二.纯跟踪算法(Pure Pursuit) 2.1 算法理解 ...
六边形溯源追踪算法编程思想与代码
六边形算法简介基于浓度梯度的六边形化学源追踪算法是Russell在2003年提出了.该追踪算法的主要思路如下: While(循环停止条件){ If( Then 在n点处逆时针旋转60°,前进固定步长 ...
Pure Pursuit纯跟踪算法Python/Matlab算法实现
本文的python源代码来自: https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit ...
基于Matlab的三维胖射线追踪算法
基于Matlab的三维胖射线追踪算法(1) 三维CT更有利于对构造的精细识别,三维CT本质上与二维CT无较大区别,但计算量增加,占用电脑内存大,计算时间更长. 文章目录基于Matlab的三维胖射线追 ...
基于Matlab的跨孔CT胖射线追踪算法（五）
基于Matlab的跨孔CT胖射线追踪算法(五) CT技术是一种无损的工程物探检测技术,因其方法简单.分辨率高.理论上更可靠.结果更直观,被广泛的应用于各种工程.胖射线追踪是CT技术的一种正演算法,本文 ...
基于Matlab的跨孔CT胖射线追踪算法（四）
基于Matlab的跨孔CT胖射线追踪算法(四) CT技术是一种无损的工程物探检测技术,因其方法简单.分辨率高.理论上更可靠.结果更直观,被广泛的应用于各种工程.胖射线追踪是CT技术的一种正演算法,本文 ...
基于Matlab的跨孔CT胖射线追踪算法（三）
基于Matlab的跨孔CT胖射线追踪算法(三) CT技术是一种无损的工程物探检测技术,因其方法简单.分辨率高.理论上更可靠.结果更直观,被广泛的应用于各种工程.胖射线追踪是CT技术的一种正演算法,本文 ...
基于Matlab的跨孔CT胖射线追踪算法（二）
基于Matlab的跨孔CT胖射线追踪算法(二) CT技术是一种无损的工程物探检测技术,因其方法简单.分辨率高.理论上更可靠.结果更直观,被广泛的应用于各种工程.胖射线追踪是CT技术的一种正演算法,本文 ...

Webots舵轮使用纯追踪算法

文章目录

前言

一、自行车模型

二、纯追踪算法

三、Webots中对舵轮使用纯追踪算法

Webots舵轮使用纯追踪算法相关推荐

最新文章

热门文章