以前に導入されたPurePursuitメソッドは、無人車両の横方向トラッキングエラーを使用してコントローラーを設計します。無人車両軌道追跡タスクでは、横方向の追跡エラーに加えて、コントローラーの安定性と有効性を向上させるのに役立つ他の情報があるかどうか。この記事で紹介したStanelyメソッドは、横方向のトラッキングエラーeye_yを組み合わせたものです。eそして方位角eθe_ {\ theta}からの偏差e私コントローラを設計します。スタンレーの横方向トラッキングエラーの計算と純粋な追跡方法には違いがあることは注目に値します。純粋な追跡は後車軸の中心を基準点として幾何学的公式を計算し、スタンレーは基準点としてのフロントアクスルの中心。
スタンレーメソッド分析とコントローラー設計
無人運転アルゴリズムを参照してください-スタンレー法を使用して、無人車両の軌道追跡を実現します
スタンレーコントローラーは次のように設計されています。
δ(k)=θe(k)+arctanλe(k)v(k)\ delta(k)= \ theta_ {e}(k)+ \ arctan \ frac {\ lambda e(k)} { v(k)} δ (k )=θE(k )+アークタンv (k )λ E (K )
その中で、θe(k)\ theta_e(k)θE(k )はkkですk時間での方位角偏差e(k)e(k)e (k )は、上の図の形状、λ\ lambdaに従って計算された横方向のトラッキングエラーです。λは調整するパラメータvvですvは無人車両の現在の速度です。
2.Pythonの例
パラメータλ= 2.0 \ lambda = 2.0λ=2 。0、一定速度V = 2.0 V = 2.0v=2 。0
"""
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.theta = theta0 # headding
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_head = np.zeros(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
dtheta = refer_head[ind]-ugv.theta
delta = dtheta+math.atan2(k*e/ugv.v, 1.0)
ugv.update(2.0, delta)
ugv.plot_duration()
上記