调整飞行器推力方向以保证飞行器控制角度不超过指定角度

问题描述

如题，最近在看 ego-planner 大佬的代码，发现作者在计算出飞行器推力$F$（对应代码中的force_）后，为确保飞行器推力方向与$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+mgz$，其中 $f$ 和 $z$ 都是向量，$z=(0,0,1)$。当检测到 $F$ 与 $z$ 的夹角超过 $\theta$ 时，只需要相应减小 $f$ 的大小，即可减小 $F$ 与 $z$ 的夹角，这里可以通过简单的力的分解与合成来进一步理解。因此，调整后的总推力可以写为 $F=sf+mgz$，其中 $s$ 是一个系数，本质上是一个缩放因子。从代码上来看，这个$s$的大小是通过求解一个二次方程得到的，那么这个二次方程是如何得到的呢，我们接着看。

根据上述过程，一旦 $F$ 与 $z$ 的夹角超过 $\theta$ ，我们需要将其调整回 $\theta$，因此其夹角的余弦值即为$cos(\theta )=c$，即
$$cos(F,z)=\frac{Fz}{|F|}=c$$
进而，$$Fz=cF$$
我们将 $F=sf+mgz$ 带入上式，得到
$$(sf+mgz)z=c|sf+mgz|$$
进而有
$$sf(2)+mg=c|sf+mgz|$$
两边取平方，则有
$$(sf(2)+mg{)}^2=c^2(sf+mgz{)}^T(sf+mgz)$$
记 $|f|=n_f$，进而得到
$$f^2(2)s^2+2mgf(2)s+m^2{g}^2=c^2(n_f^2{s}^2+2mgf(2)s+m^2{g}^2)$$
即
$$(c^2{n}_f^2-f^2(2))s^2+2mgf(2)(c^2-1)s+m^2{g}^2(c^2-1)=0$$
这便得到了上述代码中的二次方程的系数。

不解之处

代码中作者注释为Limit control angle to 45 degree，但这里 $\theta$ 实际上是$\pi /2$，也就是 $90°$，所以有些困惑。