CN109131318B - 一种基于拓扑地图的自主泊车路径协调方法 - Google Patents

Abstract

本发明公开一种基于拓扑地图的自主泊车路径协调方法，是基于V2X通信，自主泊车控制器通过停车场服务器提供的停车场拓扑地图、目标空置车位和动态道路通行矩阵，规划行车的全局路径和泊车路径，然后通过融合协调，形成平滑完整的自主泊车路径。本发明方法实现了在车辆运行前就规划出从停车场入口到车位的自主泊车路径，实现全局路径与泊车路径的无偏差融合，使低速自动驾驶与自动泊车能够平滑过渡，解决了低速自动驾驶与自动泊车的高效切换问题。

Description

一种基于拓扑地图的自主泊车路径协调方法

技术领域

本发明涉及智能网联汽车技术领域，具体涉及一种基于拓扑地图的停车场自主泊车路径协调方法。

背景技术

随着汽车智能化的发展，自动泊车辅助系统(Automated Parking Assist,APA)已在实际生活中帮助人们提高了泊车的效率与安全性。然而传统的自动泊车需要驾驶员在停车场中寻找车位，然后将车辆准确地停在泊车位置，泊车整个过程仍需要驾驶员介入，未能减轻该工况下的驾驶负担，因而研发更高智能化程度的自主泊车系统，或称为自动代客泊车(Automated Valet Parking,AVP)，使汽车能够自主的从停车场入口落客区完成整个低速自动驾驶与自主泊车任务，将人们彻底从泊车工况中解放出来，是弥补APA现有功能缺陷的有效途径。

然而自主泊车的研究尚存在诸多问题与局限性。在自动驾驶的技术背景下，将低速自动驾驶与传统的自动泊车技术相结合是实现自主泊车的有效途径。但采用高精度地图的传统自动驾驶方案不仅计算代价高，硬件成本高，也难以与现有泊车方法结合，并不适用当前APA系统升级的平滑过渡，也无法针对性地实现自主泊车功能。而低速自动驾驶与自动泊车的高效切换，即如何解决车辆停车位置、切换时机以及车辆到达车位后的再确认，是自主泊车的关键问题。

发明内容

基于现有技术的不足，本发明的目的在于提出一种基于拓扑地图的自主泊车路径协调方法，着力于解决车辆从停车场落客区到停车位之间的全局路径与泊车路径的融合问题，进而确定低速自动驾驶与泊车切换时的停车位置、切换时机，同时避免了车辆到达车位之后再确认泊车。主要涉及基于激光雷达点云数据的停车场拓扑地图构建和基于拓扑地图的自主泊车路径的协调方法。

本发明解决以上技术问题所采取的技术方案如下：一种基于拓扑地图的自主泊车路径协调方法，该方法用于融合全局路径与泊车路径，确定低速自动驾驶与泊车系统切换时的车辆停车位置与切换时机。具体如下：

基于V2X通信，自主泊车控制器根据停车场服务器提供的停车场拓扑地图、目标空置车位和动态道路通行矩阵，规划行车的全局路径和泊车路径，然后通过融合协调，形成平滑完整的自主泊车路径。

包含如下步骤：

(1)在停车场端，布置停车场服务器，所述服务器中存储有停车场拓扑地图，同时具有根据停车场监控实时更新动态道路通行矩阵和空余车位编号，为车辆分配目标空置车位的能力，所述地图中包含有道路主节点、停车位对应道路节点、各节点构建的邻接矩阵、车位信息；在车辆端，布置自主泊车控制器，所述控制器基于V2X通信从停车场服务器下载停车场拓扑地图，并接收目标空置车位信息和当前动态道路通行矩阵；

(2)所述自主泊车控制器根据车辆当前位置及停车场服务器提供的信息，基于停车场拓扑地图，规划车辆在道路上可通行的全局路径，然后根据全局路径末点的行车方向与停车位之间的方位关系确定泊车方向，再根据停车位泊车类型、全局路径末点距离车位的横向距离、停车位顶点坐标信息和车辆的外形参数规划泊车路径；

(3)经过全局路径与泊车路径的拼接、节点优化、平滑处理、路径拟合，得到无偏差融合且平滑过渡的自主泊车路径。

