一种田间作业车辆外部加速度辨识与姿态同步估计方法技术

本发明专利技术公开了一种田间作业车辆外部加速度辨识与姿态同步估计方法，包括下述步骤：基于三轴陀螺仪与三轴加速度计的测量模型，推导外部加速度的一阶低通滤波模型，融合到卡尔曼滤波器；将三轴陀螺仪测量量ωX、ωY、ωZ作为卡尔曼滤波器的状态输入量；将三轴加速度计的测量量aX、aY、aZ作为滤波器的观测输入向量，进行外部加速度的修正；建立包含3个状态向量与3个观测向量的系统卡尔曼滤波状态方程和测量方程，精准测量田间作业车辆姿态角与辨识外部加速度。本发明专利技术方法考虑车辆外部加速度对姿态测量的影响，能有效提高田间作业车辆姿态测量精度，提升作业质量。

A Method of External Acceleration Identification and Attitude Synchronization Estimation for Field Operating Vehicles

The invention discloses a method for external acceleration identification and attitude synchronization estimation of field operation vehicles, which includes the following steps: based on the measurement model of triaxial gyroscope and triaxial accelerometer, the first-order low-pass filtering model of external acceleration is derived and integrated into Kalman filter; the measurement parameters_X, _Y and_Z of triaxial gyroscope are taken as the state inputs of Kalman filter;;X, _Y, _Z;;;;;; Axis accelerometer measurements aX, aY and aZ are used as input vectors of the filter to correct the external acceleration. The Kalman filter state equation and measurement equation including three state vectors and three observation vectors are established to accurately measure the vehicle attitude angle and identify the external acceleration. The method of the invention considers the influence of the external acceleration of the vehicle on the attitude measurement, and can effectively improve the accuracy of the field operation vehicle attitude measurement and the quality of the operation.

