CN110988393A - 基于超声波风速风向仪的无人机风速风向测量及校正算法 - Google Patents

Abstract

本发明公开了一种基于超声波风速风向仪的无人机风速风向测量及校正算法，包括两种状态的风速风向测量及校正算法，一种是无人机悬停状态下的风速风向测量及校正算法，另一种是无人机前进状态下的风速风向测量及校正算法，两种风速风向测量及校正算法都是利用四元数与姿态阵之间的关系来得出无人机的三个欧拉角，进而得到无人机的倾角，通过倾角的大小来对超声波风速风向仪测得的风速风向结果进行校正。与现有技术相比，本发明的有益效果是：本发明的测量及其校正算法克服了因无人机前进或者有风吹来时机身平面倾斜导致的测量结果误差，具有测量精度高、使用寿命长的优点。

Description

基于超声波风速风向仪的无人机风速风向测量及校正算法

技术领域

本发明涉及一种测风校正算法，尤其涉及一种基于超声波风速风向仪的无人机风速风向测量及校正算法。

背景技术

目前，无人飞行器已在野外施工、勘探、交通运输、旅游、救援，特别是江河湖泊上工作等领域中得到广泛应用。随着无人机应用的深入，采用多旋翼无人机进行气象监测也正成为一个研究热点。

但是在现有的技术中，使用多旋翼无人机测风主要是通过直接在机体上部加装塔状超声波风速风向仪来实现的。现有的技术不仅会因为改变重心、增大风阻而导致无人机飞行稳定性的降低，而且还会因为超声波风速风向仪在工作时受到旋翼自身产生风场的影响而极大降低测量精度。故现有技术无法实现在多旋翼无人机上对风速、风向的精确监测。

考虑到能装在无人机上，这里我们选用了原理简单的超声波风速风向仪，发明了一种基于超声波风速风向仪的无人机风速风向测量的校正算法。利用超声波的传播特性来测量风速风向，具有测量精度高、使用寿命长的优点。它适用于多种环境，可靠性高。所以我们把这种测风方法与无人机相结合，可以充分发挥其优势。但因为传感器放置在无人机上，无人机前进或者有风吹来时机身平面会倾斜，超声波风速风向仪也随之倾斜，由于超声波风速风向仪仅测得与超声波风速风向仪平面平行的风，这势必造成测量结果的误差，因此要对其结果进行校正。

发明内容

本发明针对上述现有技术的不足，而提供一种基于超声波风速风向仪的无人机风速风向测量及校正算法。

为解决上述技术问题，本发明采用的技术方案是：

一种基于超声波风速风向仪的无人机风速风向测量及校正算法，包括两种状态的风速风向测量及校正算法，一种是无人机悬停状态下的风速风向测量及校正算法，另一种是无人机前进状态下的风速风向测量及校正算法，两种风速风向测量及校正算法都是利用四元数与姿态阵之间的关系来得出无人机的三个欧拉角，进而得到无人机的倾角，通过倾角的大小来对超声波风速风向仪测得的风速风向结果进行校正；

设有一参考坐标系R，简称R系，一刚体相对于R系作定点转动，定点为O；选取坐标系b与刚体固联，坐标系b简称b系，假设初始时刻b系与R系重合，设OA＝r为起始位置向量，OA'＝r'为旋转后的向量；根据欧拉定理，仅仅考虑初始时刻与最终时刻的位置，刚体从A位置转至A'位置等效成绕单位瞬轴

转过θ₁角度一次完成；将向量分解、旋转、合成可以得到：

r'＝rcosθ₁+(1-cosθ₁)(u·r)u+u×rsinθ₁； (1)

由三重矢积公式变换得：

r'＝r+u×rsinθ₁+(1-cosθ₁)u×(u×r)； (2)

记

所以有

令：

则：

u×r＝Ur；

u×(u×r)＝U·Ur；

所以有：

令：

则式(3)可以写成：

r'＝Dr； (5)

记初始时刻的刚体固联坐标系为b₀，由于初始时刻刚体固联坐标系b与参考坐标系R重合，所以有：

在转动过程中位置向量和b系都和刚体固联，所以位置向量和b系的相对角位置始终不变，即有：

所以得到：

r＝r'^b； (8)

将式(8)带入式(5)得：

r'＝Dr'^b； (9)

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

即：

令：

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

其中，i,j,k既是相互正交的单位向量，又是虚数单位

将q₀,q₁,q₂,q₃代入式(10)中进一步化简得：

设参考坐标系为导航坐标系n，与刚体固联的坐标系为机体坐标系，那么坐标变换矩阵

