1.一种无人驾驶车辆的局部路径规划方法,基于组合导航传感器实现,所述方法包括:步骤1)在世界坐标系中建立运动学方程,以车辆运动中的速度和转角为自变量,建立运动模型;
步骤2)基于实时载波相位动态差分技术,将组合导航传感器获取的数据转换至导航坐标系,为车辆运行提供定位信息;
步骤3)从时间和位姿两个方面,通过实施加权多目标优化的方法,建立多目标代价函数;
步骤4)通过确定车辆的运行状态,融合时间分辨率参数dt_ref进行动态调节,实现车辆稳定运行。
2.根据权利要求1所述的无人驾驶车辆的局部路径规划方法,其特征在于,所述步骤1)具体包括:
将世界坐标系OwXwYw的基坐标系设置在车辆简化的运行模型后轴中心上,设置O为运动模型的瞬时旋转中心;
建立自动驾驶车辆运动学状态方程为:T
其中,s(t)=[x(t),y(t),β(t)] 为车辆的位姿状态量, 为车辆位姿状态的导数,x(t)和y(t)分别为车辆在世界坐标系的X轴和Y轴的位移量,β(t)为t时刻车辆质心处的转向角,v(t)为t时刻车辆后轴中心速度,θ(t)为t时刻车辆的横摆角,lr为车辆后轮到车辆质心的距离。
3.根据权利要求2所述的无人驾驶车辆的局部路径规划方法,其特征在于,所述步骤2)具体包括:
步骤2‑1)将组合导航传感器获取的大地坐标系下的地理经度λ、地理纬度φ、大地高度H,转换至地理直角坐标系下的坐标值(x,y,z);
其中,N为椭球面卯酉圈的曲率半径,满足下式:e为椭球的第一偏心率,a、b分别为椭球的长、短半径,均为常数,W为第一辅助系数;
T
步骤2‑2)选取P0=[x0,y0,z0] 作为导航坐标系的原点,P0点表示车辆的初始位置在空T
间直角坐标系下的坐标,T表示转置,P=[x,y,z]表示地理直角坐标系下的坐标值;[Δx,T
Δy,Δz]表示在空间直角坐标系下当前位置与初始位置之间的差值:T
通过坐标旋转,计算点P=[x,y,z]在以点P0为坐标原点的导航坐标系下的位置(E,N,U)为:
为坐标转换矩阵,满足下式:
4.根据权利要求3所述的无人驾驶车辆的局部路径规划方法,其特征在于,所述步骤3)具体包括:
从时间和位姿两个方面,通过实施加权多目标优化的方法,建立多目标代价函数为:*
B=argminf(B)
其中,B表示由姿序列信息和时间间隔序列信息的TEB轨迹模型;f(B)表示各种约束目*
标函数之和,γk为第k个目标函数fk(B)的权重系数,1≤k≤K,B为被优化的TEB序列结果;
所述目标函数包括用惩罚函数表示的速度和加速度约束以及与轨迹有关的目标函数,所述TEB序列包括位姿序列信息和时间间隔序列信息。
5.根据权利要求4所述的无人驾驶车辆的局部路径规划方法,其特征在于,所述步骤4)的确定车辆的运行状态,具体包括:根据设定时间内的转向角度,确定车辆为直行状态或转弯状态;
根据下式确定时间分辨率参数dt_ref:其中,d为预先指定的时间间隔,Δd为转弯时的变化量。
6.根据权利要求5所述的无人驾驶车辆的局部路径规划方法,其特征在于,所述步骤4)的时间分辨率参数dt_ref动态调节方法具体包括:在初始化阶段,根据A‑Star算法生成的全局路径,生成TEB序列;
在轨迹修正阶段,根据下式计算两个相邻位姿之间的时间间隔dt:其中,pi和pi‑1分别为两个相邻的位姿点,max_vel_x为车辆最大速度;
设定dt_ref为的两个点之间的指定时间间隔,dt_hysteresis为滞后时间,dt_hysteresis∈(‑0.1*dtref,0.1*dtref);
当dt>dt_ref+dt_hysteresis,在Pi‑1点和Pi点的中间位置插入点Pi,Pi点变为Pi+1点,Pi‑1点和Pi点之间的时间间隔为原来Pi‑1点和Pi点之间的时间间隔的一半;
当dt<dt_ref+dt_hysteresis时,删除Pi点,Pi+1点成为新的Pi点,Pi‑1点和Pi点之间的时间间隔为原来Pi‑1点至Pi+1点时间间隔之和;
在轨迹修正阶段,重复进行增加和删除点的操作,直至整个TEB序列任意两个点之间的时间间隔都在指定的dt_ref范围内或达到最大迭代次数,从而得到动态调整好的局部路径。
7.一种无人驾驶车辆的局部路径规划系统,基于组合导航传感器实现,其特征在于,所述系统包括:运动模型建立模块、定位信息获取模块、代价函数建立模块和动态调节模块;
其中,
所述运动模型建立模块,用于在世界坐标系中建立运动学方程,以车辆运动中的速度和转角为自变量,建立运动模型;
所述定位信息获取模块,用于基于实时载波相位动态差分技术,将组合导航传感器获取的数据转换至导航坐标系,为车辆运行提供定位信息;
所述代价函数建立模块,用于从时间和位姿两个方面,通过实施加权多目标优化的方法,建立多目标代价函数;
所述动态调节模块,用于通过确定车辆的运行状态,融合时间分辨率参数dt_ref进行动态调节,实现车辆稳定运行。