CN109540135B - 水田拖拉机位姿检测和偏航角提取的方法及装置 - Google Patents

水田拖拉机位姿检测和偏航角提取的方法及装置 Download PDF

Info

Publication number
CN109540135B
CN109540135B CN201811328662.6A CN201811328662A CN109540135B CN 109540135 B CN109540135 B CN 109540135B CN 201811328662 A CN201811328662 A CN 201811328662A CN 109540135 B CN109540135 B CN 109540135B
Authority
CN
China
Prior art keywords
sensor
triaxial
paddy field
data measured
field tractor
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.)
Active
Application number
CN201811328662.6A
Other languages
English (en)
Other versions
CN109540135A (zh
Inventor
齐龙
李帅
马旭
龚浩
陶明
刘闯
黄旭楠
林洁平
谢梓钊
陈钊国
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China Agricultural University
Original Assignee
South China Agricultural University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China Agricultural University filed Critical South China Agricultural University
Priority to CN201811328662.6A priority Critical patent/CN109540135B/zh
Publication of CN109540135A publication Critical patent/CN109540135A/zh
Application granted granted Critical
Publication of CN109540135B publication Critical patent/CN109540135B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/06—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving measuring of drift angle; involving correction for drift
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth

Abstract

本发明公开了一种水田拖拉机位姿检测和偏航角提取的方法及装置，该方法包括：通过三轴加速度传感器测量水田拖拉机三个坐标轴上的加速度数据，从而得到水田拖拉机各个坐标轴与重力之间的夹角；通过三轴陀螺仪传感器测量水田拖拉机绕坐标轴转动的角速度数据，从而得到水田拖拉机转动的角度；通过三轴地磁传感器测量水田拖拉机各个方向的磁场数据，从而得到水田拖拉机的偏航角；采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合；采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合。本发明可以保证水田拖拉机在作业过程中获得精确的位姿信息和偏航角信息。

Description

水田拖拉机位姿检测和偏航角提取的方法及装置
技术领域
本发明涉及一种水田拖拉机位姿检测和偏航角提取的方法及装置，属于水田拖拉机导航技术领域。
背景技术
随着农业机械化的快速发展，对高精度导航技术的需求也越来越高，为了保证水田拖拉机能够准确、高效地完成田间工作，提取精确的水田拖拉机位姿信息和偏航角信息对水田拖拉机的导航和智能化工作至关重要。
发明内容
本发明的目的是为了解决上述现有技术的缺陷，提供了一种水田拖拉机位姿检测和偏航角提取的方法，该方法可以保证水田拖拉机在作业过程中获得精确的位姿信息和偏航角信息。
本发明的另一目的在于提供一种一种水田拖拉机位姿检测和偏航角提取的装置。
本发明的目的可以通过采取如下技术方案达到：
一种水田拖拉机位姿检测和偏航角提取的方法，所述方法包括：
通过三轴加速度传感器测量水田拖拉机三个坐标轴上的加速度数据，从而得到水田拖拉机各个坐标轴与重力之间的夹角；
通过三轴陀螺仪传感器测量水田拖拉机绕坐标轴转动的角速度数据，从而得到水田拖拉机转动的角度；
通过三轴地磁传感器测量水田拖拉机各个方向的磁场数据，从而得到水田拖拉机的偏航角；
采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合；
采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合。
进一步的，所述方法还包括：
根据陀螺仪传感器的温度零点偏移对陀螺仪传感器的温度进行补偿。
进一步的，所述温度零点偏移曲线为三次多项式BX＝At3+Bt2+Ct1+D，通过最小二乘法拟合之后得到各项系数。
进一步的，所述方法还包括：
通过三轴加速度传感器输入输出加速度数据，通过三轴陀螺仪传感器输入输出角速度数据，以及通过三轴地磁传感器输入输出磁场数据，利用卡尔曼滤波算法对各传感器状态进行最优估计，并通过最优估计去除各传感器中噪声和干扰对测量数据的影响。
进一步的，所述方法还包括：
通过卡尔曼滤波融合算法采集到的加速度数据、角速度数据和磁场数据进行误差补偿和滤波，将三轴陀螺仪传感器测量的角度数据作为卡尔曼滤波器的预测值，并估计过程噪声协方差；将三轴加速度传感器测量的角度数据和三轴地磁传感器测量的偏航角作为测量值，并结合三轴陀螺仪传感器误差估计测量噪声协方差矩阵，实现三轴加速度传感器、三轴陀螺仪传感器和三轴地磁传感器的融合。
进一步的，所述方法还包括：
利用三轴陀螺仪传感器测量的角速度数据作为四元数的更新，以三轴加速度传感器测量的重力加速度数据作为四元数的观测，实时解算姿态角。
进一步的，所述采用互补滤波算法将测量的加速度数据和角速度数据进行融合，具体为：
通过三轴加速度传感器测量的角度数据和上一次融合后的角度数据对三轴陀螺仪传感器的角速度数据进行修改，然后再进行积分；当水田拖拉机处于静止状态时，增大三轴加速度传感器测量的角度数据权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重。
进一步的，所述采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合，具体为：
将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合；当水田拖拉机处于静止状态时，增大三轴地磁传感器测量的偏航角数据的权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重。
本发明的另一目的可以通过采取如下技术方案达到：
一种水田拖拉机位姿检测和偏航角提取的装置，所述装置包括三轴加速度传感器、三轴陀螺仪传感器、三轴地磁传感器和控制中心，所述三轴加速度传感器、三轴陀螺仪传感器、三轴地磁传感器设置在水田拖拉机的机体上，并分别与控制中心相连；
所述三轴加速度传感器，用于测量水田拖拉机三个坐标轴上的加速度数据；
所述三轴陀螺仪传感器，用于测量水田拖拉机绕坐标轴转动的角速度数据；
所述三轴地磁传感器，用于测量水田拖拉机各个方向的磁场数据；
所述控制中心，用于读取各传感器测量的数据以及运行相应的控制算法，检测得到水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息，并进行显示。
进一步的，所述控制中心包括相连的主控芯片和显示屏，所述主控芯片用于读取各传感器测量的数据以及运行相应的控制算法，检测得到水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息，所述显示屏用于显示水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息。
本发明相对于现有技术具有如下的有益效果：
1、本发明通过三轴加速度传感器得到水田拖拉机各个坐标轴与重力之间的夹角，通过三轴陀螺仪传感器得到水田拖拉机转动的角度，以及通过三轴地磁传感器得到水田拖拉机的偏航角，采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合，从而获得准确的水田拖拉机位姿数据，以及采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合，从而获得准确的水田拖拉机偏航角数据，可以保证水田拖拉机在作业过程中获得精确的位姿信息和偏航角信息。
2、本发明采用的互补滤波算法并不是直接融合通过三轴加速度传感器和三轴陀螺仪传感器解算得到的角度数据，而是先通过三轴加速度传感器得到的角度数据和上一次融合后的角度数据对三轴陀螺仪传感器的角速度数据进行修改，然后再进行积分，有效地抑制了三轴加速度传感器和三轴陀螺仪传感器的偏差。
3、本发明根据陀螺仪传感器的温度零点偏移对陀螺仪传感器的温度进行补偿，可以在要求姿态更新速率高而微处理器计算性能有限的情况下使用，并且实时性较高。
4、本发明采用位姿解算四元数算法对数据进行处理，在数据处理完毕后再把四元数转换成姿态角(欧拉角)，能够有效结合三轴陀螺仪传感器以及三轴加速度传感器的误差特性，将角速度以及重力加速度两个互不相干的物理矢量进行互补融合。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1为本发明实施例的水田拖拉机位姿检测和偏航角提取的装置的结构框图。
图2为本发明实施例的温度补偿后的三轴陀螺仪传感器曲线图。
图3为本发明实施例的互补滤波算法的流程图。
图4为本发明实施例的卡尔曼滤波融合算法流程图。
图5为本发明实施例的卡尔曼滤波融合算法的融合流程图。
图6为本发明实施例的预测方程和更新方程的关系图。
图7为本发明实施例的传感器测量曲线与编码器的测量曲线的对比图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例：
如图1所示，本实施例提供了一种水田拖拉机位姿检测和偏航角提取的装置，该装置包括三轴加速度传感器(加速度计)1、三轴陀螺仪传感器(陀螺仪)2、三轴地磁传感器(地磁计)3和控制中心4，三轴加速度传感器1、三轴陀螺仪传感器2和三轴地磁传感器3分别与控制中心4相连。
所述控制中心4读取各传感器测量的数据以及运行相应的控制算法，检测得到水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息，并进行显示；其中，控制算法包括：互补滤波算法、姿态数据融合算法、温度补偿算法、卡尔曼滤波算法、位姿解算四元数算法和卡尔曼滤波融合算法。
进一步地，控制中心4包括主控芯片401和显示屏402，主控芯片401可以采用微处理器，如单片机等，其通过IIC(Inter-Integrated Circuit，集成电路)总线分别与三轴加速度传感器1、三轴陀螺仪传感器2、三轴地磁传感器3相连，用于读取各传感器测量的数据以及运行相应的控制算法，检测得到水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息；显示屏402可以采用液晶显示屏(Liquid Crystal Display，简称LCD)，其与主控芯片401相连，用于显示水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息，本领域技术人员可以理解，显示屏402也可以采用触摸显示屏、数码管显示屏等。
所述三轴加速度传感器1可以输出水田拖拉机三个坐标轴上的加速度数据，具体地，通过检测水田拖拉机在各个方向的形变而采样得到受力数据，根据F＝ma转换，输出水田拖拉机三个坐标轴上的加速度数据；当三轴加速度传感器1的姿态不同时，它在自身各个坐标检测到的重力加速度是不一样的，利用各方向的检测结果，根据力的分解原理，可求出水田拖拉机各个坐标轴与重力之间的夹角，因为重力方向是与地理坐标系的“天地”轴固连的，所以通过测量载体坐标系各轴与重力方向的夹角即可求得它与地理坐标系的角度旋转关系，从而获知载体姿态，但由于物体在运动时三轴加速度传感器1的数据会有非常大的数据变化，所以难以读出准确值。
所述三轴陀螺仪传感器2可以输出水田拖拉机绕坐标轴转动的角速度数据，如同将速度对时间积分可以求出路程一样，将角速度对时间积分就可以计算出水田拖拉机转动的角度，由于三轴陀螺仪传感器2测量角度时使用积分，会存在积分误差，因此需要三轴加速度传感器1来补偿误差。
所述三轴地磁传感器3可以输出水田拖拉机各个方向的磁场大小，通过检测地球磁场可以实现指南针的功能，由于地磁与地理坐标系的“南北”轴固联，利用三轴地磁传感器3的指南针功能，就可以测量出水田拖拉机的偏航角了，但三轴地磁传感器3会受到外部地磁场干扰，所以也得不到准确的偏航角。
进一步地，通过三轴加速度传感器1输入输出加速度数据，通过三轴陀螺仪传感器2输入输出角速度数据，以及通过三轴地磁传感器3输入输出磁场数据，利用卡尔曼滤波算法对各传感器状态进行最优估计，并通过最优估计去除各传感器中噪声和干扰对测量数据的影响，以此获取各传感器的准确信息。
三轴陀螺仪传感器2的温度补偿，可以通过增加扩展卡尔曼滤波的状态变量的维数，即增加三维陀螺零偏做数据融合得以解决，优点是可以动态地估计三轴陀螺仪传感器2的温度零点偏移，有较强的适应能力，缺点是卡尔曼滤波算法的计算量以维数的三次方增加，因此实时性会有所降低，在要求姿态更新速率高而微处理器计算性能有限的情况下，可以采用温度补偿算法解决，具体地，三轴陀螺仪传感器2的温度零点偏移与传感器温度、温度梯度密切相关，在三轴陀螺仪传感器2的温度变化缓慢的情况下，可以认为其温度零点偏移对应此时传感器的温度，设温度零点偏移曲线为三次多项式BX＝At3+Bt2+Ct1+D，通过最小二乘法拟合之后可得到各项系数，温度补偿后的三轴陀螺仪传感器2曲线如图2所示。
为了补偿三轴陀螺仪传感器2的积分误差，本实施例采用了互补滤波算法，互补滤波算法的流程如图3所示，因为三轴陀螺仪传感器2在测量角度时，在水田拖拉机静止状态下存在缺陷，且受时间影响，而三轴加速度传感器1在测量角度时，在水田拖拉机运动状态下存在缺陷，且不受时间影响，所以根据两者刚好互补的特性，当水田拖拉机处于静止状态时，增大三轴加速度传感器1测量的角度数据(即各个坐标轴与重力之间的夹角)的权重，当水田拖拉机处于运动时，增大三轴陀螺仪传感器2测量的角度数据(即转动的角度)的权重，从而获得更准确的姿态数据；互补滤波算法并不是直接融合通过三轴加速度传感器1和三轴陀螺仪传感器2解算得到的角度数据，而是先通过三轴加速度传感器1得到的角度数据和上一次融合后的角度数据对三轴陀螺仪传感器2的角速度数据进行修改，然后再进行积分，有效地抑制了三轴加速度传感器1和三轴陀螺仪传感器2的偏差，该互补滤波算法的调用采用中断方式。
为了得到准确的偏航角，本实施例采用了姿态数据融合算法，具体地，将三轴陀螺仪传感器测量的角度数据(即转动的角度)和三轴地磁传感器测量的偏航角数据进行融合，当水田拖拉机处于静止状态时，增大三轴地磁传感器测量的偏航角数据的权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重，从而获得水田拖拉机的偏航角数据。
优选地，由于地球参考坐标是固定不动的，坐标随着水田拖拉机的运动而不断变换，初始时令两个坐标完全重合，代表水田拖拉机水平静止，以后水田拖拉机运动时通过测量水田拖拉机机体坐标系与地球坐标系之间的角度即可获得当前水田拖拉机的姿态；水田拖拉机运动时相应的机体坐标系也会相对于参考坐标系转动，如果将机体坐标系的三轴当作三个单位向量，则机体坐标系都可以在参考坐标系上找到对应的三个方向余弦，如对于单位方向向量可以在参考系找到对应的三个方向余弦，那机体坐标便可以得个九个方向余弦，写成矩阵，便是方向余弦矩阵，上下标表示机体坐标转化为参考坐标；位姿解算四元数算法由三个实数及一个虚数组成，因而被称之为四元数，使用四元数表示姿态并不直观，但因为使用姿态角(即偏航角、横滚角及俯仰角)，表示姿态的时候会有“万向节死锁”问题，且运算比较复杂，所以一般在数据处理的时候会使用四元数，处理完毕后再把四元数转换成姿态角，即由姿态角(欧拉角)获得四元数，再求逆将四元数改成姿态角，最后便能获得水田拖拉机的位姿信息，即偏航角、俯仰角和翻滚角，能够有效结合三轴陀螺仪传感器以及三轴加速度传感器的误差特性，将角速度以及重力加速度两个互不相干的物理矢量进行互补融合；利用四元数进行数据处理具体为：利用三轴陀螺仪传感器2测量的角速度数据作为四元数的更新，以三轴加速度传感器1测量的重力加速度数据作为四元数的观测，通过主控芯片401实时解算姿态角。
为了实现多个传感器的融合，本实施例采用了卡尔曼滤波融合算法，卡尔曼滤波融合算法的流程如图4和图5所示，通过卡尔曼滤波融合算法采集到的加速度数据、角速度数据和磁场数据(即原始数据)进行误差补偿和滤波，将三轴陀螺仪传感器测量的角度数据作为卡尔曼滤波器的预测值，并估计过程噪声协方差Q；将三轴加速度传感器测量的角度数据和三轴地磁传感器测量的偏航角作为测量值，并结合三轴陀螺仪传感器误差估计测量噪声协方差矩阵R。
具体地，本实施例运用卡尔曼滤波的两个预测方程和三个更新方程，预测方法和更新方程的关系图如图6所示，预测方程根据之前的状态和控制量预测当前状态；更新方程表示相信传感器数据多些还是相信总体估计值多些(由卡尔曼增益Kg决定)，卡尔曼滤波的工作原理为：根据预测方程预测当前状态并用更新方程检测预测结果，该过程一直在重复更新当前状态。
本实施例对上述装置进行验证，将各传感器放置在与1024线光栅编码器固定连接的转动平台上，测试俯仰姿态角的测量精度及跟踪性能，与编码器测量曲线对比如图7所示，图中实线为传感器的测量值，点划线为编码器的测量值，可以看出合成后的传感器具有很好的稳定性和精确度，能够良好地实现水田拖拉机位姿检测和偏航角提取。
综上所述，本发明通过三轴加速度传感器得到水田拖拉机各个坐标轴与重力之间的夹角，通过三轴陀螺仪传感器得到水田拖拉机转动的角度，以及通过三轴地磁传感器得到水田拖拉机的偏航角，采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合，从而获得更准确的水田拖拉机位姿数据，以及采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合，从而获得更准确的水田拖拉机偏航角数据，可以保证水田拖拉机在作业过程中获得精确的位姿信息和偏航角信息。
以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明构思加以等同替换或改变，都属于本发明专利的保护范围。

Claims (7)

1.一种水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法包括：
通过三轴加速度传感器测量水田拖拉机三个坐标轴上的加速度数据，从而得到水田拖拉机各个坐标轴与重力之间的夹角；
通过三轴陀螺仪传感器测量水田拖拉机绕坐标轴转动的角速度数据，从而得到水田拖拉机转动的角度；
通过三轴地磁传感器测量水田拖拉机各个方向的磁场数据，从而得到水田拖拉机的偏航角；
采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合；
采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合；
所述采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合，具体为：
通过三轴加速度传感器测量的角度数据和上一次融合后的角度数据对三轴陀螺仪传感器的角速度数据进行修改，然后再进行积分；当水田拖拉机处于静止状态时，增大三轴加速度传感器测量的角度数据权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重；
所述采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合，具体为：
将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合；当水田拖拉机处于静止状态时，增大三轴地磁传感器测量的偏航角数据的权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重。
2.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：
根据陀螺仪传感器的温度零点偏移对陀螺仪传感器的温度进行补偿。
3.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：
通过三轴加速度传感器输入输出加速度数据，通过三轴陀螺仪传感器输入输出角速度数据，以及通过三轴地磁传感器输入输出磁场数据，利用卡尔曼滤波算法对各传感器状态进行最优估计，并通过最优估计去除各传感器中噪声和干扰对测量数据的影响。
4.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：
通过卡尔曼滤波融合算法对采集到的加速度数据、角速度数据和磁场数据进行误差补偿和滤波，将三轴陀螺仪传感器测量的角度数据作为卡尔曼滤波器的预测值，并估计过程噪声协方差；将三轴加速度传感器测量的角度数据和三轴地磁传感器测量的偏航角作为测量值，并结合三轴陀螺仪传感器误差估计测量噪声协方差矩阵，实现三轴加速度传感器、三轴陀螺仪传感器和三轴地磁传感器的融合。
5.根据权利要求1所述的水田拖拉机位姿检测和偏航角提取的方法，其特征在于，所述方法还包括：
利用三轴陀螺仪传感器测量的角速度数据作为四元数的更新，以三轴加速度传感器测量的重力加速度数据作为四元数的观测，实时解算姿态角。
6.一种水田拖拉机位姿检测和偏航角提取的装置，其特征在于，所述装置包括三轴加速度传感器、三轴陀螺仪传感器、三轴地磁传感器和控制中心，所述三轴加速度传感器、三轴陀螺仪传感器、三轴地磁传感器设置在水田拖拉机的机体上，并分别与控制中心相连；
所述三轴加速度传感器，用于测量水田拖拉机三个坐标轴上的加速度数据；
所述三轴陀螺仪传感器，用于测量水田拖拉机绕坐标轴转动的角速度数据；
所述三轴地磁传感器，用于测量水田拖拉机各个方向的磁场数据；
所述控制中心，用于读取各传感器测量的数据以及运行相应的控制算法，检测得到水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息，并进行显示；其中，采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合；采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合；
所述采用互补滤波算法将三轴加速度传感器测量的角度数据和三轴陀螺仪传感器测量的角度数据进行融合，具体为：
通过三轴加速度传感器测量的角度数据和上一次融合后的角度数据对三轴陀螺仪传感器的角速度数据进行修改，然后再进行积分；当水田拖拉机处于静止状态时，增大三轴加速度传感器测量的角度数据权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重；
所述采用姿态数据融合算法将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合，具体为：
将三轴陀螺仪传感器测量的角度数据和三轴地磁传感器测量的偏航角数据进行融合；当水田拖拉机处于静止状态时，增大三轴地磁传感器测量的偏航角数据的权重，当水田拖拉机处于运动状态时，增大三轴陀螺仪传感器测量的角度数据的权重。
7.根据权利要求6所述的水田拖拉机位姿检测和偏航角提取的装置，其特征在于，所述控制中心包括相连的主控芯片和显示屏，所述主控芯片用于读取各传感器测量的数据以及运行相应的控制算法，检测得到水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息，所述显示屏用于显示水田拖拉机的位姿信息和水田拖拉机在直线方向上的偏航角信息。
CN201811328662.6A 2018-11-09 2018-11-09 水田拖拉机位姿检测和偏航角提取的方法及装置 Active CN109540135B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811328662.6A CN109540135B (zh) 2018-11-09 2018-11-09 水田拖拉机位姿检测和偏航角提取的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811328662.6A CN109540135B (zh) 2018-11-09 2018-11-09 水田拖拉机位姿检测和偏航角提取的方法及装置

Publications (2)

Publication Number Publication Date
CN109540135A CN109540135A (zh) 2019-03-29
CN109540135B true CN109540135B (zh) 2020-07-31

Family

ID=65845441

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811328662.6A Active CN109540135B (zh) 2018-11-09 2018-11-09 水田拖拉机位姿检测和偏航角提取的方法及装置

Country Status (1)

Country Link
CN (1) CN109540135B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110057364B (zh) * 2019-04-30 2020-08-07 盐城工业职业技术学院 一种水田拖拉机位姿检测和偏航角提取方法及装置
CN110030999A (zh) * 2019-05-21 2019-07-19 杭州鸿泉物联网技术股份有限公司 一种基于惯性导航的定位方法、装置、系统和车辆
CN113108790A (zh) * 2021-05-14 2021-07-13 深圳中智永浩机器人有限公司 机器人imu角度测量方法、装置、计算机设备及存储介质
CN113703446B (zh) * 2021-08-17 2023-11-07 泉州装备制造研究所 一种基于磁钉的导引车导航方法及调度系统
CN114395953B (zh) * 2021-12-31 2023-05-09 武汉大学 一种便携式高压水射流钢轨打磨入射角标定方法及系统
CN115268442B (zh) * 2022-07-27 2023-08-04 湖州丽天智能科技有限公司 光伏清洗机器人的自动纠偏方法、纠偏系统及清洗机器人

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103702884A (zh) * 2011-08-12 2014-04-02 迪尔公司 用于确定车辆倾角的方法和系统
CN105203098A (zh) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法
EP2985686A1 (en) * 2014-08-13 2016-02-17 Samsung Electronics Co., Ltd. Method and foldable electronic device for processing images
CN105509740A (zh) * 2015-12-31 2016-04-20 广州中海达卫星导航技术股份有限公司 一种农业机械车辆姿态测量方法及测量模块
CN106882394A (zh) * 2017-02-27 2017-06-23 华南农业大学 自适应调节弹射角度的多旋翼无人机紧急伞降装置及方法
CN107014376A (zh) * 2017-03-01 2017-08-04 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法
CN107255474A (zh) * 2017-05-11 2017-10-17 杭州电子科技大学 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
WO2018007321A1 (en) * 2016-07-04 2018-01-11 Bonamico, Mercedes Rotating atomizer device for application in apparatus for land spraying
CN107990901A (zh) * 2017-11-28 2018-05-04 元力云网络有限公司 一种基于传感器的用户方向定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10021079B2 (en) * 2015-05-31 2018-07-10 Awen Labs, Llc Security system, method, and apparatus

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103702884A (zh) * 2011-08-12 2014-04-02 迪尔公司 用于确定车辆倾角的方法和系统
EP2985686A1 (en) * 2014-08-13 2016-02-17 Samsung Electronics Co., Ltd. Method and foldable electronic device for processing images
CN105203098A (zh) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 基于九轴mems传感器的农业机械全姿态角更新方法
CN105509740A (zh) * 2015-12-31 2016-04-20 广州中海达卫星导航技术股份有限公司 一种农业机械车辆姿态测量方法及测量模块
WO2018007321A1 (en) * 2016-07-04 2018-01-11 Bonamico, Mercedes Rotating atomizer device for application in apparatus for land spraying
CN106882394A (zh) * 2017-02-27 2017-06-23 华南农业大学 自适应调节弹射角度的多旋翼无人机紧急伞降装置及方法
CN107014376A (zh) * 2017-03-01 2017-08-04 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法
CN107255474A (zh) * 2017-05-11 2017-10-17 杭州电子科技大学 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN107990901A (zh) * 2017-11-28 2018-05-04 元力云网络有限公司 一种基于传感器的用户方向定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"Posture and body acceleration tracking by inertial and magnetic sensing: Application in behavioral analysis of free-ranging animals";Hassen Fourati等;《Biomedical Signal Processing and Control》;20111231;第6卷(第1期);94-104 *
"应用 MEMS 陀螺仪和加速度计的汽车运动姿态测量";曹景伟等;《重庆理工大学学报(自然科学)》;20180430;第32卷(第4期);48-54 *
"水田小型除草机器人平台控制系统设计与测试";齐龙等;《吉林大学学报（工学版）》;20130731;第43卷(第4期);991-996 *

Also Published As

Publication number Publication date
CN109540135A (zh) 2019-03-29

Similar Documents

Publication Publication Date Title
CN109540135B (zh) 水田拖拉机位姿检测和偏航角提取的方法及装置
KR100533106B1 (ko) 지자계 센서의 자세 오차 보상장치 및 방법
CN113091709B (zh) 一种新型gnss接收机倾斜测量方法
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN109916395B (zh) 一种姿态自主冗余组合导航算法
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN108731664B (zh) 机器人状态估计方法、装置、计算机设备和存储介质
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
JP2012194175A (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
US11698687B2 (en) Electronic device for use in motion detection and method for obtaining resultant deviation thereof
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN104316080B (zh) 一种姿态陀螺仪倾斜补偿四元数解算装置
CN112611380B (zh) 基于多imu融合的姿态检测方法及其姿态检测装置
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN114926547A (zh) 一种相机与imu的标定方法、电子设备及系统
CN114485641A (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
CN111141283A (zh) 一种通过地磁数据判断行进方向的方法
CN114964214B (zh) 一种航姿参考系统的扩展卡尔曼滤波姿态解算方法
CN116125789A (zh) 一种基于四元数的姿态算法参数自动匹配系统及方法
US20230349699A1 (en) Absolute heading estimation with constrained motion
RU2339002C1 (ru) Способ определения навигационных параметров управляемых подвижных объектов и устройство для его осуществления
JP3783061B1 (ja) 傾斜角と並進加速度の検出方法および検出装置
CN115356965B (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