就是姿态矩阵

在机体轴坐标系下，无人机先绕x轴转动横滚角α，再绕y轴转动俯仰角β，最后再绕z轴转动航向角γ；则在此坐标系下的变换矩阵为：

对比式(13)和式(14)可得到三个姿态角：

β＝-arcsinT₃₁＝-arcsin2(q₁q₃-q₀q₂)；

其中，T₁₁为cosβcosγ，T₂₁为cosβsinγ，T₃₁为-sinβ，T₃₂为sinαcosβ，T₃₃为cosαcosβ；

设无人机原平面的法向量为

旋转之后所得平面的法向量为：

所以无人机的倾角θ为：

θ＝arccos(cosαcosβ)； (17)

由超声波测风原理可以得到风速ν为：

其中，t₁、t₂为x轴方向传播时间，t₃、t₄为y轴方向传播时间，

风向

为：

其中，k为整数，公式后面标了各种情况下的k取什么值。

当无人机处于悬停状态下时，无人机内内置的GPS测得的无人机速度大小为0，此时超声波风速风向仪所测得的风向与实际风向一致；将风分解为与无人机平面平行的分量和垂直的分量，垂直分量对超声波风速风向仪无影响，所以超声波风速风向仪测得的风为平行分量，测得的风向角与原风向角一致，所以修正的结果为：

其中，v_实际为实际风速，

为实际风向，γ为航向角。

当无人机处于前进状态时，无人机内内置的GPS测得无人机前进的速度大小为v₁，无人机以一定的速度前进，等效于无人机悬停时，同样大小的风沿着无人机前进方向的反方向吹来；超声波风速风向仪测量的速度大小为v，此时的v_合为无人机前进状态下等效的风速v₁与实际要测的风速v₂的合成风，则：

其中,v为超声波风速风向仪测量的速度大小，

为无人机前进状态下等效的风速，

为实际要测的风速；

在机体坐标系下，无论无人机平面如何旋转都会过原点，所以无人机平面的方程为：

Ax+By+Cz＝0

又因为其法向量为：

求得：

令z＝0可得交线方程为：

Ax+By＝0； (25)

当B＞0时，合成风v_合分解为

和

为无人机前进状态下的等效风速，

为实际要测的风速，

为合风的风向角，θ₂为实际风的风向角；合风的坐标为

等效风速的坐标为

由式(23)可得：

实际风速大小为：

在这种情况下求出的实际风的风向角θ₂为：

当B＜0时的平面合风向分解示意图，此时要求的风向角为负，同理可得：

特别的，当A＝0,B＞0时，

此时

当A＝0,B＜0时，

此时

故综上所述，可得：

将θ₂转化为[0,2π)内的角度，即：

其中，θ′₂为求出的风向角转化为[0,2π)内的角；

随着时间的变化，某时刻的风向为：

其中γ为航向角。

与现有技术相比，本发明的有益效果在于：本发明的测量及其校正算法克服了因无人机前进或者有风吹来时机身平面倾斜导致的测量结果误差，具有测量精度高、使用寿命长的优点。

附图说明

图1为刚体旋转示意图；

图2为风向分解示意图；

图3为B＞0时平面合风向分解示意图；

图4为B＜0时平面合风向分解示意图。

其中，1-无人机平面，2-水平面，3-风向，4-风向沿无人机平面的分量，5-风向垂直无人机平面的分量，6-无人机平面与水平面的交线。

具体实施方式

现在结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。

一种基于超声波风速风向仪的无人机风速风向测量及校正算法，包括两种状态的风速风向测量及校正算法，一种是无人机悬停状态下的风速风向测量及校正算法，另一种是无人机前进状态下的风速风向测量及校正算法，两种风速风向测量及校正算法都是利用四元数与姿态阵之间的关系来得出无人机的三个欧拉角，进而得到无人机的倾角，通过倾角的大小来对超声波风速风向仪测得的风速风向结果进行校正；

图1是刚体旋转示意图，设有一参考坐标系R，简称R系，一刚体相对于R系作定点转动，定点为O；选取坐标系b与刚体固联，坐标系b简称b系，假设初始时刻b系与R系重合，设OA＝r为起始位置向量，OA'＝r'为旋转后的向量；根据欧拉定理，仅仅考虑初始时刻与最终时刻的位置，刚体从A位置转至A'位置等效成绕单位瞬轴

转过θ₁角度一次完成；将向量分解、旋转、合成可以得到：

r'＝rcosθ₁+(1-cosθ₁)(u·r)u+u×rsinθ₁； (1)

