CN108627155B - 一种农业机械非直线作业向心加速度估算及倾角融合方法 - Google Patents

一种农业机械非直线作业向心加速度估算及倾角融合方法 Download PDF

Info

Publication number
CN108627155B
CN108627155B CN201810274971.3A CN201810274971A CN108627155B CN 108627155 B CN108627155 B CN 108627155B CN 201810274971 A CN201810274971 A CN 201810274971A CN 108627155 B CN108627155 B CN 108627155B
Authority
CN
China
Prior art keywords
acceleration
axis
accelerometer
centripetal
centripetal acceleration
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
CN201810274971.3A
Other languages
English (en)
Other versions
CN108627155A (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 CN201810274971.3A priority Critical patent/CN108627155B/zh
Publication of CN108627155A publication Critical patent/CN108627155A/zh
Application granted granted Critical
Publication of CN108627155B publication Critical patent/CN108627155B/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/20—Instruments for performing navigational calculations

Abstract

本发明涉及一种农业机械非直线作业向心加速度估算及倾角融合方法，包括以下步骤：1)三轴加速度计静止或匀速运动的重力加速度计算；2)三轴加速度计非直线运动向心加速度获取；3)三轴加速度计非直线运动非重力加速度求解；4)三轴加速度计加速度分解；5)向心加速度变化量求解；6)从卡尔曼滤波器的测量方程中去除向心加速度，并估算去除向心加速度的融合倾角。本方法在不增加额外传感器的前提下，分析估计出传感器上的向心加速度并以卡尔曼滤波器估计出最优的倾角，增加了加速度传感器非直线运动时倾角测量的真实度，有效减少了向心加速度的影响，提高了倾角融合精度，属于智能农业机械领域。

Description

