1.一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,包括:
S1:获取全局泊车路径;
S2:根据车辆状态参数构建整车运动学模型;
S3:根据整车运动学模型和全局泊车路径,以泊车所耗时间最短为优化目标,以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束参数,构建行驶轨迹优化模型;
S4:引入权重因子,对行驶轨迹优化模型中的约束进行转化,得到行驶轨迹优化目标函数;在图优化理论g2o框架下,采用LM方法对行驶轨迹优化目标函数进行求解,得到最优解向量;根据最优解向量,得到优化的局部行驶轨迹;根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化。
2.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,整车运动学模型为:其中,x(t)表示t时刻车辆后轴中点横坐标,y(t)表示t时刻车辆后轴中点纵坐标,表示t时刻车辆航向角,δ(t)表示t时刻前轮转向角,L表示轴距,v(t)表示t时刻后轴车速。
3.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,行驶车速约束为:其中,vk表示车辆在第k个位姿点的行驶车速,Rk表示车辆在第k个位姿点的转向半径,表示车辆在第k个位姿点的与前一位姿点的航向角变化量,ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔,Υ(Sk,Sk+1)表示车辆从位姿点Sk到位姿点Sk+1的速度方向,dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量。
4.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,整车运动学约束为:其中,hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数, 表示第k个位姿点的车辆航向角, 表示第k+1个位姿点的车辆航向角,dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量,Rk表示车辆在第k个位姿点的转向半径, 表示车辆在第k个位姿点的与前一位姿点的航向角变化量,Rmin表示车辆的最小转向半径。
5.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,避障安全距离约束为:T T
ok(Sk)=[σ(Sk,O1),σ(Sk,O2),…,σ(Sk,Om)]‑[σmin,σmin,…,σmin]≥0其中,T表示矩阵的转置,ok(Sk)表示车辆在位姿点Sk的避障安全距离,σ(Sk,Om)表示车辆在位姿点Sk与第m个障碍物Om间的避障安全距离,σmin表示最小避障安全距离。
6.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,行驶轨迹优化模型为:s.t.S1=Sc,Sn=Sf,0≤ΔTk≤ΔTmax
ok(Sk)≥0
vk(Sk+1,Sk,ΔTk)≥0,(k=1,2,…,n‑1)
ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔTk)≥0,(k=2,3,…,n‑2)a1(S2,S1,ΔT1)≥0,an(Sn,Sn‑1,ΔTn‑1)≥0
其中,ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔,ΔTmax表示位姿点间最大时间间隔,n表示位姿点数量,S1表示初始位姿,Sc表示车辆初始状态,Sn表示目标位姿,Sf表示车辆最终状态,hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数,表示从位姿点Sk到位姿点Sk+1的允许转向半径,ok(Sk)表示车辆在位姿点Sk的避障安全距离,vk(Sk+1,Sk,ΔTk)表示车辆在第k个位姿点的行驶车速,ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔTk)表示车辆在第k个位姿点的加速度,a1(S2,S1,ΔT1)表示初始位姿加速度,an(Sn,Sn‑1,ΔTn‑1)表示目标位姿加速度;B表示需要优化的参数集;ΔTk+1表示车辆从位姿Sk+1到Sk+2所需的时间,ΔT1表示从位姿S1到S2所需的时间,ΔTn‑1表示从位姿Sn‑1到Sn所需的时间。
7.根据权利要求1所述的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,行驶轨迹优化目标函数为:*
其中,B 表示行驶轨迹最优解向量,B表示需要优化的参数集, 表示中间函数,S1表示初始位姿,Sn表示目标位姿,n表示位姿点数量,ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔,φ(hk,ξh)表示连续迁移指数约束, 表示转向半径约束,χ(vk,ξv)表示行驶车速约束,χ(ok,ξo)表示避障安全距离约束,χ(ak,ξa)表示第k个位姿点的加速度约束,χ(an,ξa)表示目标位姿加速度约束;hk表示车辆在第k个位姿点的连续迁移指数, 表示车辆在第k个位姿点的允许转向半径,vk表示车辆在第k个位姿点的行驶车速,ok表示车辆在第k个位姿点的避障安全距离,ak表示车辆在第k个位姿点的加速度,an表示车辆在目标位姿点的加速度;ξh表示连续迁移指数约束权重,ξr表示转向半径约束权重,ξv表示行驶车速约束权重,ξo表示避障安全距离约束权重,ξa表示加速度约束权重。
8.一种面向泊车场景的无人驾驶车辆行驶轨迹优化系统,该系统用于执行权利要求1‑
7所述的任意一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法,其特征在于,包括:信息获取模块、行驶轨迹优化模型构建模块、解算模块和控制模块;
所述信息获取模块用于获取全局泊车路径;
所述行驶轨迹优化模型构建模块用于根据全局泊车路径和车辆状态参数构建行驶轨迹优化模型;
所述解算模块用于对行驶轨迹优化模型进行解算,得到优化的车辆行驶轨迹;
所述控制模块用于根据优化的车辆行驶轨迹进行泊车操作。