apollo参考线平滑公式及推导

---

前言

本文基于Apollo的参考线平滑，主要参考以下文章
开发者说丨离散点曲线平滑原理
决策规划算法二：生成参考线

一、问题描述

一般情况下，高精地图给出的道路中心线不够平滑，会导致车辆行驶不够平稳，因此我们希望在进行planning之前能够对参考线进行平滑。
优化对象：离散中心线点坐标$(x_i,y_i)$

优化方法：二次规划
二次规划的一般形式如下：
$$\begin{array}{rl}\text{Minimize}& \frac{1}{2}{\mathbf{x}}^T\mathbf{Qx}+{\mathbf{c}}^T\mathbf{x}\\ \text{subject to}& \mathbf{Ax}\le \mathbf{b},\\ & \mathbf{Ex}=\mathbf{d},\\ & \mathbf{x}\ge 0,\end{array}$$

二、优化目标

1. 平滑度代价$cost1$
   也就是$\left|\overrightarrow{P_1{P}_3}\right|$的模长,从图中可以直观的看出当$\angle P_0{P}_1{P}_2$越大，则$\left|\overrightarrow{P_1{P}_3}\right|$越小。
   
   $cost1$的物理意义是$\overrightarrow{P_1{P}_3}$的模长，$\overrightarrow{P_1{P}_3}=\overrightarrow{P_1{P}_2}+\overrightarrow{P_1{P}_0}$.
   因此：$$\begin{array}{rl}cost1& =((x_0-x_1)+(x_2-x_1){)}^2+((y_0-y_1)+(y_2-y_1){)}^2\\ & =(x_0+x_2-2x_1{)}^2+(y_0+y_2-2y_1{)}^2\end{array}$$
   因此$cost1$推广到一般形式为：
   $$cost1=\sum _{i=1}^{n-1}(x_{i-1}+x_{i+1}-2x_i{)}^2+(y_{i-1}+y_{i+1}-2y_i{)}^2$$
   为了后续计算$Q$矩阵，将上式写成矩阵形式：
   $$cost1=[x_0+x_2-2x_1,y_0+y_2-2y_1,\dots ,x_{n-3}+x_{n-1}-2x_{n-2},y_{n-3}+y_{n-1}-2y_{n-2}]\left[\begin{array}{c}{x}_0+x_2-2x_1\\ y_0+y_2-2y_1\\ \dots \\ x_{n-3}+x_{n-1}-2x_{n-2}\\ y_{n-3}+y_{n-1}-2y_{n-2}\end{array}\right]$$
   令$X=[x_0,y_0,x_1,y_1,\dots ,x_{n-1},y_{n-1}]$，系数矩阵为A，那么$cost1=X\ast A\ast (X\ast A{)}^T=X\ast A\ast A^T\ast X$
   同时A矩阵为$$A=\left[\begin{array}{cccccc}1& 0& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ -2& 0& 0& 0& \dots & 0\\ 0& -2& 0& 0& \dots & 0\\ 1& 0& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& 0& \dots & 1\end{array}\right]$$
   其中A矩阵维度为$2n\ast 2(n-2)$.
   同时因为$cost1=X\ast A\ast (X\ast A{)}^T=X\ast A\ast A^T\ast X$，因此系数矩阵$Q_1=A\ast A^T$
   因此$$Q_1=\left[\begin{array}{cccccccc}I& -2I& I& O& \cdots & O& O& O\\ -2I& 5I& -4I& I& \cdots & O& O& O\\ I& -4I& 6I& -4I& \cdots & O& O& O\\ O& I& -4I& 6I& \cdots & O& O& O\\ ⋮& ⋮& ⋮& ⋮& \ddots & ⋮& ⋮& ⋮\\ O& O& O& O& \cdots & 6I& -4I& I\\ O& O& O& O& \cdots & -4I& 5I& -2I\\ O& O& O& O& \cdots & O& -2I& I\end{array}\right]$$
   其中$I=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$
   上面公式可以使用如下matlab代码进行验证：

