PX4轨迹生成公式推导_px4的s曲线控制

PX4轨迹生成公式推导下载链接

对于多旋翼，飞行任务的时候，通过地面站画出航点，上传给飞控，

飞控通过轨迹生成算法生成平滑的目标位置、速度及加速度，给位置控制模块控制飞机的位置、速度及加速度。

PX4轨迹生成的方法为：约束加加速度的S型速度曲线(Jerk-limited S-Curve),生成的速度曲线为S型曲线，如下图所示。

具体的代码实现在flight_tasks模块的VelocitySmoothing.cpp （v1.11版本）

约束条件：

    JMax: 最大的加加速度

    aMax:最大加速度

    a3:最后的加速度（为0）

整个过程分为三个阶段：

    T1阶段为加加速度阶段(加加速度 j=jMax)

    T2阶段为均加速阶段

    T3阶段为减加速度阶段(减加速度 j=-jMax)

输入为：

    a0:初始加速度

    v0:初始速度

    x0:初始位置

    vRef:目标速度

符号意义：

    aT1：T1阶段最后的加速度

    aT2：整个T2阶段的加速度

    vT1：T1阶段最后的速度

    vT2：T2阶段最后的速度

    vT3：T3阶段最后的速度

    xT1：T1阶段最后的位置

    xT2：T2阶段最后的位置

    xT3：T3阶段最后的位置

初始加速度a0经过T1、T2及T3三个阶段后，加速度变为a3,可以表示为：

a0 + j * T1 - j * T3 = a3

因为约束条件中 a3 = 0; 则

a0 + j * T1 - j * T3 = 0               ====>     T3 = (a0 + j * T1) / j                     (1)       对应飞控代码中函数computeT3(float T1, float a0, float j_max)

aT2 = a