CN114279446A - 飞行汽车航姿测量方法、装置及飞行汽车 - Google Patents
飞行汽车航姿测量方法、装置及飞行汽车 Download PDFInfo
- Publication number
- CN114279446A CN114279446A CN202111578204.XA CN202111578204A CN114279446A CN 114279446 A CN114279446 A CN 114279446A CN 202111578204 A CN202111578204 A CN 202111578204A CN 114279446 A CN114279446 A CN 114279446A
- Authority
- CN
- China
- Prior art keywords
- attitude
- accelerometer
- preset
- acceleration
- magnetometer
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000691 measurement method Methods 0.000 title claims abstract description 18
- 238000005259 measurement Methods 0.000 claims abstract description 130
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 68
- 238000004364 calculation method Methods 0.000 claims abstract description 33
- 238000000034 method Methods 0.000 claims abstract description 32
- 238000012545 processing Methods 0.000 claims abstract description 24
- 230000001133 acceleration Effects 0.000 claims description 81
- 238000001914 filtration Methods 0.000 claims description 43
- 238000012937 correction Methods 0.000 claims description 41
- 230000000295 complement effect Effects 0.000 claims description 29
- 230000005484 gravity Effects 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 description 16
- 238000010586 diagram Methods 0.000 description 14
- 230000008569 process Effects 0.000 description 10
- 238000004590 computer program Methods 0.000 description 4
- 230000002085 persistent effect Effects 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 239000002355 dual-layer Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
本申请涉及一种飞行汽车航姿测量方法、装置及飞行汽车。该飞行汽车航姿测量方法，包括：获取飞行汽车的惯导设备采集的惯性测量值；采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果；根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。本申请提供的技术方案，能够提高飞行汽车航姿信息输出的准确性和稳定性。
Description
技术领域
本申请涉及飞行器技术领域，尤其涉及一种飞行汽车航姿测量方法、装置及飞行汽车。
背景技术
随着科技的进步，人们出行方式也逐步趋向于多元化，飞行汽车在不久的将来，也会成为人们出行的代步工具。
在对飞行汽车进行航向控制时，需要获得飞行汽车的航姿信息，在相关的技术方案中，通过时通过卡尔曼滤波或互补滤波算法的方式对飞行汽车的航姿进行结算。其中，卡尔曼滤波算法在解算过程中存在初始时刻收敛较慢，计算量较大，同时还可能会存在发散的情况；而传统互补滤波算法，不同的应用平台需要对参数重新进行测定，并且解算过程中会存在一定的超调量。
由此可见，在解算飞行汽车航姿信息的相关技术方案中，分别存在计算量大、容易发散和受应用平台场景影响等问题，导致输出的飞行汽车航姿信息的准确性和稳定性受到影响。
发明内容
为解决或部分解决相关技术中存在的问题，本申请提供一种飞行汽车航姿测量方法、装置及飞行汽车，能提高飞行汽车航姿信息输出的准确性和稳定性。
本申请第一方面提供一种飞行汽车航姿测量方法，包括：
获取飞行汽车的惯导设备采集的惯性测量值；
采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果；
根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。
在一实施方式中，所述采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果，包括：
采用拓展卡尔曼滤波算法对所述惯性测量值进行运算，得到第一姿态运算结果；
采用互补滤波算法对所述惯性测量值进行运算，得到第二姿态运算结果。
在一实施方式中，所述根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，包括：
在所述惯导设备包括加速度计时，如果所述加速度计的加速度测量值大于预设的加速度阈值，将所述第一姿态运算结果作为飞行汽车的姿态信息。
在一实施方式中，所述根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，包括：
在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量大于预设的误差阈值时，将所述第二姿态运算结果作为飞行汽车的姿态信息。
在一实施方式中，所述根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，包括：
在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量小于或等于预设的误差阈值时，按照预设权重将所述第一姿态运算结果和所述第二姿态运算结果进行融合，得到所述飞行汽车的姿态信息。
在一实施方式中，所述根据所述加速度计和所述磁力计得到的误差修正量按以下方式确定：
通过所述加速度计确定所述飞行汽车的加速度测量值，通过所述磁力计确定所述飞行汽车所在地的地磁测量值；
基于所述加速度测量值和标准重力加速度确定加速度误差修正量，基于所述地磁测量值和理论地磁确定地磁误差修正量；
将所述加速度误差修正量与所述地磁误差修正量进行求和运算，得到所述加速度计和所述磁力计的误差修正量。
本申请第二方面提供一种飞行汽车航姿测量装置，包括：
测量模块，用于获取飞行汽车的惯导设备采集的惯性测量值；
运算模块，用于采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果；
决策模块，用于根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。
在一实施方式中，所述运算模块包括：
第一运算子模块，用于采用拓展卡尔曼滤波算法对所述惯性测量值进行运算，得到第一姿态运算结果；
第二运算子模块，用于采用互补滤波算法对所述惯性测量值进行运算，得到第二姿态运算结果。
在一实施方式中，所述决策模块包括：
第一决策子模块，用于在所述惯导设备包括加速度计时，如果所述加速度计的加速度测量值大于预设的加速度阈值，将所述第一姿态运算结果作为飞行汽车的姿态信息；或，
第二决策子模块，用于在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量大于预设的误差阈值时，将所述第二姿态运算结果作为飞行汽车的姿态信息；
第三决策子模块，用于在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量小于或等于预设的误差阈值时，按照预设权重将所述第一姿态运算结果和所述第二姿态运算结果进行融合，得到所述飞行汽车的姿态信息。
本申请第三方面提供一种飞行汽车，包括：
处理器；以及
存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如上所述的方法。
本申请提供的技术方案可以包括以下有益效果：
本申请采用两个或两个以上预设算法对所述惯性测量值分别进行运算，最后根据预设决策规则对两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，这样能够充分利用不同算法的优点进行互补，从而提高飞行汽车航姿信息输出的准确性和稳定性，为飞行汽车的航向控制提供稳定的数据源，保证飞行汽车行驶的安全性。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
通过结合附图对本申请示例性实施方式进行更详细地描述，本申请的上述以及其它目的、特征和优势将变得更加明显，其中，在本申请示例性实施方式中，相同的参考标号通常代表相同部件。
图1是本申请实施例示出的飞行汽车航姿测量方法的流程示意图；
图2是本申请实施例示出的飞行汽车航姿测量方法的另一流程示意图；
图3是本申请实施例示出的飞行汽车航姿测量方法的应用框图；
图4是本申请实施例示出的飞行汽车航姿测量方法的另一流程示意图；
图5是本申请实施例示出的飞行汽车航姿测量方法中的拓展卡尔曼滤波算法的示意图；
图6是本申请实施例示出的飞行汽车航姿测量方法中的拓互补滤波算法的示意图；
图7是本申请实施例示出的飞行汽车航姿测量装置的结构示意图；
图8是本申请实施例示出的飞行汽车航姿测量装置的另一结构示意图；
图9是本申请实施例示出的飞行汽车的结构示意图；
图10是本申请实施例示出的飞行汽车的另一结构示意图。
具体实施方式
下面将参照附图更详细地描述本申请的实施方式。虽然附图中显示了本申请的实施方式，然而应该理解，可以以各种形式实现本申请而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了使本申请更加透彻和完整，并且能够将本申请的范围完整地传达给本领域的技术人员。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语“第一”、“第二”、“第三”等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
相关技术关于解算飞行汽车航姿信息的技术方案中，分别存在计算量大、容易发散和受应用平台场景影响等问题，导致输出的飞行汽车航姿信息的准确性和稳定性受到影响。本申请提供一种飞行汽车航姿测量方法，能提高飞行汽车航姿信息输出的准确性和稳定性。
以下结合附图详细描述本申请实施例的技术方案。
图1是本申请实施例示出的飞行汽车航姿测量方法的流程示意图。
参见图1，该方法包括：
S101，获取飞行汽车的惯导设备采集的惯性测量值。
其中，飞行汽车的惯导设备例如可以是惯性测量单元但不局限于此。该步骤可以获取飞行汽车的惯性测量单元采集的惯性测量值。
S102，采用两个或两个以上预设算法对惯性测量值分别进行运算，得到两个或两个以上姿态运算结果。
其中，预设算法例如可以是拓展卡尔曼滤波算法或互补滤波算法等。采用两个或两个以上预设算法对惯性测量值分别进行运算，可以分别利用不同算法的优点。
S103，根据预设决策规则对两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。
其中，预设决策规则可以根据加速度计的加速度测量值与预设的加速度阈值的比较结果和/或根据加速度计和磁力计的误差修正量与预设的误差阈值的比较结果进行不同的决策处理，在不同情况下分别决策为：将第一姿态运算结果作为飞行汽车的姿态信息，或将第二姿态运算结果作为飞行汽车的姿态信息，或按照预设权重将第一姿态运算结果和第二姿态运算结果进行融合，得到飞行汽车的姿态信息。
从该实施例可以看出，本申请采用两个或两个以上预设算法对惯性测量值分别进行运算，最后根据预设决策规则对两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，这样能够充分利用不同算法的优点进行互补，从而提高飞行汽车航姿信息输出的准确性和稳定性，为飞行汽车的航向控制提供稳定的数据源，保证飞行汽车行驶的安全性。
图2是本申请实施例示出的飞行汽车航姿测量方法的另一流程示意图，图3是本申请实施例示出的飞行汽车航姿测量方法的应用框图。
参见图2和图3，该方法包括：
S201，获取飞行汽车的惯性测量单元采集的惯性测量值。
在本申请实施例中，以惯导设备为惯性测量单元为例说明。飞行汽车的惯性测量单元可以用于测量车辆的惯性测量值。可选的，惯性测量单元可以包括三种传感器：陀螺仪、加速度计和磁力计。
该步骤可以获取飞行汽车的陀螺仪、加速度计和磁力计采集的惯性测量值。
S202，采用拓展卡尔曼滤波算法对惯性测量值进行运算，得到第一姿态运算结果。
该步骤采用拓展卡尔曼滤波算法对陀螺仪、加速度计和磁力计采集输入的惯性测量值进行运算，得到第一姿态运算结果。
该步骤可以包括：
确定陀螺仪的偏置、加速度计的测量噪声；
基于陀螺仪的偏置采用四元数微分方程确定飞行汽车的姿态状态向量；
采用拓展卡尔曼滤波算法对姿态状态向量进行更新，并基于更新后的姿态状态向量计算飞行汽车的欧拉角；
基于测量噪声构建飞行汽车的加速度测量方程，采用加速度测量方程测量飞行汽车的加速度；
通过磁力计确定飞行汽车的航向角的测量值，并通过预设的航向角状态方程和航向角测量算法对航向角测量值进行修正，得到飞行汽车的航向角。
S203，采用互补滤波算法对惯性测量值进行运算，得到第二姿态运算结果。
该步骤采用互补滤波算法对陀螺仪、加速度计和磁力计采集输入的惯性测量值进行运算，得到第二姿态运算结果。
该步骤可以包括：
通过加速度计确定飞行汽车的加速度测量值，基于加速度测量值和标准加速度计算加速度测量值的误差；
通过磁力计确定飞行汽车所在地的地磁测量值，基于地磁测量值和理论地磁计算地磁测量值的误差；
基于加速度测量值的误差和地磁测量值的误差，采用预设的补偿算法，结合陀螺仪的偏置求解四元数，并采用四元数微分方程计算飞行车辆的欧拉角。
另外，当加速度测量值大于预设阈值时，还可以获取车载磁力计测量的地磁测量值，基于地磁测量值计算飞行汽车的航向角；通过陀螺仪确定飞行汽车的转向角。
S204，根据预设决策规则对第一姿态运算结果和第二姿态运算结果进行决策处理，确定飞行汽车的姿态信息。
其中，预设决策规则可以根据加速度计的加速度测量值与预设的加速度阈值的比较结果和/或根据加速度计和磁力计的误差修正量与预设的误差阈值的比较结果进行不同的决策处理，在不同情况下分别决策为：将第一姿态运算结果作为飞行汽车的姿态信息，或将第二姿态运算结果作为飞行汽车的姿态信息，或按照预设权重将第一姿态运算结果和第二姿态运算结果进行融合，得到飞行汽车的姿态信息。
例如：
在惯导设备包括加速度计时，如果加速度计的加速度测量值大于预设的加速度阈值，将第一姿态运算结果作为飞行汽车的姿态信息；或，
在惯导设备包括加速度计和磁力计时，如果加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据加速度计和磁力计得到的误差修正量大于预设的误差阈值时，将第二姿态运算结果作为飞行汽车的姿态信息；或，
在惯导设备包括加速度计和磁力计时，如果加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据加速度计和磁力计得到的误差修正量小于或等于预设的误差阈值时，按照预设权重将第一姿态运算结果和第二姿态运算结果进行融合，得到飞行汽车的姿态信息。
S205，输出确定的飞行汽车的姿态信息。
该步骤中，输出经过决策处理得到的飞行汽车的姿态信息。
图4是本申请实施例示出的飞行汽车航姿测量方法的另一流程示意图。图4相对于图1和图2更详细描述了本申请技术方案。
本申请的航姿测量***，可以为飞行汽车提供飞行状态下的实时姿态信息，是飞行汽车自动驾驶***的重要模块。本申请利用陀螺仪、加速度计和磁力计等传感器实时采集的数据，通过EKF(ExtendedKalmanFilter，扩展卡尔曼滤波)算法和互补滤波算法分别进行姿态解算，从而得到两组解算结果。然后再通过数据输出决策，最终输出一个精度更高、更可靠的飞行汽车姿态信息。
参见图4，该方法包括：
S401，获取飞行汽车的陀螺仪、加速度计和磁力计采集的惯性测量值。
在本申请实施例中，以惯导设备为惯性测量单元为例说明。飞行汽车的惯性测量单元可以用于测量车辆的惯性测量值。可选的，惯性测量单元可以包括三种传感器：陀螺仪、加速度计和磁力计。
在本申请实施例中，陀螺仪、加速度计以及磁力计在使用前可以进行标定，标定得到的误差参数将在后续姿态解算中使用。在本申请实施例中，陀螺仪用于测量飞行汽车的角速度，其中，该角速度包括飞行汽车三个维度的角速度，包括ωx、ωy以及ωz；加速度计用于测量飞行汽车的加速度，该加速度包括飞行汽车三个维度的加速度，包括ax、ay以及az；磁力计用于测量飞行汽车所在地的三个维度的地磁强度，包括Hx、Hy以及Hz。
在通过惯性测量单元采集测量飞行汽车的惯性测量值时，可以对三个传感器采集的数据进行实时修正，保证数据采集的准确性。
S402，采用拓展卡尔曼滤波算法对惯性测量值进行运算，得到第一姿态运算结果。
航姿测量***是一个非线性时变***。扩展卡尔曼滤波技术就是将非线性状态方程和量测方程通过一阶泰勒多项式展开，实现局部线性化，再利用卡尔曼滤波来实现的。该步骤具体的实现流程可以参见图5所示，图5是本申请实施例示出的飞行汽车航姿测量方法中的拓展卡尔曼滤波算法的示意图。
图5中公式中，其中X0为初始状态，P0为初始协方差，Q0为初始***噪声方差阵，
为k-1时刻对k时刻的状态预测值，
为k-1时刻的状态预测值，Pk/k-1为k-1时刻对k时刻的预测误差估计的协方差阵，Pk-1为k-1时刻实时误差估计协方差阵，Qk-1为k-1时刻的***噪声方差阵，Kk为k时刻的滤波增益阵，Rk为量测***的噪声方差阵，
为k时刻的状态预测值，Pk为k时刻的协方差阵，Φk/k-1表示状态一步转移矩阵，
表示量测矩阵的转置，Hk表示量测矩阵，Zk表示量测值，I表示单位矩阵。
扩展卡尔曼滤波算法可以分为预测过程和更新过程，其中预测过程分为状态预测和协方差矩阵预测；更新过程则包含了增益更新、状态更新和协方差矩阵更新。其中初始化部分可以由传感器的初值进行确定。需说明的是，这里的***噪声和观测噪声为高斯白噪声。
该步骤中，可以选取东北天坐标系为导航坐标系，根据获得的三种传感器(陀螺仪、加速度计和磁力计)的数据，利用扩展卡尔曼滤波进行姿态解算，获得姿态信息。
关于基于EKF的姿态解算过程如下所示：
基于四元数微分方程，加入陀螺仪的偏置，选择四元数和偏置作为状态向量[q b]T,建立***状态方程：
其中陀螺仪的偏置可以通过标定获取。
对上述方程进行离散化处理，可以得到如下的矩阵形式：
其中，I表示单位矩阵，ρ表示状态转移矩阵，T为更新周期，qk为时刻k的四元数，bk为时刻k的陀螺仪偏置。该矩阵是状态转移矩阵，会在EKF中更新先验状态时应用。
其中，q是状态向量，不同时刻会有不同的结果，所以会有k时刻的q，四元数初始时刻的值可以手动赋值，也可以根据当前的欧拉角进行计算得到，后续四元数可以根据四元数微分方程更新得到。
将加速度计测量得到的数据进行转换，得到***的测量方程：
Z＝f(xk)+W
其中，Z表示加速计的测量值，f(xk)表示量测方程，W表示量测噪声，该噪声可以通过数据手册获得。Z值在后续EKF更新后验状态时使用。
上面通过EKF算法得到的四元数经过计算可以得到欧拉角，通过磁力计可以得到航向角，所以这里再次建立状态方程和测量方程对航向角进行估计修正。
其中，ψ为航向角，b为陀螺仪Z轴偏置，w为陀螺仪角速度，T为更新周期，m和k表示时刻，ψm为m时刻的航向角，ψk为k时刻的航向角。
由此，可以得到航姿测量***的三个姿态角信息
EKF姿态解算的初始时刻需要一段时间收敛，这时可以将互补滤波算法解算得到的结果反馈给EKF算法，帮助其快速收敛，获得更加精确的姿态信息。
S403，采用互补滤波算法对惯性测量值进行运算，得到第二姿态运算结果。
互补滤波主要是将多个传感器数据结合互补，进行滤波处理后输出结果。航姿测量***中陀螺仪具有高频特性，动态响应特性较好；而加速度计和磁力计具有低频特性，动态响应特性较差。随着时间的积累，陀螺仪由于积分作用和温度漂移会使得计算得到的角度存在累积误差，而加速度计和磁力计则不会存在这样的问题。但是，加速度计对加速度较为敏感，通过瞬时值计算得到的角度误差也比较大。根据两者的以上特点，可以采用互补滤波融合这三种传感器的数据来获得更加精确的姿态信息，以及更好的动态性能。
本申请提出基于自适应互补滤波的姿态解算。本申请所选择的互补滤波解算方法，可以根据加速度计的测量值作为判定条件，实现不同飞行场景的自适应互补滤波解算，这样可以获得更加精确的姿态信息。该步骤具体的实现流程可以参见图6所示，图6是本申请实施例示出的飞行汽车航姿测量方法中的拓互补滤波算法的示意图。
本申请中，加速度计的标准重力加速度[0 0 1]，变换到机体坐标系后，和实际采集到的数据进行叉乘便可以得到误差；而磁力计在地磁坐标系是不相同的，所以需要时刻更新，将机体坐标系转化为地理坐标系，然后求出理论的地磁，再转化到机体坐标系，将理论值和实际值继续叉乘求出误差。最后，将求得的两个误差进行累加，利用PI(ProportionIntegration，比例积分)补偿到陀螺仪求解出四元数，再计算出欧拉角。其中利用四元数计算欧拉角，是根据四元数和欧拉角转换的计算公式得到的。
在互补滤波解算过程中，由于在高动态场景，加速度计动态响应慢，因此可以根据加速度计的输出结果，设置不同的PI值。对于短时间动态变化较大的场景，可以选择不通过互补滤波算法的解算，而是直接输出陀螺仪和磁力计融合的结果作为最终的姿态角。这种处理方式也可以提升姿态估计的精度。
上述利用EKF算法和互补滤波算法分别解算后，可以得到两个姿态信息。
S404，判断加速度计的加速度测量值是否大于预设的加速度阈值，如果是，进入S405，如果否，进入S406。
S405，将第一姿态运算结果作为飞行汽车的姿态信息，进入S411。
在互补滤波算法中，高动态场景中，由于加速度计动态响应较慢，所以可以设置加速度阈值，例如设置加速度阈值为1，如果在加速度计的加速度测量值大于加速度阈值的场景，可以直接将扩展卡尔曼滤波算法的运算结果，作为整个***的结果输出。
S406，根据加速度计和磁力计得到误差修正量。
根据加速度计和磁力计得到的误差修正量可以按以下方式确定：
通过加速度计确定飞行汽车的加速度测量值，通过磁力计确定飞行汽车所在地的地磁测量值；基于加速度测量值和标准重力加速度确定加速度误差修正量，基于地磁测量值和理论地磁确定地磁误差修正量；将加速度误差修正量与地磁误差修正量进行求和运算，得到加速度计和磁力计的误差修正量。
S407，判断误差修正量是否大于预设的误差阈值，如果是，进入S408，如果否，进入S409。
S408，将第二姿态运算结果作为飞行汽车的姿态信息，进入S411。
进一步的，在小于或等于加速度阈值的场景，根据加速度计和磁力计计算得到的误差修正量进行判定。本申请可以设置误差阈值，例如设置误差阈值为2。当误差修正量大于误差阈值时，此时对陀螺仪的修正量较大，判定互补滤波解算得到的结果更加精确，可以将互补滤波算法的运算结果，作为整个***的结果输出。其中，加速计误差修正量可以通过理论重力加速度向量和实际重力加速度向量叉乘得到，磁力计误差修正量可以通过机体坐标系下的理论地磁向量与实际地磁向量叉乘得到。
S409，确定两个算法融合的预设权重。
如果误差修正量小于或等于误差阈值，此时可以对两个算法的权重进行设置，例如设置权重分别为0.5但不局限于此。
S410，按照预设权重将第一姿态运算结果和第二姿态运算结果进行融合，得到飞行汽车的姿态信息，进入S411。
该步骤，按照预设权重将第一姿态运算结果和第二姿态运算结果进行加权融合，得到一个更加可靠、精度更高的姿态解算的运算结果。
S411，输出飞行汽车的姿态信息。
该步骤将数据输出决策得到的结果，作为航姿测量***的解算结果，输出给飞行汽车的自动驾驶***。
需说明的是，***中的EKF算法也可以用UKF(Unscented Kalman Filter，无迹卡尔曼滤波)进行替代，或者还可以进一步加入梯度下降法进行解算。
综上所描述，本申请提出了多算法融合的飞行汽车航姿测量方法，可以输出更高精度、更可靠的姿态信息，可以保证互补滤波算法在不同的工作场景输出更高精度、误差更小的姿态信息。
与前述应用功能实现方法实施例相对应，本申请还提供了一种飞行汽车航姿测量装置、飞行汽车及相应的实施例。
图7是本申请实施例示出的飞行汽车航姿测量装置的结构示意图。
参见图7，飞行汽车航姿测量装置70包括：测量模块71、运算模块72、决策模块73。
测量模块71，用于获取飞行汽车的惯导设备采集的惯性测量值。其中，飞行汽车的惯导设备例如可以是惯性测量单元但不局限于此。该步骤可以获取飞行汽车的惯性测量单元采集的惯性测量值。
运算模块72，用于采用两个或两个以上预设算法对惯性测量值分别进行运算，得到两个或两个以上姿态运算结果。其中，预设算法例如可以是拓展卡尔曼滤波算法或互补滤波算法等。采用两个或两个以上预设算法对惯性测量值分别进行运算，可以分别利用不同算法的优点。
决策模块73，用于根据预设决策规则对两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。其中，预设决策规则可以根据加速度计的加速度测量值与预设的加速度阈值的比较结果和/或根据加速度计和磁力计的误差修正量与预设的误差阈值的比较结果进行不同的决策处理，在不同情况下分别决策为：将第一姿态运算结果作为飞行汽车的姿态信息，或将第二姿态运算结果作为飞行汽车的姿态信息，或按照预设权重将第一姿态运算结果和第二姿态运算结果进行融合，得到飞行汽车的姿态信息。
从该实施例可以看出，本申请的飞行汽车航姿测量装置，采用两个或两个以上预设算法对惯性测量值分别进行运算，最后根据预设决策规则对两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，这样能够充分利用不同算法的优点进行互补，从而提高飞行汽车航姿信息输出的准确性和稳定性，为飞行汽车的航向控制提供稳定的数据源，保证飞行汽车行驶的安全性。
图8是本申请实施例示出的飞行汽车航姿测量装置的另一结构示意图
参见图8，飞行汽车航姿测量装置70包括：测量模块71、运算模块72、决策模块73。
测量模块71、运算模块72、决策模块73的功能可以参见图7中的描述。
在一实施方式中，运算模块72包括：第一运算子模块721、第二运算子模块722。
第一运算子模块721，用于采用拓展卡尔曼滤波算法对惯性测量值进行运算，得到第一姿态运算结果。
第二运算子模块722，用于采用互补滤波算法对惯性测量值进行运算，得到第二姿态运算结果。
在一实施方式中，决策模块73包括：第一决策子模块731、第二决策子模块732、第三决策子模块733。
第一决策子模块731，用于在惯导设备包括加速度计时，如果加速度计的加速度测量值大于预设的加速度阈值，将第一姿态运算结果作为飞行汽车的姿态信息。
第二决策子模块732，用于在惯导设备包括加速度计和磁力计时，如果加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据加速度计和磁力计得到的误差修正量大于预设的误差阈值时，将第二姿态运算结果作为飞行汽车的姿态信息。
第三决策子模块733，用于在惯导设备包括加速度计和磁力计时，如果加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据加速度计和磁力计得到的误差修正量小于或等于预设的误差阈值时，按照预设权重将第一姿态运算结果和第二姿态运算结果进行融合，得到飞行汽车的姿态信息。
图9是本申请实施例示出的飞行汽车的结构示意图；
参见图9，本申请提供一种飞行汽车90，包括飞行汽车航姿测量装置70，飞行汽车航姿测量装置70的结构可以参见图7或图8中的描述。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不再做详细阐述说明。
图10是本申请实施例示出的飞行汽车的另一结构示意图。
参见图10，飞行汽车1000包括存储器1010和处理器1020。
处理器1020可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器1010可以包括各种类型的存储单元，例如***内存、只读存储器(ROM)和永久存储装置。其中，ROM可以存储处理器1020或者计算机的其他模块需要的静态数据或者指令。永久存储装置可以是可读写的存储装置。永久存储装置可以是即使计算机断电后也不会失去存储的指令和数据的非易失性存储设备。在一些实施方式中，永久性存储装置采用大容量存储装置(例如磁或光盘、闪存)作为永久存储装置。另外一些实施方式中，永久性存储装置可以是可移除的存储设备(例如软盘、光驱)。***内存可以是可读写存储设备或者易失性可读写存储设备，例如动态随机访问内存。***内存可以存储一些或者所有处理器在运行时需要的指令和数据。此外，存储器1010可以包括任意计算机可读存储媒介的组合，包括各种类型的半导体存储芯片(例如DRAM，SRAM，SDRAM，闪存，可编程只读存储器)，磁盘和/或光盘也可以采用。在一些实施方式中，存储器1010可以包括可读和/或写的可移除的存储设备，例如激光唱片(CD)、只读数字多功能光盘(例如DVD-ROM，双层DVD-ROM)、只读蓝光光盘、超密度光盘、闪存卡(例如SD卡、min SD卡、Micro-SD卡等)、磁性软盘等。计算机可读存储媒介不包含载波和通过无线或有线传输的瞬间电子信号。
存储器1010上存储有可执行代码，当可执行代码被处理器1020处理时，可以使处理器1020执行上文述及的方法中的部分或全部。
此外，根据本申请的方法还可以实现为一种计算机程序或计算机程序产品，该计算机程序或计算机程序产品包括用于执行本申请的上述方法中部分或全部步骤的计算机程序代码指令。
或者，本申请还可以实施为一种计算机可读存储介质(或非暂时性机器可读存储介质或机器可读存储介质)，其上存储有可执行代码(或计算机程序或计算机指令代码)，当可执行代码(或计算机程序或计算机指令代码)被电子设备(或服务器等)的处理器执行时，使处理器执行根据本申请的上述方法的各个步骤的部分或全部。
以上已经描述了本申请的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其他普通技术人员能理解本文披露的各实施例。
Claims (10)
1.一种飞行汽车航姿测量方法，其特征在于，包括：
获取飞行汽车的惯导设备采集的惯性测量值；
采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果；
根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。
2.根据权利要求1所述的方法，其特征在于，所述采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果，包括：
采用拓展卡尔曼滤波算法对所述惯性测量值进行运算，得到第一姿态运算结果；
采用互补滤波算法对所述惯性测量值进行运算，得到第二姿态运算结果。
3.根据权利要求2所述的方法，其特征在于，所述根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，包括：
在所述惯导设备包括加速度计时，如果所述加速度计的加速度测量值大于预设的加速度阈值，将所述第一姿态运算结果作为飞行汽车的姿态信息。
4.根据权利要求2所述的方法，其特征在于，所述根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，包括：
在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量大于预设的误差阈值时，将所述第二姿态运算结果作为飞行汽车的姿态信息。
5.根据权利要求2所述的方法，其特征在于，所述根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息，包括：
在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量小于或等于预设的误差阈值时，按照预设权重将所述第一姿态运算结果和所述第二姿态运算结果进行融合，得到所述飞行汽车的姿态信息。
6.根据权利要求4或5所述的方法，其特征在于，所述根据所述加速度计和所述磁力计得到的误差修正量按以下方式确定：
通过所述加速度计确定所述飞行汽车的加速度测量值，通过所述磁力计确定所述飞行汽车所在地的地磁测量值；
基于所述加速度测量值和标准重力加速度确定加速度误差修正量，基于所述地磁测量值和理论地磁确定地磁误差修正量；
将所述加速度误差修正量与所述地磁误差修正量进行求和运算，得到所述加速度计和所述磁力计的误差修正量。
7.一种飞行汽车航姿测量装置，其特征在于，包括：
测量模块，用于获取飞行汽车的惯导设备采集的惯性测量值；
运算模块，用于采用两个或两个以上预设算法对所述惯性测量值分别进行运算，得到两个或两个以上姿态运算结果；
决策模块，用于根据预设决策规则对所述两个或两个以上姿态运算结果进行决策处理，确定飞行汽车的姿态信息。
8.根据权利要求7所述的装置，其特征在于，所述运算模块包括：
第一运算子模块，用于采用拓展卡尔曼滤波算法对所述惯性测量值进行运算，得到第一姿态运算结果；
第二运算子模块，用于采用互补滤波算法对所述惯性测量值进行运算，得到第二姿态运算结果。
9.根据权利要求8所述的装置，其特征在于，所述决策模块包括：
第一决策子模块，用于在所述惯导设备包括加速度计时，如果所述加速度计的加速度测量值大于预设的加速度阈值，将所述第一姿态运算结果作为飞行汽车的姿态信息；或，
第二决策子模块，用于在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量大于预设的误差阈值时，将所述第二姿态运算结果作为飞行汽车的姿态信息；
第三决策子模块，用于在所述惯导设备包括加速度计和磁力计时，如果所述加速度计的加速度测量值小于或等于预设的加速度阈值，则在根据所述加速度计和所述磁力计得到的误差修正量小于或等于预设的误差阈值时，按照预设权重将所述第一姿态运算结果和所述第二姿态运算结果进行融合，得到所述飞行汽车的姿态信息。
10.一种飞行汽车，其特征在于，包括：
处理器；以及
存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如权利要求1-6中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111578204.XA CN114279446B (zh) | 2021-12-22 | 2021-12-22 | 飞行汽车航姿测量方法、装置及飞行汽车 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111578204.XA CN114279446B (zh) | 2021-12-22 | 2021-12-22 | 飞行汽车航姿测量方法、装置及飞行汽车 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114279446A true CN114279446A (zh) | 2022-04-05 |
| CN114279446B CN114279446B (zh) | 2023-11-03 |
Family
ID=80874256
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111578204.XA Active CN114279446B (zh) | 2021-12-22 | 2021-12-22 | 飞行汽车航姿测量方法、装置及飞行汽车 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114279446B (zh) |
Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104567799A (zh) * | 2014-11-28 | 2015-04-29 | 天津大学 | 基于多传感器信息融合的小型旋翼无人机高度测量方法 |
| US20160131482A1 (en) * | 2014-11-10 | 2016-05-12 | MAGNETI MARELLI S.p.A. | Device for the detection of the attitude of auto-vehicles and corresponding method |
| CN105651242A (zh) * | 2016-04-05 | 2016-06-08 | 清华大学深圳研究生院 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
| CN105955302A (zh) * | 2016-06-20 | 2016-09-21 | 武汉理工大学 | 一种多旋翼无人机环境自主监测控制***及方法 |
| US20170003751A1 (en) * | 2015-06-30 | 2017-01-05 | Stmicroelectronics S.R.L. | Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus |
| CN108398128A (zh) * | 2018-01-22 | 2018-08-14 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
| CN109506646A (zh) * | 2018-11-20 | 2019-03-22 | 石家庄铁道大学 | 一种双控制器的无人机姿态解算方法及*** |
| CN111811507A (zh) * | 2020-04-08 | 2020-10-23 | 北京嘀嘀无限科技发展有限公司 | 移动设备姿态的确定方法、装置、存储介质及电子设备 |
| WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN112284376A (zh) * | 2020-10-10 | 2021-01-29 | 南京工程学院 | 一种基于多传感器融合的移动机器人室内定位建图方法 |
| CN112304330A (zh) * | 2020-10-29 | 2021-02-02 | 腾讯科技（深圳）有限公司 | 展示车辆的行驶状态的方法以及电子设备 |
| CN112298552A (zh) * | 2020-08-06 | 2021-02-02 | 北京航空航天大学 | 一种可自主增稳控制的微型双扑翼飞行器及其控制力矩产生方法 |
| CN112630813A (zh) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | 基于捷联惯导和北斗卫星导航***的无人机姿态测量方法 |
| CN112683269A (zh) * | 2020-12-07 | 2021-04-20 | 电子科技大学 | 一种附有运动加速度补偿的marg姿态计算方法 |
| WO2021078002A1 (zh) * | 2019-10-22 | 2021-04-29 | 深圳市道通智能航空技术有限公司 | 无人机航向角初值选取方法、装置及无人机 |
| CN112965505A (zh) * | 2021-01-29 | 2021-06-15 | 广东汇天航空航天科技有限公司 | 载人飞行器飞行姿态控制方法、***及载人飞行器 |
| CN113063416A (zh) * | 2021-02-05 | 2021-07-02 | 重庆大学 | 一种基于自适应参数互补滤波的机器人姿态融合方法 |
-
2021
- 2021-12-22 CN CN202111578204.XA patent/CN114279446B/zh active Active
Patent Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160131482A1 (en) * | 2014-11-10 | 2016-05-12 | MAGNETI MARELLI S.p.A. | Device for the detection of the attitude of auto-vehicles and corresponding method |
| CN104567799A (zh) * | 2014-11-28 | 2015-04-29 | 天津大学 | 基于多传感器信息融合的小型旋翼无人机高度测量方法 |
| US20170003751A1 (en) * | 2015-06-30 | 2017-01-05 | Stmicroelectronics S.R.L. | Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus |
| CN105651242A (zh) * | 2016-04-05 | 2016-06-08 | 清华大学深圳研究生院 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
| CN105955302A (zh) * | 2016-06-20 | 2016-09-21 | 武汉理工大学 | 一种多旋翼无人机环境自主监测控制***及方法 |
| CN108398128A (zh) * | 2018-01-22 | 2018-08-14 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
| CN109506646A (zh) * | 2018-11-20 | 2019-03-22 | 石家庄铁道大学 | 一种双控制器的无人机姿态解算方法及*** |
| WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| WO2021078002A1 (zh) * | 2019-10-22 | 2021-04-29 | 深圳市道通智能航空技术有限公司 | 无人机航向角初值选取方法、装置及无人机 |
| CN111811507A (zh) * | 2020-04-08 | 2020-10-23 | 北京嘀嘀无限科技发展有限公司 | 移动设备姿态的确定方法、装置、存储介质及电子设备 |
| CN112298552A (zh) * | 2020-08-06 | 2021-02-02 | 北京航空航天大学 | 一种可自主增稳控制的微型双扑翼飞行器及其控制力矩产生方法 |
| CN112284376A (zh) * | 2020-10-10 | 2021-01-29 | 南京工程学院 | 一种基于多传感器融合的移动机器人室内定位建图方法 |
| CN112304330A (zh) * | 2020-10-29 | 2021-02-02 | 腾讯科技（深圳）有限公司 | 展示车辆的行驶状态的方法以及电子设备 |
| CN112630813A (zh) * | 2020-11-24 | 2021-04-09 | 中国人民解放军国防科技大学 | 基于捷联惯导和北斗卫星导航***的无人机姿态测量方法 |
| CN112683269A (zh) * | 2020-12-07 | 2021-04-20 | 电子科技大学 | 一种附有运动加速度补偿的marg姿态计算方法 |
| CN112965505A (zh) * | 2021-01-29 | 2021-06-15 | 广东汇天航空航天科技有限公司 | 载人飞行器飞行姿态控制方法、***及载人飞行器 |
| CN113063416A (zh) * | 2021-02-05 | 2021-07-02 | 重庆大学 | 一种基于自适应参数互补滤波的机器人姿态融合方法 |
Non-Patent Citations (2)
| Title |
|---|
| YUTTHANA PITITEERAPHAB 等: "The effect of average filter for complementary filter and Kalman filter based on measurement angle", 《2016 9TH BIOMEDICAL ENGINEERING INTERNATIONAL CONFERENCE (BMEICON)》, pages 1 - 4 * |
| 张栋 等: "互补滤波和卡尔曼滤波的融合姿态解算方法", 传感器与微***, vol. 36, no. 3, pages 62 - 69 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114279446B (zh) | 2023-11-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11015957B2 (en) | Navigation system | |
| CN109001787A (zh) | 一种姿态角解算与定位的方法及其融合传感器 | |
| KR102340630B1 (ko) | 항법 시스템 | |
| CN106500693B (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
| US20090048779A1 (en) | Systems and methods for gyrocompass alignment using dynamically calibrated sensor data and an iterated extended kalman filter within a navigation system | |
| RU2762143C2 (ru) | Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области | |
| CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及*** | |
| CN112066985B (zh) | 一种组合导航***初始化方法、装置、介质及电子设备 | |
| US20160131482A1 (en) | Device for the detection of the attitude of auto-vehicles and corresponding method | |
| EP3748293B1 (en) | Systems and methods for compensating for the absence of a sensor measurement in a heading reference system | |
| CN114061570A (zh) | 车辆定位方法、装置、计算机设备和存储介质 | |
| JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| CN112046491B (zh) | 车轮侧偏刚度的估算方法、装置、车辆及可读存储介质 | |
| US11268813B2 (en) | Integrated inertial gravitational anomaly navigation system | |
| JP7028223B2 (ja) | 自己位置推定装置 | |
| CN115839718B (zh) | 一种基于运动约束的融合定位方法及装置 | |
| CN116559923A (zh) | 用于gnss天线杆臂和里程计杆臂的在线估计方法、设备及介质 | |
| CN113917512B (zh) | 自动驾驶车辆的定位方法、装置、电子设备及存储介质 | |
| CN112577513A (zh) | 一种状态量误差确定方法及车载终端 | |
| CN114279446B (zh) | 飞行汽车航姿测量方法、装置及飞行汽车 | |
| CN111649762B (zh) | 一种惯性多普勒全参量高精度标定方法及装置 | |
| CN113959433A (zh) | 一种组合导航方法及装置 | |
| Oei et al. | Attitude estimation for ground vehicles using low-cost sensors with in-vehicle calibration | |
| CN114353825B (zh) | 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及*** |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |