Fast Planner的代码中，前端路径搜索时需要评估路径的启发成本（函数KinodynamicAstar::estimateHeuristic），涉及到一元四次方程和一元三次方程的求解计算。Kinodynamic Astar函数推导详见Faster Planner——Kinodynamic Astar详解。
1. 一元三次方程求解[1]
在求解三次多项式时，Fast planner代码中使用了两种方法，当判别式大于0及等于0的情况利用了求根公式法，判别式小于0的情况则使用了三角函数法。
1.1 判别式
a
x
3
+
b
x
2
+
c
x
+
d
=
0
,
其中
a
≠
0
(1.1)
a x^3+b x^2+c x+d=0, 其中a \neq 0 \tag{1.1}
ax3+bx2+cx+d=0,其中a=0(1.1)
定义判别式
Δ
\Delta
Δ为：
Δ
=
(
b
c
6
a
2
−
b
3
27
a
3
−
d
2
a
)
2
+
(
c
3
a
−
b
2
9
a
2
)
3
(1.2)
\Delta=\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3 \tag{1.2}
Δ=(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3(1.2)
（1）当
Δ
>
0
\Delta>0
Δ>0 时，方程有一个实根和两个共轭复根；
（2）当
Δ
=
0
\Delta=0
Δ=0 时，方程有三个实根:
当
(
b
c
6
a
2
−
b
3
27
a
3
−
d
2
a
)
2
=
−
(
c
3
a
−
b
2
9
a
2
)
3
=
0
(1.3)
\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2=-\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3=0 \tag{1.3}
(6a2bc−27a3b3−2ad)2=−(3ac−9a2b2)3=0(1.3)
时，方程有一个三重实根；
当
(
b
c
6
a
2
−
b
3
27
a
3
−
d
2
a
)
2
=
−
(
c
3
a
−
b
2
9
a
2
)
3
≠
0
(1.4)
\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2=-\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3 \neq 0 \tag{1.4}
(6a2bc−27a3b3−2ad)2=−(3ac−9a2b2)3=0(1.4)
时，方程的三个实根中有两个相等;
（3）当
Δ
<
0
\Delta<0
Δ<0 时，方程有三个不等的实根。
1.2 求根公式法
a x 3 + b x 2 + c x + d = 0 , 其中 a ≠ 0 (1.5) a x^3+b x^2+c x+d=0, 其中a \neq 0 \tag{1.5} ax3+bx2+cx+d=0,其中a=0(1.5)
x 1 = − b 3 a + b c 6 a 2 − b 3 27 a 3 − d 2 a + ( b c 6 a 2 − b 3 27 a 3 − d 2 a ) 2 + ( c 3 a − b 2 9 a 2 ) 3 3 + b c 6 a 2 − b 3 27 a 3 − d 2 a − ( b c 6 a 2 − b 3 27 a 3 − d 2 a ) 2 + ( c 3 a − b 2 9 a 2 ) 3 3 x_1=-\frac{b}{3 a}+\sqrt[3]{\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}+\sqrt{\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3}}+\sqrt[3]{\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}-\sqrt{\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3}} x1=−3ab+36a2bc−27a3b3−2ad+(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3+36a2bc−27a3b3−2ad−(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3
x 2 = − b 3 a + − 1 + 3 i 2 b c 6 a 2 − b 3 27 a 3 − d 2 a + ( b c 6 a 2 − b 3 27 a 3 − d 2 a ) 2 + ( c 3 a − b 2 9 a 2 ) 3 3 + − 1 − 3 i 2 b c 6 a 2 − b 3 27 a 3 − d 2 a − ( b c 6 a 2 − b 3 27 a 3 − d 2 a ) 2 + ( c 3 a − b 2 9 a 2 ) 3 3 x_2=-\frac{b}{3 a}+\frac{-1+\sqrt{3} \mathrm{i}}{2} \sqrt[3]{\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}+\sqrt{\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3}} +\frac{-1-\sqrt{3} \mathrm{i}}{2}\sqrt[3]{\left.\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}-\sqrt{\left(\frac{b c}{6 a^2}\right.}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3} x2=−3ab+2−1+3i36a2bc−27a3b3−2ad+(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3+2−1−3i36a2bc−27a3b3−2ad−(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3
x 3 = − b 3 a + − 1 − 3 i 2 b c 6 a 2 − b 3 27 a 3 − d 2 a + ( b c 6 a 2 − b 3 27 a 3 − d 2 a ) 2 + ( c 3 a − b 2 9 a 2 ) 3 3 + − 1 + 3 i 2 b c 6 a 2 − b 3 27 a 3 − d 2 a − ( b c 6 a 2 − b 3 27 a 3 − d 2 a ) 2 + ( c 3 a − b 2 9 a 2 ) 3 3 x_3=-\frac{b}{3 a}+\frac{-1-\sqrt{3} \mathrm{i}}{2} \sqrt[3]{\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}+\sqrt{\left(\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3}} +\frac{-1+\sqrt{3} \mathrm{i}}{2}\sqrt[3]{\left.\frac{b c}{6 a^2}-\frac{b^3}{27 a^3}-\frac{d}{2 a}-\sqrt{\left(\frac{b c}{6 a^2}\right.}-\frac{b^3}{27 a^3}-\frac{d}{2 a}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3} x3=−3ab+2−1−3i36a2bc−27a3b3−2ad+(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3+2−1+3i36a2bc−27a3b3−2ad−(6a2bc−27a3b3−2ad)2+(3ac−9a2b2)3
1.3 三角函数法
a
x
3
+
b
x
2
+
c
x
+
d
=
0
,
其中
a
≠
0
(1.9)
a x^3+b x^2+c x+d=0, 其中a \neq 0 \tag{1.9}
ax3+bx2+cx+d=0,其中a=0(1.9)
若令
Δ
=
(
−
b
3
27
a
3
−
d
2
a
+
b
c
6
a
2
)
2
+
(
c
3
a
−
b
2
9
a
2
)
3
=
α
2
+
β
3
<
0
(1.10)
\Delta=\left(\frac{-b^3}{27 a^3}-\frac{d}{2 a}+\frac{b c}{6 a^2}\right)^2+\left(\frac{c}{3 a}-\frac{b^2}{9 a^2}\right)^3=\alpha^2+\beta^3<0 \tag{1.10}
Δ=(27a3−b3−2ad+6a2bc)2+(3ac−9a2b2)3=α2+β3<0(1.10)
则
x
1
=
−
b
3
a
+
2
−
β
cos
[
arccos
α
(
−
β
)
3
2
3
]
(1.11)
x_1=-\frac{b}{3 a}+2 \sqrt{-\beta} \cos \left[\frac{\arccos \frac{\alpha}{(-\beta)^{\frac{3}{2}}}}{3}\right] \tag{1.11}
x1=−3ab+2−βcos
3arccos(−β)23α
(1.11)
x
2
=
−
b
3
a
+
2
−
β
cos
[
arccos
α
(
−
β
)
3
2
+
2
π
3
]
(1.12)
x_2=-\frac{b}{3 a}+2 \sqrt{-\beta} \cos \left[\frac{\arccos \frac{\alpha}{(-\beta)^{\frac{3}{2}}}+2 \pi}{3}\right] \tag{1.12}
x2=−3ab+2−βcos
3arccos(−β)23α+2π
(1.12)
x
3
=
−
b
3
a
+
2
−
β
cos
[
arccos
α
(
−
β
)
3
2
−
2
π
3
]
(1.13)
x_3=-\frac{b}{3 a}+2 \sqrt{-\beta} \cos \left[\frac{\arccos \frac{\alpha}{(-\beta)^{\frac{3}{2}}}-2 \pi}{3}\right] \tag{1.13}
x3=−3ab+2−βcos
3arccos(−β)23α−2π
(1.13)
程序中一元三次方程求解函数代码：
vector<double> KinodynamicAstar::cubic(double a, double b, double c, double d)
{
vector<double> dts;
double a2 = b / a;
double a1 = c / a;
double a0 = d / a;
double Q = (3 * a1 - a2 * a2) / 9;
double R = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 54;
double D = Q * Q * Q + R * R;
if (D > 0)
{
double S = std::cbrt(R + sqrt(D));
double T = std::cbrt(R - sqrt(D));
dts.push_back(-a2 / 3 + (S + T));
return dts;
}
else if (D == 0)
{
double S = std::cbrt(R);
dts.push_back(-a2 / 3 + S + S);
dts.push_back(-a2 / 3 - S);
return dts;
}
else
{
double theta = acos(R / sqrt(-Q * Q * Q));
dts.push_back(2 * sqrt(-Q) * cos(theta / 3) - a2 / 3);
dts.push_back(2 * sqrt(-Q) * cos((theta + 2 * M_PI) / 3) - a2 / 3);
dts.push_back(2 * sqrt(-Q) * cos((theta + 4 * M_PI) / 3) - a2 / 3);
return dts;
}
}
2. 一元四次方程求解[2-5]
代码中一元四次方程的求解使用了费拉里方法。
给定一元四次方程形式如下：
a
x
4
+
b
x
3
+
c
x
2
+
d
x
+
e
=
0
(2.1)
ax^4+bx^3+cx^2+dx+e = 0 \tag{2.1}
ax4+bx3+cx2+dx+e=0(2.1)
当
a
≠
0
a≠0
a=0时，均可以化为：
x
4
+
a
3
x
3
+
a
2
x
2
+
a
1
x
+
a
0
=
0
(2.2)
x^4+a_3x^3+a_2x^2+a_1x+a_0 = 0 \tag{2.2}
x4+a3x3+a2x2+a1x+a0=0(2.2)
移项：
x
4
+
a
3
x
3
=
−
a
2
x
2
−
a
1
x
−
a
0
(2.3)
x^4+a_3x^3=-a_2x^2-a_1x-a_0 \tag{2.3}
x4+a3x3=−a2x2−a1x−a0(2.3)
对等式左边进行配方，需等式两边同时加上 ( 1 2 a 3 x ) 2 (\frac{1}{2} a_3 x)^2 (21a3x)2 :
x
4
+
a
3
x
3
+
(
1
2
a
3
x
)
2
=
(
1
2
a
3
x
)
2
−
a
2
x
2
−
a
1
x
−
a
0
(2.4)
x^4+a_3 x^3+\left(\frac{1}{2} a_3 x\right)^2=\left(\frac{1}{2} a_3 x\right)^2-a_2 x^2-a_1 x-a_0 \tag{2.4}
x4+a3x3+(21a3x)2=(21a3x)2−a2x2−a1x−a0(2.4)
配方可得：
(
x
2
+
1
2
a
3
x
)
2
=
(
1
4
a
3
2
−
a
2
)
x
2
−
a
1
x
−
a
0
(2.5)
\left(x^2+\frac{1}{2} a_3 x\right)^2=\left(\frac{1}{4} a_3^2-a_2\right) x^2-a_1 x-a_0 \tag{2.5}
(x2+21a3x)2=(41a32−a2)x2−a1x−a0(2.5)
若等式右侧也为平方公式，则便于方程求解。为对等式右边配方，引入变量
y
y
y（对于任意值，等式都成立）：
(
x
2
+
1
2
a
3
x
+
y
2
)
2
−
(
x
2
+
1
2
a
3
x
)
y
−
1
4
y
2
=
(
1
4
a
3
2
−
a
2
)
x
2
−
a
1
x
−
a
0
(2.6)
\left(x^2+\frac{1}{2} a_3 x+\frac{y}{2}\right)^2-\left(x^2+\frac{1}{2} a_3 x\right) y-\frac{1}{4} y^2=\left(\frac{1}{4} a_3^2-a_2\right) x^2-a_1 x-a_0 \tag{2.6}
(x2+21a3x+2y)2−(x2+21a3x)y−41y2=(41a32−a2)x2−a1x−a0(2.6)
移项，有：
(
x
2
+
1
2
a
3
x
+
y
2
)
2
=
(
x
2
+
1
2
a
3
x
)
y
+
1
4
y
2
+
(
1
4
a
3
2
−
a
2
)
x
2
−
a
1
x
−
a
0
(2.7)
\left(x^2+\frac{1}{2} a_3 x+\frac{y}{2}\right)^2=\left(x^2+\frac{1}{2} a_3 x\right) y+\frac{1}{4} y^2+\left(\frac{1}{4} a_3^2-a_2\right) x^2-a_1 x-a_0 \tag{2.7}
(x2+21a3x+2y)2=(x2+21a3x)y+41y2+(41a32−a2)x2−a1x−a0(2.7)
对等式右边进行整理，有：
(
x
2
+
1
2
a
3
x
+
y
2
)
2
=
(
1
4
a
3
2
−
a
2
+
y
)
x
2
+
(
1
2
a
3
y
−
a
1
)
x
+
y
2
4
−
a
0
(2.8)
\left(x^2+\frac{1}{2} a_3 x+\frac{y}{2}\right)^2=\left(\frac{1}{4} a_3^2-a_2+y\right) x^2+\left(\frac{1}{2} a_3 y-a_1\right) x+\frac{y^2}{4}-a_0 \tag{2.8}
(x2+21a3x+2y)2=(41a32−a2+y)x2+(21a3y−a1)x+4y2−a0(2.8)
可见式（2.8）左侧为完全平方式，右侧为 x x x 的二次多项式 (系数中含有 y y y )。为了右侧也是完全平方，需要选择合适的 y y y 值，需其判别式等于 0 ，即:
Δ = ( 1 2 a 3 y − a 1 ) 2 − 4 ( 1 4 a 3 2 − a 2 + y ) ( y 2 4 − a 0 ) = 0 (2.9) \Delta=\left(\frac{1}{2} a_3 y-a_1\right)^2-4\left(\frac{1}{4} a_3^2-a_2+y\right)\left(\frac{y^2}{4}-a_0\right)=0 \tag{2.9} Δ=(21a3y−a1)2−4(41a32−a2+y)(4y2−a0)=0(2.9)
整理得:
y
3
−
a
2
y
2
+
(
a
3
a
1
−
4
a
0
)
y
+
(
4
a
2
a
0
−
a
1
2
−
a
3
2
a
0
)
=
0
(2.10)
y^3-a_2 y^2+\left(a_3 a_1-4 a_0\right) y+\left(4 a_2 a_0-a_1^2-a_3^2 a_0\right)=0 \tag{2.10}
y3−a2y2+(a3a1−4a0)y+(4a2a0−a12−a32a0)=0(2.10)
式（2.10）是关于
y
y
y 的一元三次方程，可对其进行求解（利用上文中一元三次方程求解方法），
y
1
y_1
y1 为其任一实数解，则式（2.8）右侧方程可配成
(
R
x
+
F
)
2
(Rx+F)^2
(Rx+F)2的形式，即：
(
x
2
+
1
2
a
3
x
+
y
1
2
)
2
=
(
R
x
+
F
)
2
(2.11)
\left(x^2+\frac{1}{2} a_3 x+\frac{y_1}{2}\right)^2=\left(R x+F\right)^2 \tag{2.11}
(x2+21a3x+2y1)2=(Rx+F)2(2.11)
其中：
R
=
1
4
a
3
2
−
a
2
+
y
1
(2.12)
R=\sqrt{\frac{1}{4} a_3^2-a_2+y_1} \tag{2.12}
R=41a32−a2+y1(2.12)
F
=
y
2
4
−
a
0
(2.13)
F=\sqrt{\frac{y^2}{4} -a_0} \tag{2.13}
F=4y2−a0(2.13)
将
y
1
y_1
y1 代入式（2.8）得:
(
x
2
+
1
2
a
3
x
+
y
1
2
)
2
=
(
1
2
a
3
2
−
4
a
2
+
4
y
1
x
+
1
2
y
1
2
−
4
a
0
)
2
(2.14)
\left(x^2+\frac{1}{2} a_3 x+\frac{y_1}{2}\right)^2=\left(\frac{1}{2} \sqrt{a_3^2-4 a_2+4 y_1} x+\frac{1}{2} \sqrt{y_1^2-4 a_0}\right)^2 \tag{2.14}
(x2+21a3x+2y1)2=(21a32−4a2+4y1x+21y12−4a0)2(2.14)
等式两边开方，有：
x
2
+
1
2
a
3
x
+
y
1
2
=
±
(
1
2
a
3
2
−
4
a
2
+
4
y
1
x
+
1
2
y
1
2
−
4
a
0
)
(2.15)
x^2+\frac{1}{2} a_3 x+\frac{y_1}{2}=\pm\left(\frac{1}{2} \sqrt{a_3^2-4 a_2+4 y_1} x+\frac{1}{2} \sqrt{y_1^2-4 a_0}\right) \tag{2.15}
x2+21a3x+2y1=±(21a32−4a2+4y1x+21y12−4a0)(2.15)
整理得:
x
2
+
1
2
(
a
3
±
a
3
2
−
4
a
2
+
4
y
1
)
x
+
1
2
(
y
1
±
y
1
2
−
4
a
0
)
=
0
(2.16)
x^2+\frac{1}{2}\left(a_3 \pm \sqrt{a_3^2-4 a_2+4 y_1}\right) x+\frac{1}{2}\left(y_1 \pm \sqrt{y_1^2-4 a_0}\right)=0 \tag{2.16}
x2+21(a3±a32−4a2+4y1)x+21(y1±y12−4a0)=0(2.16)
可用一元二次方程的求根公式对其求解 (即原一元四次方程的解)，有：
x
1
=
−
1
4
a
3
+
1
2
R
+
1
2
D
x
2
=
−
1
4
a
3
+
1
2
R
−
1
2
D
x
3
=
−
1
4
a
3
−
1
2
R
+
1
2
E
x
4
=
−
1
4
a
3
−
1
2
R
−
1
2
E
(2.17)
\begin{aligned} & x_1=-\frac{1}{4} a_3+\frac{1}{2} R+\frac{1}{2} D \\ & x_2=-\frac{1}{4} a_3+\frac{1}{2} R-\frac{1}{2} D \\ & x_3=-\frac{1}{4} a_3-\frac{1}{2} R+\frac{1}{2} E \\ & x_4=-\frac{1}{4} a_3-\frac{1}{2} R-\frac{1}{2} E \end{aligned} \tag{2.17}
x1=−41a3+21R+21Dx2=−41a3+21R−21Dx3=−41a3−21R+21Ex4=−41a3−21R−21E(2.17)
其中:
R
=
1
4
a
3
2
−
a
2
+
y
1
(2.18)
R=\sqrt{\frac{1}{4} a_3^2-a_2+y_1} \tag{2.18}
R=41a32−a2+y1(2.18)
当
R
≠
0
R \neq 0
R=0 时：
D
=
3
4
a
3
2
−
R
2
−
2
a
2
+
1
4
(
4
a
3
a
2
−
8
a
1
−
a
3
3
)
R
−
1
(2.19)
D=\sqrt{\frac{3}{4} a_3^2-R^2-2 a_2+\frac{1}{4}\left(4 a_3 a_2-8 a_1-a_3^3\right) R^{-1}} \tag{2.19}
D=43a32−R2−2a2+41(4a3a2−8a1−a33)R−1(2.19)
E
=
3
4
a
3
2
−
R
2
−
2
a
2
−
1
4
(
4
a
3
a
2
−
8
a
1
−
a
3
3
)
R
−
1
(2.20)
E=\sqrt{\frac{3}{4} a_3^2-R^2-2 a_2-\frac{1}{4}\left(4 a_3 a_2-8 a_1-a_3^3\right) R^{-1}} \tag{2.20}
E=43a32−R2−2a2−41(4a3a2−8a1−a33)R−1(2.20)
当
R
=
0
R=0
R=0 时：
D
=
3
4
a
3
2
−
2
a
2
+
2
y
1
2
−
4
a
0
(2.21)
D=\sqrt{\frac{3}{4} a_3^2-2 a_2+2 \sqrt{y_1^2-4 a_0}} \tag{2.21}
D=43a32−2a2+2y12−4a0(2.21)
E
=
3
4
a
3
2
−
2
a
2
−
2
y
1
2
−
4
a
0
(2.22)
E=\sqrt{\frac{3}{4} a_3^2-2 a_2-2 \sqrt{y_1^2-4 a_0}} \tag{2.22}
E=43a32−2a2−2y12−4a0(2.22)
程序中一元四次方程求解函数代码：
vector<double> KinodynamicAstar::quartic(double a, double b, double c, double d, double e)
{
vector<double> dts;
double a3 = b / a;
double a2 = c / a;
double a1 = d / a;
double a0 = e / a;
vector<double> ys = cubic(1, -a2, a1 * a3 - 4 * a0, 4 * a2 * a0 - a1 * a1 - a3 * a3 * a0);
double y1 = ys.front();
double r = a3 * a3 / 4 - a2 + y1;
if (r < 0)
return dts;
double R = sqrt(r);
double D, E;
if (R != 0)
{
D = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 + 0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R);
E = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 - 0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R);
}
else
{
D = sqrt(0.75 * a3 * a3 - 2 * a2 + 2 * sqrt(y1 * y1 - 4 * a0));
E = sqrt(0.75 * a3 * a3 - 2 * a2 - 2 * sqrt(y1 * y1 - 4 * a0));
}
if (!std::isnan(D))
{
dts.push_back(-a3 / 4 + R / 2 + D / 2);
dts.push_back(-a3 / 4 + R / 2 - D / 2);
}
if (!std::isnan(E))
{
dts.push_back(-a3 / 4 - R / 2 + E / 2);
dts.push_back(-a3 / 4 - R / 2 - E / 2);
}
return dts;
}
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
