CN113419523A - 一种适用于无角度传感器的农机获得前轮期望角度的方法 - Google Patents

一种适用于无角度传感器的农机获得前轮期望角度的方法 Download PDF

Info

Publication number
CN113419523A
CN113419523A CN202110628073.5A CN202110628073A CN113419523A CN 113419523 A CN113419523 A CN 113419523A CN 202110628073 A CN202110628073 A CN 202110628073A CN 113419523 A CN113419523 A CN 113419523A
Authority
CN
China
Prior art keywords
vehicle
angle
front wheel
angle sensor
agricultural machine
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.)
Withdrawn
Application number
CN202110628073.5A
Other languages
English (en)
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.)
Wuxi Kalman Navigation Technology Co ltd
Original Assignee
Wuxi Kalman Navigation Technology Co ltd
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 Wuxi Kalman Navigation Technology Co ltd filed Critical Wuxi Kalman Navigation Technology Co ltd
Priority to CN202110628073.5A priority Critical patent/CN113419523A/zh
Publication of CN113419523A publication Critical patent/CN113419523A/zh
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface

Abstract

本发明涉及一种农机自动驾驶方法，具体说是适用于无角度传感器的农机获得前轮期望角度的方法。它先构建农机运动学模型，再采用LQR控制方法求解，即可得到反馈控制的前轮目标角度即期望角度，从而无需角度传感器，即可获得农机前轮的期望角度。采用该方法的农机无需角度传感器，工作效率较高，不会对农业的生产应用造成损失。

Description

