固定翼位置控制_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