参考https://github.com/MPC-Berkeley
计算轨迹点误差方法
离线分析路径跟踪误差
1.离散采样参考路径
# Compute cumulative distance along an array of waypoints, aka s.
def compute_path_s(xy_arr):
cdists = []
for i in range(xy_arr.shape[0]):
if i == 0:
cdists.append(0.0) # s = 0
else:
dx = xy_arr[i,0] - xy_arr[i-1,0]
dy = xy_arr[i,1] - xy_arr[i-1,1]
d = math.sqrt( dx**2 + dy**2 ) + cdists[-1]
cdists.append(d) # s = s_prev + dist(z[i], z[i-1])
return np.array(cdists)
s_pr = compute_path_s(xy_pr)
# START INTERPOLATION
# Discretize the path finely to help with analysis.
path_disc = 0.1 # m
s_interp = np.arange(0.0, s_pr[-1], path_disc)
2.找出两组航路点之间的最近点和相应的距离/横向误差。（1）最近点索引是xy_实际中每个索引的xy_参考中最近点的索引。（2）错误是xy_-ref中最近点与xy_-actual中匹配索引之间的无符号距离。
它高估了误差，因为它包括横向误差和一小部分纵向误差。（3）我们应该使用lat_错误。它将横向误差计算为有符号距离。
3.横向误差，点到最近点索引以及索引下一点组成直线的距离。（点到直线的距离）
def compute_lateral_error(xy_query, xy_path_prev, xy_path_next):
'''
Assumptions: path is locally linear. In this region, it is represented
as a vector starting at xy_path_prev and ending at xy_path_next.
Let v1 = xy_path_next-xy_path_prev
v2 = v2 = xy_query - xy_path_prev
We can use the cross product v1 x v2 to find the area of the parallelogram spanned by these vectors.
Dividing by the norm of v1 (i.e. the "base") gives the height, aka lateral error.
The sign of the cross product gives the sign of the lateral error, as well.
# Source: https://stackoverflow.com/questions/39840030/distance-between-point-and-a-line-from-two-points
'''
v1 = xy_path_next - xy_path_prev
v2 = xy_query - xy_path_prev
lat_error = np.cross(v1,v2)/np.linalg.norm(v1)
return lat_error
画轨迹路，画中画局部放大
import rospy
import numpy as np
import matplotlib.pyplot as plt
from gps_utils import ref_gps_traj as r
from genesis_path_follower.msg import state_est
from genesis_path_follower.msg import mpc_path
from plot_utils.getVehicleFrame import plotVehicle
from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
class PlotGPSTrajectory():
'''
A class to plot the global GPS trajectory and the vehicle's path tracking behavior.
Also includes the vehicle geometry, thanks to code written by Nitin Kapania, found at:
https://github.com/nkapania/Wolverine/blob/master/utils/sim_lib.py
'''
def __init__(self):
# Load Global Trajectory
if rospy.has_param('mat_waypoints'):
mat_name = rospy.get_param('mat_waypoints')
else:
raise ValueError("No Matfile of waypoints were provided!")
if not (rospy.has_param('lat0') and rospy.has_param('lon0') and rospy.has_param('yaw0')):
raise ValueError('Invalid rosparam global origin provided!')
lat0 = rospy.get_param('lat0')
lon0 = rospy.get_param('lon0')
yaw0 = rospy.get_param('yaw0')
grt = r.GPSRefTrajectory(mat_filename=mat_name, LAT0=lat0, LON0=lon0, YAW0=yaw0) # only 1 should be valid.
# Set up Data
self.x_global_traj = grt.get_Xs()
self.y_global_traj = grt.get_Ys()
self.x_ref_traj = self.x_global_traj[0]; self.y_ref_traj = self.y_global_traj[0]
self.x_mpc_traj = self.x_global_traj[0]; self.y_mpc_traj = self.y_global_traj[0]
self.x_vehicle = self.x_global_traj[0]; self.y_vehicle = self.y_global_traj[0]; self.psi_vehicle = yaw0
self.df_vehicle = 0.0 # rad (steering angle)
# This should be include in a launch file/yaml for the future.
self.a = 1.5213 # m (CoG to front axle)
self.b = 1.4987 # m (CoG to rear axle)
self.vW = 1.89 # m (vehicle width)
self.rW = 0.3 # m (wheel radius, estimate based on Shelley's value of 0.34 m).
# Set up Plot: includes full ("global") trajectory, target trajectory, MPC prediction trajectory, and vehicle.
self.f = plt.figure()
self.ax = plt.gca()
plt.ion()
# Trajectory
self.l1, = self.ax.plot(self.x_global_traj, self.y_global_traj, 'k')
self.l2, = self.ax.plot(self.x_ref_traj, self.y_ref_traj, 'rx')
self.l3, = self.ax.plot(self.x_mpc_traj, self.y_mpc_traj, 'g*')
# Vehicle coordinates
FrontBody, RearBody, FrontAxle, RearAxle, RightFrontTire, RightRearTire, LeftFrontTire, LeftRearTire = \
plotVehicle(self.x_vehicle, self.y_vehicle, self.psi_vehicle, self.df_vehicle, self.a, self.b, self.vW, self.rW)
self.vl0, = self.ax.plot(self.x_vehicle, self.y_vehicle, 'bo', MarkerSize= 8)
self.vl1, = self.ax.plot(FrontBody[0,:], FrontBody[1,:], 'gray', LineWidth = 2.5)
self.vl2, = self.ax.plot(RearBody[0,:], RearBody[1,:], 'gray', LineWidth = 2.5)
self.vl3, = self.ax.plot(FrontAxle[0,:], FrontAxle[1,:], 'gray', LineWidth = 2.5)
self.vl4, = self.ax.plot(RearAxle[0,:], RearAxle[1,:], 'gray', LineWidth = 2.5)
self.vl5, = self.ax.plot(RightFrontTire[0,:], RightFrontTire[1,:], 'r', LineWidth = 3)
self.vl6, = self.ax.plot(RightRearTire[0,:], RightRearTire[1,:], 'k', LineWidth = 3)
self.vl7, = self.ax.plot(LeftFrontTire[0,:], LeftFrontTire[1,:], 'r', LineWidth = 3)
self.vl8, = self.ax.plot(LeftRearTire[0,:], LeftRearTire[1,:], 'k', LineWidth = 3)
plt.xlabel('X (m)'); plt.ylabel('Y (m)')
plt.axis('equal')
# Zoomed Inset Plot: Based off tutorial/code here: http://akuederle.com/matplotlib-zoomed-up-inset
self.ax_zoom = zoomed_inset_axes(self.ax, 5, loc=2) # axis, zoom_factor, location (2 = upper left)
self.window = 25 # m
self.zl1, = self.ax_zoom.plot(self.x_global_traj, self.y_global_traj, 'k')
self.zl2, = self.ax_zoom.plot(self.x_ref_traj, self.y_ref_traj, 'rx')
self.zl3, = self.ax_zoom.plot(self.x_mpc_traj, self.y_mpc_traj, 'g*')
self.zvl0,= self.ax_zoom.plot(self.x_vehicle, self.y_vehicle, 'bo', MarkerSize= 8)
self.zvl1,= self.ax_zoom.plot(FrontBody[0,:], FrontBody[1,:], 'gray', LineWidth = 2.5)
self.zvl2,= self.ax_zoom.plot(RearBody[0,:], RearBody[1,:], 'gray', LineWidth = 2.5)
self.zvl3,= self.ax_zoom.plot(FrontAxle[0,:], FrontAxle[1,:], 'gray', LineWidth = 2.5)
self.zvl4,= self.ax_zoom.plot(RearAxle[0,:], RearAxle[1,:], 'gray', LineWidth = 2.5)
self.zvl5,= self.ax_zoom.plot(RightFrontTire[0,:], RightFrontTire[1,:], 'r', LineWidth = 3)
self.zvl6,= self.ax_zoom.plot(RightRearTire[0,:], RightRearTire[1,:], 'k', LineWidth = 3)
self.zvl7,= self.ax_zoom.plot(LeftFrontTire[0,:], LeftFrontTire[1,:], 'r', LineWidth = 3)
self.zvl8,= self.ax_zoom.plot(LeftRearTire[0,:], LeftRearTire[1,:], 'k', LineWidth = 3)
self.ax_zoom.set_xlim(self.x_vehicle - self.window, self.x_vehicle + self.window)
self.ax_zoom.set_ylim(self.y_vehicle - self.window, self.y_vehicle + self.window)
plt.yticks(visible=False)
plt.xticks(visible=False)
rospy.init_node('vehicle_plotter', anonymous=True)
rospy.Subscriber('state_est', state_est, self.update_state, queue_size=1)
rospy.Subscriber('mpc_path', mpc_path, self.update_mpc_trajectory, queue_size=1)
self.loop()
def loop(self):
# Main Plotting Loop. Updates plot with info from subscriber callbacks.
r = rospy.Rate(10)
while not rospy.is_shutdown():
# Update MPC OL + Reference Trajectories.
self.l2.set_xdata(self.x_ref_traj); self.l2.set_ydata(self.y_ref_traj)
self.l3.set_xdata(self.x_mpc_traj); self.l3.set_ydata(self.y_mpc_traj)
self.zl2.set_xdata(self.x_ref_traj); self.zl2.set_ydata(self.y_ref_traj)
self.zl3.set_xdata(self.x_mpc_traj); self.zl3.set_ydata(self.y_mpc_traj)
# Update Vehicle Plot.
FrontBody, RearBody, FrontAxle, RearAxle, RightFrontTire, RightRearTire, LeftFrontTire, LeftRearTire = \
plotVehicle(self.x_vehicle, self.y_vehicle, self.psi_vehicle, self.df_vehicle, self.a, self.b, self.vW, self.rW)
self.vl0.set_xdata(self.x_vehicle)
self.vl0.set_ydata(self.y_vehicle)
self.vl1.set_xdata(FrontBody[0,:])
self.vl1.set_ydata(FrontBody[1,:])
self.vl2.set_xdata(RearBody[0,:])
self.vl2.set_ydata(RearBody[1,:])
self.vl3.set_xdata(FrontAxle[0,:])
self.vl3.set_ydata(FrontAxle[1,:])
self.vl4.set_xdata(RearAxle[0,:])
self.vl4.set_ydata(RearAxle[1,:])
self.vl5.set_xdata(RightFrontTire[0,:])
self.vl5.set_ydata(RightFrontTire[1,:])
self.vl6.set_xdata(RightRearTire[0,:])
self.vl6.set_ydata(RightRearTire[1,:])
self.vl7.set_xdata(LeftFrontTire[0,:])
self.vl7.set_ydata(LeftFrontTire[1,:])
self.vl8.set_xdata(LeftRearTire[0,:])
self.vl8.set_ydata(LeftRearTire[1,:])
self.zvl0.set_xdata(self.x_vehicle)
self.zvl0.set_ydata(self.y_vehicle)
self.zvl1.set_xdata(FrontBody[0,:])
self.zvl1.set_ydata(FrontBody[1,:])
self.zvl2.set_xdata(RearBody[0,:])
self.zvl2.set_ydata(RearBody[1,:])
self.zvl3.set_xdata(FrontAxle[0,:])
self.zvl3.set_ydata(FrontAxle[1,:])
self.zvl4.set_xdata(RearAxle[0,:])
self.zvl4.set_ydata(RearAxle[1,:])
self.zvl5.set_xdata(RightFrontTire[0,:])
self.zvl5.set_ydata(RightFrontTire[1,:])
self.zvl6.set_xdata(RightRearTire[0,:])
self.zvl6.set_ydata(RightRearTire[1,:])
self.zvl7.set_xdata(LeftFrontTire[0,:])
self.zvl7.set_ydata(LeftFrontTire[1,:])
self.zvl8.set_xdata(LeftRearTire[0,:])
self.zvl8.set_ydata(LeftRearTire[1,:])
self.ax_zoom.set_xlim(self.x_vehicle - self.window, self.x_vehicle + self.window)
self.ax_zoom.set_ylim(self.y_vehicle - self.window, self.y_vehicle + self.window)
self.f.canvas.draw()
plt.pause(0.001)
r.sleep()
def update_state(self, msg):
# Update vehicle's position.
self.x_vehicle = msg.x
self.y_vehicle = msg.y
self.psi_vehicle = msg.psi
self.df_vehicle = msg.df
def update_mpc_trajectory(self, msg):
# Update the MPC planned (open-loop) trajectory.
self.x_mpc_traj = msg.xs
self.y_mpc_traj = msg.ys
# Update the reference for the MPC module.
self.x_ref_traj = msg.xr
self.y_ref_traj = msg.yr
if __name__=='__main__':
p = PlotGPSTrajectory()
模型/预测时序
参考论文 http://www.me.berkeley.edu/~frborrel/pdfpub/IV_KinematicMPC_jason.pdf
预测时域内参考点
1.根据参考速度，使用距离信息插值
2.根据参考路径中的时间信息，使用时间插值
def get_waypoints(self, X_init, Y_init, yaw_init, v_target=None):
XY_traj = self.trajectory[:,4:6] # full XY global trajectory
xy_query = np.array([[X_init,Y_init]]) # the vehicle's current position (XY)
# find the index of the closest point on the trajectory to the initial vehicle pose
diff_dists = np.sum( (XY_traj-xy_query)**2, axis=1)
closest_traj_ind = np.argmin(diff_dists)
# TODO: this function does not handle well the case where the car is far from the recorded path! Ill-defined behavior/speed.
# May use np.min(diff_dists) and add appropriate logic to handle this edge case.
if v_target is not None:
return self.__waypoints_using_vtarget(closest_traj_ind, v_target, yaw_init) #v_ref given, use distance information for interpolation
else:
return self.__waypoints_using_time(closest_traj_ind, yaw_init) #no v_ref, use time information for interpolation
''' Helper functions: you shouldn't need to call these! '''
def __waypoints_using_vtarget(self, closest_traj_ind, v_target, yaw_init):
start_dist = self.trajectory[closest_traj_ind,6] # s0, cumulative dist corresponding to closest point
dists_to_fit = [x*self.traj_dt*v_target + start_dist for x in range(0,self.traj_horizon+1)] # edit bounds
# NOTE: np.interp returns the first value x[0] if t < t[0] and the last value x[-1] if t > t[-1].
self.x_interp = np.interp(dists_to_fit, self.trajectory[:,6], self.trajectory[:,4]) # x_des = f_interp(d_des, d_actual, x_actual)
self.y_interp = np.interp(dists_to_fit, self.trajectory[:,6], self.trajectory[:,5]) # y_des = f_interp(d_des, d_actual, y_actual)
psi_ref = np.interp(dists_to_fit, self.trajectory[:,6], self.trajectory[:,3]) # psi_des = f_interp(d_des, d_actual, psi_actual)
self.psi_interp = self.__fix_heading_wraparound(psi_ref, yaw_init)
# Send a stop command if the end of the trajectory is within the horizon of the waypoints.
# Alternatively, could use start_dist as well: if start_dist + some delta_s > end_dist, then stop.
stop_cmd = False
if self.x_interp[-1] == self.trajectory[-1,4] and self.y_interp[-1] == self.trajectory[-1,5]:
stop_cmd = True
return self.x_interp, self.y_interp, self.psi_interp, stop_cmd
def __waypoints_using_time(self, closest_traj_ind, yaw_init):
start_tm = self.trajectory[closest_traj_ind,0] # t0, time of recorded trajectory corresponding to closest point
times_to_fit = [h*self.traj_dt + start_tm for h in range(0,self.traj_horizon+1)]
# NOTE: np.interp returns the first value x[0] if t < t[0] and the last value x[-1] if t > t[-1].
self.x_interp = np.interp(times_to_fit, self.trajectory[:,0], self.trajectory[:,4]) # x_des = f_interp(t_des, t_actual, x_actual)
self.y_interp = np.interp(times_to_fit, self.trajectory[:,0], self.trajectory[:,5]) # y_des = f_interp(t_des, t_actual, y_actual)
psi_ref = np.interp(times_to_fit, self.trajectory[:,0], self.trajectory[:,3]) # psi_des = f_interp(t_des, t_actual, psi_actual)
self.psi_interp = self.__fix_heading_wraparound(psi_ref, yaw_init)
# Send a stop command if the end of the trajectory is within the horizon of the waypoints.
# Alternatively, could use start_dist as well: if start_dist + some delta_s > end_dist, then stop.
stop_cmd = False
if self.x_interp[-1] == self.trajectory[-1,4] and self.y_interp[-1] == self.trajectory[-1,5]:
stop_cmd = True
return self.x_interp, self.y_interp, self.psi_interp, stop_cmd
记录参考路径
时间（接收到的时间）经度 纬度 航向 速度（东向速度/北向速度）加速度（速度的微分求得）车轮转角
模型参考目标函数/约束
非线性求解julia
目标函数，位置误差（x,y）航向误差 速度误差 输入 输入平滑（加速度） 加加速度
kmpc.update_cost(9.0, 9.0, 10.0, 0.0, 100.0, 1000.0, 0.0, 0.0) # x,y,psi,v,da,ddf,a,df
# Cost function.
@NLobjective(mdl, Min, sum{ C_x*(x[i] - x_r[i])^2 + C_y*(y[i] - y_r[i])^2 + C_psi*(psi[i] - psi_r[i])^2 , i=2:(N+1)} +
C_v *sum{ (v[i] - v_target)^2, i = 2:N} +
C_acc*sum{(acc[i])^2, i=1:N} +
C_df*sum{(d_f[i])^2, i=1:N} +
C_dacc*sum{(acc[i+1] - acc[i])^2, i=1:(N-1)} +
C_ddf*sum{(d_f[i+1] - d_f[i])^2, i=1:(N-1)} +
sum{ sl_df[i] + sl_acc[i], i=1:N}
)
参数
控制周期0.05，模型离散时间0.20，预测步长 8
网友评论