其中，停车场拓扑地图构建的方法是，基于激光雷达点云数据：

1)在激光雷达采集前的预定义坐标系下，从激光雷达采集的点云数据中提取停车位顶点坐标、车道线节点坐标，停车位顶点指车位的四个角点，车道线节点指各路段车道线的端点；

2)根据预定义坐标系的平面中心原点与平面法向量，将步骤1)提取的坐标经过投影变换、四元数变换后变为平面直角坐标系下的坐标；

3)选取停车场内某一点作为坐标系原点，建立停车场坐标系，然后将2)中的坐标转至停车场坐标系下；

4)设计停车位顶点、道路主节点和停车位对应道路节点：

(a)每个停车位的四个角点都作为停车位顶点；

(b)道路主节点：选取同向车道中心线之间的交点作为道路主节点，且主节点之间连线与对应道路侧的车位线平行；

(c)停车位对应道路节点：在预设的车位坐标系基础上确定：即在车位中心面向道路方向时，以车位左后顶点为车位坐标系原点，左后顶点到右后顶点的方向为车位坐标系x轴正方向，左后顶点到左前顶点的方向为y轴正方向；然后，过车位中心且平行于y轴的直线与所面向道路的两个道路主节点连线的交点作为该车位对应的道路节点；

5)构建邻接矩阵：对步骤4)设计的道路主节点和停车位对应道路节点统一编号，对于任意两节点i,j，根据从节点i到节点j的直接连接情况，确定矩阵中的元素a_i,j为0还是1，可通行设置为1，不可通行为0，最终根据节点总数N构建出N*N的邻接矩阵A_r+p；

6)对地图信息统一描述，包括道路拓扑信息和车位信息：

(a)道路拓扑信息Map包括由道路主节点信息Node_r和车位对应道路节点信息Node_p构成的所有节点信息Node_r+p，以及邻接矩阵A_r+p：

Map＝G(Node_r+p,A_r+p)

式中，Node_r和Node_p中各节点坐标由停车场坐标系横、纵坐标表示，N_r为道路主节点个数，N_p为停车位个数也即车位对应道路节点个数，Nr+N_p＝N；A_r+p由描述节点之间连接关系的元素a_i,j构成，a_i,j∈{0,1}；

(b)车位信息表述如下：

P_p,i＝{n_p,i,T_p,i,W_i,L_i,D_i,h_i,P_pv,i}

其中，停车场所有车位的车位顶点信息如下:

N_p为停车位个数，每个车位四个顶点V_lr,V_rr,V_rf,V_lf分别表示车位左后、右后、右前、左前顶点；

n_p,i代表第i个车位的车位编号，T_p,i代表第i个车位的车位类型，W_i，L_i分别代表第i个车位的宽和长，D_i表示第i个车位前方的通车道路宽，h_i表示第i个车位泊车起点的横向位移参数；P_pv,i代表第i个车位的四个顶点的坐标；

并构建车位编号与停车位对应道路节点之间的对应关系如下：

P2R＝{n_p,i,Node_p,i,DP_i,LP_i}

Node_p,i代表第i个车位对应的停车位对应道路节点；DP_i代表第i个车位所允许的泊车方向；LP_i代表第i个车位直线泊车需要添加的节点编号。

其中，基于停车场道路节点设计原则与自主泊车路径规划原则，融合全局路径与泊车路径：

1)基于停车场道路节点设计原则，根据车位信息中的泊车类型，以全局路径Path_global末点

的方向判断泊车方向，

距离车位的横向距离作为泊车规划的初始条件，进而规划出泊车路径Path_parking：

Drt_driving,i∈{-1,0,1}

式中，(x_gp,i,y_gp,i)为路径第i个节点的坐标，N_gp为路径节点数，Path_parking,i为第i段泊车路径，(x_ppi,j,y_ppi,j)为第i段路径第k个节点的坐标，N_ppi为第i段路径的节点数，Drt_driving,i为第i段路径允许车辆的行驶方向，定义1为前进，-1为倒车，0表示路径不存在；

2)将全局路径末点替换为泊车起始点，若泊车路径存在前行部分，在节点替换后将全局路径与泊车路径拼接共同作为前进段路径Path_forward，倒车部分作为倒车段路径Path_backward，表述如下：

