1.一种基于差分GPS的园区车辆智能驾驶路径拟合跟踪控制方法,其特征在于,通过以下方法来实现:首先在园区道路上每间隔一段距离周期性地采集经纬度坐标,设采集的坐标点数为n,然后将采集的基于大地坐标系的坐标点转化为n个平面坐标点;接着,利用n个平面坐标点拟合出园区道路的路径曲线,然后计算出车辆当前位置坐标距离路径曲线的最短距离Dmin,再根据车辆当前位置坐标及路径曲线上距离车辆当前位置最近的采集点坐标,计算出车辆方向盘的转向;最后,控制车辆进行转动角度大小与车辆与路径曲线的最短距离Dmin成正比的左转向和右转向,来控制车辆在园区道路上的自动驾驶。
2.根据权利要求1所述的基于差分GPS的园区车辆智能驾驶路径拟合跟踪控制方法,其特征在于:具体通过以下步骤来实现:a).路径坐标信息采集,基于差分GPS采集路径上点的经纬度坐标信息,采样规律按照直线段每隔5m采集坐标点一次、曲线段每隔1m采集坐标点一次的方式进行采集,所采集的坐标点是基于大地坐标系的,设所采集的坐标点数量为n个,n个坐标点中Pi点的坐标表示为Pi(Bi,Li,Hi),i=1,2,3,...,n;
b).平面坐标转换,利用高斯投影正算公式(1),将Pi点的大地坐标Pi(Bi,Li,Hi)转换为平面坐标Pi(xi,yi):其中,角度均为弧度,l″=Li-L0,L0为中央子午线经度,N为椭球面卯酉圈的曲率半径,e为椭球的第一偏心率,记a、b分别为椭球的长短半径,f为椭球扁率,W为第一辅助系数;
a=6378137.000m,b=
6356752.314m;f=1/298.257223563;t=tanBi;η2=e2cos2B,X为子午线弧长,其通过如下公式进行求取:
其中,a0,a2,a4,a6为基本常量,按照公式(3)计算:其中,m0,m2,m4,m6,m8为基本常量,按照如下公式计算:
2 2
m0=a(1-e); m4=5em2;
c).路径曲线拟合,设所采集的某条待拟合的道路坐标点依次为P1、P2、…、Pn,共计n个坐标点,且n>4;依次经步骤b)转换为平面坐标,在平面坐标系内利用最小二乘法将n个离散的点拟合成一条曲线,所拟合的曲线用一元三次方程函数fn(x)表示为:fn(x)=ax3+bx2+cx+d (4)
公式(4)中,a、b、c、d为拟合曲线的系数;
d).路径跟踪,计算车辆当前位置点Pm(xm,ym)到路径曲线的最短距离,设点Pm(xm,ym)到路径曲线fn(x)最短路径与曲线交点为P(x,y),则当前位置点到路径曲线的距离为:令:
L=(xm-x)2+(ym-y)2 (6)
将预设路径方程:
3 2
y=ax+bx+cx+d (7)
代入公式(6)中,化简得:
将方程式(8)对x一次求导化简得:
利用牛顿法求取方程L′=0的近似解:
令:
迭代初值为:
x0=xm-0.3 (11)
迭代公式:
xn+1=xn-f(xn)/f′(xn) (12)精度阈值10-12,迭代次数限定100,假定求得的近似解为x=xd,代入式(7)中获得曲线上一点Pd(xd,yd),过Pd点做曲线(7)的切线,所获得切线的斜率为:tanθ1=K (14)
记θ2为点Pd(xd,yd)与点Pm(xm,ym)两点之间的连线与竖直方向的夹角,根据三角形内角和等于180°以及相似三角形定理得:θ1=θ2;
根据公式(5)求取车辆当前位置Pm(xm,ym)到曲线的最短距离为:e).车辆方向控制,计算车辆当前位置位于路径曲线的左侧还是右侧,Pm(xm,ym)为车辆当前位置坐标,P″(x″,y″)为拟合出的曲线方程fn(x)=ax3+bx2+cx+d上沿车辆行进方向距离Pm(xm,ym)最近的采集点坐标,将xm坐标代入拟合出的曲线方程得到坐标点P(xm,y);θ3为点Pm(xm,ym)与点P″(x″,y″)两点间的连线与x轴的夹角,θ4为点P(xm,y)与点P″(x″,y″)两点间的连线与x轴的夹角;
通过公式(16)、公式(17)和公式(18)求出θ3、θ4对应的反正切值phid、phir;atan2函数的计算公式为:phid、phir的计算公式为:
phid=atan2(y″-ym,x″-xm) (17)phir=atan2(y″-y,x″-xm) (18)经公式(16)、公式(17)和公式(18)求出phid、phir,由于坐标的象限可能不同,通过公式(19)求出差值以后、还需要通过公式(20)求出实际反正切差值err_phi:err_phi=phid-phir (19)
当err_phi大于0时,表示车辆当前位置位于路径曲线的左侧,应向右打方向盘,方向盘的转动角度大小与车辆距离路径曲线的距离Dmin成正比,使车辆向右行驶;当err_phi小于0时,表示车辆当前位置位于路径曲线的右侧,应向左打方向盘,方向盘的转动角度大小与车辆距离路径曲线的距离Dmin成正比,使车辆向左行驶;通过周期性的、不间断的调整车辆的方向盘,进而控制车辆的行驶路径,实现车辆的自动驾驶。