问题描述
如题,最近在看 ego-planner 大佬的代码,发现作者在计算出飞行器推力
F
F
F(对应代码中的force_
)后,为确保飞行器推力方向与
z
z
z轴的夹角不超过
θ
\theta
θ,编写了以下代码:
// Limit control angle to 45 degree
double theta = M_PI / 2;
double c = cos(theta);
Eigen::Vector3d f;
f.noalias() = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1);
if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
{
double nf = f.norm();
double A = c * c * nf * nf - f(2) * f(2);
double B = 2 * (c * c - 1) * f(2) * mass_ * g_;
double C = (c * c - 1) * mass_ * mass_ * g_ * g_;
double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
// Limit control angle to 45 degree
脉路梳理
其主要思路应该如下:
首先,对于一开始所求出的总推力
F
F
F,其可以写成
F
=
f
+
m
g
z
F = f + mgz
F=f+mgz,其中
f
f
f 和
z
z
z 都是向量,
z
=
(
0
,
0
,
1
)
z = (0,0,1)
z=(0,0,1)。当检测到
F
F
F 与
z
z
z 的夹角超过
θ
\theta
θ 时,只需要相应减小
f
f
f 的大小,即可减小
F
F
F 与
z
z
z 的夹角,这里可以通过简单的力的分解与合成来进一步理解。因此,调整后的总推力可以写为
F
=
s
f
+
m
g
z
F = s f + mgz
F=sf+mgz,其中
s
s
s 是一个系数,本质上是一个缩放因子。从代码上来看,这个
s
s
s的大小是通过求解一个二次方程得到的,那么这个二次方程是如何得到的呢,我们接着看。
根据上述过程,一旦
F
F
F 与
z
z
z 的夹角超过
θ
\theta
θ ,我们需要将其调整回
θ
\theta
θ,因此其夹角的余弦值即为
c
o
s
(
θ
)
=
c
cos (\theta) = c
cos(θ)=c,即
c
o
s
(
F
,
z
)
=
F
z
∣
F
∣
=
c
cos(F,z) = \frac{F z}{|F|} = c
cos(F,z)=∣F∣Fz=c
进而,
F
z
=
c
F
Fz = cF
Fz=cF
我们将
F
=
s
f
+
m
g
z
F = s f + mgz
F=sf+mgz 带入上式,得到
(
s
f
+
m
g
z
)
z
=
c
∣
s
f
+
m
g
z
∣
(s f + mgz)z = c |s f + mgz|
(sf+mgz)z=c∣sf+mgz∣
进而有
s
f
(
2
)
+
m
g
=
c
∣
s
f
+
m
g
z
∣
s f(2) + mg = c |s f + mgz|
sf(2)+mg=c∣sf+mgz∣
两边取平方,则有
(
s
f
(
2
)
+
m
g
)
2
=
c
2
(
s
f
+
m
g
z
)
T
(
s
f
+
m
g
z
)
( s f(2) + mg )^2 = c^2 (s f + mgz)^T(s f + mgz)
(sf(2)+mg)2=c2(sf+mgz)T(sf+mgz)
记
∣
f
∣
=
n
f
|f| = n_f
∣f∣=nf,进而得到
f
2
(
2
)
s
2
+
2
m
g
f
(
2
)
s
+
m
2
g
2
=
c
2
(
n
f
2
s
2
+
2
m
g
f
(
2
)
s
+
m
2
g
2
)
f^2(2)s^2 + 2mgf(2)s + m^2g^2 = c^2 (n^2_f s^2 + 2mgf(2)s + m^2g^2)
f2(2)s2+2mgf(2)s+m2g2=c2(nf2s2+2mgf(2)s+m2g2)
即
(
c
2
n
f
2
−
f
2
(
2
)
)
s
2
+
2
m
g
f
(
2
)
(
c
2
−
1
)
s
+
m
2
g
2
(
c
2
−
1
)
=
0
(c^2 n^2_f - f^2(2)) s^2 + 2mgf(2)(c^2 - 1)s + m^2g^2(c^2 - 1) = 0
(c2nf2−f2(2))s2+2mgf(2)(c2−1)s+m2g2(c2−1)=0
这便得到了上述代码中的二次方程的系数。
不解之处
代码中作者注释为Limit control angle to 45 degree
,但这里
θ
\theta
θ 实际上是
π
/
2
\pi/2
π/2,也就是
90
°
90°
90°,所以有些困惑。