[发明专利]一种多无人驾驶车辆路径协同规划方法及系统

权利要求书

1.一种多无人驾驶车辆路径协同规划方法，其特征在于，包括：

建立多无人驾驶车辆行驶的环境模型；所述环境模型包括：车辆行驶区域、静态障碍物和动态障碍物；

建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型；所述防碰撞包裹圆模型为两个无人驾驶车辆的包裹圆不重叠；所述包裹圆的半径是根据对应无人驾驶车辆的前悬长度、后悬长度、车宽以及前后轴之间的距离进行确定；所述包裹圆的圆心是根据对应无人驾驶车辆的后轴中间坐标、在大地坐标系中的姿态角以及前后轴之间的距离进行确定；

根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型；

根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径。

2.根据权利要求1所述的一种多无人驾驶车辆路径协同规划方法，其特征在于，所述建立多无人驾驶车辆行驶的环境模型，具体包括：

获取动态障碍物的起始点位置、运动时间以及运动速度；

根据动态障碍物的起始点位置、运动时间以及运动速度确定动态障碍物的运动轨迹。

3.根据权利要求1所述的一种多无人驾驶车辆路径协同规划方法，其特征在于，所述建立每一无人驾驶车辆的运动学模型，并根据运动模型建立无人驾驶车辆之间的防碰撞包裹圆模型，具体包括：

利用公式确定无人驾驶车辆之间的防碰撞包裹圆模型；

其中，为第i个无人驾驶车辆的包裹圆的半径，为第j个无人驾驶车辆的包裹圆的半径，，为第i个无人驾驶车辆的包裹圆的圆心坐标，，为第j个无人驾驶车辆的包裹圆的圆心坐标，N为无人驾驶车辆的个数。

4.根据权利要求3所述的一种多无人驾驶车辆路径协同规划方法，其特征在于，所述根据无人驾驶车辆的之间防碰撞包裹圆模型以及环境模型，采用分离超平面定理，建立无人驾驶车辆与环境中动静态障碍物防碰撞模型，具体包括：

利用公式确定无人驾驶车辆与环境中动静态障碍物防碰撞模型；

其中，为超平面的法向量，为超平面的偏移量，与被参数化为样条，超平面的运动被描述为时间的函数，为第i个无人驾驶车辆的运动轨迹，为第i个无人驾驶车辆的纵向速度，t为第i个无人驾驶车辆的运动时间，为超平面的法向量的转置矩阵，时间[0，T]为运动的总时间。

5.根据权利要求4所述的一种多无人驾驶车辆路径协同规划方法，其特征在于，所述根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型，基于交替方向乘子法逐步迭代对多无人驾驶车辆进行解耦式运动规划，确定每一无人驾驶车辆的最优路径，具体包括：

根据环境模型、运动学模型、防碰撞包裹圆模型以及无人驾驶车辆与环境中动静态障碍物防碰撞模型确定每一无人驾驶车辆的起点与终点，并根据每一无人驾驶车辆的起点与终点，采用B样条曲线，确定每一无人驾驶车辆的运动轨迹的B样条曲线函数；

根据每一无人驾驶车辆的运动轨迹的B样条曲线函数，建立以所有无人驾驶车辆的运动轨迹的总和最小为目标的总和函数；

根据总和函数建立增加二次惩罚项的增广拉格朗日函数；

根据增加二次惩罚项的增广拉格朗日函数，利用交替方向乘子法进行迭代更新，确定每一无人驾驶车辆的最优路径。