一种适用于无角度传感器的农机获得前轮期望角度的方法
技术领域
本发明涉及一种农机自动驾驶方法，具体说是适用于无角度传感器的农机获得前轮期望角度的方法。
背景技术
随着农业自动化的发展、国家惠农政策的普及以及农民老龄化的比例上升，精准农业逐渐成为国际研究热点。精准农业的普及可以解放双手提高工作效率、提高土地利用率、及时播种收割、节约种子等投入，达到减少劳动力、减少投入、增加产量的目标。
在农机自动驾驶领域，传统的自动驾驶系统一般由四部分组成：一是终端显示单元比如平板。二是控制单元如ECU控制盒。三是执行机构如力矩电机或液压阀。四是角度传感器。其中角度传感器安装在农机前轮的转向节处，用于测量前轮转角的角度。由于角度传感器安装在前轮转向节处，农机工作环境比较恶劣，在工作过程中经常造成角度传感器的损坏或者连接线束的损坏。而角度传感器一旦不能工作，整个自动驾驶系统则处于瘫痪状态，即降低了农业生产的效率又耽误农时，给农业的生产应用带来了重大的损失。
发明内容
本发明要解决的技术问题是提供一种适用于无角度传感器的农机获得前轮期望角度的方法，采用该方法的农机无需安装角度传感器，工作效率较高，不会对农业的生产应用造成损失。
为解决上述问题，提供以下技术方案：
本发明的适用于无角度传感器的农机获得前轮期望角度的方法，其特征在于包括如下步骤：
第一步，构建农机运动学模型
当车辆没有安装角度传感器时，其前轮转角δ无法直接获得，根据阿克曼转向条件，可以间接得到车辆前轮转角：
Figure BDA0003103681620000011
由于车辆航向角速率
Figure BDA0003103681620000012
来自于GNSS测量航向
Figure BDA0003103681620000013
的差分或者车身天向陀螺，其精度比角度传感器要差，且具有较大的延迟滞后，所以由此间接估算的车辆前轮转角不再符合上述运动学模型；根据控制系统延迟特性和系统辨识结果，建立改进的车辆运动学模型如下：
Figure BDA0003103681620000021
其中，δ为车辆前轮转角；L为车辆前后轴轴距，直接测量得到；V为车辆后轴中心处前进速度，由车辆的GNSS测量得到；(x，y)为车辆在后轴中心在大地坐标系下的坐标，由车辆的GNSS测量得到；
Figure BDA0003103681620000022
为车辆航向角度，由车辆的GNSS测量得到；k为增益系数；τ为时间常数；δcmd为控制指令；
第二步，采用LQR控制方法求解车辆前轮转角δ
上述车辆运动学模型近似写成下面的状态空间表达式：
Figure BDA0003103681620000023
其中：
Figure BDA0003103681620000024
U＝δcmd
LQR控制的最优目标函数是：累计跟踪误差和累计控制输入的加权，其表达式为：
Figure BDA0003103681620000025
其中，J为目标函数；Q为非负定的状态加权权阵；R为正定的控制加权权阵；
采用传统方法解算黎卡提方程可得到最优反馈控制角度：
δf＝-KX
其中，K为状态反馈调节增益矩阵，由解算黎卡提方程后得到，δf：期望角度，即农机自动驾驶时用于控制农机行走的期望角度，从而实现无角度传感器的农机自动驾驶。
其中，第一步构建农机运动学模型中，k取2；τ：取0.2。
采取以上方案，具有以下优点：
由于本发明的适用于无角度传感器的农机获得前轮期望角度的方法先构建农机运动学模型，再采用LQR控制方法求解，即可得到反馈控制的前轮目标角度即期望角度，从而无需角度传感器，即可获得前轮期望角度。因而，采用本方法的农机不会因角度传感器损坏而处于瘫痪状态，从而大大提高了农业生产的效率，不会耽误农时，不会对农业的生产应用造成损失。
附图说明
图1是本发明的适用于无角度传感器的农机获得前轮期望角度的方法在给定农机初始横向偏差的仿真结果图；
图2是本发明的适用于无角度传感器的农机获得前轮期望角度的方法在给定农机初始横向/航向偏差的仿真结果图；
图3是本发明的适用于无角度传感器的农机获得前轮期望角度的方法在给定农机初始横向/航向/角度偏差仿真结果图。
具体实施方式
以下结合附图对本发明做进一步详细说明。
本发明的适用于无角度传感器的农机获得前轮期望角度的方法，其特征在于包括如下步骤：
第一步，构建农机运动学模型
当车辆没有安装角度传感器时，其前轮转角δ无法直接获得，根据阿克曼转向条件，可以间接得到车辆前轮转角：
Figure BDA0003103681620000031
由于车辆航向角速率
Figure BDA0003103681620000032
来自于GNSS测量航向
Figure BDA0003103681620000033
的差分或者车身天向陀螺，其精度比角度传感器要差，且具有较大的延迟滞后，所以由此间接估算的车辆前轮转角不再符合上述运动学模型；根据控制系统延迟特性和系统辨识结果，建立改进的车辆运动学模型如下：
Figure BDA0003103681620000041
其中，δ为车辆前轮转角；L为车辆前后轴轴距，直接测量得到；V为车辆后轴中心处前进速度，由车辆的GNSS测量得到；(x，y)为车辆在后轴中心在大地坐标系下的坐标，由车辆的GNSS测量得到；
Figure BDA0003103681620000042
为车辆航向角度，由车辆的GNSS测量得到；k为增益系数，这里取值2。；τ为时间常数，这里取0.2；δcmd为控制指令。
第二步，采用LQR控制方法求解车辆前轮转角δ
上述车辆运动学模型近似写成下面的状态空间表达式：
Figure BDA0003103681620000043
其中：
Figure BDA0003103681620000044
LQR控制的最优目标函数是：累计跟踪误差和累计控制输入的加权，其表达式为：
Figure BDA0003103681620000045
其中，J为目标函数；Q为非负定的状态加权权阵；R为正定的控制加权权阵。
采用传统方法解算黎卡提方程可得到最优反馈控制角度：
δf＝-KX
其中，K为状态反馈调节增益矩阵，由解算黎卡提方程后得到，δf：期望角度，即农机自动驾驶时用于控制农机行走的期望角度，从而实现无角度传感器的农机自动驾驶。
给定农机的初始横向偏差为0.3米，初始航向偏差和前轮角度偏差为0，采用本发明适用于无角度传感器的农机获得前轮期望角度的方法进行仿真如图1所示。仿真结果表明5秒时间横向偏差即可收敛到2.5厘米以内，满足农机自动驾驶的需求。
给定农机的初始横向偏差为0.3米，初始车头航向偏差为-0.1弧度即-5.7度，初始前轮角度偏差为0，采用本发明适用于无角度传感器的农机获得前轮期望角度的方法进行仿真如图2所示。仿真结果表明4秒时间内横向偏差即可收敛到2.5cm以内，车头航向偏差即可收敛到2度以内，6秒时间可以完全收敛。满足农机自动驾驶的需求。
给定农机的初始横向偏差为0.3米，初始车头航向偏差为-0.1弧度即-5.7度，初始前轮角度偏差为0.1弧度即5.7度，采用本发明适用于无角度传感器的农机获得前轮期望角度的方法进行仿真如图3所示。仿真结果表明1秒时间前轮角度偏差可以收敛到1度以内，5秒时间横向偏差可以收敛到2.5厘米以内，航向偏差收敛到2度以内。完全满足农机自动驾驶的需求。

Claims (2)

