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

问题描述

如题,最近在看 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)=FFz=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=csf+mgz
进而有
s f ( 2 ) + m g = c ∣ s f + m g z ∣ s f(2) + mg = c |s f + mgz| sf(2)+mg=csf+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 (c2nf2f2(2))s2+2mgf(2)(c21)s+m2g2(c21)=0
这便得到了上述代码中的二次方程的系数。

不解之处

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

  • 6
    点赞
  • 16
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值