其他分享
首页 > 其他分享> > Apollo 参考线平滑方法Fem Pos Deviation Smoother

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中的注释

/*

平滑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: 1e10

weight_ref_deviation: 1.0

weight_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 - 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='--')



#计算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)
    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

标签：Deviation,deviation,Smoother,weight,Fem,pos,length,array,append
来源： https://blog.csdn.net/weixin_43683789/article/details/122020645