由三重矢积公式变换得：

r'＝r+u×rsinθ₁+(1-cosθ₁)u×(u×r)； (2)

记

所以有

令：

则：

u×r＝Ur；

u×(u×r)＝U·Ur；

所以有：

令：

则式(3)可以写成：

r'＝Dr； (5)

记初始时刻的刚体固联坐标系为b₀，由于初始时刻刚体固联坐标系b与参考坐标系R重合，所以有：

在转动过程中位置向量和b系都和刚体固联，所以位置向量和b系的相对角位置始终不变，即有：

所以得到：

r＝r'^b； (8)

将式(8)带入式(5)得：

r'＝Dr'^b； (9)

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

即：

令：

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

其中，i,j,k既是相互正交的单位向量，又是虚数单位

将q₀,q₁,q₂,q₃代入式(10)中进一步化简得：

设参考坐标系为导航坐标系n，与刚体固联的坐标系为机体坐标系，那么坐标变换矩阵

就是姿态矩阵

在机体轴坐标系下，无人机先绕x轴转动横滚角α，再绕y轴转动俯仰角β，最后再绕z轴转动航向角γ；则在此坐标系下的变换矩阵为：

对比式(13)和式(14)可得到三个姿态角：

β＝-arcsinT₃₁＝-arcsin2(q₁q₃-q₀q₂)；

其中，T₁₁为cosβcosγ，T₂₁为cosβsinγ，T₃₁为-sinβ，T₃₂为sinαcosβ，T₃₃为cosαcosβ；

设无人机原平面的法向量为

旋转之后所得平面的法向量为：

所以无人机的倾角θ为：

θ＝arccos(cosαcosβ)； (17)

由超声波测风原理可以得到风速ν为：

其中，t₁、t₂为x轴方向传播时间，t₃、t₄为y轴方向传播时间，

风向

为：

其中，k为整数，公式后面标了各种情况下的k取什么值。

图2为风向分解示意图，1为无人机平面，2为水平面，3为风向，4为风向沿无人机平面的分量，5为风向垂直无人机平面的分量，6为无人机平面与水平面的交线。

当无人机处于悬停状态下时，GPS测得的无人机速度大小为0，此时风向3即为实际的风向。将风分解为与无人机平面平行的分量和垂直的分量，垂直分量对超声波风速风向仪无影响，所以超声波风速风向仪测得的风为平行分量，测得的风向角与原风向角一致，所以修正的结果为：

其中，v_实际为实际风速，

为实际风向，γ为航向角。

当无人机处于前进状态时，GPS测得无人机前进的速度大小为v₁，无人机以一定的速度前进，等效于无人机悬停时，同样大小的风沿着无人机前进方向的反方向吹来。超声波风速风向仪测量的速度大小为v，此时的v_合为无人机前进状态下等效的风速v₁与实际要测的风速v₂的合成风，则：

其中，v为超声波风速风向仪测量的速度大小，

为无人机前进状态下等效的风速，

为实际要测的风速。

在机体坐标系下，无论无人机平面如何旋转都会过原点，所以无人机平面的方程为：

Ax+By+Cz＝0

又因为其法向量为：

求得：

令z＝0可得交线方程为：

Ax+By＝0； (25)

图3为B＞0时的平面合风向分解示意图，合成风v_合分解为

和

为无人机前进状态下的等效风速，

为实际要测的风速，

为合风的风向角，θ₂为实际风的风向角。合风的坐标为

等效风速的坐标为

由式(23)可得

实际风速大小为：

在这种情况下求出的风向角为：

图4为B＜0时的平面合风向分解示意图，此时要求的风向角为负，同理可得：

特别的，当A＝0,B＞0时，

此时

当A＝0,B＜0时，

此时

故综上所述，可得：

将θ₂转化为[0,2π)内的角度，即

其中，θ′₂为求出的风向角转化为[0,2π)内的角；

随着时间的变化，某时刻的风向为：

其中γ为航向角。

以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

Claims (3)

1.一种基于超声波风速风向仪的无人机风速风向测量及校正算法，包括两种状态的风速风向测量及校正算法，一种是无人机悬停状态下的风速风向测量及校正算法，另一种是无人机前进状态下的风速风向测量及校正算法，其特征在于：两种风速风向测量及校正算法都是利用四元数与姿态阵之间的关系来得出无人机的三个欧拉角，进而得到无人机的倾角，通过倾角的大小来对超声波风速风向仪测得的风速风向结果进行校正；