Path_forward包括两段路径，若泊车规划不存在车辆前进部分，Path_forward仅取式中第一部分；

3)对拼接后的路径进行节点优化，根据各节点的方向信息判断相关联的节点是否在同一道路直线上，如果在同一条直线上，仅保留首末节点，中间点剔除，节点优化后的结果如下：

式中，角标f/b表示前进段或倒车段路径，(x_f,i,y_f,i)、(x_b,i,y_b,i)、ψ_f,i、ψ_b,i、N_f、N_b分别表示节点优化后的坐标，对应坐标的航向角和行车路径节点编号；

4)对节点优化后的前进段路径进行平滑处理，对倒车段路径拟合，获得曲率由0～ρ_max～0连续变化的可驾驶路径，ρ_max为最大曲率。

与现有技术相比，本发明显著的有益效果体现在：

1.提出了基于停车场拓扑地图的自主泊车路径设计及协调方法，该方法基于停车场道路节点设计原则和自主泊车路径规划原则，能够在车辆运行前规划出从停车场入口到进入车位的自主泊车路径，实现全局路径与泊车路径的无偏差融合，进而确定出理想的车辆停车位置与切换时机，使低速自动驾驶与自动泊车能够平滑过渡，从路径规划的角度解决了低速自动驾驶与自动泊车的高效切换问题。

2.提出了基于激光雷达点云数据的停车场拓扑地图构建方法，合理利用了现有的激光雷达数据，所建地图数据结构简洁，在地图中设计车位信息，能够避免车辆到达车位后再确认车位，提高了自主泊车效率，该方法可推广至无GPS信号的停车场地图构建。

3.提出了基于动态道路通行矩阵和改进A*算法的全局路径规划方法，该方法将停车场拓扑地图中各连接节点之间实际道路的可通行能力，反映在动态道路通行矩阵中，该矩阵与改进A*算法中目标函数相结合，可在车辆路径规划阶段考虑当前时刻停车场中的各路段通行能力，规划更加合理的行车路径。

本发明的其他特征和优点将在随后的说明书中阐述，并且部分的从说明书中变得显而易见，或者通过实施本发明而了解。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制。

图1是自主泊车系统架构图；图中泊车路径前进段用虚线框表示，表示某些泊车规划没有前进段路径；车辆定位信息用虚箭头表示，因为该定位信息来自于车辆定位系统，而并非直接来自于自主泊车控制器。

图2是自主泊车路径协调方法示意图，图中各关键节点：S1、B、S2、P1、P2、P3、P4、M1、M2、M3、M4。

图3是停车场拓扑地图中自主泊车路线图，图中点画线表示自主泊车路径。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明。但本领域的技术人员应该知道，以下实施例并不是对本发明技术方案作的唯一限定，凡是在本发明技术方案精神实质下所做的任何等同变换或改动，均应视为属于本发明的保护范围。

本发明提出的一种基于拓扑地图的自主泊车路径协调方法，引用了包括道路拓扑图与车位信息在内的停车场拓扑地图信息，然后基于V2X通信，自主泊车控制器针对停车场服务器提供的停车场拓扑地图、目标空置车位和动态道路通行矩阵，规划融合全局路径与泊车路径构成的自主泊车路径，在规划阶段保证低速自动驾驶与自动泊车能够平滑过渡；实际运行过程中，可根据道路实际通行情况，在线调整自主泊车路径。

如图1所示，在停车场端，布置停车场服务器，该服务器存储有包含道路拓扑图和车位信息的停车场拓扑地图，同时具有根据停车场监控实时更新动态道路通行矩阵和空余车位编号，为车辆分配目标空置车位的能力。在车辆端，布置自主泊车控制器，该控制器基于V2X通信从停车场服务器下载停车场拓扑地图，并接收目标空置车位编号和当前的动态道路通行矩阵。停车场服务器和自主泊车控制器都可在获知道路信息、空余车位信息等情况下，做出自主泊车路径规划，包括全局路径规划、泊车路径规划和路径协调。

因此，方案实施主要包含如下过程：

一、基于激光雷达点云数据的停车场拓扑地图构建

二、基于动态道路通行矩阵与改进A*算法的全局路径规划

三、泊车路径规划