clc;clear all;
n = 10; #点个数
a=zeros(2*n,(2*n)-4);
for i=1:(2*n)-4
    a(i,i)=1;
    a(i+2,i)=-2;
    a(i+4,i)=1;
end
matrix = a*a';
% 获取矩阵的行数和列数
[n, m] = size(matrix);

% 确定块矩阵的大小
block_size = 2;
block_rows = n / block_size;
block_cols = m / block_size;

% 初始化分块矩阵C
C = cell(block_rows, block_cols);

% 填充分块矩阵C
for i = 1:block_rows
    for j = 1:block_cols
        C{i, j} = matrix(block_size*(i-1)+1:block_size*i, block_size*(j-1)+1:block_size*j);
    end
end

% 输出结果
disp('分块矩阵C：');
for i = 1:block_rows
    for j = 1:block_cols
        fprintf('C{%d, %d} = \n', i, j);
        disp(C{i, j});
    end
end
res = zeros(block_rows,block_cols);
for i =1:block_rows
    for j=1:block_cols
        res(i,j) =C{i,j}(1,1)/1;
    end
end
disp(res);

运行结果如下图：

2. 均匀性$cost2$
   平滑后的参考线的每两个相邻点之间的长度尽量均匀一致。
   
   因此$cost2:$
   $$\begin{array}{rl}cost2& ={\left|\overrightarrow{P_1{P}_2}\right|}^2+{\left|\overrightarrow{P_2{P}_3}\right|}^2\\ & =(x_2-x_1{)}^2+(y_2-y_1{)}^2+(x_3-x_2{)}^2+(y_3-y_2{)}^2\end{array}$$
   因此推广到一般形式：
   $$cost2=\sum _{i=0}^{n-1}(x_i-x_{i+1}{)}^2+(y_i-y_{i+1}{)}^2$$
   将上式推广到矩阵形式：
   $$cost2=[(x_i-x_{i+1}),(y_i-y_{i+1})]\left[\begin{array}{c}(x_i-x_{i+1})\\ (y_i-y_{i+1})\end{array}\right]$$,
   ，那么$cost2=X\ast A_1\ast (X\ast A_1{)}^T=X\ast A_1\ast A_1^T\ast X$,其中$A_1$为：
   $$A_1=\left[\begin{array}{cccccc}1& 0& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ -1& 0& 0& 0& \dots & 0\\ 0& -1& 0& 0& \dots & 0\\ 0& 0& 0& 0& \dots & 0\\ 0& 0& 0& 0& \dots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& 0& \dots & -1\end{array}\right]$$ 其中$A_1$矩阵维度为$2n\ast 2(n-1)$,那么$Q_2=A_1\ast A_1^T$,
   $$Q_2=\left[\begin{array}{cccccc}I& -I& O& \cdots & O& O\\ -I& 2I& -I& \cdots & O& O\\ O& -I& 2I& \cdots & O& O\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮\\ O& O& O& \cdots & 2I& -I\\ O& O& O& \cdots & -I& I\end{array}\right]$$
   其中$I=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$
   上面公式可以使用如下matlab代码进行验证：

clc;clear all;
n = 10;
a=zeros(2*n,(2*(n-1)));
for i=1:(2*(n-1))
    a(i,i)=1;
    a(i+2,i)=-1;
end
matrix = a*a';
% 获取矩阵的行数和列数
[n, m] = size(matrix);

% 确定块矩阵的大小
block_size = 2;
block_rows = n / block_size;
block_cols = m / block_size;

% 初始化分块矩阵C
C = cell(block_rows, block_cols);

% 填充分块矩阵C
for i = 1:block_rows
    for j = 1:block_cols
        C{i, j} = matrix(block_size*(i-1)+1:block_size*i, block_size*(j-1)+1:block_size*j);
    end
end

% 输出结果
disp('分块矩阵C：');
for i = 1:block_rows
    for j = 1:block_cols
        fprintf('C{%d, %d} = \n', i, j);
        disp(C{i, j});
    end
end
res = zeros(block_rows,block_cols);
for i =1:block_rows
    for j=1:block_cols
        res(i,j) =C{i,j}(1,1)/1;
    end