设有一参考坐标系R，简称R系，一刚体相对于R系作定点转动，定点为O；选取坐标系b与刚体固联，坐标系b简称b系，假设初始时刻b系与R系重合，设OA＝r为起始位置向量，OA'＝r'为旋转后的向量；根据欧拉定理，仅仅考虑初始时刻与最终时刻的位置，刚体从A位置转至A'位置等效成绕单位瞬轴

转过θ₁角度一次完成；将向量分解、旋转、合成可以得到：

r'＝rcosθ₁+(1-cosθ₁)(u·r)u+u×rsinθ₁；(1)

由三重矢积公式变换得：

r'＝r+u×rsinθ₁+(1-cosθ₁)u×(u×r)；(2)

记

所以有

令：

则：

u×r＝Ur；

u×(u×r)＝U·Ur；

所以有：

令：

则式(3)可以写成：

r'＝Dr； (5)

记初始时刻的刚体固联坐标系为b₀，由于初始时刻刚体固联坐标系b与参考坐标系R重合，所以有：

在转动过程中位置向量和b系都和刚体固联，所以位置向量和b系的相对角位置始终不变，即有：

所以得到：

r＝r'^b； (8)

将式(8)带入式(5)得：

r'＝Dr'^b； (9)

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

即：

令：

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

其中，i,j,k既是相互正交的单位向量，又是虚数单位

将q₀,q₁,q₂,q₃代入式(10)中进一步化简得：

设参考坐标系为导航坐标系n，与刚体固联的坐标系为机体坐标系，那么坐标变换矩阵

就是姿态矩阵

在机体轴坐标系下，无人机先绕x轴转动横滚角α，再绕y轴转动俯仰角β，最后再绕z轴转动航向角γ；则在此坐标系下的变换矩阵为：

对比式(13)和式(14)可得到三个姿态角：

β＝-arcsinT₃₁＝-arcsin2(q₁q₃-q₀q₂)；

其中，T₁₁为cosβcosγ，T₂₁为cosβsinγ，T₃₁为-sinβ，T₃₂为sinαcosβ，T₃₃为cosαcosβ；

设无人机原平面的法向量为

旋转之后所得平面的法向量为：

所以无人机的倾角θ为：

θ＝arccos(cosαcosβ)；(17)

由超声波测风原理可以得到风速v为：

其中，t₁、t₂为x轴方向传播时间，t₃、t₄为y轴方向传播时间，

风向

为：

其中，k为整数，公式后面标了各种情况下的k取什么值。

2.根据权利要求1所述的基于超声波风速风向仪的无人机风速风向测量及校正算法，其特征在于：当无人机处于悬停状态下时，无人机内内置的GPS测得的无人机速度大小为0，此时超声波风速风向仪所测得的风向与实际风向一致；将风分解为与无人机平面平行的分量和垂直的分量，垂直分量对超声波风速风向仪无影响，所以超声波风速风向仪测得的风为平行分量，测得的风向角与原风向角一致，所以修正的结果为：

其中，

为实际风速，

为实际风向，γ为航向角。

3.根据权利要求1所述的基于超声波风速风向仪的无人机风速风向测量及校正算法，其特征在于：当无人机处于前进状态时，无人机内内置的GPS测得无人机前进的速度大小为v₁，无人机以一定的速度前进，等效于无人机悬停时，同样大小的风沿着无人机前进方向的反方向吹来；超声波风速风向仪测量的速度大小为v，此时的v_合为无人机前进状态下等效的风速v₁与实际要测的风速v₂的合成风，则：

其中,v为超声波风速风向仪测量的速度大小，

为无人机前进状态下等效的风速，

为实际要测的风速；

在机体坐标系下，无论无人机平面如何旋转都会过原点，所以无人机平面的方程为：

Ax+By+Cz＝0

又因为其法向量为：

求得：

令z＝0可得交线方程为：

Ax+By＝0； (25)

当B＞0时，合成风v_合分解为

和

为无人机前进状态下的等效风速，

为实际要测的风速，

为合风的风向角，θ₂为实际风的风向角；合风的坐标为

等效风速的坐标为

由式(23)可得：

实际风速大小为：

在这种情况下求出的实际风的风向角θ₂为：

当B＜0时的平面合风向分解示意图，此时要求的风向角为负，同理可得：

特别的，当A＝0,B＞0时，

此时

θ₂＝0；

当A＝0,B＜0时，

此时

θ₂＝π；

故综上所述，可得：

将θ₂转化为[0,2π)内的角度，即：

其中，θ′₂为求出的风向角转化为[0,2π)内的角；

随着时间的变化，某时刻的风向为：

其中γ为航向角。

Images