Apollo 参考线平滑方法Fem Pos Deviation Smoother

配置文件:

平滑器的配置文件
位置：

modules/planning/conf/planning.conf

–smoother_config_filename=/apollo/modules/planning/conf/discrete_points_smoother_config.pb.txt

内容：

max_constraint_interval : 0.25
longitudinal_boundary_bound : 2.0
max_lateral_boundary_bound : 0.5
min_lateral_boundary_bound : 0.1
curb_shift : 0.2
lateral_buffer : 0.2

discrete_points {
smoothing_method: FEM_POS_DEVIATION_SMOOTHING
fem_pos_deviation_smoothing {
weight_fem_pos_deviation: 1e10
weight_ref_deviation: 1.0
weight_path_length: 1.0
apply_curvature_constraint: false
max_iter: 500
time_limit: 0.0
verbose: false
scaled_termination: true
warm_start: true}
}
问题：

参考discretized_points_smoothing/fem_pos_deviation_smoother.h中的注释

@brief:
This class solve an optimization problem:
Y
|
| P(x1, y1) P(x2, y2)
| P(x0, y0) … P(x(k-1), y(k-1))
|P(start)
|
|________________________________________________________ X
Given an initial set of points from 0 to k-1, The goal is to find a set of
points which makes the line P(start), P0, P(1) … P(k-1) “smooth”.
*/

平滑K个点即P0~PK-1。

方法：

在ReferenceLineProvider的构造函数中，有选择平滑器的相关代码：

if (smoother_config_.has_qp_spline()) {
smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_));
} else if (smoother_config_.has_spiral()) {
smoother_.reset(new SpiralReferenceLineSmoother(smoother_config_));
} else if (smoother_config_.has_discrete_points()) {
smoother_.reset(new DiscretePointsReferenceLineSmoother(smoother_config_));
} else {
ACHECK(false) << "unknown smoother config "
<< smoother_config_.DebugString();
}
Ps:apply_curvature_constraint_ =false

这分别是利用不同求解器实现了这个方法。如果考虑参考线的曲率约束，其优化问题是非线性的，可以使用ipopt非线性求解器求解（内点法），也可以使用osqp二次规划求解器来用SQP方法求解；如果不考虑曲率约束，则直接用osqp求解二次规划问题，Apollo默认使用FemPosDeviationSmoother::QpWithOsqp进行求解来减少计算量。

目标函数设计：

Apollo设计了三个代价函数，分别代表了平滑性、曲线总长度和参考点距离。大部分场景都是只考虑平滑性（平滑性权重默认设置为1e10，其他的权重设置为1）

weight_fem_pos_deviation: 1e10weight_ref_deviation: 1.0weight_path_length: 1.0

因此此处只考虑平滑性进行计算：

上述公式可以理解为：从中间那个点到第一个点的向量和从中间那个点到最后一个点的向量的矢量和的模的平方。显然，如果这三个点在一条直线上，那么这个值最小；三个点组成的两个线段的夹角越小，即曲线越“弯”，这个值就越大。

示例代码（python）用20个点模拟测试一下效果见图：

优化前后的轨迹点X,Y，kappa值

用途：

可对任意个X,Y坐标组成数组进行Fem Pos Deviation Smoother方法的平滑处理。

#!/usr/bin/python
# -*- coding: UTF-8 -*-
import osqp
import numpy as np
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
#matplotlib widget
from scipy import sparse#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 - zP = 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.xprob.update(l=ly, u=uy)
res = prob.solve()
opt_y = res.x#plot x - y , opt_x - opt_y , lb - ubfig1 = 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='--')#计算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.0s_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)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()

参考链接：

https://www.cnblogs.com/icathianrain/p/14407757.html

https://zhuanlan.zhihu.com/p/365371616

https://zhuanlan.zhihu.com/p/342740447