1.一种适用于无角度传感器的农机获得前轮期望角度的方法，其特征在于包括如下步骤：
第一步，构建农机运动学模型
当车辆没有安装角度传感器时，其前轮转角δ无法直接获得，根据阿克曼转向条件，可以间接得到车辆前轮转角：
Figure FDA0003103681610000011
由于车辆航向角速率
Figure FDA0003103681610000012
来自于GNSS测量航向
Figure FDA0003103681610000013
的差分或者车身天向陀螺，其精度比角度传感器要差，且具有较大的延迟滞后，所以由此间接估算的车辆前轮转角不再符合上述运动学模型；根据控制系统延迟特性和系统辨识结果，建立改进的车辆运动学模型如下：
Figure FDA0003103681610000014
其中，δ为车辆前轮转角；L为车辆前后轴轴距，直接测量得到；V为车辆后轴中心处前进速度，由车辆的GNSS测量得到；(x，y)为车辆在后轴中心在大地坐标系下的坐标，由车辆的GNSS测量得到；
Figure FDA0003103681610000015
为车辆航向角度，由车辆的GNSS测量得到；k为增益系数；τ为时间常数；δcmd为控制指令；
第二步，采用LQR控制方法求解车辆前轮转角δ
上述车辆运动学模型近似写成下面的状态空间表达式：
Figure FDA0003103681610000016
其中：
Figure FDA0003103681610000017
U＝δcmd
LQR控制的最优目标函数是：累计跟踪误差和累计控制输入的加权，其表达式为：
Figure FDA0003103681610000021
其中，J为目标函数；Q为非负定的状态加权权阵；R为正定的控制加权权阵；
采用传统方法解算黎卡提方程可得到最优反馈控制角度：
δf＝-KX
其中，K为状态反馈调节增益矩阵，由解算黎卡提方程后得到，δf：期望角度，即农机自动驾驶时用于控制农机行走的期望角度，从而实现无角度传感器的农机自动驾驶。
2.如权利要求1所述的适用于无角度传感器的农机获得前轮期望角度的方法，其特征在于第一步构建农机运动学模型中，k取2；τ：取0.2。
CN202110628073.5A 2021-06-08 2021-06-08 一种适用于无角度传感器的农机获得前轮期望角度的方法 Withdrawn CN113419523A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110628073.5A CN113419523A (zh) 2021-06-08 2021-06-08 一种适用于无角度传感器的农机获得前轮期望角度的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110628073.5A CN113419523A (zh) 2021-06-08 2021-06-08 一种适用于无角度传感器的农机获得前轮期望角度的方法

Publications (1)

Publication Number Publication Date
CN113419523A true CN113419523A (zh) 2021-09-21

Family

ID=77713907

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110628073.5A Withdrawn CN113419523A (zh) 2021-06-08 2021-06-08 一种适用于无角度传感器的农机获得前轮期望角度的方法

Country Status (1)

Country Link
CN (1) CN113419523A (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101093396A (zh) * 2007-07-04 2007-12-26 华南农业大学 一种农业机械的导航控制方法
CN101866181A (zh) * 2009-04-16 2010-10-20 中国农业大学 农用机械的导航方法、导航装置和农用机械
CN104477237A (zh) * 2014-11-11 2015-04-01 深圳职业技术学院 一种四轮独立转向电动车转向控制方法及系统
CN110348115A (zh) * 2019-07-09 2019-10-18 顺丰科技有限公司 一种叉车的lqr控制方法、装置、存储介质和控制器

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101093396A (zh) * 2007-07-04 2007-12-26 华南农业大学 一种农业机械的导航控制方法
CN101866181A (zh) * 2009-04-16 2010-10-20 中国农业大学 农用机械的导航方法、导航装置和农用机械
CN104477237A (zh) * 2014-11-11 2015-04-01 深圳职业技术学院 一种四轮独立转向电动车转向控制方法及系统
CN110348115A (zh) * 2019-07-09 2019-10-18 顺丰科技有限公司 一种叉车的lqr控制方法、装置、存储介质和控制器

Similar Documents

Publication Publication Date Title
Kayacan et al. Experimental validation of linear and nonlinear MPC on an articulated unmanned ground vehicle
AU2009217477B2 (en) Control of passive, towed implements
Eaton et al. Autonomous farming: Modelling and control of agricultural machinery in a unified framework
CN108646747A (zh) 农用车辆路径跟踪控制方法
Oksanen et al. Guidance system for agricultural tractor with four wheel steering
CN111133853B (zh) 一种拖拉机耕深多参数联合自动调节系统及方法
WO2012027082A1 (en) Automatic control of passive, towed implements
EP3254547A1 (en) System and method for vehicle steering calibration
Wang et al. Adaptive turning control for an agricultural robot tractor
Raikwar et al. Navigation and control development for a four-wheel-steered mobile orchard robot using model-based design
CN109804757A (zh) 基于卫星导航的自动化播深调控伸缩式平台
Oksanen Accuracy and performance experiences of four wheel steered autonomous agricultural tractor in sowing operation
Yin et al. Trajectory tracking based on adaptive sliding mode control for agricultural tractor
Jingtao et al. Cascaded navigation control for agricultural vehicles tracking straight paths
CN113419523A (zh) 一种适用于无角度传感器的农机获得前轮期望角度的方法
Wang et al. Review of electro-hydraulic hitch system control method of automated tractors
CN112148020B (zh) 一种基于eps的农机自动驾驶系统及控制方法
US20200050205A1 (en) System and method for updating a mapped area
Ding et al. Design and experiment of motion controller for information collection platform in field with Beidou positioning
EP3618600B1 (en) Guidance working depth compensation
Eaton et al. Robust sliding mode control of an agricultural tractor under the influence of slip
CN114485680A (zh) 一种秸秆旋埋还田导航系统设计与试验方法
Ospina et al. Improved inclination correction method applied to the guidance system of agricultural vehicles.
CN113960921B (zh) 果园履带车辆视觉导航控制方法与系统
CN115576326A (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
WW01 Invention patent application withdrawn after publication

Application publication date: 20210921

WW01 Invention patent application withdrawn after publication