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