基于速度障碍的无人车实时轨迹规划方法

摘要

本发明公开一种基于速度障碍的无人车实时轨迹规划方法，包括：1：基于无人车A的当前位置构造防碰机动搜索树的节点n_i,j；2：基于无人车A及障碍物速度构造无人车A的可达防碰速度集合RAV_j(t_i)；3：通过三次多项式光滑可控单元计算当前规划时刻的最佳速度；4：设置速度风险因子risk_i()；5：基于可达速度集合RV_j(t_i)和risk_i()选择新的速度，即t_i时刻操作符o_i,j,l；6：基于节点n_i,j和操作符o_i,j,l构造树枝e_j,k，获得时刻t_i+1或t_i+T_s时刻节点n_i+1,k；7：如n_i+1,k节点状态位于目标状态g的极小邻域内，则算法完成，否则返回第一步循环计算。本发明适用于无人驾驶车辆换道、交叉路口转弯、避障等各种机动情况下的实时轨迹规划，在多个动态障碍物存在条件下能够保证实时性和规划结果的高精度，并最大限度地满足规划速度及轨迹平滑性的要求。

主权项

基于速度障碍的无人车实时轨迹规划方法，其特征在于，包括以下步骤：步骤一，基于无人车A的当前位置构造防碰机动搜索树的节点n_i,j，表示时刻t_i无人车A的位置和速度：节点：$n_{i,j}=\{x_{i,j}=(X_j\left(t_i\right),Y_j\left(t_i\right)),{\overrightarrow{v}}_{i,j}=({\upsilon }_{j,x}\left(t_i\right),{\upsilon }_{j,y}\left(t_i\right))\}---\left(1\right)$其中n_i,j是t_i时刻第j个节点，x_i,j＝(X_j(t_i),Y_j(t_i))是A的位置信息，是A的速度信息；步骤二，基于无人车A的速度及障碍物速度获得无人车A的可达防碰速度集合RAV_j(t_i)，构造防碰机动搜索树的操作符o_i,j,l：操作符：$O_{i,j,l}=\left\{{\overrightarrow{v}}_l\right|{\overrightarrow{v}}_l\in {\mathrm{RAV}}_j\left(t_i\right),{\overrightarrow{v}}_l=({\upsilon }_{l,x}\left(t_i\right),{\upsilon }_{l,y}\left(t_i\right))={\overrightarrow{v}}_{i,j}+A_{\mathrm{XY}}{T}_s\}---\left(2\right)$可达防碰速度集合RAV_j(t_i)中的速度作为防碰机动搜索树中的操作符，通过其扩展t_i时刻的节点到t_i+1时刻的后继节点，且t_i+1＝t_i+T_s；其中，RAV_j(t_i)是为节点n_i,j计算的可达避障速度，A_XY为无人车A在二维平面的可行加速度区域，o_i,j,l是节点n_i,j上第l个操作符；步骤三，通过三次多项式光滑可控单元计算当前规划时刻的最佳速度步骤四，设置速度风险因子速度风险因子是所选择的速度同所有障碍物发生碰撞的最短预测时间以及同之间距离的函数：${\mathrm{risk}}_i\left({\overrightarrow{v}}_l^{\prime }\right)=p_t/{\mathrm{tc}}_i\left({\overrightarrow{v}}_l^{\prime }\right)+p_{\upsilon }\left|\right|{{\overrightarrow{v}}_l}^{\mathrm{perf}}-{{\overrightarrow{v}}_l}^{\prime }\left|\right|---\left(3\right)$其中p_t和p_υ分别是碰撞时间项和速度偏差项的加权因子，p_t根据采样间隔T_s选取，p_υ根据最大加速度和采样间隔T_s选取；步骤五，基于可达速度集合RV和选择新的速度即t_i时刻操作符o_i,j,l；步骤六，基于节点n_i,j和操作符o_i,j,l构造树枝e_j,k，获得时刻t_i+1或t_i+T_s时刻节点n_i+1,k：树枝：$e_{j,k}=\left\{(n_{i,j},n_{i+1,k})\right|n_{i+1,k}=n_{i,j}+((O_{i,j,l}+{\overrightarrow{v}}_{i,j})T_s/2)---\left(4\right)$e_j,k是防碰机动搜索树中t_i时刻节点n_i,j同t_i+1时刻节点n_i+1,k之间的边；步骤七，如n_i+1,k节点状态为目标状态g，则算法完成，否则返回第一步循环计算。