【技术实现步骤摘要】
一种田间作业车辆外部加速度辨识与姿态同步估计方法
本专利技术属于惯性导航的
，具体涉及一种田间作业车辆外部加速度辨识与姿态同步估计方法。
技术介绍
惯性导航技术是20世纪中期发展起来的完全自主式导航技术。通过惯性测量组件测量载体相对惯性空间的角速率与加速度信息，利用牛顿运动定律自动推算载体瞬时速度与位置信息，具有不依赖外界信息、不向外界辐射能量、不受干扰、隐蔽性好的特点。能连续地提供载体的全部导航、制导参数(位置、线速度、角速度、姿态角)，广泛运用于航天、航空、航海领域，特别是军事领域。而农机具姿态角的准确估量是实现农业机械精准作业的关键参数。对农业机械而言，如导航作业时，由于田面的不平整，单纯依赖GNSS定位并不能保证拖拉机直线作业，只有借助车身实时横滚角与俯仰角信息进行投影校正，方能保证拖拉机按照规划路径准确行走作业。又如平地机与喷雾机作业时，作业质量的保证取决于平地铲与喷杆的水平，这要求农机具姿态检测信息的实时反馈，以保证水平作业。同时准确的姿态倾角检测更是农业机械与农机具动力学建模、作业安全预警的研究的基础。目前姿态角检测多采用惯性传感器、图像处理等方法获取，还有一些采用多种传感器融合技术进行姿态监测。其中惯性传感器一般采用加速度计和陀螺仪、倾角传感器、超声波传感器等对姿态进行监测。传感器融合技术主要是将多个传感器结合起来对姿态角进行监测。利用MEMS传感器实对姿态倾角测量是目前应用最广泛的方法，其经济性、稳定性和适应性指标相比其他测量方法都具有突出优势。姿态倾角更新解算是惯性导航系统的关键算法，传统的姿态倾角更新算法有欧拉角法、方向余弦和四元素法。四元素法计算时需要进行泰勒展开，通常采用忽略其高阶项将非线性转化成线性进行姿态估算，存在误差。欧拉角法是直接迭代欧拉角微分方程，存在奇点。陀螺仪存在固有零偏，长时间积分运算存在累积误差，不适合单独使用。加速度计可以解算准静态峡物体欧拉姿态角，无积分运算不存在累积误差问题，但其解算的姿态角受外部加速度影响，无法在动态条件下应用。所以，如何开发一种进行姿态角融合与外部加速度辨识来提高倾角测量精度、稳定性的方法，是本领域技术人员的研究方向之一。
技术实现思路
本专利技术的主要目的在于克服现有技术的缺点与不足，提供一种田间作业车辆外部加速度辨识与姿态同步估计方法，来有效提高田间作业车辆姿态测量精度，提升作业质量。为了达到上述目的，本专利技术采用以下技术方案：本专利技术提供一种田间作业车辆外部加速度辨识与姿态同步估计方法，包括下述步骤：S1、基于三轴陀螺仪与三轴加速度计的测量模型，推导外部加速度的一阶低通滤波模型，融合到卡尔曼滤波器；S2、将三轴陀螺仪测量量ωX、ωY、ωZ作为卡尔曼滤波器的状态输入量；S3、将三轴加速度计的测量量aX、aY、aZ作为滤波器的观测输入向量，进行外部加速度的修正；S4、建立包含3个状态向量与3个观测向量的系统卡尔曼滤波状态方程和测量方程，精准测量田间作业车辆姿态角与辨识外部加速度。优选的技术方案，在步骤S1之前，还包括下述步骤：搭建硬件平台，所述硬件平台包括3轴加速度计和3轴陀螺仪，所述3轴加速度计和3轴陀螺仪的输出频率不小于100Hz。优选的技术方案，在步骤S1中，三轴陀螺仪测量模型为：yG＝ω+nG，三轴加速度计测量模型为：yA＝g+α+nA，外部加速度的一阶低通模型为：αt＝kαt-1+εt。优选的技术方案，在步骤S3中，外部加速度估计误差为：基于方向余弦矩阵的姿态角解算为：其中yG，yA分别是三轴陀螺仪与三轴加速度计测量值，由此可估算外部加速度。优选的技术方案，在步骤S4中，所述系统卡尔曼滤波状态方程具体如下：系统的状态向量为[c31,c32,c33]；系统状态状态转移矩阵为系统状态噪声矩阵为系统状态噪声协方差矩阵为优选的技术方案，在步骤S4中，所述测量方程如下：陀螺仪测量噪声协方差矩阵为系统观测矩阵为系统输入矩阵为H＝gI；系统观测噪声矩阵为系统观测噪声协方差矩阵为Rt-1＝∑α+∑A；外部加速度模型的测量误差协方差矩阵为加速度计测量噪声协方差矩阵为t表示本次计算的时序，Δt表示融合时间。本专利技术与现有技术相比，具有如下优点和有益效果：1、本专利技术建立外部加速度计模型，考虑外部加速度对姿态测量影响，提高测量精度。2、本专利技术采用方向余弦矩阵地其中3个参数进行姿态解算，减少运算量，提高运行效率。3、本专利技术搭建系统的具有3个状态向量和3个观测向量的外部拓展卡尔曼滤波模型进行姿态角融合与外部加速度辨识，保证田间作业车辆姿态测量精度。综上所述，本专利技术提供一种田间作业车辆外部加速度辨识与姿态同步估计方法，采用3轴加速度计与3轴陀螺仪惯性传感器；建立一阶外部加速度模型考虑外部加速度影响，搭建具有3个状态向量与3个观测向量的外部拓展卡尔曼滤波模型进行姿态角融合与外部加速度辨识，保证了田间作业车辆姿态倾角测量精度与稳定性。附图说明图1为一种田间作业车辆外部加速度辨识与姿态同步估计系统示意图；图2为外部加速度辨识试验结果示意图；图3本专利技术横滚倾角与MTi-300比较示意图；图4本专利技术俯仰倾角与MTi-300比较示意图。具体实施方式为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合附图及实例，对本专利技术进行进一步详细说明。此处所描述的具体实例仅仅用以解释本专利技术，并不用于限定本专利技术。此外，下面所描述的本专利技术各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以互相结合。实施例本专利技术一种田间作业车辆外部加速度辨识与姿态同步估计方法，主要是基于外部加速度模型的卡尔曼滤波算法融合6轴微惯性传感器信息，实现田间作业车辆外部加速度辨识与姿态同步估计，其主要技术方案为：采用一阶低通滤波外部加速度模型实现外部加速度的辨识，并借助方向余弦矩阵法进行倾角姿态解算；建立包含3个状态向量和3个观测向量的卡尔曼滤波状态方程和测量方程进行实时估算姿态角与辨识外部加速度。本专利技术方法考虑车辆外部加速度对姿态测量的影响，能有效提高田间作业车辆姿态测量精度，提升作业质量。如图1所示，上述一种田间作业车辆外部加速度辨识与姿态同步估计方法具体包括下述步骤：首先是硬件平台的搭建，本专利技术的硬件平台具有稳定可靠3轴加速度计、3轴陀螺仪，输出频率不小于100Hz。由于本专利技术重点在于算法介绍，故详细的硬件平台信息不在此展开介绍。在平台式惯性导航系统中，从导航坐标系n到车体坐标系b的旋转矩阵(后文简写为C)可用一个3x3的方向余弦矩阵表示：若运用常规的Z-Y-X旋转顺序的欧拉角表示，旋转矩阵C等同于：式中：式中φ、θ和ψ分别代表横滚角、俯仰角与航向角。整理可推导出姿态角的解算公式为：故根据方向余弦矩阵法，只需求解第3行3个元素便可得到相应的姿态角。如图1所示：建立姿态倾角参考系统的外部拓展卡尔曼滤波状态方程与测量方程：其中x为状态矢量，z为测量矢量，Φ为状态转移矩阵，H为观测矩阵；w和v为状态噪声和观测噪声，其中：系统的状态向量为[c31,c32,c33]；系统状态状态转移矩阵为系统状态噪声矩阵为系统状态噪声协方差矩阵为陀螺仪测量噪声协方差矩阵为系统观测矩阵为系统输入矩阵为H＝gI；系统观测噪声矩阵为系统观测噪声协方差矩阵为Rt-1＝∑α+∑A；外部加速度模型的测量误差协方差矩阵为加速本文档来自技高网...

