导航路径优化(一)——平滑
文章目录
- 一、路径需求
- 二、路径优化
- 1. 平滑代价
- 1.1 三点路径平滑
- 1.1.1 直接法
- 1.1.2 海塞矩阵法
- 1.2 N点路径平滑
- 1.2.1 直接法
- 1.2.2 海塞矩阵法
一、路径需求
一般地，对局部规划器生成的路径需要进行优化，我们希望传给机器人用于导航的路径有如下的需求：
- 路径尽可能光滑，无拐点、无曲率突变(平滑)；
- 路径点间距尽可能均匀(均匀)；
- 平滑后的路径尽可能贴近原路径(几何相似性)；
二、路径优化
针对路径优化的需求，从三个方面考虑：平滑、均匀、几何相似性，这三个方面用代价体现后，总代价最小即为我们需要的最优路径。即最优路径 c o s t m i n = c o s t s + c o s t l + c o s t g cost_{min}=cost_s + cost_l + cost_g costmin=costs+costl+costg。
1. 平滑代价
1.1 三点路径平滑
以三个路径点为例：
如上图所示：原路径上存在三个路径点： A ( x 1 , y 1 ) A(x_1, y_1) A(x1,y1)、 B ( x 2 , y 2 ) B(x_2,y_2) B(x2,y2)、 C ( x 3 , y 3 ) C(x_3,y_3) C(x3,y3)，以此三个点做两个向量 B A → \overrightarrow {BA} BA 和 B C → \overrightarrow {BC} BC，两向量相加即可得到向量 v ⃗ \vec{v} v，那么向量 v ⃗ \vec {v} v 的范数越小则可以说明路径点 A 、 B 、 C A、B、C A、B、C 越接近于直线，当向量 v ⃗ \vec {v} v 的范数为0则路径点 A 、 B 、 C A、B、C A、B、C 共线。
v ⃗ = B A → + B C → = ( x 1 − x 2 , y 1 − y 2 ) + ( x 3 − x 2 , y 3 − y 2 ) = ( x 1 + x 3 − 2 x 2 , y 1 + y 3 − 2 y 2 ) \vec {v}=\overrightarrow {BA}+\overrightarrow {BC}=(x_1-x_2,y_1-y_2)+(x_3-x_2,y_3-y_2)=(x_1+x_3-2x_2,y_1+y_3-2y_2) v=BA+BC=(x1−x2,y1−y2)+(x3−x2,y3−y2)=(x1+x3−2x2,y1+y3−2y2)
向量指向谁，谁作为被减数
1.1.1 直接法
c o s t s = ∣ ∣ v ⃗ ∣ ∣ 2 = ( x 1 + x 3 − 2 x 2 ) 2 + ( y 1 + y 3 − 2 y 2 ) 2 = [ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 ] [ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 ] \begin{aligned} cost_s=||\vec {v}||^2&=(x_1+x_3-2x_2)^2+(y_1+y_3-2y_2)^2 \\[2ex] &=\left[ \begin{matrix} x_1+x_3-2x_2 & y_1+y_3-2y_2 \end{matrix} \right] \left[ \begin{matrix} x_1+x_3-2x_2\\ y_1+y_3-2y_2 \end{matrix} \right] \end{aligned} costs=∣∣v∣∣2=(x1+x3−2x2)2+(y1+y3−2y2)2=[x1+x3−2x2y1+y3−2y2][x1+x3−2x2y1+y3−2y2]
令 X = [ x 1 y 1 x 2 y 2 x 3 y 3 ] T \begin{aligned} X=\left[ \begin{matrix} x_1 &y_1 & x_2&y_2&x_3&y_3 \end{matrix} \right]^T \end{aligned} X=[x1y1x2y2x3y3]T，则:
[ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 ] = [ x 1 y 1 x 2 y 2 x 3 y 3 ] [ 1 0 0 1 − 2 0 0 − 2 1 0 0 1 ] = X T A s \begin{aligned} \left[ \begin{matrix} x_1+x_3-2x_2 & y_1+y_3-2y_2 \end{matrix} \right]=\left[ \begin{matrix} x_1 &y_1 & x_2&y_2&x_3&y_3 \end{matrix} \right]\left[ \begin{array}{rr} 1 &0 \\ 0&1\\-2&0 \\ 0&-2 \\1&0 \\ 0 &1 \end{array} \right]=X^TA_s \end{aligned} [x1+x3−2x2y1+y3−2y2]=[x1y1x2y2x3y3] 10−2010010−201 =XTAs
此时，
c o s t s = ∣ ∣ v ⃗ ∣ ∣ 2 = [ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 ] [ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 ] = X T A s ( X T A s ) T = X T A s A s T X = X T P s X \begin{aligned} cost_s=||\vec {v}||^2 &=\left[ \begin{matrix} x_1+x_3-2x_2 & y_1+y_3-2y_2 \end{matrix} \right] \left[ \begin{matrix} x_1+x_3-2x_2\\ y_1+y_3-2y_2 \end{matrix} \right]\\[2ex] &=X^TA_s(X^TA_s)^T\\[1.5ex] &=X^TA_sA_s^TX\\[1.5ex] &=X^TP_sX \end{aligned} costs=∣∣v∣∣2=[x1+x3−2x2y1+y3−2y2][x1+x3−2x2y1+y3−2y2]=XTAs(XTAs)T=XTAsAsTX=XTPsX
1.1.2 海塞矩阵法
f = c o s t s = ∣ ∣ v ⃗ ∣ ∣ 2 = ( x 1 + x 3 − 2 x 2 ) 2 + ( y 1 + y 3 − 2 y 2 ) 2 \begin{aligned} f=cost_s=||\vec {v}||^2&=(x_1+x_3-2x_2)^2+(y_1+y_3-2y_2)^2 \\[2ex] \end{aligned} f=costs=∣∣v∣∣2=(x1+x3−2x2)2+(y1+y3−2y2)2
一阶偏导数：
∂ f ∂ x 1 = ∂ f ∂ x 3 = 2 ( x 1 + x 3 − 2 x 2 ) ∂ f ∂ x 2 = − 4 ( x 1 + x 3 − 2 x 2 ) ∂ f ∂ y 1 = ∂ f ∂ y 3 = 2 ( y 1 + y 3 − 2 y 2 ) ∂ f ∂ y 2 = − 4 ( y 1 + y 3 − 2 y 2 ) \begin{aligned} \frac{\partial f}{\partial x_1}=\frac{\partial f}{\partial x_3}&=2(x_1+x_3-2x_2) \\[2ex] \frac{\partial f}{\partial x_2}&=-4(x_1+x_3-2x_2)\\[2ex] \frac{\partial f}{\partial y_1}=\frac{\partial f}{\partial y_3}&=2(y_1+y_3-2y_2) \\[2ex] \frac{\partial f}{\partial y_2}&=-4(y_1+y_3-2y_2)\\[2ex] \end{aligned} ∂x1∂f=∂x3∂f∂x2∂f∂y1∂f=∂y3∂f∂y2∂f=2(x1+x3−2x2)=−4(x1+x3−2x2)=2(y1+y3−2y2)=−4(y1+y3−2y2)
二阶偏导数：
∂ 2 f ∂ x 1 2 = ∂ ( 2 ( x 1 + x 3 − 2 x 2 ) ) ∂ x 1 = 2 ∂ 2 f ∂ x 1 ∂ y 1 = ∂ ( 2 ( x 1 + x 3 − 2 x 2 ) ) ∂ y 1 = 0 ∂ 2 f ∂ x 1 ∂ x 2 = ∂ ( 2 ( x 1 + x 3 − 2 x 2 ) ) ∂ x 2 = − 4 ∂ 2 f ∂ x 1 ∂ y 2 = ∂ ( 2 ( x 1 + x 3 − 2 x 2 ) ) ∂ y 2 = 0 ∂ 2 f ∂ x 1 ∂ x 3 = ∂ ( 2 ( x 1 + x 3 − 2 x 2 ) ) ∂ x 3 = 2 ∂ 2 f ∂ x 1 ∂ y 3 = ∂ ( 2 ( x 1 + x 3 − 2 x 2 ) ) ∂ y 3 = 0 ⋮ ∂ 2 f ∂ y 3 ∂ x 1 = ∂ ( 2 ( y 1 + y 3 − 2 y 2 ) ) ∂ x 1 = 0 ∂ 2 f ∂ y 3 ∂ y 1 = ∂ ( 2 ( y 1 + y 3 − 2 y 2 ) ) ∂ y 1 = 2 ∂ 2 f ∂ y 3 ∂ x 2 = ∂ ( 2 ( y 1 + y 3 − 2 y 2 ) ) ∂ x 2 = 0 ∂ 2 f ∂ y 3 ∂ y 2 = ∂ ( 2 ( y 1 + y 3 − 2 y 2 ) ) ∂ y 2 = − 4 ∂ 2 f ∂ y 3 ∂ x 3 = ∂ ( 2 ( y 1 + y 3 − 2 y 2 ) ) ∂ x 3 = 0 ∂ 2 f ∂ y 3 2 = ∂ ( 2 ( y 1 + y 3 − 2 y 2 ) ) ∂ y 3 = 2 \begin{aligned} \frac{\partial^2 f}{\partial x_1^2}&=\frac{\partial (2(x_1+x_3-2x_2))}{\partial x_1}=2 \\[2ex] \frac{\partial^2 f}{\partial x_1\partial y_1}&=\frac{\partial (2(x_1+x_3-2x_2))}{\partial y_1}=0 \\[2ex] \frac{\partial^2 f}{\partial x_1\partial x_2}&=\frac{\partial (2(x_1+x_3-2x_2))}{\partial x_2}=-4 \\[2ex] \frac{\partial^2 f}{\partial x_1\partial y_2}&=\frac{\partial (2(x_1+x_3-2x_2))}{\partial y_2}=0 \\[2ex] \frac{\partial^2 f}{\partial x_1\partial x_3}&=\frac{\partial (2(x_1+x_3-2x_2))}{\partial x_3}=2 \\[2ex] \frac{\partial^2 f}{\partial x_1\partial y_3}&=\frac{\partial (2(x_1+x_3-2x_2))}{\partial y_3}=0 \\[2ex] & \vdots\\ \frac{\partial^2 f}{\partial y_3\partial x_1}&=\frac{\partial (2(y_1+y_3-2y_2) )}{\partial x_1}=0 \\[2ex] \frac{\partial^2 f}{\partial y_3\partial y_1}&=\frac{\partial (2(y_1+y_3-2y_2) )}{\partial y_1}=2 \\[2ex] \frac{\partial^2 f}{\partial y_3\partial x_2}&=\frac{\partial (2(y_1+y_3-2y_2) )}{\partial x_2}=0 \\[2ex] \frac{\partial^2 f}{\partial y_3\partial y_2}&=\frac{\partial (2(y_1+y_3-2y_2) )}{\partial y_2}=-4 \\[2ex] \frac{\partial^2 f}{\partial y_3\partial x_3}&=\frac{\partial (2(y_1+y_3-2y_2) )}{\partial x_3}=0 \\[2ex] \frac{\partial^2 f}{\partial y_3^2}&=\frac{\partial (2(y_1+y_3-2y_2) )}{\partial y_3}=2 \\[2ex] \end{aligned} ∂x12∂2f∂x1∂y1∂2f∂x1∂x2∂2f∂x1∂y2∂2f∂x1∂x3∂2f∂x1∂y3∂2f∂y3∂x1∂2f∂y3∂y1∂2f∂y3∂x2∂2f∂y3∂y2∂2f∂y3∂x3∂2f∂y32∂2f=∂x1∂(2(x1+x3−2x2))=2=∂y1∂(2(x1+x3−2x2))=0=∂x2∂(2(x1+x3−2x2))=−4=∂y2∂(2(x1+x3−2x2))=0=∂x3∂(2(x1+x3−2x2))=2=∂y3∂(2(x1+x3−2x2))=0⋮=∂x1∂(2(y1+y3−2y2))=0=∂y1∂(2(y1+y3−2y2))=2=∂x2∂(2(y1+y3−2y2))=0=∂y2∂(2(y1+y3−2y2))=−4=∂x3∂(2(y1+y3−2y2))=0=∂y3∂(2(y1+y3−2y2))=2
海塞矩阵：
H = P s = [ ∂ 2 f ∂ x 1 2 ∂ 2 f ∂ x 1 ∂ y 1 … ∂ 2 f ∂ x 1 ∂ y 3 ∂ 2 f ∂ y 1 ∂ x 1 ∂ 2 f ∂ y 1 2 … ∂ 2 f ∂ y 1 ∂ y 3 ⋮ ⋮ ⋱ ⋮ ∂ 2 f ∂ y 3 ∂ x 1 ∂ 2 f ∂ y 3 ∂ x 2 … ∂ 2 f ∂ y 3 2 ] = [ 2 0 − 4 0 2 0 0 2 0 − 4 0 2 − 4 0 8 0 − 4 0 0 − 4 0 8 0 − 4 2 0 − 4 0 2 0 0 2 0 − 4 0 2 ] \begin{aligned} H=P_s= \left[ \begin{matrix} \frac{\partial^2 f}{\partial x_1^2} & \frac{\partial^2 f}{\partial x_1 \partial y_1} & \dots &\frac{\partial^2 f}{\partial x_1 \partial y_3} \\[3ex] \frac{\partial^2 f}{\partial y_1 \partial x_1} &\frac{\partial^2 f}{\partial y_1^2} & \dots &\frac{\partial^2 f}{\partial y_1 \partial y_3} \\[3ex] \vdots & \vdots & \ddots & \vdots\\[3ex] \frac{\partial^2 f}{\partial y_3 \partial x_1} &\frac{\partial^2 f}{\partial y_3 \partial x_2} & \dots &\frac{\partial^2 f}{\partial y_3^2}\\ \end{matrix} \right] =\left[ \begin{array}{rrrrrr} 2& 0 & -4 &0 &2 & 0 \\ 0& 2& 0 &-4 &0 & 2 \\ -4& 0 & 8 &0 &-4 & 0 \\ 0& -4 & 0 &8 &0 & -4 \\ 2& 0 & -4 &0 &2 & 0 \\ 0& 2 & 0 &-4 &0 & 2 \\ \end{array} \right] \end{aligned} H=Ps= ∂x12∂2f∂y1∂x1∂2f⋮∂y3∂x1∂2f∂x1∂y1∂2f∂y12∂2f⋮∂y3∂x2∂2f……⋱…∂x1∂y3∂2f∂y1∂y3∂2f⋮∂y32∂2f = 20−4020020−402−4080−400−4080−420−4020020−402
令 I = [ 1 0 0 1 ] \begin{aligned} I=\left[ \begin{matrix} 1 &0\\0 & 1 \end{matrix} \right] \end{aligned} I=[1001]，则：
P s = [ 2 I − 4 I 2 I 8 I − 4 I 2 I ] = 2 × [ I − 2 I I 4 I − 2 I I ] \begin{aligned} P_s= \left[ \begin{array}{rrr} 2I &-4I &2I \\ & 8I & -4I \\ & & 2I \end{array} \right] =2 \times \left[ \begin{array}{rrr} I &-2I &I \\ & 4I & -2I \\ & & I \end{array} \right] \end{aligned} Ps= 2I−4I8I2I−4I2I =2× I−2I4II−2II
总结：
- 直接法：预处理简单，后期处理复杂 A s A s T A_s{A_s}^T AsAsT 计算维数高，计算消耗大；
- 海塞矩阵法：预处理繁琐，需求解二阶偏导数；
1.2 N点路径平滑
1.2.1 直接法
对于路径存在N个路径点：
c o s t s = ∑ i = 1 n − 2 ∣ ∣ v ⃗ i + 1 ∣ ∣ 2 = ∑ i = 1 n − 2 [ ( x i + x i + 2 − 2 x i + 1 ) 2 + ( y i + y i + 2 − 2 y i + 1 ) 2 ] = [ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 x 2 + x 4 − 2 x 3 … x n − 2 + x n − 2 x n − 1 y n − 2 + y n − 2 y n − 1 ] [ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 x 2 + x 4 − 2 x 3 ⋮ x n − 2 + x n − 2 x n − 1 y n − 2 + y n − 2 y n − 1 ] \begin{aligned} cost_s=\sum_{i=1}^{n-2}||\vec{v}_{i+1}||^2&=\sum_{i=1}^{n-2}[(x_i+x_{i+2}-2x_{i+1})^2+(y_i+y_{i+2}-2y_{i+1})^2] \\[2ex] &=\left[ \begin{matrix} x_1+x_3-2x_2 & y_1+y_3-2y_2 & x_2+x_4-2x_3 & \dots & x_{n-2}+x_n-2x_{n-1} & y_{n-2}+y_n-2y_{n-1} \end{matrix} \right] \left[ \begin{matrix} x_1+x_3-2x_2 \\ y_1+y_3-2y_2 \\ x_2+x_4-2x_3 \\ \vdots \\ x_{n-2}+x_n-2x_{n-1} \\ y_{n-2}+y_n-2y_{n-1} \end{matrix} \right] \end{aligned} costs=i=1∑n−2∣∣vi+1∣∣2=i=1∑n−2[(xi+xi+2−2xi+1)2+(yi+yi+2−2yi+1)2]=[x1+x3−2x2y1+y3−2y2x2+x4−2x3…xn−2+xn−2xn−1yn−2+yn−2yn−1] x1+x3−2x2y1+y3−2y2x2+x4−2x3⋮xn−2+xn−2xn−1yn−2+yn−2yn−1
令 X = [ x 1 y 1 x 2 y 2 … x n y n ] T \begin{aligned} X=\left[ \begin{matrix} x_1 &y_1 & x_2&y_2&\dots&x_n&y_n \end{matrix} \right]^T \end{aligned} X=[x1y1x2y2…xnyn]T，则:
[ x 1 + x 3 − 2 x 2 y 1 + y 3 − 2 y 2 x 2 + x 4 − 2 x 3 … x n − 2 + x n − 2 x n − 1 y n − 2 + y n − 2 y n − 1 ] = [ x 1 y 1 x 2 y 2 … x n y n ] [ 1 0 0 0 1 0 − 2 0 1 0 0 − 2 0 1 1 0 − 2 0 0 1 0 − 2 1 0 0 1 … ⋮ ] { 2 n } × { 2 n − 4 } = X T A s \begin{aligned} &\left[ \begin{matrix} x_1+x_3-2x_2 & y_1+y_3-2y_2 & x_2+x_4-2x_3 & \dots & x_{n-2}+x_n-2x_{n-1} & y_{n-2}+y_n-2y_{n-1} \end{matrix} \right] \\[3ex] &=\left[ \begin{matrix} x_1 &y_1 & x_2&y_2&\dots&x_n&y_n \end{matrix} \right]\left[ \begin{array}{rrrr} 1 &0 &0\\ 0&1&0\\-2&0 &1&0\\ 0&-2 &0&1\\1&0 &-2&0\\ 0&1&0&-2\\ &&1&0\\ &&0&1&\dots \\ &&&\vdots \end{array} \right]_{\{2n\}\times\{2n-4\} }\\[3ex] &=X^TA_s \end{aligned} [x1+x3−2x2y1+y3−2y2x2+x4−2x3…xn−2+xn−2xn−1yn−2+yn−2yn−1]=[x1y1x2y2…xnyn] 10−2010010−2010010−2010010−201⋮… {2n}×{2n−4}=XTAs
此时，同 1.1.1 直接法
c o s t s = X T A s ( X T A s ) T = X T A s A s T X = X T P s X \begin{aligned} cost_s =X^TA_s(X^TA_s)^T =X^TA_sA_s^TX =X^TP_sX \end{aligned} costs=XTAs(XTAs)T=XTAsAsTX=XTPsX
令 I = [ 1 0 0 1 ] O = [ 0 0 0 0 ] \begin{aligned} I=\left[ \begin{matrix} 1 &0\\0 & 1 \end{matrix} \right] \end{aligned} \quad \begin{aligned} O=\left[ \begin{matrix} 0 &0\\0 & 0 \end{matrix} \right] \end{aligned} I=[1001]O=[0000]，则：
A s = [ I − 2 I I I − 2 I I … ⋮ ] A_s= \begin{aligned} \left[ \begin{array}{rrrr} I \\ -2I & I \\ I & -2I \\ & I & \dots \\ & \vdots \end{array} \right] \end{aligned} As= I−2III−2II⋮…
当 n = 3 n=3 n=3 时,
P s = A s A s T = [ I − 2 I I 4 I − 2 I I ] P_s=A_sA_s^T= \begin{aligned} \left[ \begin{array}{rrrr} I & -2I & I \\ & 4I &-2I\\ && I \end{array} \right] \end{aligned} Ps=AsAsT= I−2I4II−2II
当 n ⩾ 4 n\geqslant4 n⩾4 时,
P s = A s A s T = [ I − 2 I I O O … O 5 I − 4 I I O … O 6 I − 4 I I … O ⋮ ⋮ ⋮ 6 I − 4 I I 5 I − 2 I I ] P_s=A_sA_s^T= \begin{aligned} \left[ \begin{array}{rrrrrrr} I & -2I & I & O&O& \dots &O\\ & 5I&-4I &I&O&\dots & O\\ && 6I&-4I & I &\dots&O\\ && & & \vdots &\vdots&\vdots\\ &&&& 6I&-4I & I\\ &&&&& 5I&-2I\\ &&&&&&I\\ \end{array} \right] \end{aligned} Ps=AsAsT= I−2I5II−4I6IOI−4IOOI⋮6I………⋮−4I5IOOO⋮I−2II
1.2.2 海塞矩阵法
同 1.1.2 海塞矩阵法