ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 其他分享> 文章详细

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

2021-12-19 09:58:13  阅读：286  来源： 互联网

标签：Deviation deviation Smoother weight Fem pos length array append


配置文件:

平滑器的配置文件
位置：

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: 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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有