一种无人农机路径跟随控制方法、装置与电子设备制造方法及图纸

本发明专利技术公开了一种无人农机路径跟随控制方法、装置与电子设备，基于北斗定位系统和惯性导航系统组成组合定位系统，设计了无人农机的路径跟随系统，使用模糊自适应纯追踪算法设计路径跟随控制器，通过控制方向盘以及纵向速度对其横向偏差和纵向速度进行控制，实现无人农机的路径跟随功能。该系统和方法实现了基于北斗定位系统的无人农机路径跟随控制系统，并实现了纯追踪前视距离的模糊自适应调节。实现了纯追踪前视距离的模糊自适应调节。实现了纯追踪前视距离的模糊自适应调节。

【技术实现步骤摘要】
一种无人农机路径跟随控制方法、装置与电子设备


[0001]本专利技术属于无人农机导航定位控制
，具体涉及一种无人农机路径跟随控制方法、装置与电子设备。

技术介绍

[0002]无人农机可以推动精准农业的实施，提高资源利用率和劳动生产率，降低对农机手熟练度的要求，降低工作强度，无人农机驾驶拖拉机已经成为近年来的研究热点。其中无人农机的路径跟随功能是无人农机完成作业最重要的环节之一。无人农机路径跟随是指给定路径，控制无人农机沿着给定路径行走，进而使得无人农机完成作业。
[0003]目前国内无人农机所使用的定位系统大多都是基于GPS，使用北斗系统定位系统的较少。同时在无人农机路径跟随控制系统中，纯追踪算法被提出的时间很早，此方法是基于几何的追踪方法，可靠性较高，在直线段路径跟踪的效果较好，但是在环境复杂和曲率变化的场合精度较低，该算法的精度取决于前视距离，如果前视距离可以适时调节，该算法的精度将会提高。

技术实现思路

