固定翼位置控制_Tecs

Tecs在位置控制中主要控制纵向的高度，因为升降舵可以控制飞机的高度，油门可以控制飞机的速度，但是单单通过升降舵改变高度会使速度下降或者上升，单单通过油门改变速度会使高度改变，所以我们通过

协调油门和俯仰角要求来控制飞机的高度和速度达到预设的值。

Tecs算法的功能:输入设定的高度和速度，输出目标油门和俯仰角。

油门控制方法

一些简单的概念

飞机总能量： E=mgh+12mV2

总能量变化率(将E对时间求导)： E=mg dhdt+m V*dvdt

 

对于控制油门，因为油门和飞机总能量变化率E成线性关系，

T=mgEV+D

T正比于ΔE

我们可以由飞机总能量变化率求出预测油门throttle_predicted

预测油门 =巡航油门 + ( 总能量变化率  / 最大总能量变化率 ) *(最大油门值-巡航油门)

不难发现当总能量变化率最大时，飞机的预测油门取最大油门

 

获得预测油门后我们需要对其进行修正，利用总能量差的PID修正

油门修正值=(PID修正值)*总能量到油门的增益

其中，总能量到油门的增益=1/(油门时间常数*(最大总能量变化率-最小总能量变化率))

 

最终目标油门大小_throttle_setpoint=预测油门+油门修正值

 

 

 

 

 

 

 

 

 

 

 

俯仰角控制方法

一些简单的概念

控制权重：决定了能量的控制重要性，其中有势能控制权重和动能控制权重，二者相加总合为2

能量平衡(SEB)B=势能*势能的控制权重 - 动能*动能的控制权重

_SEB_error: 飞机此时的energy balance和目标位置的energy balance之间的差值

能量变化率平衡(SEB_rate) B = 势能变化率*势能的控制权重 –动能变化率*动能的控制权重_SEB_rate_error:飞机此时的energy balance rate和目标位置的energy balance rate之间的差值

 

通过对

俯仰角和能量变化率平衡  存在转化关系

   θ=/V*g

由预定位置的 sp 我们可以得到预测俯仰角 /(v*g)

我们利用能量平衡差值_SEB_error进行PID修正。

修正值SEB_correction=_SEB_error+ _pitch_integ_state + _SEB_rate_error * _pitch_damping_gain

将SEB_correction/（tp(俯仰时间常数)*V*g） 得到俯仰角修正值

其中climb_angle_to_SEB_rate=tp*V*g

最终的俯仰角_pitch_setpoint=SEB_correction/climb_angle_to_SEB_rate+