四、全局路径与泊车路径的协调

下面一一予以细述：

一、基于激光雷达点云数据的停车场拓扑地图构建

停车场拓扑地图离线设计，存储于停车场服务器中，通过V2X通信模块与车辆交互，在接收到车辆发出的泊车请求后，将其发送给车载自主泊车控制器。

本发明提出的基于激光雷达点云数据设计停车场拓扑地图的步骤如下：

(1)利用现有的激光雷达点云数据，在采集时预定义的坐标系下，提取停车位顶点坐标、车道线节点坐标，停车位顶点指车位的四个角点，车道线节点指各路段车道线的端点；

(2)根据(1)中坐标系的原点与平面法向量，将提取的坐标经过投影变换、四元数变换后处理为平面直角坐标系下的坐标；

(3)选取停车场坐标系基准点，建立停车场的平面直角坐标系，停车场基准点选取方式基于实际情况任意选取，例如可以选取一个车位的某一顶点作为基准点，该车位长和宽边分别作为x轴与y轴，然后将(2)中的结果经过平面坐标转换至停车场坐标系下；

(4)设计停车位顶点、道路主节点和停车位对应道路节点：

(a)选取每个停车位的四个角点都作为车位顶点；

(b)道路主节点：设计道路节点时，需保证靠近车道的车位线与对应侧道路节点连线平行。针对有车道线的停车场，选取同向车道中心线之间的交点作为道路主节点，且主节点之间连线应与对应道路侧的车位线平行；无车道线的停车场，应在道路中间假设一条平行于对应道路侧车位的直线，作为等效车道中心线，且该直线与车位间要保持安全通行距离，单双向通行视车道宽度选定，以等效车道中心线之间的交点作为道路主节点。将该步骤中涉及到的平行原则称为停车场道路节点设计原则。

(c)停车位对应道路节点：首先建立车位坐标系：站在车位中心(四个顶点对角线交点)面向道路方向时，以车位左后顶点为车位坐标系原点，左后顶点到右后顶点的方向为车位坐标系x轴正方向，左后顶点到左前顶点的方向为y轴正方向；然后，过车位中心且平行于y轴的直线与所面向道路的两个道路主节点连线的交点为该车位对应的道路节点。

(5)构建邻接矩阵：对步骤(4)设计的道路主节点和车位对应的道路节点统一编号，并基于实际的道路连通情况建立描述节点连接关系的邻接矩阵A_r+p，即对于任意两节点i,j，根据从节点i到节点j的直接连接情况，设定A_r+p中的元素a_i,j为0或1，可通行设置为1，不可通行为0。同理节点j到节点i，以a_j,i描述，最终根据节点总数N构建出N*N的邻接矩阵A_r+p。

(6)对地图信息统一描述，包括道路拓扑信息和车位信息：

(a)道路拓扑信息Map包括由道路主节点Node_r和车位对应道路节点Node_p构成的节点信息Node_r+p，以及邻接矩阵A_r+p：

Map＝G(Node_r+p,A_r+p)

式中，Node_r和Node_p中各节点坐标由停车场坐标系横、纵坐标表示，N_r为道路主节点个数，N_p为停车位个数也即车位对应道路节点个数，Nr+N_p＝N；A_r+p由描述节点i,j之间连接关系的元素a_i,j(或a_j,i)构成，a_j,i、a_i,j∈{0,1}。

(b)车位信息除了包含自身车位信息如车位顶点之外，还要考虑其他相关信息，包括车位编号，车位类型(包括平行车位、垂直车位、斜向车位等)，车位的宽和长，车位前方的通车道路宽，泊车起点的横向位移参数，车位所允许的泊车方向(包括右侧、左侧泊车和直线倒车方式)，直线泊车需要添加的节点编号。

其中，停车场所有车位的车位顶点信息集合如下:

N_p为停车位个数，每个车位由四个顶点表述，V_lr,V_rr,V_rf,V_lf分别表示车位左后、右后、右前、左前四个顶点，且每个顶点由停车场坐标系横、纵坐标表示。

车位信息表述如下：

P_p,i＝{n_p,i,T_p,i,W_i,L_i,D_i,h_i,P_pv,i}