[0004]为解决上述技术问题，本专利技术提供一种无人农机路径跟随控制方法，其支持北斗系统差分定位，同时可以和惯性导航进行组合定位，保证无人农机定位的准确性和可靠性。另外，该路径跟随方法较传统的纯追踪路径跟随方法，可以实现前视距离的自适应调节，路径跟踪的精度和鲁棒性都有所提升。
[0005]本专利技术实现其目的所采取的技术方案是：
[0006]一种无人农机路径跟随控制方法，包括如下步骤：
[0007]步骤(1)基于北斗定位系统，为无人农机提供位置坐标以及航向信息；
[0008]步骤(2)利用惯性传感器进行航位推算，获取无人农机位置坐标；
[0009]步骤(3)利用北斗定位系统和惯性传感器组成组合定位系统，采用卡尔曼滤波器融合无人农机位置坐标、位置以及航向信息；
[0010]步骤(4)将步骤(3)的融合的无人农机位置坐标、位置以及航向信息传递给规划控制单元，进行路径规划，将规划好的路径传递给规划控制单元；
[0011]步骤(5)建立无人农机运动学模型；
[0012]步骤(6)设计模糊自适应纯追踪控制器；
[0013]步骤(7)设置模糊变量以及其论域和模糊集；
[0014]步骤(8)设置模糊控制规则；
[0015]步骤(9)计算纯追踪模型的前轮转角；
[0016]步骤(10)通过方向盘控制前轮转角，控制无人农机的横向误差趋近于0。
[0017]进一步地，所述步骤(1)包括经过RTK差分定位获取无人农机的经纬度并将其转化为通用横墨卡托坐标系中的无人农机位置坐标。
[0018]进一步地，所述步骤(3)包括采用卡尔曼滤波器融合无人农机位置坐标(x,y)，以无人农机位置坐标(x,y)为状态变量，惯性传感器航位推算的无人农机位置坐标为预测量，北斗定位系统测量的坐标数据作为观测量，组合定位系统的状态方程为：
[0019]X
k
＝AX
k
‑1+BU
k
‑1+W (1)
[0020]其中，Δx和Δy分别为采样时刻内x、y方向的增量，W为过程噪声且W～N(0,Q)，N(0,Q)代表噪声W符合均值为0，方差为Q的标准正太分布；
[0021]北斗定位系统和惯性传感器的组合定位系统的观测方程为：
[0022]Z
k
＝HX
k
+V (2)
[0023]其中，Z
k
代表了k时刻北斗定位系统观测得到的无人农机坐标，H为状态转换矩阵，V为观测噪声且V～N(0,R),N(0,R)代表噪声V符合均值为0，方差为R的标准正太分布。
[0024]进一步地，所述步骤(5)中的运动学模型为：
[0025][0026]其中，x为无人农机的横向坐标，y是无人农机的纵向坐标，θ是无人农机航向，表示横向坐标对时间的导数，表示纵向坐标对时间的导数，表示航向对时间的导数，v代表无人农机速度，δ
f
为前轮转角值，L代表前轮和后轮轴心的轴距。
[0027]进一步地，所述步骤(6)包括：首先确定纯追踪控制器中前视距离F，采用模糊自适应的方法确定前视距离F，以横向偏差p
e
和农机速度v作为输入变量，不考虑变化率的影响，将前视距离F作为输出变量，建立一个二维模糊控制器。
[0028]进一步地，所述步骤(7)中横向偏差p
e
论域设置为[
‑
100,100]，单位为cm，模糊集为{NL,NS,Z,PS,PL}，无人农机速度v论域设置为[0,6]，单位为m/s,模糊集为{Z,PS,PL}，前视距离F的论域为：[0,6]，单位为m，模糊集为{Z,PS,PM,PL}，其中NL代表负大，NS代表负小，Z代表零，PS代表正小，PM代表正小PL代表正大。
[0029]进一步地，所述步骤(8)中的模糊控制规则为：横向偏差越小时，前视距离F越大越稳定；横向偏差越大时，前视距离F越小越稳定；稳定运行时，速度越大，前视距离F越大；稳定运行时，速度越小，前视距离F越小。
[0030]进一步地，所述步骤(9)包括通过模糊规则表输出自适应前视距离F中距离无人农机最近的点，假设寻找目标点为(x
g
,y
g
)，其中XOY为全局坐标系，X
c
OY
c
为无人农机坐标系，L
d
为弦长，(x,y)为无人农机在全局坐标系下的位置坐标，(x
g
,y
g
)为前视距离范围中的目标点，x
c
和y
c
分别代表在无人农机坐标系下与目标点的横向和纵向距离，α和θ分别代表了无人农机、目标点和全局坐标系的夹角，R代表无人农机的运动半径；
[0031]则有：
[0032][0033]无人农机距离目标点的横向偏差x
c
：
[0034][0035]其中，p
e
为农机车辆与规划路径之间横向距离，为车体当前航向角和规划路径航向角之间的夹角；
[0036]在运动学模型中得到前轮转角和转弯半径R的关系为：
[0037]δ＝arctan(L/R)，其中L为前后两轮中心的轴距；
[0038]最终得到前轮转角为：
[0039][0040]本专利技术还提供一种无人农机路径跟随控制装置，其特征在于，包括：
[0041]北斗定位终端模块，基于北斗定位系统，为无人农机提供位置坐标以及航向信息；
[0042]惯性传感器测量模块，利用惯性传感器进行航位推算，获取无人农机位置坐标；
[0043]组合定位模块，利用北斗定位系统和惯性传感器组成组合定位系统，采用卡尔曼滤波器融合无人农机位置坐标、位置以及航向信息；将融合的无人农机位置坐标、位置以及航向信息传递给规划控制单元，进行路径规划，将规划好的路径传递给规划控制单元；
[0044]模糊控制本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种无人农机路径跟随控制方法，其特征在于，包括如下步骤：步骤(1)基于北斗定位系统，为无人农机提供位置坐标以及航向信息；步骤(2)利用惯性传感器进行航位推算，获取无人农机位置坐标；步骤(3)利用北斗定位系统和惯性传感器组成组合定位系统，采用卡尔曼滤波器融合无人农机位置坐标、位置以及航向信息；步骤(4)将步骤(3)的融合的无人农机位置坐标、位置以及航向信息传递给规划控制单元，进行路径规划，将规划好的路径传递给规划控制单元；步骤(5)建立无人农机运动学模型；步骤(6)设计模糊自适应纯追踪控制器；步骤(7)设置模糊变量以及其论域和模糊集；步骤(8)设置模糊控制规则；步骤(9)计算纯追踪模型的前轮转角；步骤(10)通过方向盘控制前轮转角，控制无人农机的横向误差趋近于0。2.根据权利要求1所述的一种无人农机路径跟随控制方法，其特征在于，所述步骤(1)包括经过RTK差分定位获取无人农机的经纬度并将其转化为通用横墨卡托坐标系中的无人农机位置坐标。3.根据权利要2所述的一种无人农机路径跟随控制方法，其特征在于，所述步骤(3)包括采用卡尔曼滤波器融合无人农机位置坐标(x,y)，以无人农机位置坐标(x,y)为状态变量，惯性传感器航位推算的无人农机位置坐标为预测量，北斗定位系统测量的坐标数据作为观测量，组合定位系统的状态方程为：X
k
＝AX
k
‑1+BU
k
‑1+W (1)其中，Δx和Δy分别为采样时刻内x、y方向的增量，W为过程噪声且W～N(0,Q)，N(0,Q)代表噪声W符合均值为0，方差为Q的标准正太分布；北斗定位系统和惯性传感器的组合定位系统的观测方程为：Z
k
＝HX
k
+V (2)其中，Z
k
代表了k时刻北斗定位系统观测得到的无人农机坐标，H为状态转换矩阵，V为观测噪声且V～N(0,R),N(0,R)代表噪声V符合均值为0，方差为R的标准正太分布。4.根据权利要3所述的一种无人农机路径跟随控制方法，其特征在于，所述步骤(5)中的运动学模型为：其中，x为无人农机的横向坐标，y是无人农机的纵向坐标，θ是无人农机航向，表示横向坐标对时间的导数，表示纵向坐标对时间的导数，表示航向对时间的导数，v代表无人农机速度，δ
f
为前轮转角值，L代表前轮和后轮轴心的轴距。
5.根据权利要4所述的一种无人农机路径跟随控制方法，其特征在于，所述步骤(6)包括：首先确定纯追踪控制器中前视距离F，采用模糊自适应的方法确定前视距离F，以横向偏差p
e
和农机速度v作为输入变量，不考虑变化率的影响，将前视距离F作为输出变量，建立一个二维模糊控制器。6.根据权利要5所述的一种无人农机路径跟随控制方法，其特征在于，所述步骤(7)中横向偏差p
e
论域设置为[
‑

【专利技术属性】
技术研发人员：王亮亮，郭继冬，李子申，楚焕鑫，刘炳成，汪亮，刘振耀，常坤，
申请(专利权)人：中国科学院空天信息创新研究院，
类型：发明
国别省市：