[发明专利]一种无人机航向角动态校定方法

权利要求书

1.一种无人机航向角动态校定方法，其特征在于：通过惯性测量单元IMU姿态解算出的航向，通过GPS获得无人机在地理坐标系N系下实时北和东方向的速度分量可以得出无人机实际飞行速度方向，再由GPS得到的无人机实际飞行速度方向与IMU姿态解算给出的飞行航向的偏差积分，来对航向角进行纠偏，实现动态校定无人机的航向角的功能，具体步骤如下：

步骤1，根据陀螺仪采集到的旋转角度数据通过计算得出的四元数q₀，q₁，q₂，q₃；

步骤2，对无人机飞行过程中的陀螺仪、地磁计和加速度计的数据进行融合，通过惯性测量单元IMU利用四元数姿态解算算法算出无人机的飞行航向ψ，所述的无人机的飞行航向ψ，如下式所示：

步骤3，利用GPS获得无人机在地理坐标系N系下实时北和东方向的速度分量为vn，ve，再根据反正切函数可解算出无人机实际飞行速度方向ψ₀，如下式所示：

ψ₀＝-atan2(ve，vn)；

步骤4，将GPS得到的实际无人机飞行速度方向ψ₀与IMU姿态解算后给出的无人机飞行航向ψ作差，俩者差值作为误差值E_k,如下式所示:

E_k＝ψ₀-ψ

步骤5，通过GPS得到的实际无人机飞行速度方向与IMU姿态解算给出的飞行航向的偏差E_k积分，来对航向角进行纠偏：

积分结果补偿到IMU的最终航向角ψ：

其中，n为IMU更新的频率相对于GPS更新频率的倍数；

所述的步骤1中四元数q₀，q₁，q₂，q₃的计算过程如下:

设有参考坐标系R，坐标轴X₀,Y₀,Z₀,坐标轴方向的单位向量i₀,j₀,k₀，刚体相对R系做定点转动，定点为O，取坐标系b与刚体固联，b系的坐标轴为x,y,z，坐标轴方向的单位向量i,j,k，假设初始时刻b系与R系重合，刚体上取一点A，转动点O至该点引位置向量OA；

设刚体以ω＝ω_xi+ω_yj+ω_zk相对R系旋转，初始时刻位置向量处于OA＝r,经过一段时间OA’＝r’，根据欧拉定理，仅考虑刚体在O时刻和t时刻的角位置时，刚体从A位置转到A’位置的转动可等效成转轴绕轴单位向量u转过α角的位置转动，位置向量做圆锥运动，A和A’位于同一圆上，r和r’位于同一圆锥上；下面分析r和r’的关系，在圆上取一点B使角AO’B为90度，可得如下等效式：

OO′＝(r·u)u

O′A＝r-OO′＝r-(r·u)u

O′B＝u×O′A＝u×r-u×(r·u)u＝u×r

O′A′＝O′Acosα+O′Bsinα＝rcosα-(r·u)ucosα+u×rsinα

可以得出：r′＝OO′+O′A′＝rcosα+(1-cosα)×(r·u)u+u×rsinα

由矢量三重积计算公式：

u×(u×r)＝u(u·r)-(u·u)r＝(r·u)u-r

即

(r·u)u＝r+u×(u×r)

所以

r′＝rcosα+(1-cosα)[r+u×(u×r)]+u×rsinα

＝r+u×rsinα+(1-cosα)u×(u×r)

将上式向R系投影：

r′^R＝r^R+(u×r)^Rsinα+(1-cosα)[u×(u×r)]^R

记

又根据叉乘关系表达式

记

则

(u×r)^R＝Ur^R

[u×(u×r)]^R＝U·Ur^R

所以

令

得到

r′^R＝Dr^R

记初始时刻的刚体固联坐标系为b₀，由于初始时刻刚体固联坐标系与参考坐标系重合所以r^R＝r^b0而在转动过程中，位置向量和b系都同刚体固联，所以位置向量和b系的相对角位置始终不变，即有：

r^R＝r′^R＝Dr′^b

该式说明D即为b系至R系的坐标变换矩阵；

即

以q₀，q₁，q₂，q₃构造四元数：