Pure Pursuit纯跟踪算法Python/Matlab算法实现

本文的python源代码来自：
https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit/pure_pursuit.py

纯跟踪算法的原理，详见https://blog.csdn.net/gophae/article/details/100012763

我们对纯跟踪算法进行一次仿真，python 我已经改过，如下：

import numpy as np
import math
import matplotlib.pyplot as plt
#定义常数
k = 0.1  # look forward gain
Lfc = 1.0  # look-ahead distance
Kp = 1.0  # speed propotional gain
dt = 0.1  # [s]
L = 2.9  # [m] wheel base of vehicleshow_animation = Trueclass 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) * dtstate.v = state.v + a * dtreturn statedef PIDControl(target, current):#PID控制，定速巡航a = Kp * (target - current)return adef 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.yawif state.v < 0:  # 如果是倒车的话，就要反过来alpha = math.pi - alphaLf = k * state.v + Lfcdelta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)#核心计算公式return delta, inddef calc_target_index(state, cx, cy):
# 找到与车辆当前位置最近点的序号# search nearest point indexdx = [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# search look ahead target point indexwhile 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 main():#  target course ，随机出来一条sin函数曲线cx = np.arange(0, 50, 0.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  # max simulation time# initial statestate = VehicleState(x=-0.0, y=-3.0, yaw=0.0, v=0.0)lastIndex = len(cx) - 1time = 0.0x = [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 = PIDControl(target_speed, state.v)di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)state = update(state, ai, di)time = time + dtx.append(state.x)y.append(state.y)yaw.append(state.yaw)v.append(state.v)t.append(time)if show_animation:plt.cla()plt.plot(cx, cy, ".r", label="course")plt.plot(x, y, "-b", label="trajectory")plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")plt.axis("equal")plt.grid(True)plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])plt.pause(0.001)plt.show()if __name__ == '__main__':print("Pure pursuit path tracking simulation start")main()

将本段代码改为MATLAB版本：

k = 0.1;  % look forward gain
Lfc = 1.0;  % look-ahead distance
Kp = 1.0 ; % speed propotional gain
dt = 0.1  ;% [s]
L = 2.9  ;% [m] wheel base of vehicle
cx = 0:0.1:50;
cx = cx';
for i = 1:length(cx)
cy(i) = sin(cx(i)/5)*cx(i)/2;
endi = 1;
target_speed = 10/3.6;
T = 80;
lastIndex = length(cx);
x = 0; y = -3; yaw = 0; v = 0;
time = 0;Lf = k * v + Lfc;figure
while T > time target_ind= calc_target_index(x,y,cx,cy,Lf)ai = PIDcontrol(target_speed, v,Kp);di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf);[x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L)time = time + dt;
%     pause(0.1)plot(cx,cy,'b',x,y,'r-*')drawnowhold on
end
% plot(cx,cy,x,y,'*')
% hold onfunction [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L)x = x + v * cos(yaw) * dt;y = y + v * sin(yaw) * dt;yaw = yaw + v / L * tan(delta) * dt;v = v + a * dt;
endfunction [a] = PIDcontrol(target_v, current_v, Kp)
a = Kp * (target_v - current_v);
endfunction [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Lf)tx = cx(ind);ty = cy(ind);alpha = atan((ty-y)/(tx-x))-yaw;Lf = k * v + Lfc;delta = atan(2*L * sin(alpha)/Lf)  ;
endfunction [ind] = calc_target_index(x,y, cx,cy,Lf)
N =  length(cx);
Distance = zeros(N,1);
for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);
end
[~, location]= min(Distance);
ind = location;
% LL = 0;
%     while Lf > LL && (ind + 1) < length(cx)
%         dx = cx(ind + 1 )- cx(ind);
%         dy = cx(ind + 1) - cx(ind);
%         LL = LL + sqrt(dx * 2 + dy * 2);
%         ind  = ind + 1;
%     end
%     ind = ind + 10
end

仿真结果如下：

Pure Pursuit纯跟踪算法Python/Matlab算法实现相关推荐

LQR轨迹跟踪算法Python/Matlab算法实现_代码(2)
本文根据LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)使用代码实现,进行仿真: clc clear allKp = 1.0 ; dt =0.1 ;% [s] L = ...
LQR轨迹跟踪算法Python/Matlab算法实现2
这里对上一篇LQR轨迹跟踪算法Python/Matlab算法实现进行勘误: clc clear allKp = 1.0 ; dt = 0.1 ;% [s] L = 2.9 ;% [m] wheel b ...
LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导
对于文章 LQR轨迹跟踪算法Python/Matlab算法实现中的LQR推导的问题,我简单写了一下手稿,不高兴做成公式了:
Stanley轨迹跟踪算法Python/Matlab算法实现
本文针对Python/Matlab实现Stanley进行介绍: Stanley 的数学几何原理来自: https://blog.csdn.net/gophae/article/details/1000 ...
pure pursuit纯跟踪
Pure Pursuit是一种几何追踪方法,速度越小,performance越好; :汽车前轮转角 L:前后轮轴距(车长) R:转弯半径将车辆模型简化为自行车模型(这里默认左轮和右轮的旋转是一致的) ...
LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)
本文提供了另一种LQR状态空间矩阵的推导方式:
[运动控制算法]Pure Pursuit纯路径跟踪算法
纯路径跟踪 Pure Pursuit是一种用于路径跟踪的控制算法.它通过计算角速度控制机器人从当前位置移到机器人前方的某个预瞄点.假定线速度是恒定的,当然可以随意更改机器人的线速度.该算法会根据机器人 ...
pure pursuit：无人车轨迹追踪算法
对于无人车辆来说,在规划好路径以后(这个路径我们通常称为全局路径),全局路径由一系列路径点构成,这些路径点只要包含空间位置信息即可,也可以包含姿态信息,但是不需要与时间相关,这些路径点被称为全局路径点 ...
【老生谈算法】matlab算法离合器——离合器
基于Matlab算法离合器设计与试验 1.原文下载: 本算法原文如下,有需要的朋友可以点击进行下载序号原文(点击下载) 本项目原文 [老生谈算法]基于Matlab算法离合器设计与试验.doc 2. ...

Pure Pursuit纯跟踪算法Python/Matlab算法实现

Pure Pursuit纯跟踪算法Python/Matlab算法实现相关推荐

最新文章

热门文章