1.一种面向果园车辆自主导航算法的仿真测试方法,其特征在于,该方法包括以下步骤:S1、果园地图配置步骤:根据用户录入的果园种植参数,生成树干中心位置和树干直径,基于树干信息建立果园种植模型模拟果园环境,输出果园地图M;
S2、雷达配置及测量步骤:根据用户录入的雷达参数,建立LiDAR测量模型,该测量模型能够根据t时刻世界坐标系xOWy下的车辆位姿,获得车辆坐标系xOVy下LiDAR测量点的极坐标;
S3、路径生成与跟踪步骤:根据t时刻LiDAR测量点的极坐标,首先采用待测试的导航算法计算导航路径,输出t时刻导航参数 然后采用待测试的导航算法进行路径跟踪引导车辆按照导航路径行驶,输出t时刻左右轮期望车速S4、仿真执行步骤:在步骤S1配置的果园地图上,按照步骤S2配置的LiDAR测量模型执行测量,根据步骤S3中待测试导航算法生成的导航参数和左右轮期望车速配置车辆运行参数,执行Δt时刻,得到t+Δt时刻的车辆位姿,返回S2继续执行,获取仿真结果即车辆运行轨迹。
2.根据权利要求1所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S1具体为:S1‑1、用户录入的果园种植参数,包括行数N1、株数N2、行距D1、株距D2、树干直径D3;
S1‑2根据所述种植参数,采用下述公式生成树干中心坐标(xT(k,j),yT(k,j))及树干直径dT(k,j):其中:k表示树干行号,j表示树干株号,ε1表示x轴方向树干种植位置随机扰动,ε2表示y轴方向树干种植位置随机扰动,ε3表示树干直径随机扰动;
S1‑3、建立果园种植模型模拟果园环境,以白色背景上的黑色圆点表示树干,根据树干中心坐标(xT,yT)及树干直径dT,在世界坐标系xOWy下绘制并输出果园地图M。
3.根据权利要求2所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S1‑2中:为模拟曲线树行,第i行第j株果树的树干中心行坐标采用下述公式:其中:λ表示正弦扰动的波长、A表示正弦扰动的振幅;
为模拟缺树树行,每行随机产生N3个范围在[1,N2]的整数,作为缺树株号。
4.根据权利要求1所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S2具体为:S2‑1、用户录入雷达参数,包括LiDAR的最大测量距离rmax、扫描角范围θrange、扫描角分辨率Δθ和起始扫描角θ0;用户录入世界坐标系xOWy下的初始车辆位姿(xV(0),yV(0),αV(0));
S2‑2、根据所述雷达参数,建立LiDAR测量模型计算扫描线的数量N和各扫描线的扫描角θi:θi=θ0+Δθ(i‑1)
其中:N表示扫描线的总数,i表示扫描线的编号;
令LiDAR扫描线的起始位置与车辆质心OV重合,t时刻第i条扫描线在世界坐标系xOWy下的坐标为xL、yL:其中:r为扫描线极径;(xV(t),yV(t),αV(t))表示t时刻世界坐标系xOWy下的车辆位姿;
对每条扫描线,令r从0开始按间隔Δr增长,执行扫描;
S2‑3、果园地图中,当扫描线坐标(xL,yL)处为黑色像素时,认为扫描线触碰到树干,记*录此时的扫描线极径作为测量值ri,采用下述公式获取第i条扫描线返回的测量距离ri:*
ri(t)=ri+εL
其中:εL为LiDAR随机测量误差;
若第i条扫描线在果园地图范围内或最大测量距离范围内未找到树干点,令ri(t)=rmax;
最终,得到在车辆坐标系xOVy下的t时刻LiDAR测量点极坐标
5.根据权利要求4所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,间隔Δr小于等于果树地图单个像素尺寸。
6.根据权利要求1所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S4中,配置车辆运行参数具体包括:S4‑1、电机控制:建立电机模型模拟电机力矩变化,采用PID控制器实现左右电机控制,基于t时刻左右轮期望车速 和实际车速(vL(t),vR(t)),获得t时刻左右电机力矩(τL(t),τR(t));
S4‑2、车辆动力学控制:建立两轮差速驱动车辆动力学模型模拟车辆速度变化,基于t时刻左右电机力矩(τL(t),τR(t)),获得t时刻左右轮实际车速(vL(t),vR(t));
S4‑3、车辆运动学控制:建立车辆运动学模型模拟车辆位姿变化,基于t时刻左右轮实际车速(vL(t),vR(t)),获得t时刻世界坐标系下xOWy的车辆位姿(xV(t),yV(t),αV(t));
令S4‑1至S4‑3运行Δt时间,得到t+Δt时刻的车辆位姿(xV(t+Δt),yV(t+Δt),αV(t+Δt)),然后再从S2开始执行。
7.根据权利要求6所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S4‑1中,电机控制具体为:S4‑1a、根据t时刻左右轮期望车速 和实际车速(vL(t),vR(t)),计算左右电机速度误差(eL(t),eR(t)):S4‑1b、采用PID参数整定方法计算左右电机控制电压(uL(t),uR(t)):其中:KP、KI和KD分别为PID参数整定方法预设的比例参数、积分参数和微分参数;
分别表示eL(t)和eR(t)的微分;
S4‑1c、采用线性饱和法将左、右电机控制电压幅值限制在[‑umax,umax]内,即当|uL(t)|>|umax|或者|uR(t)|>|umax|时,令|uL(t)|=|umax|,|uR(t)|=|umax|,电压方向不变;
S4‑1d、以左右电机控制电压(uL(t),uR(t))为输入变量,以左右电机转速(nL(t),nR(t))为状态变量,根据下述电机模型获取t时左右电机力矩(τL(t),τR(t)):其中:ke、kc、Ja和Ra分别为电机反电动势系数、电机转矩系数、电机转动惯量和电机电枢电阻; 分别表示nR(t)、nL(t)的微分。
8.根据权利要求7所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S4‑2中,车辆动力学控制具体为:基于t时刻左右电机力矩(τL(t),τR(t))采用下述车辆动力学模型获取t时刻左右轮实际车速(vL(t),vR(t)):其中:D表示车辆轮距,d表示车辆质心与两个驱动轮几何中心之间的距离,m表示车辆质量,J表示车辆转动惯量,rV表示车轮半径,分别表示vL(t)、vR(t)的微分。
9.根据权利要求8所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,所述的S4‑3中,车辆运动学控制具体为:基于t时刻左右轮实际车速(vL(t),vR(t)),采用下述车辆运动学模型获取t时刻世界坐标系下xOWy的车辆位姿(xV(t),yV(t),αV(t)):其中: 别为xV(t),yV(t),αV(t)的微分。
10.一种面向果园车辆自主导航算法的仿真测试平台,基于权利要求1‑9之一所述的面向果园车辆自主导航算法的仿真测试方法,其特征在于,该平台包括:果园地图配置单元:根据用户录入的果园种植参数,生成树干中心位置和树干直径,基于树干信息建立果园种植模型模拟果园环境,输出果园地图M;
雷达配置及测量单元:根据用户录入的雷达参数,建立LiDAR测量模型,该测量模型能够根据t时刻世界坐标系xOWy下的车辆位姿,获得车辆坐标系xOVy下LiDAR测量点的极坐标;
路径生成与跟踪单元:根据t时刻LiDAR测量点的极坐标,首先采用待测试的导航算法计算导航路径,输出t时刻导航参数 然后采用待测试的导航算法进行路径跟踪引导车辆按照导航路径行驶,输出t时刻左右轮期望车速仿真执行单元:根据配置的果园地图、LiDAR测量模型、导航参数和左右轮期望车速配置车辆运行参数,执行仿真,获取仿真结果即车辆运行轨迹。