一种农业机械非直线作业向心加速度估算及倾角融合方法
技术领域
本发明涉及智能农业机械领域，具体涉及一种农业机械非直线作业向心加速度估算及倾角融合方法。
背景技术
智能农业机械的作业以直线行走为主，但田间作业地头转弯等非直线作业过程中其可视为圆周运动，外部加速度(向心加速度)是连续存在的，对于采用惯性传感系统必然引起倾斜角度的测量误差。根据牛顿第二定律，在农业机械圆周运动过程中，计算向心加速度的转弯角速度可通过转向陀螺仪测出。但转弯半径或切向线速度在不增加外部传感器的情况下难以通过惯性传感器准确测量，因此，仅采用加速度计难以消除向心加速度对倾角测量的影响。
针对此问题，胡炼等(2011年)利用转向陀螺仪检测平地机转弯运动，并设定控制阈值，达到设定阈值后即停止平地铲的控制驱动，使平地铲保持“转弯”前直线行驶时的状态，即平地机转弯时不对平地铲进行自动调平控制，此算法简单，但是并未消除转弯过程中的外部加速度，水田硬底层不平时平地铲仍会产生倾斜，影响平整度。林潮兴(2015年)提出在平地机转弯时，通过调节卡尔曼滤波算法的过程协方差矩阵和观测协方差矩阵的权重，增大观测协方差矩阵的权重，减小过程协方差矩阵的权重，这样的操作相当于在转弯过程中增加陀螺仪测量的可信程度，减少加速度计测量的可信程度，算法有一定的效果，但需通过大量试验选择出最优参数，此外，平地机转弯过程速度较高时，倾角测量精度下降，难以保持稳定的精度。
发明内容
针对现有技术中存在的技术问题，本发明的目的是：提供一种适用于低成本加速度计的农业机械非直线作业向心加速度估算及倾角融合方法。
为了达到上述目的，本发明采用如下技术方案：
一种农业机械非直线作业向心加速度估算及倾角融合方法，包括以下步骤：
1)三轴加速度计静止或匀速运动的重力加速度计算；
2)三轴加速度计非直线运动向心加速度获取；
3)三轴加速度计非直线运动非重力加速度求解；
4)三轴加速度计加速度分解；
5)向心加速度变化量求解；
6)从卡尔曼滤波器的测量方程中去除向心加速度，并估算去除向心加速度的融合倾角。
具体为：
1)三轴加速度计静止或匀速运动的重力加速度计算
根据三轴加速度计的工作原理，静止或匀速运动(直线作业)时，三轴加速度数据和重力加速度满足：
Figure BDA0001613431700000021
式中，gx,0、gy,0、gz,0分别为重力加速度在加速度计的三轴上的测量分量(m/s2)；g0为静止或匀速运动时重力加速度的测量值(m/s2)。
2)三轴加速度计非直线运动的加速度获取
农业机械在转弯等非直线作业过程中，其所受的向心力产生的向心加速度必然使重力加速度的测量值偏离g0。假设在k时刻，由加速度计测量的三轴分量分别为gx,k、gy,k、gz,k。则转弯过程中加速度计的测量值由下式确定：
Figure BDA0001613431700000022
式中，gk为k时刻非直线行驶加速度计的测量值(m/s2)。
3)三轴加速度计非直线运动的非重力加速度求解
Figure BDA0001613431700000023
4)三轴加速度计加速度分解
非直线运动时，向心加速度主要叠加在加速度计的Y轴上，在X轴和Z轴上小到可以不考虑，
Figure BDA0001613431700000024
k时刻Y轴上的加速度应包括直线行驶时Y轴上的加速度分量和加速度变化量，即：
gy,k＝gy,0+Δak (5)
式中，Δak表示k时刻向心加速度的变化量(m/s2)。
5)k时刻向心加速度变化量求解
Figure BDA0001613431700000031
根据一元二次方程的求解公式虽可得到k时刻向心加速度的变化量，但会出现多解或无解(即虚根)现象，不能在工程上应用。由于直线行驶时Y轴上的重力加速度分量很小，可近似处理为0，即：
Figure BDA0001613431700000032
6)去除向心加速度的倾角融合
基于卡尔曼滤波器的去除向心加速度倾角融合算法为式(8)－(12)
Figure BDA0001613431700000033
Figure BDA0001613431700000034
Figure BDA0001613431700000035
Figure BDA0001613431700000036
Figure BDA0001613431700000037
其中，Zk为k时刻利用加速度计测量计算的角度值，其测量方程为：
Figure BDA0001613431700000038
其中，
Figure BDA0001613431700000039
Figure BDA00016134317000000310
为真实倾角(°)；Δb为陀螺仪的零位偏差(°/s)；
Figure BDA00016134317000000311
为系统更新周期(s)；
Figure BDA00016134317000000312
为k时刻的预测结果；
Figure BDA00016134317000000313
为(k-1)时刻的最优解；
Figure BDA00016134317000000314
为预测协方差；
Figure BDA00016134317000000315
为k时刻的协方差最优解；AT为A矩阵的转置矩阵；
Figure BDA00016134317000000316
为系统过程协方差矩阵，其为非负定矩阵；Qg为陀螺仪测量误差的协方差；Qb为加速度计估计陀螺仪零位偏差的协方差；Kk为k时刻卡尔曼增益；
Figure BDA00016134317000000317
HT是H的转置矩阵；Zk为k时刻利用加速度计测量计算的角度值；R＝[Ra]为测量误差的协方差矩阵，其为正定矩阵；Ra为加速计测量误差的协方差；
Figure BDA00016134317000000318
为单位矩阵；gy，k为k时刻转弯行驶重力加速度y轴分量的测量值(m/s2)；gk为k时刻转弯行驶重力加速度的测量值(m/s2)；g0为直线行驶重力加速度的测量值(m/s2)；g为重力加速度常量(9.8m/s2)；wa，k为k时刻加速度计测量的高斯白噪声(dB)。
总的说来，本发明具有如下优点：
(1)本发明通过加速度传感器非直线运动向心加速度产生原理，在不增加额外传感器的前提下，分析估计出传感器上的向心加速度并以卡尔曼滤波器估计出最优的倾角。
(2)本发明的倾角融合方法去除了估计出的向心加速度，增加了加速度传感器非直线运动时倾角测量的真实度，有效减少了向心加速度的影响，提高了倾角融合精度。
附图说明
图1为向心加速度估算及倾角融合方法流程图。
图2为农业机械转弯作业示意图。
图3为三轴多功能转台试验中角速度为140°/s的倾角算法融合曲线。
图4为三轴多功能转台试验中角速度为-140°/s的倾角算法融合曲线。
其中，“KF”为普通卡尔曼滤波算法的倾角，“TIAKF”为去除向心加速度后的倾角，“AHRS”为参考姿态传感器输出倾角，“GYROz”为传感器的运动角速度，区域A和区域E为静止状态，区域B为变加速过程，区域D为变减速过程，区域C为匀速转弯过程。
具体实施方式
下面来对本发明做进一步详细的说明。
本实施例MEMS传感器采用加速度计ADIS16300的Y轴加速度和单轴陀螺ADXL453，处理器为TI公司TMS320F28069，参考姿态传感器为MTi-100。
如图1所示，对于农业机械平台采用三轴加速度计实时测量平台倾角，非直线作业过程中向心加速度估算和倾角融合共6个步骤：
1)三轴加速度计静止或匀速运动的重力加速度计算
根据三轴加速度计的工作原理，静止或匀速运动(直线作业)时，三轴加速度数据和重力加速度满足：
Figure BDA0001613431700000041
式中，gx,0、gy,0、gz,0分别为重力加速度在加速度计的三轴上的测量分量(m/s2)；g0为静止或匀速运动时重力加速度的测量值(m/s2)。
2)三轴加速度计非直线运动的加速度获取
农业机械在转弯等非直线作业过程中，其所受的向心力产生的向心加速度必然使重力加速度的测量值偏离g0。假设在k时刻，由加速度计测量的三轴分量分别为gx,k、gy,k、gz,k。则转弯过程中加速度计的测量值由下式确定：
Figure BDA0001613431700000051
式中，gk为k时刻非直线行驶加速度计的测量值(m/s2)。
3)三轴加速度计非直线运动的非重力加速度求解
Figure BDA0001613431700000052
4)三轴加速度计加速度分解
非直线运动时，向心加速度主要叠加在加速度计的Y轴上，在X轴和Z轴上小到可以不考虑，
Figure BDA0001613431700000053
k时刻Y轴上的加速度应包括直线行驶时Y轴上的加速度分量和加速度变化量，即：
gy,k＝gy,0+Δak
式中，Δak表示k时刻向心加速度的变化量(m/s2)。
5)k时刻向心加速度变化量求解
Figure BDA0001613431700000054
根据一元二次方程的求解公式虽可得到k时刻向心加速度的变化量，但会出现多解或无解(即虚根)现象，不能在工程上应用。由于直线行驶时Y轴上的重力加速度分量很小，可近似处理为0，即：
Figure BDA0001613431700000055
6)从卡尔曼滤波器的测量方程中去除向心加速度，并估算去除向心加速度的融合倾角
基于卡尔曼滤波器的去除向心加速度倾角融合算法为
Figure BDA0001613431700000056
Figure BDA0001613431700000057
Figure BDA0001613431700000058
Figure BDA0001613431700000059
Figure BDA00016134317000000510
其中，Zk为k时刻利用加速度计测量计算的角度值，其测量方程为：
Figure BDA0001613431700000061
在三轴多功能转台上模拟如图2所示转弯非直线运动时的数据如图3和图4所示。
图3为转台外框角速度为140°/s顺时针旋转的试验数据融合曲线，转台外框角加速度为10°/s2；图4为转台外框角速度为-140°/s逆时针旋转的试验数据融合曲线，图中C代表匀速圆周运动过程。
由图3和图4可知，在转台匀速圆周运动(C)过程中，本发明算法和姿态航向参考系统AHRS的数据近似吻合，但普通的融合算法最大误差严重偏离了姿态航向参考系统的测量值。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (3)

