本发明属于无人驾驶车辆技术领域，具体涉及一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及系统；该方法包括：获取全局泊车路径；根据车辆状态参数构建整车运动学模型；根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束参数，构建行驶轨迹优化模型；采用Levenberg‑Marquardt(LM)方法对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化；本发明提高了在泊车场景下，无人驾驶车辆行驶的安全性，实用性高。
一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及系统
技术领域
[0001] 本发明属于无人驾驶车辆技术领域，具体涉及一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及系统。
背景技术
[0002] 近年来随着汽车智能化的不断演进，无人驾驶车辆技术受到越来越多的关注。自主规划是无人驾驶车辆的关键属性，它不仅要求基于算法找到一条从起始点到目标点的无碰撞路径，还要求无人驾驶车辆在复杂环境中完成自适应避障。自主泊车是无人驾驶车辆最可能实现商用化的场景之一，然而仅使用全局路径规划算法，如Dijkstrs算法、A*算法、RRT算法等不能满足泊车场景下的无人驾驶车辆驾驶需求，而已有的局部路径规划算法即全局路径优化算法，如Lattice Planner算法、DWA算法和TEB算法等大多适用于两轮机器人，较少考虑无人驾驶车辆在实际行驶过程中的运动学、速度、加速度、允许转向半径及避障安全距离等约束条件，对于无人驾驶车辆的使用具有较大的安全隐患。
[0003] 综上所述，亟需一种一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及系统，以解决泊车场景下，现有全局路径规划算法及其优化算法不能满足无人驾驶车辆安全行驶需求的问题。
发明内容
[0004] 针对现有技术存在的不足，本发明提出了一种基于用户意识和演化博弈的信息传播控制方法，该方法包括：
[0005] S1：获取全局泊车路径；
[0006] S2：根据车辆状态参数构建整车运动学模型；
[0007] S3：根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束参数，构建行驶轨迹优化模型；
[0008] S4：采用LM方法对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化。
[0009] 优选的，整车运动学模型为：
[0010]
[0011] 其中，x(t)表示t时刻车辆后轴中点横坐标，y(t)表示t时刻车辆后轴中点纵坐标，表示t时刻车辆航向角，δ(t)表示t时刻前轮转向角，L表示轴距，v(t)表示t时刻后轴车速。
[0012] 优选的，行驶车速约束为：
[0013]
[0014] 其中，vk表示车辆在第k个位姿点的行驶车速，Rk表示车辆在第k个位姿点的转向半径， 表示车辆在第k个位姿点的与前一位姿点的航向角变化量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，Υ(Sk,Sk+1)表示车辆从位姿点Sk到位姿点Sk+1的速度方向，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量。
[0015] 优选的，整车运动学约束为：
[0016]
[0017]
[0018] 其中，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数， 表示第k个位姿点的车辆航向角， 表示第k+1个位姿点的车辆航向角，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，Rk表示车辆在第k个位姿点的转向半径， 表示车辆在第k个位姿点的与前一位姿点的航向角变化量，Rmin表示车辆的最小转向半径。
[0019] 优选的，避障安全距离约束为：
[0020] ok(Sk)＝[σ(Sk,O1),σ(Sk,O2),…,σ(Sk,Om)]T‑[σmin,σmin,…,σmin]T≥0[0021] 其中，ok(Sk)表示车辆在位姿点Sk的避障安全距离，σ(Sk,Om)表示车辆在位姿点Sk与障碍物Om间的避障安全距离，σmin表示最小避障安全距离。
[0022] 优选的，行驶轨迹优化模型为：
[0023]
[0024] s.t.S1＝Sc,Sn＝Sf,0≤ΔTk≤ΔTmax
[0025]
[0026] ok(Sk)≥0
[0027] vk(Sk+1,Sk,ΔTk)≥0,(k＝1,2,…,n‑1)
[0028] ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)≥0,(k＝2,3,…,n‑2)
[0029] a1(S2,S1,ΔT1)≥0,an(Sn,Sn‑1,ΔTn‑1)≥0
[0030] 其中，Δ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,ΔT)表示车辆在第k个位姿点的加速度，a1(S2,S1,ΔT1)表示初始位姿加速度，an(Sn,Sn‑1,ΔTn‑1)表示目标位姿加速度。
[0031] 优选的，对行驶轨迹优化模型进行求解的过程包括：引入权重因子，对行驶轨迹优化模型中的约束进行转化，得到行驶轨迹优化目标函数；在图优化理论g2o框架下，采用LM方法对行驶轨迹优化目标函数进行求解，得到最优解向量；根据最优解向量，得到优化的局部行驶轨迹。
[0032] 进一步的，行驶轨迹优化目标函数为：
[0033]
[0034]*
[0035] 其中，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表示加速度约束权重。
[0036] 一种面向泊车场景的无人驾驶车辆行驶轨迹优化系统，包括：信息获取模块、行驶轨迹优化模型构建模块、解算模块和控制模块；
[0037] 所述信息获取模块用于获取全局泊车路径；
[0038] 所述行驶轨迹优化模型构建模块用于根据全局泊车路径和车辆状态参数构建行驶轨迹优化模型；
[0039] 所述解算模块用于对行驶轨迹优化模型进行解算，得到优化的车辆行驶轨迹；
[0040] 所述控制模块用于根据优化的车辆行驶轨迹进行泊车操作。
[0041] 本发明的有益效果为：本发明设计的面向泊车场景的无人驾驶车辆行驶轨迹优化方法基于局部路径规划算法思想，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束条件，构建行驶轨迹优化模型，通过对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹，进而实现完整的车辆行驶轨迹优化；与采用现有的全局路径规划算法及其优化算法对车辆轨迹进行优化相比，本发明优化的车辆行驶轨迹考虑了实际泊车过程中符合整车行驶特点的约束条件，满足分层导航体系机构中的局部优化需求，提高了在泊车场景下，无人驾驶车辆行驶的安全性，实用性高。
附图说明
[0042] 图1为本发明中面向泊车场景的无人驾驶车辆行驶轨迹优化方法流程图；
[0043] 图2为本发明中车辆运动学模型示意图；
[0044] 图3为本发明中运动学位移姿态迁移图；
[0045] 图4为本发明中泊车行驶轨迹优化效果示意图。
具体实施方式
[0046] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0047] 本发明提出了一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法，如图1所示，所述方法包括以下内容：
[0048] S1：获取全局泊车路径。
[0049] 以已知的全局泊车路径为基础，插入若干个整车位姿点，并在相邻位姿点之间加入时间间隔信息。
[0050] S2：根据车辆状态参数构建整车运动学模型。
[0051] 车辆状态参数包括车辆后轴中点坐标、前轮转向角、轴距、车辆航向角、后轴车速(行驶车速)等。如图2所示，以车辆后轴中点为整车状态参考点，整车的位姿状态向量为整车运动学模型表达式为：
[0052]
[0053] 其中，x(t)表示t时刻车辆后轴中点横坐标，y(t)表示t时刻车辆后轴中点纵坐标，表示t时刻车辆航向角，δ(t)表示t时刻前轮转向角，L表示轴距，v(t)表示t时刻后轴车速。
[0054] 整车的控制输入向量为U＝[v,δ]T，由上式可得：
[0055]
[0056] 其中，Υ(·)表示速度方向。
[0057] S3：根据整车运动学模型和全局泊车路径，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束条件，构建行驶轨迹优化模型。
[0058] 根据整车的结构特点，其转向半径满足R＝L/tan(δ)。与最大转向角δmax对应的转向半径便是最小转向半径Rmin。用Sk,k＝1,2,...n表示满足控制输入的一系列离散位姿点，整车直线运动和转向运动之间过渡过程的两个位姿Sk和Sk+1需满足同一个恒定曲率的圆弧，进而避免整车行驶过程中曲率不连续现象的出现。根据整车运动学模型和全局泊车路径，可得到整车的运动学位姿迁移过程，如图3所示，在k时刻，整车位姿Sk与向量dk＝[xk+1‑Txk,yk+1‑yk,0]之间的夹角用θk,k表示。在k+1时刻，整车位姿Sk+1与向量dk＝[xk+1‑xk,yk+1‑yk,T
0]之间的夹角用θk,k+1表示。当且仅当θk,k与θk,k+1相等时方可满足整车位姿的连续迁移过程，即符合整车的运动学特点，其必须满足：
[0059]
[0060] 车辆的转向半径约束为：
[0061]
[0062] 其中，hk(Sk+1,Sk)表示从位姿点Sk到位姿点Sk+1的连续迁移指数， 表示第k个位姿点的车辆航向角， 表示第k+1个位姿点的车辆航向角，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，Rk表示车辆在第k个位姿点的转向半径， 表示车辆在第k个位姿点的与前一位姿点的航向角变化量，Rmin表示车辆的最小转向半径。
[0063] 在泊车场景下，整车运动学约束为连续迁移指数约束和转向半径约束。
[0064] 行驶车速表示为：
[0065]
[0066] 构建方向向量 在距离向量dk上的投影：
[0067]
[0068] 其中，vk表示车辆在第k个位姿点的行驶车速，Rk表示车辆在第k个位姿点的转向半径， 表示车辆在第k个位姿点的与前一位姿点的航向角变化量，ΔTk表示车辆在第k个位姿点的与前一位姿点的时间间隔，Υ(Sk,Sk+1)表示车辆从位姿点Sk到位姿点Sk+1的速度方向，dk表示车辆在位姿点Sk指向位姿点Sk+1的方向向量，表示比例因子。
[0069] 行驶车速约束为：
[0070]
[0071] 进一步的，角速度 但是其必须满足|ωk|≤ωmax，其中ωmax＝vmax/T
Rmin，因此行驶车速约束表达式为：vk(Sk+1,Sk,ΔTk)＝[vmax‑|vk|,ωmax‑|ωk|] 。ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔTk)表示加速度约束，其中，加速度表达式为：
[0072]
[0073] 其中，ak表示车辆在第k个位姿点的加速度。
[0074] k时刻的车速由k和k+1时刻共同决定，所以k时刻的加速度由k、k+1和k+2时刻共同决定，进而加速度约束的表达式可表示为：
[0075] ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔTk)＝amax‑|ak|
[0076] 加速度约束为：
[0077] ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)≥0,(k＝2,3,…,n‑2)
[0078] a1(S2,S1,ΔT1)≥0,an(Sn,Sn‑1,ΔTn‑1)≥0
[0079] 避障安全距离约束表示为：
[0080] ok(Sk)＝[σ(Sk,O1),σ(Sk,O2),…,σ(Sk,Om)]T‑[σmin,σmin,…,σmin]T≥0[0081] 其中，ok(Sk)表示车辆在位姿点Sk的避障安全距离，σ(Sk,Om)表示车辆在位姿点Sk与障碍物Om间的避障安全距离，σmin表示最小避障安全距离。
[0082] 采用Timed‑Elastic‑Band(TEB)思想即局部路径规划算法思想，构建行驶轨迹优化模型，其表达式为：
[0083]
[0084] s.t.S1＝Sc,Sn＝Sf,0≤ΔTk≤ΔTmax
[0085]
[0086] ok(Sk)≥0
[0087] vk(Sk+1,Sk,ΔTk)≥0,(k＝1,2,…,n‑1)
[0088] ak(Sk+2,Sk+1,Sk,ΔTk+1,ΔT)≥0,(k＝2,3,…,n‑2)
[0089] a1(S2,S1,ΔT1)≥0,an(Sn,Sn‑1,ΔTn‑1)≥0
[0090] 其中，Δ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,ΔT)表示车辆在第k个位姿点的加速度，a1(S2,S1,ΔT1)表示初始位姿加速度，an(Sn,Sn‑1,ΔTn‑1)表示目标位姿加速度。
[0091] S4：采用LM方法对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹；根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化。
[0092] 引入权重因子，对行驶轨迹优化模型中的约束进行转化，得到行驶轨迹优化目标函数。
[0093] 将精确的非线性规划问题转化为近似非线性平方优化问题，并针对行驶轨迹优化模型中的等式和不等式约束，引入与之对应的权重因子。
[0094] 对于等式约束，例如，连续迁移指数约束，其转化约束为：
[0095]
[0096] 对于不等式约束，例如，行驶速度约束，其转化约束为：
[0097]
[0098] 对于其他约束，同样采用上述方法，此处不再赘述。
[0099] 最终，将非线性规划问题表达为具有目标函数的优化问题，得到行驶轨迹优化目标函数即：
[0100]
[0101]
[0102] 其中，B*表示行驶轨迹最优解向量， 表示中间函数，φ(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表示加速度约束权重。
[0103] 在图优化理论g2o框架下，采用LM方法对行驶轨迹优化目标函数进行求解，得到最优解向量；根据最优解向量，得到优化的局部行驶轨迹。
[0104] 具体的，利用Levenberg‑Marquardt(LM)方法即列文伯格‑马夸尔特法具有鲁棒性和高效计算的特点，求解行驶轨迹优化目标函数。进一步，在图优化理论g2o框架下，求解获*取的解向量B，进而得到最优解向量B；分析最优解向量，得到优化的局部行驶轨迹。
[0105] 根据优化的局部行驶轨迹实现泊车场景下的完整车辆行驶轨迹优化；在无人驾驶情况下，车辆根据优化的完整车辆行驶轨迹进行自动泊车。
[0106] 本发明还提供一种面向泊车场景的无人驾驶车辆行驶轨迹优化系统，该系统用于执行本发明设计的一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法；包括：信息获取模块、行驶轨迹优化模型构建模块、解算模块和控制模块；
[0107] 所述信息获取模块用于获取全局泊车路径；
[0108] 所述行驶轨迹优化模型构建模块用于根据全局泊车路径和车辆状态参数构建行驶轨迹优化模型；
[0109] 所述解算模块用于对行驶轨迹优化模型进行解算，得到优化的车辆行驶轨迹；
[0110] 所述控制模块用于根据优化的车辆行驶轨迹进行泊车操作。
[0111] 如图4所示，从图中可以看出的，优化后车辆行驶轨迹不再以路径的长度为单一维度考虑，而是进一步考虑了行驶轨迹与障碍物之间的安全距离，换言之，在同一车速约束条件下更符合实际行驶安全的需要。
[0112] 本发明设计的面向泊车场景的无人驾驶车辆行驶轨迹优化方法基于局部路径规划算法思想，以泊车所耗时间最短为优化目标，以自动泊车起始点位姿、目标点位姿、行驶车速、整车运动学以及避障安全距离为基本约束条件，构建行驶轨迹优化模型，通过对行驶轨迹优化模型进行求解，得到优化的局部行驶轨迹，进而实现完整的车辆行驶轨迹优化；与采用现有的全局路径规划算法及其优化算法对车辆轨迹进行优化相比，本发明优化的车辆行驶轨迹，考虑了实际泊车过程中符合整车行驶特点的约束条件，满足分层导航体系机构中的局部优化需求，提高了在泊车场景下，无人驾驶车辆行驶的安全性，实用性高。
[0113] 以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
