# 无人车系统（六）：轨迹跟踪Stanley方法

## Stanley 方法分析与控制器设计

Stanley 控制器设计如下：

$\delta(k)=\theta_{e}(k)+\arctan \frac{\lambda e(k)}{v(k)}$

## 2. python示例

"""
Stanley method
"""

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 display

plt.ion()
plt.figure(figsize=(18, 3))

class UGV_model:
def __init__(self, x0, y0, theta0, L, v0, T): # L:wheel base
self.x = x0 # X
self.y = y0 # Y
self.l = L  # wheel base
self.v = v0  # speed
self.dt = T  # decision time periodic
def update(self, vt, deltat):  # update ugv's state
dx = self.v*np.cos(self.theta)
dy = self.v*np.sin(self.theta)
dtheta = self.v*np.tan(deltat)/self.l
self.x += dx*self.dt
self.y += dy*self.dt
self.theta += dtheta*self.dt

def 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)
ugv = UGV_model(0, 1.0, 0, 2.0, 2.0, 0.1)
k = 2.0
pind = 0
ind = 0
for i in range(1000):
robot_state = np.zeros(2)
robot_state[0] = ugv.x
robot_state[1] = ugv.y
_, ind = refer_tree.query(robot_state)
if ind < pind:
ind = pind
else:
pind = ind

dist = np.linalg.norm(robot_state-refer_path[ind])
dx, dy = refer_path[ind] - robot_state
alpha = math.atan2(dy, dx)
e = np.sign(np.sin(alpha-ugv.theta))*dist
delta = dtheta+math.atan2(k*e/ugv.v, 1.0)
ugv.update(2.0, delta)
ugv.plot_duration()


12-24 1449

06-12 3888

12-11 2898

03-01 668

03-05 30

11-20 552

11-07 2万+

12-12 1178

08-04 5412

01-06 3696

12-11 710

03-08 2211

03-06

05-08

01-07

06-12

12-20 2669

06-27

12-14 809

06-03 2万+

09-26

12-10 1309

12-10 711

03-22 5万+

03-25 4万+

05-14 2万+

04-09 3万+

03-27 1万+

03-26 4万+

03-21 7762

#### 无人驾驶轨迹跟踪之纯轨迹跟踪（Pure Pursuit）

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie