|
Apollo的的规划算法基于Frenet坐标系，因此道路中心线的平滑性控制着车辆是否左右频繁晃动，而高精地图的道路中心线往往不够平滑。
离散点平滑法，包括
1，FEM_POS_DEVIATION_SMOOTHING有限元位置差异
2，COS_THETA_SMOOTHING余弦）
3，QpSplineReferenceLineSmoother（三次样条插值法）
4，SpiralReferenceLineSmoother（螺旋曲线法）
本文只讲解FEM_POS_DEVIATION_SMOOTHING有限元位置差异的方法。
一，为什么需要重新生成参考线
导航路径存在的问题：1，导航路径过长；2，不平滑
导致的结果：
1，不利于主车以及障碍物寻找投影点，甚至出现多个投影点。
2，原本的导航路径不平滑，车辆无法直接进行轨迹跟踪。
解决方案：根据导航路径，生成参考线。
二，生成参考线的具体步骤与方法
1，找到匹配点以及投影点
2，以匹配点或者投影点为原点，往后取30个点，往前取150个点。对这180个点进行平滑处理，生成符合要求的参考线，作为该规划周期内的参考线，并以此参考线为Frenet坐标系。
三，参考线平滑方法：QP
方法步骤：
1，设计Cost:平滑程度，与原路径的偏离程度，路径点距离分布的均匀程度。
2，整理Cost目标函数，使其符合QP的表达形式。
3，考虑约束条件，并整理为QP的约束表达式形式。
4，运用合适的QP求解器进行求解，获取优化后的结果。
四，设计Cost：平滑程度，与原路径的偏离程度，路径点距离分布的均匀程度（长度代价）
1，平滑程度
衡量指标： 。其值越小， 路径越平滑。
2，与原路径的偏离程度
如上图所示，平滑前的点记为 ,平滑后的点记为 。
虽然路径平滑了，但与原路径距离偏差太大，不符合要求。
衡量指标：
3,路径点距离分布的均匀程度（长度代价）
衡量指标： 。其值越小，分布越均匀。
五，实例讲解
1，以3个点为例求解目标函数
如上图所示，已知变量：原路径点为： ，未知变量：优化后的路径点为： ，求优化后的路径点坐标。
2，二次规划的一般形式
3,n个点的代价函数求解（重点）：
平滑性代价：
其中：
令
则
均匀性代价（长度代价）：
其中，
令
则
与原路径的距离偏差代价：
由于 是常数，对目标函数的变化没有影响，故可以删除，以简化表达。
令，
则：
综述所述：
一般情况下，平滑的权重系数远远大于其他2项的权重系数
二次规划的标准形式为：
则
求解约束条件：
至此，参考线平滑问题转化为二次规划的标准形式，采用成熟的求解器求解即可获得 ,从而实现导航轨迹的平滑，获得符合要求的参考线轨迹点。
import osqp
import numpy as np
import matplotlib.pyplot as plt
from scipy import sparse
#计算kappa用来评价曲线
def calcKappa(x_array,y_array):
s_array = []
k_array = []
if(len(x_array) != len(y_array)):
return(s_array , k_array)
length = len(x_array)
temp_s = 0.0
s_array.append(temp_s)
for i in range(1 , length):
temp_s += np.sqrt(np.square(y_array[i] - y_array[i - 1]) + np.square(x_array[i] - x_array[i - 1]))
s_array.append(temp_s)
xds,yds,xdds,ydds = [],[],[],[]
for i in range(length):
if i == 0:
xds.append((x_array[i + 1] - x_array[i]) / (s_array[i + 1] - s_array[i]))
yds.append((y_array[i + 1] - y_array[i]) / (s_array[i + 1] - s_array[i]))
elif i == length - 1:
xds.append((x_array[i] - x_array[i-1]) / (s_array[i] - s_array[i-1]))
yds.append((y_array[i] - y_array[i-1]) / (s_array[i] - s_array[i-1]))
else:
xds.append((x_array[i+1] - x_array[i-1]) / (s_array[i+1] - s_array[i-1]))
yds.append((y_array[i+1] - y_array[i-1]) / (s_array[i+1] - s_array[i-1]))
for i in range(length):
if i == 0:
xdds.append((xds[i + 1] - xds[i]) / (s_array[i + 1] - s_array[i]))
ydds.append((yds[i + 1] - yds[i]) / (s_array[i + 1] - s_array[i]))
elif i == length - 1:
xdds.append((xds[i] - xds[i-1]) / (s_array[i] - s_array[i-1]))
ydds.append((yds[i] - yds[i-1]) / (s_array[i] - s_array[i-1]))
else:
xdds.append((xds[i+1] - xds[i-1]) / (s_array[i+1] - s_array[i-1]))
ydds.append((yds[i+1] - yds[i-1]) / (s_array[i+1] - s_array[i-1]))
for i in range(length):
k_array.append((xds[i] * ydds[i] - yds[i] * xdds[i]) / (np.sqrt(xds[i] * xds[i] + yds[i] * yds[i]) * (xds[i] * xds[i] + yds[i] * yds[i]) + 1e-6));
return(s_array,k_array)
def referenceline():
#add some data for test
x_array = [0.5,1.0,2.0,3.0,4.0,
5.0,6.0,7.0,8.0,9.0,
10.0,11.0,12.0,13.0,14.0,
15.0,16.0,17.0,18.0,19.0]
y_array = [0.1,0.3,0.2,0.4,0.3,
-0.2,-0.1,0,0.5,0,
0.1,0.3,0.2,0.4,0.3,
-0.2,-0.1,0,0.5,0]
length = len(x_array)
#weight , from config
weight_fem_pos_deviation_ = 1e10 #cost1 - x
weight_path_length = 1 #cost2 - y
weight_ref_deviation = 1 #cost3 - z
P = np.zeros((length,length))
#set P matrix,from calculateKernel
#add cost1
P[0,0] = 1 * weight_fem_pos_deviation_
P[0,1] = -2 * weight_fem_pos_deviation_
P[1,1] = 5 * weight_fem_pos_deviation_
P[length - 1 , length - 1] = 1 * weight_fem_pos_deviation_
P[length - 2 , length - 1] = -2 * weight_fem_pos_deviation_
P[length - 2 , length - 2] = 5 * weight_fem_pos_deviation_
for i in range(2 , length - 2):
P[i , i] = 6 * weight_fem_pos_deviation_
for i in range(2 , length - 1):
P[i - 1, i] = -4 * weight_fem_pos_deviation_
for i in range(2 , length):
P[i - 2, i] = 1 * weight_fem_pos_deviation_
with np.printoptions(precision=0):
print(P)
P = P / weight_fem_pos_deviation_
P = sparse.csc_matrix(P)
#set q matrix , from calculateOffset
q = np.zeros(length)
#set Bound(upper/lower bound) matrix , add constraints for x
#from CalculateAffineConstraint
#In apollo , Bound is from road boundary,
#Config limit with (0.1,0.5) , Here I set a constant 0.2
bound = 0.2
A = np.zeros((length,length))
for i in range(length):
A[i, i] = 1
A = sparse.csc_matrix(A)
lx = np.array(x_array) - bound
ux = np.array(x_array) + bound
ly = np.array(y_array) - bound
uy = np.array(y_array) + bound
#solve
prob = osqp.OSQP()
prob.setup(P,q,A,lx,ux)
res = prob.solve()
opt_x = res.x
prob.update(l=ly, u=uy)
res = prob.solve()
opt_y = res.x
#plot x - y , opt_x - opt_y , lb - ub
fig1 = plt.figure(dpi = 100 , figsize=(12, 8))
ax1_1 = fig1.add_subplot(2,1,1)
ax1_1.plot(x_array,y_array , ".--", color = "grey", label="orig x-y")
ax1_1.plot(opt_x, opt_y,".-",label = "opt x-y")
# ax1_1.plot(x_array,ly,".--r",label = "bound")
# ax1_1.plot(x_array,uy,".--r")
ax1_1.legend()
ax1_1.grid(axis="y",ls='--')
#plot kappa
ax1_2 = fig1.add_subplot(2,1,2)
s_orig,k_orig = calcKappa(x_array,y_array)
s_opt ,k_opt = calcKappa(opt_x,opt_y)
ax1_2.plot(s_orig , k_orig , ".--", color = "grey", label="orig s-kappa")
ax1_2.plot(s_opt,k_opt,".-",label="opt s-kappa")
ax1_2.legend()
ax1_2.grid(axis="y",ls='--')
plt.show()
if __name__ == '__main__':
referenceline()
平滑前后的坐标和曲率对比：
|