n_p,i代表第i个车位的车位编号，T_p,i代表第i个车位的车位类型，W_i，L_i分别代表第i个车位的宽和长，D_i表示第i个车位前方的通车道路宽，h_i表示第i个车位泊车起点的横向位移参数；P_pv,i代表第i个车位的四个顶点的坐标。

构建车位编号与车位对应道路节点之间的对应关系如下：

P2R＝{n_p,i,Node_p,i,DP_i,LP_i}

Node_p,i代表第i个车位对应的停车位对应道路节点；DP_i代表第i个车位所允许的泊车方向；LP_i代表第i个车位直线泊车需要添加的节点编号。

以上步骤均为人工离线完成，停车场拓扑地图数据制作完成后，将Map、P_p,i、P2R等信息存储于停车场服务器中作为停车场地图的原始数据。

二、基于动态道路通行矩阵与改进A*算法的全局路径规划方法

为了使自主泊车控制器能够根据当前时刻的停车场道路通行能力规划出更合理的自主泊车路径，为自主泊车控制器全局规划模块设计了改进的A*算法，通过利用停车服务器向自主泊车控制器提供动态道路通行矩阵，规划更加合理的行车路径。

动态道路通行矩阵B_r+p与停车场拓扑地图中的邻接矩阵A_r+p具有相同矩阵维度，表述如下：

式中，b_i,j是表征节点i与节点j之间连接关系的元素，b_j,i同理，b_j,i、b_i,j∈[0,1]，根据道路的实际通行情况在0～1之间动态调整，0表示两道路节点间不可通行，1表示可直接通行，0与1之间的数表示可通行，但是通行能力不同。

定义道路实际可通行矩阵C_r+p，满足

c_j,i为矩阵中的元素。

设计改进A*算法的价值函数：

h(i)＝|x_f-x_r+p,i|+|y_f-y_r+p,i|

c_j,i＝a_j,i·b_j,i

故式中f(i)为改进的A*算法的价值函数，k(k≠0)为加权系数，加权系数根据工程经验确定，计算时c_j,i∈(0,1]，g(j)为起点到上一步检索节点的累计距离，g(j,i)为上一步检索的节点j到当前节点i的距离，h(i)为启发值函数，估计当前节点到目标节点的距离；(x_f,y_f)为目标车位对应的道路节点坐标，(x_r+p,i,y_r+p,i)为当前节点对应的Node_r+p数据中的坐标。

工作步骤如下：

(1)首先，停车场服务器基于V2X通信模块接收停车场中运行车辆的GPS信息，并实时发送到道路占用信息处理单元，该单元根据接收的车辆GPS信息并将其与停车场拓扑地图匹配，确定各车位置是在哪两个节点之间，若在i与j节点之间，则减小b_i,j与b_j,i的值，具体变化量自行设计，然后将当前时刻更新后的邻接矩阵A_r+p存储于停车场服务器中。

(2)自主泊车控制器接收来自停车场服务器的停车场地图信息、空余车位编号信息、指定的目标空置车位信息。如图1所示，利用改进A*算法从初始节点开始，算法每一步从A_r+p中搜索出相邻节点，利用改进的目标函数结合C_r+p计算出各相邻节点的f(i)，并对比找出该值最小的节点作为下一节点，如此循环直至找到目标空置车位的停车位对应道路节点，最终获得当前函数下的全局路径Path_global，整个检索过程与传统A*算法相同，此处不赘述。

式中，(x_gp,i,y_gp,i)为路径第i个节点的坐标，N_gp为路径节点数。

三、泊车路径规划

基于停车场道路节点设计原则，根据车位信息P_p,i中的T_p,i确定泊车类型(垂直、平行、斜向)，以全局路径Path_global末点

的方向判断泊车方向(左侧、右侧)，

距离车位的横向距离作为泊车规划的初始条件，规划出泊车路径Path_parking，以上称为自主泊车路径规划原则。

Drt_driving,i∈{-1,0,1}

式中Path_parking,i为第i段泊车路径，(x_ppi,k,y_ppi,k)为第i段路径第k个节点的坐标，N_ppi为第i段路径的节点数，Drt_driving,i用以描述第i段路径允许车辆的行驶方向，定义1为前进，-1为倒车，0表示路径不存在。

四、全局路径与泊车路径的协调