【技术保护点】
1.一种田间作业车辆外部加速度辨识与姿态同步估计方法，其特征在于，包括下述步骤：S1、基于三轴陀螺仪与三轴加速度计的测量模型，推导外部加速度的一阶低通滤波模型，融合到卡尔曼滤波器；S2、将三轴陀螺仪测量量ωX、ωY、ωZ作为卡尔曼滤波器的状态输入量；S3、将三轴加速度计的测量量aX、aY、aZ作为滤波器的观测输入向量，进行外部加速度的修正；S4、建立包含3个状态向量与3个观测向量的系统卡尔曼滤波状态方程和测量方程，精准测量田间作业车辆姿态角与辨识外部加速度。

【技术特征摘要】
1.一种田间作业车辆外部加速度辨识与姿态同步估计方法，其特征在于，包括下述步骤：S1、基于三轴陀螺仪与三轴加速度计的测量模型，推导外部加速度的一阶低通滤波模型，融合到卡尔曼滤波器；S2、将三轴陀螺仪测量量ωX、ωY、ωZ作为卡尔曼滤波器的状态输入量；S3、将三轴加速度计的测量量aX、aY、aZ作为滤波器的观测输入向量，进行外部加速度的修正；S4、建立包含3个状态向量与3个观测向量的系统卡尔曼滤波状态方程和测量方程，精准测量田间作业车辆姿态角与辨识外部加速度。2.根据权利要求1所述的田间作业车辆外部加速度辨识与姿态同步估计方法，其特征在于，在步骤S1之前，还包括下述步骤：搭建硬件平台，所述硬件平台包括3轴加速度计和3轴陀螺仪，所述3轴加速度计和3轴陀螺仪的输出频率不小于100Hz。3.根据权利要求1所述的田间作业车辆外部加速度辨识与姿态同步估计方法，其特征在于，在步骤S1中，三轴陀螺仪测量模型为：yG＝ω+nG，三轴加速度计测量模型为：yA＝g+α+nA...

【专利技术属性】
技术研发人员：张智刚，黄培奎，罗锡文，张健，何杰，刘兆朋，王辉，岳斌斌，高维炜，
申请(专利权)人：华南农业大学，
类型：发明
国别省市：广东,44