CN108415435B - 一种农业机械圆曲线自动驾驶控制方法 - Google Patents
一种农业机械圆曲线自动驾驶控制方法 Download PDFInfo
- Publication number
- CN108415435B CN108415435B CN201810295678.5A CN201810295678A CN108415435B CN 108415435 B CN108415435 B CN 108415435B CN 201810295678 A CN201810295678 A CN 201810295678A CN 108415435 B CN108415435 B CN 108415435B
- Authority
- CN
- China
- Prior art keywords
- equation
- vehicle
- definite symmetric
- positive
- weighting array
- 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 title claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000005516 engineering process Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
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/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means
Abstract
本发明提供了一种农业机械圆曲线自动驾驶控制方法,包括以下步骤:步骤(1):建立车辆运动学模型;步骤(2):获取实时数据,并计算得到半正定对称加权阵、正半定对称时变加权阵和正定对称时变加权阵;步骤(3):基于步骤(2)计算控制量;步骤(4):通过PWM技术控制电磁阀或者电机驱动前轮转向,从而实现圆曲线路径跟踪自动控制,本发明针对实际应用中对圆目标轨迹跟踪需求,设计了一种圆曲线自动控制方法,进而填补这一技术空白。
Description
技术领域
本发明涉及自动驾驶控制算法领域,具体涉及到一种农业机械圆曲线自动驾驶控制方法。
背景技术
随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展,精准农业正在快速变成一种趋势。目前农业机械自动驾驶大部分是以AB直线的行驶进行作业,然而在实际使用过程中,对于一些丘陵或者特殊应用的场合,需要使用圆曲线进行自动驾驶。
发明内容
为了解决上述的缺陷,本发明提供了一种农业机械圆曲线自动驾驶控制方法,本发明针对实际应用中对圆目标轨迹跟踪需求,设计了一种圆曲线自动控制方法,进而填补这一技术空白。
本发明提供了一种农业机械圆曲线自动驾驶控制方法,包括以下步骤:
步骤(1):建立车辆运动学模型;
步骤(2):获取实时数据,并计算得到半正定对称加权阵、正半定对称时变加权阵和正定对称时变加权阵;
步骤(3):基于步骤(2)计算控制量;
步骤(4):通过PWM技术控制电磁阀或者电机驱动前轮转向,从而实现圆曲线路径跟踪自动控制。
上述的自动驾驶控制方法,其中,所述步骤(1)具体包括:
步骤(1.1):基于圆曲线运动模型,得到其中R0为圆半径,α0为圆轨迹起始角度,α为车辆转过的角度,l2为质心到控制点的距离,其中,控制点和参考点重合,l2=0,ξ为航向修正角,在公式(1)中,定义ξ顺时针为正,则车辆控制点位于参考点的后面,所以l2<0;
步骤(1.2):在惯性坐标系下,车辆的航向角计算公式为:
其中ψe、ψ、δ、l1、Vx、r分别为航向偏差、航向角、前轮转角、前后轮距离、车速、曲率半径;
步骤(1.3):车辆稳态前轮角度计算方式为:
步骤(1.4):车辆前轮角度偏差动态模型为:
步骤(1.5):基于公式4,如果曲率半径恒定,则公式4变化为公式5;
上述的自动驾驶控制方法,其中,所述步骤(1)还包括:
步骤(1.6):基于公式5得到车辆跟踪误差模型为:
x=[ψe δe re]T
y(t)=C(t)x(t)
e(t)=yr(t)-y(t) (公式7)
其中,x为状态变量,A为状态转移矩阵,B为控制量系数,C为观测矩阵,yr为目标值向量。
上述的自动驾驶控制方法,其中,所述步骤(2)还包括:
基于步骤(1)中的公式7,选择以下性能指标函数:
其中F为半正定对称加权阵,Q(t)为正半定对称时变加权阵,R(t)为正定对称时变加权阵,t0、tf分别为起始和结束时间。
上述的自动驾驶控制方法,其中,所述步骤(3)包括:根据计算黎卡提方程:
u(t)=-R-1BTP(t)x(t) (公式10)。
本发明具有以下有益效果:
(1)本发明实现的圆曲线自动控制方法简单可行,计算量小对硬件要求低。
(2)本发明的圆曲线自动控制方法控制目标跟踪精度在±2.5cm,满足实际用户需求。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图,重点在于示出本发明的主旨。
图1为本发明提供的圆曲线运动模型示意图。
具体实施方式
在下文的描述中,给出了大量具体的细节以便提供对本发明更为彻底的理解。然而,对于本领域技术人员而言显而易见的是,本发明可以无需一个或多个这些细节而得以实施。在其他的例子中,为了避免与本发明发生混淆,对于本领域公知的一些技术特征未进行描述。
为了彻底理解本发明,将在下列的描述中提出详细的步骤以及详细的结构,以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下,然而除了这些详细描述外,本发明还可以具有其他实施方式。
如图1所示,本发明的圆曲线运动模型示意图,图中R0为圆半径,α0为圆轨迹起始角度,α为车辆转过的角度,l2为质心到控制点的距离,如果控制点和参考点重合,l2=0,ξ为航向修正角。
在公式(1)中,定义ξ顺时针为正,在图1中,车辆控制点位于参考点的后面,所以l2<0。
在惯性坐标系下,车辆的航向角计算公式如公式(2)所示:
其中ψe、ψ、δ、l1、Vx、r分别为航向偏差、航向角、前轮转角、前后轮距离、车速、曲率半径。
车辆稳态前轮角度计算通过公式3计算:
车辆前轮角度偏差动态模型:
在公式4中,如果曲率半径恒定,则公式4变化为公式5:其中u为控制量
车辆跟踪误差模型:
由以上模型分析,建立车辆运动学模型,模型建立和控制量计算:
x=[ψe δe re]T
y(t)=C(t)x(t)
e(t)=yr(t)-y(t) (公式7)
其中,x为状态变量,A为状态转移矩阵,B为控制量系数,C为观测矩阵,yr为目标值向量。
选择以下性能指标函数:
以下提供一具体的实施例
实施例1
一种农业机械圆曲线自动驾驶控制方法,包括步骤(1):建立车辆运动学模型,具体为通过上述公式2、4、6建立运动学模型,如公式7所示。
步骤(2):获取实时数据,计算公式7中的x,A,B,C和e,计算公式8中的F、Q、R、t0、tf;
步骤(3):根据计算黎卡提方程:
u(t)=-R-1BTP(t)x(t) (公式10)
步骤(4):通过PWM技术控制电磁阀或者电机驱动前轮转向,从而实现圆曲线路径跟踪自动控制。本实施例实现的圆曲线自动控制方法简单可行,计算量小对硬件要求低,以及圆曲线自动控制方法控制目标跟踪精度在±2.5cm,满足实际用户需求。
以上对本发明的较佳实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施;任何熟悉本领域的技术人员,在不脱离本发明技术方案范围情况下,都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰,或修改为等同变化的等效实施例,这并不影响本发明的实质内容。因此,凡是未脱离本发明技术方案的内容,依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰,均仍属于本发明技术方案保护的范围内。
Claims (1)
1.一种农业机械圆曲线自动驾驶控制方法,其特征在于,包括以下步骤:
步骤(1):建立车辆运动学模型;
步骤(2):获取实时数据,并计算得到半正定对称加权阵、正半定对称时变加权阵和正定对称时变加权阵;
步骤(3):基于步骤(2)计算控制量;步骤(4):通过PWM技术控制电磁阀或者电机驱动前轮转向,从而实现圆曲线路径跟踪自动控制,所述步骤(1)具体包括:
步骤(1.1):基于圆曲线运动模型,得到其中R0为圆半径,l2为质心到控制点的距离,其中,控制点和参考点重合,l2=0,ξ为航向修正角,在公式(1)中,定义ξ顺时针为正,则车辆控制点位于参考点的后面,所以l2<0;
步骤(1.2):在惯性坐标系下,车辆的航向角计算公式为:
步骤(1.3):车辆稳态前轮角度计算方式为:
步骤(1.4):车辆前轮角度偏差动态模型为:
步骤(1.6):基于公式5得到车辆跟踪误差模型为:
x=[ψe δe re]T
y(t)=C(t)x(t)
e(t)=yr(t)-y(t) (公式7)
其中,x为状态变量,A为状态转移矩阵,B为控制量系数,C为观测矩阵,yr为目标值向量,所述步骤(2)还包括:
基于步骤(1)中的公式7,选择以下性能指标函数:
u(t)=-R-1BTP(t)x(t) (公式10)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810295678.5A CN108415435B (zh) | 2018-04-04 | 2018-04-04 | 一种农业机械圆曲线自动驾驶控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810295678.5A CN108415435B (zh) | 2018-04-04 | 2018-04-04 | 一种农业机械圆曲线自动驾驶控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108415435A CN108415435A (zh) | 2018-08-17 |
CN108415435B true CN108415435B (zh) | 2021-08-31 |
Family
ID=63134436
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810295678.5A Active CN108415435B (zh) | 2018-04-04 | 2018-04-04 | 一种农业机械圆曲线自动驾驶控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108415435B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109358621B (zh) * | 2018-09-30 | 2019-05-07 | 山东省计算中心(国家超级计算济南中心) | 自动驾驶汽车轨迹跟踪控制方法 |
CN109407678A (zh) * | 2018-12-27 | 2019-03-01 | 上海司南卫星导航技术股份有限公司 | 一种车辆驾驶控制系统以及计算机可读介质 |
CN112985401A (zh) * | 2021-02-02 | 2021-06-18 | 上海华测导航技术股份有限公司 | 一种headline全路径规划和跟踪方法 |
CN113290554B (zh) * | 2021-04-28 | 2022-06-17 | 浙江工业大学 | 一种基于值迭代的Baxter机械臂智能优化控制方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101093396A (zh) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | 一种农业机械的导航控制方法 |
CN104571109A (zh) * | 2015-01-04 | 2015-04-29 | 南京农业大学 | 一种基于扰动观测器的农用车辆自主导航转向控制方法 |
CN105005196A (zh) * | 2015-05-14 | 2015-10-28 | 南京农业大学 | 农用车辆自主导航转向控制方法 |
CN105867377A (zh) * | 2016-04-13 | 2016-08-17 | 浙江理工大学 | 一种农业机械自动导航控制方法 |
CN106647770A (zh) * | 2017-01-22 | 2017-05-10 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的田间掉头路径规划及其控制方法 |
CN106681335A (zh) * | 2017-01-22 | 2017-05-17 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的避障路径规划及其控制方法 |
-
2018
- 2018-04-04 CN CN201810295678.5A patent/CN108415435B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101093396A (zh) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | 一种农业机械的导航控制方法 |
CN104571109A (zh) * | 2015-01-04 | 2015-04-29 | 南京农业大学 | 一种基于扰动观测器的农用车辆自主导航转向控制方法 |
CN105005196A (zh) * | 2015-05-14 | 2015-10-28 | 南京农业大学 | 农用车辆自主导航转向控制方法 |
CN105867377A (zh) * | 2016-04-13 | 2016-08-17 | 浙江理工大学 | 一种农业机械自动导航控制方法 |
CN106647770A (zh) * | 2017-01-22 | 2017-05-10 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的田间掉头路径规划及其控制方法 |
CN106681335A (zh) * | 2017-01-22 | 2017-05-17 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的避障路径规划及其控制方法 |
Non-Patent Citations (3)
Title |
---|
农机自动驾驶监控终端关键技术研究与系统开发;张亚娇;《中国优秀硕士学位论文全文数据库 信息科技辑》;20170315;全文 * |
拖拉机自动驾驶液压转向控制系统分析及仿真;张媛媛;《中国优秀硕士学位论文全文数据库 农业科技辑》;20170615;全文 * |
自动驾驶农机转向控制研究;连志鹏;《中国优秀硕士学位论文全文数据库 农业科技辑》;20161215;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN108415435A (zh) | 2018-08-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108415435B (zh) | 一种农业机械圆曲线自动驾驶控制方法 | |
CN108490943B (zh) | 一种农业机械自适应曲线自动驾驶控制方法 | |
CN105867377B (zh) | 一种农业机械自动导航控制方法 | |
CN107415939B (zh) | 一种分布式驱动电动汽车转向稳定性控制方法 | |
CN103676949B (zh) | 车道线跟踪控制系统及其控制方法 | |
CN111301425B (zh) | 用于自动驾驶车辆的利用动态模型的有效最优控制 | |
CN105172793B (zh) | 自动驾驶汽车的位姿估算方法 | |
CN108614426A (zh) | 一种基于干扰观测器的多移动机器人编队鲁棒控制方法 | |
CN104811588B (zh) | 一种基于陀螺仪的船载稳像控制方法 | |
CN105929842A (zh) | 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法 | |
CN111123950B (zh) | 行车控制方法、装置及车辆 | |
CN111045432A (zh) | 一种欠驱动水面船非线性路径跟踪控制系统及方法 | |
CN110162046A (zh) | 基于事件触发型模型预测控制的无人车路径跟随方法 | |
CN108062024B (zh) | 一种考虑阻力的移动机器人反演滑模控制方法 | |
CN109204457A (zh) | 确定用于机动车辆应用的转向角 | |
CN109739094A (zh) | 一种基于自适应滑模控制的移动机器人轨迹跟踪新方法 | |
CN111497872A (zh) | 一种自动生成偏离因子修正轨迹漂移的方法 | |
CN108931986B (zh) | 一种两轮汽车自平衡控制方法、装置及存储介质 | |
CN113534817A (zh) | 移动机器人动力学建模和轨迹跟踪控制方法及装置 | |
CN107807516A (zh) | 一种基于模糊自适应pid控制的模拟转台控制系统 | |
EP3410074B1 (en) | Method and device for improving performance of relative-position sensor, and computer storage medium | |
CN111976716A (zh) | 一种低成本自适应巡航弯道半径估算及速度控制方法 | |
CN103954289B (zh) | 一种光学成像卫星敏捷机动姿态确定方法 | |
CN108068938B (zh) | 一种双轮车速度控制方法及系统 | |
CN103170976B (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 |