1.一种农业机械非直线作业向心加速度估算及倾角融合方法，其特征在于，包括以下步骤：
1)三轴加速度计静止或匀速运动的重力加速度计算；
2)三轴加速度计非直线运动向心加速度获取；
3)三轴加速度计非直线运动非重力加速度求解；
4)三轴加速度计加速度分解；
5)向心加速度变化量求解；
6)从卡尔曼滤波器的测量方程中去除向心加速度，并估算去除向心加速度的融合倾角；
步骤1)中，根据三轴加速度计的工作原理，静止或匀速运动且直线作业时，三轴加速度数据和重力加速度满足
Figure FDA0002636840260000011
gx,0、gy,0、gz,0分别为重力加速度在三轴加速度计的三轴上的测量分量；g0为静止或匀速运动时重力加速度的测量值；
步骤2)中，农业机械在非直线作业过程中，其所受的向心力产生的向心加速度必然使重力加速度的测量值偏离g0；假设在k时刻，由三轴加速度计测量的三轴分量分别为gx,k、gy,k、gz,k，则非直线作业过程中三轴加速度计的测量值由下式确定：
Figure FDA0002636840260000012
式中，gk为k时刻非直线行驶三轴加速度计的测量值；
步骤3)中，三轴加速度计非直线运动的非重力加速度由非直线运动的非重力加速度减去静止或匀速运动的重力加速度，
Figure FDA0002636840260000013
步骤4)中，在非直线运动时，向心加速度主要叠加在三轴加速度计的Y轴上，在X轴和Z轴上小到不考虑，分解得到Y轴上的加速度应包括直线行驶时Y轴上的加速度分量和加速度变化量，gy,k＝gy,0+Δak，Δak表示k时刻向心加速度的变化量；
步骤5)中，由于直线行驶时Y轴上的重力加速度分量很小，近似处理为0，则
Figure FDA0002636840260000014
2.按照权利要求1所述的一种农业机械非直线作业向心加速度估算及倾角融合方法，其特征在于：步骤6)中，测量方程为
Figure FDA0002636840260000015
其中Zk为k时刻利用三轴加速度计测量计算的角度值，wa，k为k时刻三轴加速度计测量的高斯白噪声，单位为dB,g为重力加速度常量,取值9.8m/s2。
3.按照权利要求1所述的一种农业机械非直线作业向心加速度估算及倾角融合方法，其特征在于：非直线作业包括农业机械转弯作业。
CN201810274971.3A 2018-03-30 2018-03-30 一种农业机械非直线作业向心加速度估算及倾角融合方法 Active CN108627155B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810274971.3A CN108627155B (zh) 2018-03-30 2018-03-30 一种农业机械非直线作业向心加速度估算及倾角融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810274971.3A CN108627155B (zh) 2018-03-30 2018-03-30 一种农业机械非直线作业向心加速度估算及倾角融合方法

