CN109407696B - 一种无人机航向角动态校定方法 - Google Patents

Abstract

本发明是一种无人机航向角动态校定方法，通过惯性测量单元IMU姿态解算出的航向，当飞行航向出现干扰时，通过GPS获得无人机在地理坐标系N系下实时北和东方向的速度分量可以得出无人机实际飞行速度方向，再由GPS得到的无人机实际飞行速度方向与IMU姿态解算给出的飞行航向的偏差积分，来对航向角进行纠偏，实现动态校定无人机的航向角的功能，当飞行航向未被干扰时，可通过无人机飞控系统直接控制无人机的飞行航向。该种校定方法通过全球定位系统GPS得到的无人机实际飞行速度方向与给出的飞行航向的偏差积分，来对航向角进行纠偏，达到航向角动态校定的目的，从而提高无人机的飞行性能。

Description

一种无人机航向角动态校定方法

技术领域

本发明是涉及无人机控制技术领域，具体的说是一种无人机航向角动态校定方法。

背景技术

近年来，多旋翼无人机技术不断发展成熟，由地面到天空，已然成为二维向三维的革命式创新。多旋翼无人机一般采用卫星导航设备(如全球定位系统(GPS))，地磁计，陀螺仪，加速度计等传感器，完成对无人机航向，飞行姿态，位置等航行参数的测量。由于飞机在动态飞行过程中易受众多不确定因素干扰，易偏离设定的航线，从而会导致飞行任务的失败。要使无人机准确无误的完成指定任务，首当其冲必须保证无人机在飞行过程中沿着指定航向作业，因此对无人机航向角动态校定变得至关重要。

发明内容

本发明针对现有技术中的不足，提供一种无人机航向角动态校定方法。

为实现上述目的，本发明采用以下技术方案：

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

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

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

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

ψ₀＝-atan2(ve，vn)；

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

E_k＝ψ₀-ψ

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

积分结果补偿到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-(r·u)u＝u×r

O′A′＝O′A cosα+O′B sinα＝rcosα-(r·u)u cosα+u×r sinα

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

由矢量三重积计算公式：

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

即

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

所以

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

＝r+u×r sinα+(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₃构造四元数：

本发明的有益效果是：提供了一种无人机航向角动态校定方法，通过全球定位系统GPS得到的无人机实际飞行速度方向与给出的飞行航向的偏差积分，来对航向角进行纠偏，达到航向角动态校定的目的，从而提高无人机的飞行性能。具有确度高、实现方便、效率高、实时性好的优势。

附图说明

图1为本发明一种无人机航向角动态校定方法的图。

图2为本发明一种无人机航向角动态校定方法机体坐标系与地理坐标系的关系示意图。

图3为本发明刚体旋转一定角度示意图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

如图1所示，通过惯性测量单元IMU姿态解算出的航向，当飞行航向出现干扰时，通过GPS获得无人机在地理坐标系N系下实时北和东方向的速度分量可以得出无人机实际飞行速度方向，再由GPS得到的无人机实际飞行速度方向与IMU姿态解算给出的飞行航向的偏差积分，来对航向角进行纠偏，实现动态校定无人机的航向角的功能，当飞行航向未被干扰时，可通过无人机飞控系统直接控制无人机的飞行航向。

通过惯性测量单元IMU姿态解算出的航向，在外乱不大的情况下，无人机应该按照这个方向进行飞行。但如果航向角有偏差的话，无人机会偏离这个航向。当无人机偏离航向时通过上述方法，实现动态校定无人机的航向角的功能。其中涉及多个坐标系，地理坐标系是指以无人机的重心为原点，Xn轴指向地理北向，Yn轴指向地理东向。机体坐标系是以无人机的重心为原点，各个轴沿无人机的俯仰，横滚，偏航方向。

如图2所示，OX_bY_bZ为机体坐标系；OX_nY_nZ_n为地理坐标系；θ为俯仰角；γ为横滚角；ψ为航向角。当飞行航向出现干扰时，无人机航向角的动态校定步骤如下：

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

本实施例中，实际飞行中姿态求解主要就是利用搭载机体上的陀螺仪所测数据来进行解算的，但陀螺仪的漂移问题会导致误差不断增大，此时需要加速度计和地磁计对系统进行补偿处理。

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

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

ψ₀＝-atan2(ve，vn)；

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

E_k＝ψ₀-ψ

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

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

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

如图3所示，步骤1中四元数q₀，q₁，q₂，q₃的计算过程如下：

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

图3b中，设刚体以ω＝ω_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-(r·u)u＝u×r

O′A′＝O′A cosα+O′B sinα＝r cosα-(r·u)u cosα+u×r sinα

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

由矢量三重积计算公式：

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

即

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

所以

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

＝r+u×r sinα+(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₃构造四元数：

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (1)

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₃构造四元数：

Images