自主泊车控制器接收到停车场服务器发送的停车场拓扑地图信息、目标空置车位编号和动态道路通行矩阵后，规划此时从当前车辆位置到目标空置车位的自主泊车路径：

(1)基于停车场道路节点设计原则和自主泊车路径规划原则，以规划的全局路径和泊车路径作为路径融合的前提条件，保证全局路径末点与泊车路径起点在同一车道线上。若泊车路径存在前行部分，则将全局路径末点替换为泊车起始点，将泊车起始点与全局路径拼接共同作为前进段路径Path_forward，倒车部分作为倒车段路径Path_backward，表述如下：

式中，Path_forward包括两段路径，故Path_backward中路径从i＝2开始拼接剩余J-i+1段倒车路径，其中J为倒车路径总段数，若泊车规划不存在车辆前进部分，Path_forward仅需式中第一部分，而Path_backward从i＝1开始拼接。

(2)对路径拼接后的结果进行节点优化，根据各节点的方向信息判断相关联的节点是否在同一道路直线上，如果在同一条直线上，仅保留首末节点，中间点剔除。

节点优化后的结果如下：

式中，角标f/b表示前进段或倒车段路径(f＝forword,b＝backword)，(x_f,i,y_f,i)、(x_b,i,y_b,i)、ψ_f,i、ψ_b,i、N_f、N_b分别表示节点优化后的坐标，对应坐标的航向角和行车路径节点编号。

(3)对节点优化后的前进段路径进行平滑处理，如图2中的S1-S2段与中M1-M2段，所采用的平滑处理方法不限，但需输出的可驾驶路径需保证曲率连续的基本原则；然后对倒车段路径拟合，如图2中M2-M3(或M2-M3-M4)段曲线，拟合方法不限，获得曲率0～ρ_max～0连续变化的倒车路径。

自主泊车控制器将最终得到的自主泊车路径作为参考路径对车辆进行控制，如图3中点画线所示。

Claims (3)

1.一种基于拓扑地图的自主泊车路径协调方法，其特征在于，基于V2X通信，自主泊车控制器根据停车场服务器提供的停车场拓扑地图、目标空置车位和动态道路通行矩阵，规划行车的全局路径和泊车路径，然后通过融合协调，形成平滑完整的自主泊车路径；

包含如下步骤：

(1)在停车场端，布置停车场服务器，所述服务器中存储有停车场拓扑地图，同时具有根据停车场监控实时更新动态道路通行矩阵和空余车位编号，为车辆分配目标空置车位的能力，所述地图中包含有道路主节点、停车位对应道路节点、各节点构建的邻接矩阵、车位信息；在车辆端，布置自主泊车控制器，所述控制器基于V2X通信从停车场服务器下载停车场拓扑地图，并接收目标空置车位信息和当前动态道路通行矩阵；

(2)所述自主泊车控制器根据车辆当前位置及停车场服务器提供的信息，基于停车场拓扑地图，规划车辆在道路上可通行的全局路径，然后根据全局路径末点的行车方向与停车位之间的方位关系确定泊车方向，再根据停车位泊车类型、全局路径末点距离车位的横向距离、停车位顶点坐标信息和车辆的外形参数规划泊车路径；

(3)经过全局路径与泊车路径的拼接、节点优化、平滑处理、路径拟合，得到无偏差融合且平滑过渡的自主泊车路径。

2.根据权利要求1所述的基于拓扑地图的自主泊车路径协调方法，其特征在于，停车场拓扑地图构建的方法是：基于激光雷达点云数据，

1)在激光雷达采集前的预定义坐标系下，从激光雷达采集的点云数据中提取停车位顶点坐标、车道线节点坐标，停车位顶点指车位的四个角点，车道线节点指各路段车道线的端点；

2)根据预定义坐标系的平面中心原点与平面法向量，将步骤1)提取的坐标经过投影变换、四元数变换后变为平面直角坐标系下的坐标；

3)合理选取坐标系原点，建立停车场坐标系，然后将2)中的坐标转换至停车场坐标系下；

4)设计停车位顶点、道路主节点和停车位对应道路节点：

(a)每个停车位的四个角点都作为停车位顶点；

(b)道路主节点：选取同向车道中心线之间的交点作为道路主节点，且主节点之间连线与对应道路侧的车位线平行；