Publications (2)

Publication Number Publication Date
CN108627155A CN108627155A (zh) 2018-10-09
CN108627155B true CN108627155B (zh) 2020-10-23

Family

ID=63696482

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810274971.3A Active CN108627155B (zh) 2018-03-30 2018-03-30 一种农业机械非直线作业向心加速度估算及倾角融合方法

Country Status (1)

Country Link
CN (1) CN108627155B (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE10216690B4 (de) * 2002-04-16 2006-07-13 Daimlerchrysler Ag Verfahren zur Darstellung einer auf einen Fahrer wirkenden Längsbeschleunigung in einer Fahrsimulation
JP4321554B2 (ja) * 2006-06-23 2009-08-26 トヨタ自動車株式会社 姿勢角検出装置と姿勢角検出方法
CN102297693B (zh) * 2010-06-24 2013-03-27 鼎亿数码科技(上海)有限公司 测量物体位置和方位的方法
CN105371846B (zh) * 2015-11-13 2018-01-05 广州周立功单片机科技有限公司 载体姿态检测方法及其系统
CN106197376B (zh) * 2016-09-23 2018-08-07 华南农业大学 基于单轴mems惯性传感器的车身倾角测量方法
CN107014376B (zh) * 2017-03-01 2019-09-10 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法

Also Published As

Publication number Publication date
CN108627155A (zh) 2018-10-09

Similar Documents

Publication Publication Date Title
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
WO2017063388A1 (zh) 一种惯导装置初始对准方法
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN105371846B (zh) 载体姿态检测方法及其系统
CN104596514B (zh) 加速度计和陀螺仪的实时降噪系统和方法
AU2015305864B2 (en) Earthmoving machine comprising weighted state estimator
CN108132053B (zh) 一种行人轨迹构建方法、系统及惯性测量装置
CN103913171A (zh) 基于加速度计和陀螺仪的载体转速与转角补偿测量方法
CN106153069A (zh) 自主导航系统中的姿态修正装置和方法
CN113340298A (zh) 一种惯导和双天线gnss外参标定方法
US10466054B2 (en) Method and system for estimating relative angle between headings
CN104819717A (zh) 一种基于mems惯性传感器组的多旋翼飞行器姿态检测方法
KR101226767B1 (ko) 주행 장치를 위한 위치측정 시스템 및 방법
CN109033017B (zh) 一种丢包环境下的车辆侧倾角与俯仰角估计方法
CN108871323A (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
CN106197376B (zh) 基于单轴mems惯性传感器的车身倾角测量方法
Carratù et al. IMU self-alignment in suspensions control system
CN109506674B (zh) 一种加速度的校正方法及装置
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN108627155B (zh) 一种农业机械非直线作业向心加速度估算及倾角融合方法
CN108413923B (zh) 一种基于鲁棒混合滤波的车辆侧倾角与俯仰角估计方法
CN109674480B (zh) 一种基于改进互补滤波的人体运动姿态解算方法
CN112577513A (zh) 一种状态量误差确定方法及车载终端
CN108773378B (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