CN113467460A - 一种基于双圆形前视距离的农业机械路径跟踪方法及*** - Google Patents
一种基于双圆形前视距离的农业机械路径跟踪方法及*** Download PDFInfo
- Publication number
- CN113467460A CN113467460A CN202110779238.9A CN202110779238A CN113467460A CN 113467460 A CN113467460 A CN 113467460A CN 202110779238 A CN202110779238 A CN 202110779238A CN 113467460 A CN113467460 A CN 113467460A
- Authority
- CN
- China
- Prior art keywords
- path
- circular
- agricultural machine
- steer
- feedforward
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000009977 dual effect Effects 0.000 claims description 8
- 238000010276 construction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Guiding Agricultural Machines (AREA)
Abstract
本发明提供了一种基于双圆形前视距离的农业机械路径跟踪方法及***,属于智能农机领域。本发明具体为:前馈控制器基于规划路径的路径点构造前馈期望转角,反馈控制器基于圆形前视距离计算导航偏差,并采用PID算法计算期望转角;基于前馈期望转角、期望转角,得到路径跟踪转角,导航控制器根据路径跟踪转角,得到期望转角,再结合农机当前转角控制电动方向盘,实现农业机械路径跟踪。本发明解决了直线前视距离在曲线路径跟踪中的常值横向偏差问题,减少路径跟踪的稳态误差。
Description
技术领域
本发明属于智能农机领域,具体涉及一种基于双圆形前视距离的农业机械路径跟踪方法及***。
背景技术
农业机械路径跟踪是自动驾驶***的重要部分,基于纯追踪模型的路径跟踪方法具有无需建模的特点,其在农机直线路径跟踪中精度较高,但受前视点距离和路径曲率的影响,其转弯掉头时的精度有待进一步提高。随着无人驾驶农机应用场合和作业效率局限性日益彰显,纯追踪算法对前视距离要求较高的问题亟待解决。为实现全田块农机的自动驾驶,需要根据路径曲率和车辆速度动态调整前视距离,研究人员相继提出基于模糊逻辑和神经网络的前视距离动态调整方法,但其实施困难且应用效果不佳。
发明内容
针对现有技术中存在不足,本发明提供了一种基于双圆形前视距离的农业机械路径跟踪方法及***,基于圆形前视轨迹解决直线前视距离导致的横向偏差偏置,实现农机自动掉头转弯和快速直线行驶时高精度的路径跟踪。
本发明是通过以下技术手段实现上述技术目的的。
一种基于双圆形前视距离的农业机械路径跟踪方法,具体为:
前馈控制器基于规划路径的路径点构造前馈期望转角δFF_steer;反馈控制器基于圆形前视距离计算导航偏差Dos,并采用PID算法计算期望转角δFB_steer;所述 其中Kp、Kd、Ki为导航误差项的对应系数;
基于前馈期望转角δFF_steer、期望转角δFB_steer,得到路径跟踪转角δsteer=δFF_steer+δFB_steer;
进一步地,所述前馈控制器基于规划路径的路径点构造前馈期望转角δFFsteer,具体为:
在高斯平面坐标系下,选择农机前进方向上规划路径的三个路径点A(xa,ya)、B(xb,yb)、C(xc,yc),构造过所述三各路径点圆形的圆心坐标(Xff,Yff)和半径Rff,并根据农机轴距LJK和半径Rff计算前馈期望转角δFF_steer=arctan(LJK/Rff)。
更进一步地,所述圆心坐标(Xff,Yff)计算过程为:
设线段AB的斜率为k1,线段BC的斜率为k2,则有:
更进一步地,所述反馈控制器基于圆形前视距离计算导航偏差Dos,具体为:
假设当前时刻农机转向角可用于下一更新时刻,基于当前时刻农机转向角构造预测圆形路径Spre;基于当前时刻农机位置M和规划路径上的路径点A、B得到下一时刻的期望圆形路径Sdes;以农机当前位置(XC,YC)为圆心,以半径为构造圆形轨迹Sveh_1、Sveh_2,所述圆形轨迹Sveh_1、Sveh_2分别与路径Spre、Sdes在农机行进方向前方相交于两点(X1_1,Y1_1)、(X1_2,Y1_2)和(X2_1,Y2_1)、(X2_2,Y2_2),则有导航偏差:
且有Dos=m Dos_1+n Dos_2,m、n分别为圆形轨迹Svel_1、Svel_2计算导航偏差的权重系数。
更进一步地,所述构造预测圆形路径Spre,具体为:
设(Xcp,Ycp)、Rcp分别为Spre的圆心和半径;
其中,θ为农机航向角。
更进一步地,所述交点(X1_1,Y1_1)、(X1_2,Y1_2)的计算公式为:
X1_1=Xc+a1(Xcp-Xc)/L±b1(Ycp-Yc)/L
X1_2=Xc+a2(Xcp-Xc)/T±b2(Ycp-Yc)/T
其中:中间量
更进一步地,所述期望圆形路径Sdes,具体为:
设(Xdp,Ydp)、Rdp分别为期望圆形路径Sdes的圆心和半径,期望路径上存在三个路径点记为M(XC,YC)、A(xa,ya)、B(xb,yb),其中A为距离当前农机位置最近路径点的下一路径点;则:
其中k3为线段MA的斜率。
更进一步地,所述交点(X2_1,Y2_1)、(X2_2,Y2_2)的计算公式为:
X2_1=Xc+a1~(Xcp-Xc)/L±b1~(Ycp-Yc)/L
X2_2=Xc+a2~(Xcp-Xc)/T±b2~(Ycp-Yc)/T
其中:中间量
一种基于双圆形前视距离的农业机械路径跟踪***,包括:
RTK定位模块,用于采集地理信息生成导航路径点;
前馈控制器,根据路径点决策出前馈期望转角δFF_steer;
反馈控制器,根据前视圆形距离计算反馈期望转角δFB_steer;
速度传感器,测量农机速度,用以设置反馈控制器计算的导航偏差Dos_1、Dos_2权重系数;
导航控制器,接收前馈转角δFF_steer和反馈转角δFB_steer,并得到期望转角δsteerCom;
角度传感器,用于测量农机当前转角。
本发明的有益效果为:
1)本发明基于圆形前视距离计算导航偏差,解决直线前视距离在曲线路径跟踪中的常值横向偏差,使得路径跟踪的稳态误差进一步的减小,提高的纯跟踪模型理论精度和适应场合;
2)本发明将前视距离的动态调整问题转化为双前视圆形半径的构造上,可以根据农机规划路径曲率和作业速度,调整导航偏差Dos_1、Dos_2的权重系数,使得期望转角δFB_steer决策更精确。
附图说明
图1为本发明农机当前转向角预测圆形轨迹图;
图2为本发明农机双前视圆形距离导航偏差计算示意图。
具体实施方式
下面结合附图以及具体实施例对本发明作进一步的说明,但本发明的保护范围并不限于此。
一种基于双圆形前视距离的农业机械路径跟踪***,包括RTK定位模块、前馈控制器、反馈控制器、速度传感器、导航控制器和角度传感器,其中RTK定位模块采集地理信息生成导航路径点,前馈控制器根据路径点决策出前馈期望转角δFF_steer,反馈控制器根据前视圆形距离计算反馈期望转角δFB_steer,速度传感器测量农机速度,并基于模糊逻辑控制设置反馈控制器计算的导航偏差Dos_1、Dos_2权重系数(为现有技术),导航控制器接收前馈转角δFF_steer和反馈转角δFB_steer并输出期望转角δsteerCom,角度传感器测量农机当前转角,并与δsteerCom相减得到偏差,利用PID计算得到输出转角命令。
一种基于双圆形前视距离的农业机械路径跟踪方法,具体为:前馈控制器基于规划路径的路径点构造前馈期望转角δFF_steer;反馈控制器基于圆形前视距离计算导航偏差Dos,并采用PID算法计算期望转角δFB_steer;基于前馈期望转角δFF_steer、期望转角δFB_steer,得到路径跟踪转角δsteer=δFF_steer+δFB_steer;导航控制器根据路径跟踪转角,得到期望转角再结合农机当前转角控制电动方向盘,实现农业机械路径跟踪;其中a、b分别为角速度和角度对应的权值系数。
如图1所示,前馈控制器基于规划路径的路径点构造前馈期望转角δFFsteer的步骤包括:在高斯平面坐标系下,选择农机前进方向上规划路径的三个路径点A(xa,ya)、B(xb,yb)、C(xc,yc),构造过三点圆形(即过A、B、C三个路径点)的圆心坐标(Xff,Yff)和半径Rff,并根据农机轴距LJK和半径Rff计算前馈期望转角δFF_steer=arctan(LJK/Rff)。
过三点圆形的圆心坐标(Xff,Yff)计算过程为:设线段AB的斜率为k1,线段BC的斜率为k2,则有:
过三点圆形的半径Rff的计算过程为:1)找出距离农机当前位置最近的路径点,将在农机前进方向的下一点A作为农机导航决策下一更新周期的路径参考点;2)由A点坐标和圆心坐标计算得到:
如图2所示,反馈控制器基于圆形前视距离计算导航偏差的步骤包括:
1)假设当前时刻农机转向角可用于下一更新时刻,则基于当前时刻农机转向角可构造预测圆形路径Spre;2)基于当前时刻农机位置M和规划路径上的两点A、B可得到下一时刻的期望圆形路径Sdes;3)以农机当前位置(XC,XC)为圆心,以半径为构造圆形轨迹Sveh_1、Sveh_2,圆形轨迹Sveh_1、Sveh_2分别与轨迹Spre、Sdes在农机行进方向前方相交于两点(X1_1,Y1_1)、(X1_2,Y1_2)和(X2_1,Y2_1)、(X2_2,Y2_2),则有导航偏差 且有Dos=mDos_1+nDos_2,m、n分别为圆形轨迹Svel_1、Svel_2计算导航偏差的权重系数。
如图1所示,预测圆形路径Spre的构造步骤为:设(Xcp,Ycp)、Rcp分别为Spre的圆心和半径,计算方法为:
其中,θ为农机航向角。
Sveh_1与Spre、Sdes的交点(X1_1,Y1_1)、(X1_2,Y1_2)的计算步骤为:
X1_1=Xc+a1(Xcp-Xc)/L±b1(Ycp-Yc)/L (10)
X1_2=Xc+a2(Xcp-Xc)/T±b2(Ycp-Yc)/T (15)
其中:L、a1、b1、T、a2、b2为计算过程中间变量。
期望圆形路径Sdes的获取步骤为:设(Xdp,Ydp)、Rdp分别为期望圆形路径Sdes的圆心和半径,期望路径上存在三个路径点记为M(XC,YC)、A(xa,ya)、B(xb,yb),其中M为农机当前位置,A为距离当前农机位置最近路径点的下一路径点,且线段MA和AB的斜率分别为k3、k1,则有:
Sveh_2与Spre、Sdes的交点(X2_1,Y2_1)、(X2_2,Y2_2)的计算步骤为:
X2_1=Xc+a1~(Xcp-Xc)/L±b1~(Ycp-Yc)/L (23)
X2_2=Xc+a2~(Xcp-Xc)/T±b2~(Ycp-Yc)/T (28)
其中:a1~、b1~、a2~、b2~为计算过程中间变量。
所述实施例为本发明的优选的实施方式,但本发明并不限于上述实施方式,在不背离本发明的实质内容的情况下,本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。
Claims (9)
2.根据权利要求1所述的基于双圆形前视距离的农业机械路径跟踪方法,其特征在于,所述前馈控制器基于规划路径的路径点构造前馈期望转角δFF_steer,具体为:
在高斯平面坐标系下,选择农机前进方向上规划路径的三个路径点A(xa,ya)、B(xb,yb)、C(xc,yc),构造过所述三各路径点圆形的圆心坐标(Xff,Yff)和半径Rff,并根据农机轴距LJK和半径Rff计算前馈期望转角δFF_steer=arctan(LJK/Rff)。
4.根据权利要求2所述的基于双圆形前视距离的农业机械路径跟踪方法,其特征在于,所述反馈控制器基于圆形前视距离计算导航偏差Dos,具体为:
假设当前时刻农机转向角可用于下一更新时刻,基于当前时刻农机转向角构造预测圆形路径Spre;基于当前时刻农机位置M和规划路径上的路径点A、B得到下一时刻的期望圆形路径Sdes;以农机当前位置(XC,YC)为圆心,以半径为构造圆形轨迹Sveh_1、Sveh_2,所述圆形轨迹Sveh_1、Sveh_2分别与路径Spre、Sdes在农机行进方向前方相交于两点(X1_1,Y1_1)、(X1_2,Y1_2)和(X2_1,Y2_1)、(X2_2,Y2_2),则有导航偏差:
且有Dos=mDos_1+nDos_2,m、n分别为圆形轨迹Svel_1、Svel_2计算导航偏差的权重系数。
9.一种实现权利要求1-8任一项所述的基于双圆形前视距离的农业机械路径跟踪方法的农业机械路径跟踪***,其特征在于,包括:
RTK定位模块,用于采集地理信息生成导航路径点;
前馈控制器,根据路径点决策出前馈期望转角δFF_steer;
反馈控制器,根据前视圆形距离计算反馈期望转角δFB_steer;
速度传感器,测量农机速度,用以设置反馈控制器计算的导航偏差Dos_1、Dos_2权重系数;
导航控制器,接收前馈转角δFF_steer和反馈转角δFB_steer,并得到期望转角δsteerCom;
角度传感器,用于测量农机当前转角。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110779238.9A CN113467460B (zh) | 2021-07-09 | 2021-07-09 | 一种基于双圆形前视距离的农业机械路径跟踪方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110779238.9A CN113467460B (zh) | 2021-07-09 | 2021-07-09 | 一种基于双圆形前视距离的农业机械路径跟踪方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113467460A true CN113467460A (zh) | 2021-10-01 |
CN113467460B CN113467460B (zh) | 2024-03-12 |
Family
ID=77879589
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110779238.9A Active CN113467460B (zh) | 2021-07-09 | 2021-07-09 | 一种基于双圆形前视距离的农业机械路径跟踪方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113467460B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106909150A (zh) * | 2017-01-22 | 2017-06-30 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的避障、掉头路径规划及其控制方法 |
CN107992069A (zh) * | 2017-11-29 | 2018-05-04 | 上海无线电设备研究所 | 一种无人机路径跟踪控制的制导律设计方法 |
CN109388140A (zh) * | 2018-09-13 | 2019-02-26 | 江苏大学 | 一种改进的用于地面车辆路径跟踪的纯追踪控制方法 |
CN109947123A (zh) * | 2019-02-27 | 2019-06-28 | 南京航空航天大学 | 一种基于视线导引律的无人机路径跟踪与自主避障方法 |
US20200133289A1 (en) * | 2018-10-31 | 2020-04-30 | Ubtech Robotics Corp Ltd | Path tracking method and mobile robot using the same |
-
2021
- 2021-07-09 CN CN202110779238.9A patent/CN113467460B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106909150A (zh) * | 2017-01-22 | 2017-06-30 | 无锡卡尔曼导航技术有限公司 | 用于农机无人驾驶的避障、掉头路径规划及其控制方法 |
CN107992069A (zh) * | 2017-11-29 | 2018-05-04 | 上海无线电设备研究所 | 一种无人机路径跟踪控制的制导律设计方法 |
CN109388140A (zh) * | 2018-09-13 | 2019-02-26 | 江苏大学 | 一种改进的用于地面车辆路径跟踪的纯追踪控制方法 |
US20200133289A1 (en) * | 2018-10-31 | 2020-04-30 | Ubtech Robotics Corp Ltd | Path tracking method and mobile robot using the same |
CN109947123A (zh) * | 2019-02-27 | 2019-06-28 | 南京航空航天大学 | 一种基于视线导引律的无人机路径跟踪与自主避障方法 |
Non-Patent Citations (1)
Title |
---|
朱欣华;王健;郭民环;姚速瑞;苏岩;: "基于位置约束的两轮驱动机器人路径跟踪控制方法", 中国惯性技术学报, no. 05 * |
Also Published As
Publication number | Publication date |
---|---|
CN113467460B (zh) | 2024-03-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108673496B (zh) | 一种基于纯追踪模型的工业机器人路径跟踪方法 | |
CN110703763A (zh) | 无人车路径跟踪及避障方法 | |
CN112519882B (zh) | 一种车辆参考轨迹跟踪方法及*** | |
CN102145808A (zh) | 工业型激光导引agv的双闭环控制***及控制方法 | |
CN110006419A (zh) | 一种基于预瞄的车辆轨迹跟踪点确定方法 | |
CN113104037B (zh) | 一种车辆方向盘转向角度确定方法及*** | |
CN112462760B (zh) | 一种双舵轮agv路径跟踪方法 | |
CN111487976B (zh) | 一种倒车轨迹跟踪方法 | |
Wang et al. | Path tracking control for autonomous harvesting robots based on improved double arc path planning algorithm | |
CN113110500B (zh) | 一种自动驾驶汽车低速自动泊车横向控制方法 | |
CN117320950A (zh) | 用于重型车辆的自适应路径跟随算法 | |
CN113741450A (zh) | 一种车辆自动驾驶横向自适应控制方法 | |
CN107450308B (zh) | 存储设备、机器人 | |
CN113525366A (zh) | 一种针对钢轮压路机液压横向控制器的横向控制方法 | |
CN113467460B (zh) | 一种基于双圆形前视距离的农业机械路径跟踪方法及*** | |
Ni et al. | Path tracking motion control method of tracked robot based on improved LQR control | |
CN111674406A (zh) | 自动驾驶***车辆横向控制的方法 | |
CN113721454B (zh) | 一种铰接式车辆路径跟踪控制方法 | |
CN116691668A (zh) | 一种大型智能车辆的横向控制方法 | |
CN114089730A (zh) | 机器人的运动规划方法和自动引导车 | |
CN113914406A (zh) | 一种电动装载机路径跟踪***及控制方法 | |
CN113325849A (zh) | 一种针对高地隙植保机的运动控制方法 | |
Xiao et al. | Smooth trajectory tracking using longitudinal distance constraint for a 4WS4WD unmanned ground vehicle | |
Yan et al. | A New Path Tracking Algorithm for Four-Wheel Differential Steering Vehicle | |
CN111830980A (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 |