PX4多旋翼位置控制

多悬翼的位置控制由内外环控制，外环P控制作用于位置差，产生期望速度，内环PID作用于速度差，产生期望油门，然后将油门解算成期望姿态。

 

• 外环位置控制的三种控制源

1.Manual

  手动控制下通过手动的输入直接产生期望速度，然后进入内环速度控制（不经过外环）

2.Offboard

  离线外部控制模式，即飞机所需要的一些数据：位置、速度都是来源于飞控外部，期望的位置、速度航向，都不是计算出来的，而是由外面的直接给出来的。

3.Auto

通过内外环控制，通过期望位置计算出期望姿态

 

• 以下是位置控制流程，我们不难发现系统的输入除了期望位置以外，还输入了期望偏航角用于解算油门

 

• 外环控制

• 内环控制

 

• Control_auto 

这个函数用于自动控制模式下，让飞机进行航线任务，除了内外环PID控制之外，还需要对系统输入的期望位置进行修正，并且这个修正较为复杂。

在自动模式下，期望位置修正分为两部分，第一部分是Z方向，第二部分是XY方向,Z方向控制优先

Z方向上

其中

Vel_sp_z=_vel_sp(2)+Z轴最大加速度* dt  （加速过程）

       min_vel_z+slope*dist_to_current_z (减速过程)

       dis_to_current_z * P参数         （离当前期望位置较近）

 

XY方向上分为两部分，当前位置在Current航点之后，当前位置在Current航点之前

Vx是Vel_sp_along_track,它的计算与当前位置和prev,current两点的位置有关。