固定翼位置控制_L1

L1算法控制航向。

 

飞机从现在位置到设定位置,需要进行转弯。转弯需要一个横向的加速度来改变速度的方向。

这里横向加速度的计算公式 a=V2R  又因为R=2*sin (η) / L1

可得 a=2*V2L1*sin⁡(η) 

L1现在位置到设定位置的距离,η是速度方向与飞机到设定位置连线的夹角

 

滚转角和横向加速度a的关系

 

得到a就可以得到roll, 下面是代码部分

 

要得到a,说明我们需要知道V,L1 ,η

V是飞机速度,可以从传感器直接得到

L1 给出了计算公式

_L1_distance = _L1_ratio * ground_speed;

_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;

 

现在只需要计算η,根据控制模式有不同的计算方法

L1航线模式

普通航线:由多个航点组成,轨迹为容易形状

定向航线:没有具体的位置要求,只有方向要求

 

盘旋航线:轨迹是一个等高的圆形航线,航线信息只有两个,圆心和半径

 

普通航线:

在普通航线中,我们不知道L1参考点,只知道L1的长度(用公式可以得出),为了计算a,我们需要知道L1和V的夹角

飞机需要向AB航线校正,则需要知道速度V和飞机到A点L1 的夹角 eta=eta1+eta2

求eta1:

V点乘AB单位向量得V在AB方向的投影

V叉乘AB单位向量得V在AB垂直方向上的投影

利用tan(eta1)的反三角函数得eta1

求eta2:

V叉乘AB单位向量得到P到AB方向的最短距离

L1可以用公式求

利用sin(eta2)的反三角函数求eta2

最后求得eta夹角,可以计算出a,滚转角roll

 

航线模式的特殊情况

 

  1. 飞机距离A点过远
  2. AP与AP的夹角大于135度

 此时L1参考点取在P到A的连线上,此时我们可以一次求出eta

先用V点乘PA单位向量得到V在PA方向上的投影,再用V叉乘PA单位向量得到V在PA垂直方向上的投影,

Eta=arctan(叉乘值/点乘值)

 

以下是代码实现部分

 

以下是官方的图

 

 

 

盘旋航线:

盘旋航线有两种模式

第一种是capture模式,飞机离圆的边界较远,引导飞机向圆心飞行

第二种是circle模式,飞机接近圆的边界,绕圆心飞行

Capture模式

盘旋航线是飞机绕圆心飞行,当飞机在圆外飞行时,L1参考点取在飞机和圆心之间,要求得横向加速度,只需要知道夹角eta,原理与航线模式下大于135读的特殊情况求eta角的方法一样

Eta=arctan(V % vector_A_to_airplane_unit  / (-V)* vector_A_to_airplane_unit )

然后利用 a=2*V2L1*sin⁡(eta)

 

Circle模式

当飞机离目标航线较近时,我们不在使用L1进行控制了,而是使用简单的PD控制加上前馈控制。我们知道飞机最后在规划的航线上做圆周运动时的向心加速度为:v^2/R。但此时飞机距离规划的航线还有d米,因此我们用PD加上前馈的控制方式来进行位置控制,公式如下:

 

 

 

 

定向航线:

定向航线中L1参考点一直在飞机和期望方向的连线上,

 

 

void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const Vector2f  &ground_speed_vector)

eta=目标航向-现在航向,然后可以求得a

评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值