end
disp(res);

运行结果为：

3. 参考线偏移程度cost3
   平滑后的参考线，希望能够保留原始道路的几何信息，不会把弯道的处的参考线平滑成一条直线。使用平滑后点与原始点的距离来表示
   
   $$\begin{array}{rl}cost3& ={\left|\overrightarrow{P_1{P}_{1}r}\right|}^2+{\left|\overrightarrow{P_2{P}_{2}r}\right|}^2\\ & =(x_1-x_{1r}{)}^2+(y_1-y_{1r}{)}^2+(x_2-x_{2r}{)}^2+(y_2-y_{2r}{)}^2\end{array}$$
   因此推广到一般形式：
   $$cost3=\sum _{i=0}^{n-1}(x_i-x_{ir}{)}^2+(y_i-y_{ir}{)}^2=\sum _{i=0}^{n-1}(x_i^2+x_{ir}^2-2\ast x_i\ast x_{ir})$$,因为常数项不影响优化结果，因此去掉常数项并写成矩阵形式：令$X=[x_0,y_0,x_1,y_1,\dots ,x_{n-1},y_{n-1}]$，$cost3=X\ast A_3\ast X^T-2\ast c\ast X^T$,
   $$cost3=X\ast \left[\begin{array}{cccccc}1& 0& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ 0& 0& 0& 0& \dots & 0\\ 0& 0& 0& 0& \dots & 0\\ 0& 0& 0& 0& \dots & 0\\ 0& 0& 0& 0& \dots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 0& 0& \dots & 1\end{array}\right]\ast X^T-2\ast [x_{0r},y_{0r},\dots ,x_{n-1r},y_{n-1r}]\ast X^T$$,其中$A_3$是$2n\ast 2n$的方阵.
   因此:
   $$Q_3=\left[\begin{array}{cccc}I& O& \cdots & O\\ O& I& \cdots & O\\ ⋮& ⋮& \ddots & ⋮\\ O& O& O& I\end{array}\right]$$,
   其中$I=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

综上所述$Q=x\ast Q_1+y\ast Q_2+z\ast Q_3$,其中$x,y,z$分别为，平滑度，均匀性，参考线偏移程度的权重，综上因此$Q$矩阵可以写成：
$$\left[\begin{array}{cccccc}X+Y+Z& -2X-Y& X& 0& 0& 0\\ 0& 5X+2Y+Z& -4X-Y& X& 0& 0\\ 0& 0& 6X+2Y+Z& -4X-Y& X& 0\\ 0& 0& 0& 6X+2Y+Z& -4X-Y& X\\ 0& 0& 0& 0& 5X+2Y+Z& -2X-Y\\ 0& 0& 0& 0& 0& X+Y+Z\end{array}\right]$$

其中$X=x\ast \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],Y=y\ast \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],,Z=z\ast \left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$
注意：这个Q矩阵只写了6个点情况，也就是12*12维，更高维只需要写$X,-4X-Y，5X+2Y+Z$这三项

三、添加约束

参考线约束的平滑较为简单，主要只有和原始参考线距离这一部分，具体内容如下,只考虑边界线约束:
$$\begin{gathered} x_{i,lower}\le x_i\le x_{i,upper} \\ {y}_{i,lower}\le y_i\le y_{i,upper} \end{gathered}$$,
可以继续写成：
$$\begin{gathered} x_{i,r}-bound\le x_i\le x_{i,r}+bound \\ {y}_{i,r}-bound\le y_i\le y_{i,r}+bound \end{gathered}$$
同时对于端点，需要特殊处理，使其等于起始点因此：
$$\begin{gathered} x_{0,r}\le x_0\le x_{0,r} \\ {y}_{0,r}\le y_0\le y_{0,r} \\ {x}_{n-1,r}\le x_{n-1}\le x_{n-1,r} \\ {y}_{n-1,r}\le y_{n-1}\le y_{n-1,r} \end{gathered}$$

四、总结

具体的c++代码实现可以参考apollo参考线平滑部分