一天精通无人机第 45 讲 高级篇系列:姿态解算

file

由于四旋翼无人机的对称性,俯仰和滚转姿态可以采用相同的方式进行处理,在初始时刻,无人机处在地面位置上,我们可以根据加速度计测量出其在水平位置的姿态角,也就是俯仰角和滚转角的初始值;而航向角则需要通过磁罗盘的读数来确定,当然在计算时还需要移除磁偏角。而当无人机起飞之后根据陀螺仪的测量读数进行后续计算。具体方法如下:

设在上一时刻t_0无人机的姿态角为θ_0;当前时刻t_1所得到的陀螺仪读数为 ω;其中从上一时刻到当前时刻的时间间隔 ∆t为:

file

于是我们可以得到由t_0时刻到t_1时刻无人机的角度增量 ∆θ为:

file

由此得无人机在t_1时刻的姿态角θ_1为:

file

在前面我们讲过,在无人机姿态解算的过程中使用的是四元数,而不是姿态角。我们可以设在t_0时刻表示无人机姿态的四元数为q_0,设x、y和z轴的角增量分别为 ∆θ_x、 ∆θ_y、 ∆θ_z而在t_1时刻四元数增量 ∆q的计算公式为:

file

那么,在t_1时刻用于表示无人机姿态的四元数为:

file

这样,我们就得到了在t_1时刻的四元数,也就得到在t_1时刻无人机的姿态角。注意,对于俯仰角和滚转角只能通过陀螺仪的积分进行计算,而航向角还需要通过磁罗盘对其做融合,也就是进行卡尔曼滤波。在t_0时刻得到航向角的最优估计为yaw_0,在t_1时刻通过陀螺仪所得到的航向增量为 ∆yaw,而通过磁罗盘所测量得到的航向角为yaw_mag,从而得到在t_1时刻航向角的预测值yaw_pre为:

file

我们根据卡尔曼滤波公式最优估计yaw_1为:

file

最后根据在t_1时刻无人机的姿态通过公式得出机体坐标系到本地坐标系的变换矩阵。得到变换矩阵之后就可以进行下一步对速度和位置进行融合计算了。因为加速度计、气压计、GPS这些传感器所得到物理量读数都不再同一个坐标系下。因此,为了将这些传感器数据融合在一起得到当前时刻的最优估计值,需要将不同坐标系下的数据通过变换矩阵转到同一个坐标系下。因此变换矩阵就至关重要了。

下期预告:《状态估计(下):速度与位置估计》

file