CN110530361B - 基于农业机械双天线gnss自动导航系统的转向角度估计器 - Google Patents
基于农业机械双天线gnss自动导航系统的转向角度估计器 Download PDFInfo
- Publication number
- CN110530361B CN110530361B CN201910787902.7A CN201910787902A CN110530361B CN 110530361 B CN110530361 B CN 110530361B CN 201910787902 A CN201910787902 A CN 201910787902A CN 110530361 B CN110530361 B CN 110530361B
- Authority
- CN
- China
- Prior art keywords
- vehicle body
- course
- state observer
- order state
- steering angle
- 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
Links
- 238000000034 method Methods 0.000 claims abstract description 14
- 238000012545 processing Methods 0.000 claims abstract description 9
- 238000005259 measurement Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 7
- 238000012360 testing method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000003321 amplification Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003199 nucleic acid amplification method Methods 0.000 description 2
- FFBHFFJDDLITSX-UHFFFAOYSA-N benzyl N-[2-hydroxy-4-(3-oxomorpholin-4-yl)phenyl]carbamate Chemical compound OC1=C(NC(=O)OCC2=CC=CC=C2)C=CC(=C1)N1CCOCC1=O FFBHFFJDDLITSX-UHFFFAOYSA-N 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011900 installation process Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
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/165—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 combined with non-inertial navigation instruments
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Guiding Agricultural Machines (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
技术领域
本发明属于农业机械自动导航领域,具体涉及一种基于单陀螺仪与状态观测理论的农业机械双天线GNSS自动导航系统的转向角度估计器。
背景技术
随着农业机械自动化水平的提高,农业机械自动导航技术应用越来越广泛,尤其是双天线GNSS自动导航系统在输出定位信息同时能够输出航向与俯仰两个车体姿态信息,避免了增加姿态传感器的成本,越来越受到青睐。
前轮转向角度是农业机械自动导航技术必需的基本信息。针对农业机械前轮转向角度的获取问题,现有技术中有多种技术方案:如授权公告号为【CN101326094B】的发明专利公开一种基于转向组件角度及角速率传感器的车轮陀螺仪,用来估算一系统的至少两个部分之间相对坐标,该方案中采用两个陀螺仪,一个安装在车身上,一个安装在车轮上,采用卡尔曼滤波的方式对两个陀螺仪信息进行融合;而采用两个陀螺仪安装非常麻烦,对很多车辆来说是安装不上的,只具有理论基础,实用性较差;或者是采用安装霍尔式轴角传感器的方式,但是,由于农业机械转向机构的差异性,轴角传感器安装过程中往往存在安装困难,易于损坏等问题,限制了农业农机自动导航技术的发展。
发明内容
本发明利用状态观测理论提出一种农业机械双天线GNSS自动导航系统的转向角度估计器,采用二阶状态观测器对通过陀螺仪和双天线GNSS所获得的信息进行融合,有效提高了角度估计的精度和鲁棒性。
本发明是采用以下的技术方案实现的:一种基于农业机械双天线GNSS自动导航系统的转向角度估计器,包括航向校正器、第一三阶状态观测器、量测角度计算器、陀螺仪信号处理单元、第二三阶状态观测器和二阶状态观测器;
所述航向校正器用以将双天线GNSS输出的航向信息校正为农业机械车头方向在北-东-地坐标系下的车体航向信息j表示时刻;第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测;量测角度计算器基于车辆的轴距L、车速V和车体航向角速率根据车辆动力学模型对车辆转向轮角θm(j)进行推算;
所述陀螺仪信号处理单元用以对安装于前轮转轴的陀螺仪输出的角速率信号进行预处理,以输出滤波后的陀螺仪角速率信号ωg(j);陀螺仪角速率信号ωg(j)与车体航向角速率相减得到前轮相对于车身的车轮转向角速率ωs(j),并输入第二三阶状态观测器;第二三阶状态观测器用以对转向角加速度进行观测;
进一步的,所述车辆转向轮角θm(j)通过以下方式获得:
进一步的,所述陀螺仪信号处理单元包括AD转换器和惯性滤波器,分别对应的对陀螺仪输出的角速率信号进行AD采样和惯性滤波,所述惯性滤波器的表达形式如下:
ωg(j)=k1ωg(j)+k2ωg(j-1)+k3ωg(j-2) (5)
式中,k1,k2,k3满足条件:k1+k2+k3=1。
进一步的,当航向信息方向指示为车体左侧至车体右侧时,航向校正器则通过以下方式校正:
当航向信息方向指示为车体右侧至车体左侧时,航向校正器则通过以下方式校正:
与现有技术相比,本发明的优点和积极效果在于:
本发明所述方案在双天线导航系统基础上只需使用一个陀螺仪,安装方便,成本低;采用三阶状态观测器实现对车体航向信息和陀螺仪角速率信号的微分运算,利用积分代替微分,避免了微分运算对误差的放大作用;并采用二阶状态观测器对通过陀螺仪和双天线GNSS所获得的信息进行融合,有效提高了角度估计的精度和鲁棒性,具有较高的实际应用和推广价值。
附图说明
图1为本发明实施例所述硬件安装结构原理图;
图2为本发明实施例所述估计器的原理框图;
图3为本发明实施例三阶状态观测器原理框图;
图4为本发明实施例二阶状态观测器原理框图;
图5为本发明实施例测试试验1情况下的测量角度与估计角度的对比曲线图;
图6为图5中测量角度与估计角度的差值曲线示意图;
图7为本发明实施例所述方案与霍尔传感器所得结果的追踪误差示意图。
具体实施方式
为了能够更清楚的理解本发明的上述目的和优点,下面结合附图对本发明的具体实施方式做详细地描述:
为同时获取车辆的航向角和横滚角信息,农业机械导航系统的双天线通常垂直于车辆前进方向安装。本发明所述的转向角度估计器硬件安装如图1所示,陀螺仪1安装于前轮轴11上,根据具体需要可安装在左前轮轴或右前轮轴,为北-东-地坐标系下车体前进方向航向角,θ为待估计的车轮转向角,参考坐标系为车头方向X,车体右侧方向Y,指向地为Z。
实施例,一种农业机械双天线GNSS自动导航系统的转向角度估计器,如图2所示,陀螺仪输出的角速率信号经陀螺仪信号处理单元进行AD采样及惯性滤波后,输出滤波后的陀螺仪角速率信号ωg(j);AD转换器对陀螺仪输出的角速率模拟信号进行采样完成模拟信号到数字信号的转换;惯性滤波器目的在于抑制陀螺仪信号中的随机干扰;
航向校正器将双天线输出的航向信息校正为农业机械车头方向在北-东-地坐标系下的航向信息第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测;量测角度计算器利用车辆的轴距L,车速V,车体航向角速率根据车辆动力学模型对车辆转向轮角θm(j)进行推算,并作为二阶状态观测器的量测信息;
陀螺仪角速率信号ωg(j)与车体航向角速率相减得到另一组车轮转向角速率ωs(j);第二三阶状态观测器对转向角加速度进行观测;最后通过二阶状态观测器根据利用动力学模型计算得到的车辆转向轮角θm(j)与转向角加速度信息对车轮转向角θ(j)进行观测,该系统设计鲁棒性高,能够获得精确的转向角。
本实施例中,双天线GNSS板卡输出的航线信息是基于北-东-地参考坐标系的,当航向信息方向指示为车体左侧至车体右侧时,航向校正器实现的数学公式为:
当航向信息指示为车体右侧至车体左侧时,航向校正器实现的数学公式为:
另外,本实施例中,所述惯性滤波器的实现公式为:
ωg(j)=k1ωg(j)+k2ωg(j-1)+k3ωg(j-2) (5)
式中k1,k2,k3需满足条件:k1+k2+k3=1
Io(j)=Io(j-1)+TsIi(j) (7)
本实施例采用状态观测器理论获取安装于前轮转轴的陀螺仪与安装于车体的双天线GNSS间相对转向角度,在农业车辆北-东-地坐标系下的陀螺仪角速率信号ωg和车体航向角已知的前提下,定义车体航向角速率并采用两种方式获得车辆转向角:1)对ωg与做差得到前轮相对于车身的车轮转向角速率ωs,对ωs积分:根据车辆动力学模型,利用车体航向角速率车辆速度V,车辆轴距L,车辆前轮转向角度表示为:考虑到对于双天线系统而言,采用公式(1)所获取的前轮转向角度存在积分累计误差;采用公式(2)获取的转向角度由于运算过程中的差分运算以及反正切运算,对信号干扰存在放大作用,精度低,本实施例创造性的设计二阶状态观测器对两种数据进行融合,以有效提高角度估计的精度与鲁棒性。
为便于验证本发明方案的效果,进行了实物实验。实物实验平台按照图1搭建,其中双天线GNSS输出航向信息为车体左侧天线指向右侧天线,同时在车轮转向轴安装霍尔式轴角传感器作为评估本发明估计精度的基准。惯性滤波器的参数取值为k1=0.6,k2=0.2,k3=0.2,第一三阶状态观测器和第二三阶状态观测器的参数取值均为k3θ=60,k3ω=128,k3α=10000;二阶状态观测器参数取值为k2θ=100,k2ω=150;双天线板卡采用司南K728,输出航向信息为车体左侧至车体右侧,采用公式(3)进行车身航向校正;测试农业车辆为某厂家1004拖拉机,车辆转向轮轴同时安装有霍尔式轮角传感器,作为转向轮角的参考,传感器型号为德国NOVOTECHNIK公司生产的RF4000-120。
测试试验1:由驾驶员驾驶车辆,车辆前进速度为6km/h,行进路线为直线和S型,角度车辆值和估计值如图5所示,两者之差如图6所示;测试过程中,0-40s为直线驾驶,40-60s为S型路线驾驶,图中实线为本发明所得前轮转向角度估计值,虚线为霍尔传感器测量的前轮转向角度值,根据图5,直线行进过程中两者之差小于1°,S型路线行进过程中,由于估计器对于频率信号的滤波效果,两者之差小于4.3°;
测试试验2:需要说明的是由于霍尔式轮角传感器本身存在的测量误差问题,测试过程1所得两者之差仅可作为评估两种方式所获取转向角度的比较依据;直线追踪精度是农业机械自动导航系统的主要技术指标,为进一步验证本发明在农业机械自动导航系统中的适用性,分别由霍尔传感器的角度值和本发明的估计器所得角度估计值作为农业机械导航系统轮角值的输入完成农业机械直线导航的路径追踪,导航控制的其他硬件条件及算法均相同,两者所得直线追踪误差如图7所示,0-50s采用霍尔轮角传感器获取轮角值完成直线追踪,50-100s采用本发明的轮角估计器获取轮角值完成直线追踪,根据图7,采用本发明的角度估计器所得前轮转向角度值完成直线路径追踪,追踪误差小于5cm,与采用霍尔传感器测量角度实现直线路径追踪精度相当,精度高,鲁棒性好,可实现双天线农业机械自动导航系统轮角测量的完全互换。
以上所述,仅是本发明的较佳实施例而已,并非是对本发明作其它形式的限制,任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域,但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型,仍属于本发明技术方案的保护范围。
Claims (4)
1.基于农业机械双天线GNSS自动导航系统的转向角度估计器,其特征在于,包括航向校正器、第一三阶状态观测器、量测角度计算器、陀螺仪信号处理单元、第二三阶状态观测器和二阶状态观测器;
所述航向校正器用以将双天线GNSS输出的航向信息校正为农业机械车头方向在北-东-地坐标系下的车体航向信息j表示时刻;第一三阶状态观测器利用车体航向信息对车体航向角速率进行观测;量测角度计算器基于车辆的轴距L、车速V和车体航向角速率根据车辆动力学模型对车辆转向轮角θm(j)进行推算;
所述陀螺仪信号处理单元用以对安装于前轮转轴的陀螺仪输出的角速率信号进行预处理,以输出滤波后的陀螺仪角速率信号ωg(j);陀螺仪角速率信号ωg(j)与车体航向角速率相减得到前轮相对于车身的车轮转向角速率ωs(j),并输入第二三阶状态观测器;第二三阶状态观测器用以对转向角加速度进行观测;
所述二阶状态观测器接收量测角度计算器得到的车辆转向轮角θm(j)与第二三阶状态观测器得到的转向角加速度信息,以对车轮转向角θ(j)进行观测,其中,二阶状态观测器利用和θm(j)对车轮转向角进行估计,并表示为:
3.根据权利要求1所述的基于农业机械双天线GNSS自动导航系统的转向角度估计器,其特征在于:所述陀螺仪信号处理单元包括AD转换器和惯性滤波器,分别对应的对陀螺仪输出的角速率信号进行AD采样和惯性滤波,所述惯性滤波器的表达形式如下:
ωg(j)=k1ωg(j)+k2ωg(j-1)+k3ωg(j-2)
式中,k1,k2,k3满足条件:k1+k2+k3=1。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910787902.7A CN110530361B (zh) | 2019-08-26 | 2019-08-26 | 基于农业机械双天线gnss自动导航系统的转向角度估计器 |
AU2020100627A AU2020100627A4 (en) | 2019-08-26 | 2020-04-24 | Steering angle estimator based on dual-antenna global navigation satellite system (gnss) for agricultural machinery |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910787902.7A CN110530361B (zh) | 2019-08-26 | 2019-08-26 | 基于农业机械双天线gnss自动导航系统的转向角度估计器 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110530361A CN110530361A (zh) | 2019-12-03 |
CN110530361B true CN110530361B (zh) | 2021-02-09 |
Family
ID=68664175
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910787902.7A Active CN110530361B (zh) | 2019-08-26 | 2019-08-26 | 基于农业机械双天线gnss自动导航系统的转向角度估计器 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN110530361B (zh) |
AU (1) | AU2020100627A4 (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111238471B (zh) * | 2020-01-17 | 2021-08-24 | 青岛农业大学 | 一种适用于农业机械直线导航的侧滑角度估计方法及估计器 |
CN114633800B (zh) * | 2020-12-16 | 2023-08-29 | 西安合众思壮导航技术有限公司 | 一种转向轮角度的检测方法、装置及设备 |
CN112379678B (zh) * | 2020-12-18 | 2021-06-11 | 深圳市瑞芬科技有限公司 | 一种基于双陀螺仪控制的自动驾驶农机 |
CN113391336A (zh) * | 2021-06-17 | 2021-09-14 | 上海联适导航技术股份有限公司 | 一种航向角的检测方法、装置、设备及可读存储介质 |
CN114162139B (zh) * | 2021-12-06 | 2024-03-26 | 上海华测导航技术股份有限公司 | 一种车辆控制方法、控制器、系统、装置及存储介质 |
CN114771656B (zh) * | 2022-05-11 | 2024-06-21 | 山东理工大学 | 一种拖拉机前轮转向角测量方法及系统 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9002565B2 (en) * | 2003-03-20 | 2015-04-07 | Agjunction Llc | GNSS and optical guidance and machine control |
DE112014001809B4 (de) * | 2013-05-02 | 2022-08-25 | GM Global Technology Operations LLC (n. d. Ges. d. Staates Delaware) | Integriertes Schätzen von Schräglage und Rollen unter Verwendung einer Trägheitsmessvorrichtung mit drei Achsen |
CN106772517A (zh) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | 基于双天线gnss接收机/陀螺仪信息融合的农机横滚角测试方法 |
CN107063241B (zh) * | 2017-03-16 | 2023-08-25 | 上海联适导航技术股份有限公司 | 基于双gnss天线及单轴mems陀螺的前轮测角系统 |
CN107943060B (zh) * | 2017-12-29 | 2021-12-07 | 上海司南卫星导航技术股份有限公司 | 一种自动驾驶仪、沿着跟踪直线引导车辆的方法以及计算机可读介质 |
CN110133694B (zh) * | 2019-04-18 | 2023-11-03 | 同济大学 | 基于双天线gnss航向和轮速辅助的车辆定位方法及系统 |
-
2019
- 2019-08-26 CN CN201910787902.7A patent/CN110530361B/zh active Active
-
2020
- 2020-04-24 AU AU2020100627A patent/AU2020100627A4/en not_active Ceased
Also Published As
Publication number | Publication date |
---|---|
AU2020100627A4 (en) | 2020-06-04 |
CN110530361A (zh) | 2019-12-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110530361B (zh) | 基于农业机械双天线gnss自动导航系统的转向角度估计器 | |
CN105203129B (zh) | 一种惯导装置初始对准方法 | |
US20210240192A1 (en) | Estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery | |
CN107063241A (zh) | 基于双gnss天线及单轴mems陀螺的前轮测角系统 | |
CN110077392B (zh) | 一种自动泊车定位系统的航迹推算方法 | |
CN104061899A (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
CN110341714B (zh) | 一种同时估计车辆质心侧偏角和扰动的方法 | |
CN114076610B (zh) | Gnss/mems车载组合导航系统的误差标定、导航方法及其装置 | |
AU2020104234A4 (en) | An Estimation Method and Estimator for Sideslip Angle of Straight-line Navigation of Agricultural Machinery | |
CN112326990A (zh) | 一种作业车辆测速方法及系统 | |
CN110133695A (zh) | 一种双天线gnss位置延迟时间动态估计系统及方法 | |
CN103217158A (zh) | 一种提高车载sins/od组合导航精度的方法 | |
WO2024120066A1 (zh) | 车辆导航方法、装置、电子设备及可读存储介质 | |
CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN114475581B (zh) | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 | |
CN206540555U (zh) | 基于双gnss天线及单轴mems陀螺的前轮测角系统 | |
CN112014599A (zh) | 一种集装箱卡车的轮速计速度修正方法及装置 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN109033017B (zh) | 一种丢包环境下的车辆侧倾角与俯仰角估计方法 | |
CN110926483B (zh) | 一种用于自动驾驶的低成本传感器组合定位系统及方法 | |
CN113048987A (zh) | 一种车载导航系统定位方法 | |
CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 | |
CN114932909A (zh) | 一种基于互补滤波实现加速度校正的坡度估算方法 | |
CN109596139B (zh) | 基于mems的车载导航方法 |
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 |