(c)停车位对应道路节点：在预设的车位坐标系基础上确定：即在车位中心面向道路方向时，以车位左后顶点为车位坐标系原点，左后顶点到右后顶点的方向为车位坐标系x轴正方向，左后顶点到左前顶点的方向为y轴正方向；然后，过车位中心且平行于y轴的直线与所面向道路的两个道路主节点连线的交点作为该车位对应的道路节点；

5)构建邻接矩阵：对步骤4)设计的道路主节点和停车位对应道路节点统一编号，对于任意两节点i,j，根据从节点i到节点j的直接连接情况，确定矩阵中的元素a_i,j为0还是1，可通行设置为1，不可通行为0，最终根据节点总数N构建出N*N的邻接矩阵A_r+p；

6)对地图信息统一描述，包括道路拓扑信息和车位信息：

(a)道路拓扑信息Map包括由道路主节点信息Node_r和车位对应道路节点信息Node_p构成的所有节点信息Node_r+p，以及邻接矩阵A_r+p：

Map＝G(Node_r+p,A_r+p)

式中，Node_r和Node_p中各节点坐标由停车场坐标系横、纵坐标表示，N_r为道路主节点个数，N_p为停车位个数也即车位对应道路节点个数，N_r+N_p＝N；

(b)车位信息表述如下：

P_p,i＝{n_p,i,T_p,i,W_i,L_i,D_i,h_i,P_pv,i}

其中，停车场所有车位的车位顶点信息如下:

N_p为停车位个数，每个车位四个顶点V_lr,V_rr,V_rf,V_lf分别表示车位左后、右后、右前、左前顶点；

n_p,i代表第i个车位的车位编号，T_p,i代表第i个车位的车位类型，W_i，L_i分别代表第i个车位的宽和长，D_i表示第i个车位前方的通车道路宽，h_i表示第i个车位泊车起点的横向位移参数；P_pv,i代表第i个车位的四个顶点的坐标；

并构建车位编号与停车位对应道路节点之间的对应关系如下：

P2R＝{n_p,i,Node_p,i,DP_i,LP_i}

Node_p,i代表第i个车位对应的停车位对应道路节点；DP_i代表第i个车位所允许的泊车方向；LP_i代表第i个车位直线泊车需要添加的节点编号。

3.根据权利要求1所述的基于拓扑地图的自主泊车路径协调方法，其特征在于，基于停车场道路节点设计原则与自主泊车路径规划原则，融合全局路径与泊车路径：

1)基于停车场道路节点设计原则，根据车位信息中的泊车类型，以全局路径Path_global末点

的方向判断泊车方向，

距离车位的横向距离作为泊车规划的初始条件，进而规划出泊车路径Path_parking：

Drt_driving,i∈{-1,0,1}

式中，(x_gp,i,y_gp,i)为路径第i个节点的坐标，N_gp为路径节点数，Path_parking,i为第i段泊车路径，(x_ppi,j,y_ppi,j)为第i段路径第k个节点的坐标，N_ppi为第i段路径的节点数，Drt_driving,i为第i段路径允许车辆的行驶方向，定义1为前进，-1为倒车，0表示路径不存在；

2)将全局路径末点替换为泊车起始点，若泊车路径存在前行部分，在节点替换后将全局路径与泊车路径拼接共同作为前进段路径Path_forward，倒车部分作为倒车段路径Path_backward，表述如下：

Path_forward包括两段路径，若泊车规划不存在车辆前进部分，Path_forward仅取式中第一部分；

3)对拼接后的路径进行节点优化，根据各节点的方向信息判断相关联的节点是否在同一道路直线上，如果在同一条直线上，仅保留首末节点，中间点剔除，节点优化后的结果如下：

式中，角标f/b表示前进段或倒车段路径，(x_f,i,y_f,i)、(x_b,i,y_b,i)、ψ_f,i、ψ_b,i、N_f、N_b分别表示节点优化后的坐标，对应坐标的航向角和行车路径节点编号；

4)对节点优化后的前进段路径进行平滑处理，对倒车段路径拟合，获得曲率由0～ρ_max～0连续变化的可驾驶路径，ρ_max为最大曲率。

Images