地下车库自动泊车的定位方法及装置与流程

本发明涉及自动驾驶技术领域，尤其涉及一种地下车库自动泊车的定位方法及装置。

背景技术：

自动泊车而言，地下停车场是一个不能回避的场所。室内环境全球定位系统信号很弱或完全没有，无法像室外为汽车提供精准位置信息。现有自动驾驶泊车系统多采用超声波雷达或摄像头检测周围环境信息。在单一检测模式下，超声波雷达仅能识别障碍物的位置信息，需要人为参与识别障碍物种类并判断是否为泊车位置；摄像头可识别车位线，但受环境影响较大，在光照和天气条件不佳或车位线遮挡的情况下识别效果往往会受到一定影响。

雷达、监控、视觉等系统在地下环境的精准度和稳定度不足。随着科学技术的不断进步，室内定位技术不断发展，有蓝牙室内定位技术、红外线室内定位技术、超宽带室内定位技术、zigbee室内定位技术、wifi室内定位技术等。但这些技术使用不便或成本高，不能满足量产自动泊车的定位需求。因此自动泊车急需一种稳定、精度较高、成本适中且使用便捷的定位方法。

结合超声波雷达及摄像头的检测定位方式是使用超声波雷达检测障碍物位置信息，判断是否存在足够空间用于泊车，结合摄像头检测到的车位线等车位标识信息判断是否为允许的泊车场景，进而检测出自身相对于车位线的位姿，基于相对位置关系生成入库轨迹。

仅利用车辆配置的超声波雷达和摄像头可以识别车位并得出相对位姿关系，但这种方案需要对停车场进行大范围的改装，对每个车位安装超声波传感器和射频识别单元，成本较高。

技术实现要素：

本发明实施例的目的是提供一种地下车库自动泊车的定位方法及装置，以解决现有技术中的定位成本高，不能满足量产自动泊车的定位需求。

为解决上述问题，第一方面，本发明提供了一种地下车库自动泊车的定位方法，所述方法包括：

当车辆进入地下车库，循着规划好的全局规划路线行驶时，在k-1时刻，采集第一数量个第一类型标签图像，得到第一数量个第一类型标签相对于所述车辆的第一时刻位姿信息；

获取k-1时刻车辆的航向角速率和速度；

根据所述k-1时刻车辆的航向角速率和速度，计算车辆在k时刻的位置和航向；

根据车辆在k时刻的位置和航向,以及车辆在k-1时刻的位置和航向，确定车辆的第一位置增量和第一角度增量；

通过所述第一位置增量和所述第一角度增量，对第一数量个第一类型标签的第一时刻位姿信息进行修正，得到k时刻，第一数量个第一类型标签相对于所述车辆的第二时刻位姿信息；

在k时刻，采集第二数量个第一类型标签图像，得到第二数量个第一类型标签相对于所述车辆的第二时刻位姿信息；

根据所述第一数量个、第二数量个第一类型标签相对于所述车辆的第二时刻位姿信息，对所述第一数量个第一类型标签、第二数量个第一类型标签经过聚类之后进行曲线拟合，得到第一局部路径；

所述车辆沿着所述第一局部路径行驶时，当k+n时刻，采集到第二类型标签的图像时，判断所述第二类型标签的图像中是否包括目标车位编号；

当包括目标车位编号时,经过欧拉变换，计算车辆相对于目标车位的第三时刻位姿信息；

根据k+n-1时刻的航向角速率和速度，计算车辆k+n时刻在车位坐标系的位置和航向；

根据当前时刻的时间信息，对k+n时刻，所述车辆的位置、航向与所述第三时刻位姿信息进行同步处理；

根据同步处理结果，通过滤波进行数据融合，得到车辆相对于目标车位的目标航向和目标位置。

在一种可能的实现方式中，所述方法之前还包括：

接收终端发送的目标车位信息；所述目标车位信息包括目标车位在第一坐标系的位置信息；

根据车辆的当前位置信息和所述目标车位信息，生成全局规划路线。

在一种可能的实现方式中，所述获取k-1时刻车辆的航向角速率和速度具体包括：

获取imu或陀螺仪测量的航向角速率；

获取轮速计测量的速度。

在一种可能的实现方式中，所述根据所述k-1时刻车辆的航向角速率和速度，计算车辆在k时刻的位置和航向具体包括：

利用公式计算车辆在k时刻的位置和航向；

其中，为车辆在k时刻的位置和航向，为车辆在k-1时刻的位置和航向，v为k-1时刻的速度，yawk-1为k-1时刻的航向，groz为k-1时刻的航向角速率，δt为k时刻与k-1时刻的时间差值。

在一种可能的实现方式中，所述根据车辆在k时刻的位置和航向,以及车辆在k-1时刻的位置和航向，确定车辆的第一位置增量和第一角度增量，具体包括：

利用公式计算第一航向角增量和第一位置增量；

其中，为k时刻的位置和航向，为k-1时刻的位置和航向；为第一位置增量和第一角度增量。

在一种可能的实现方式中，所述通过所述第一位置增量和所述第一角度增量，对第一数量个第一类型标签的第一时刻位姿信息进行修正具体包括：

根据第一位置增量和第一角度增量对多个第一类型标签在第一时刻的位姿信息进行欧拉变换。

在一种可能的实现方式中，所述滤波包括扩展卡尔曼滤波或蒙特卡洛粒子滤波。

第二方面，本发明提供了一种地下车库自动泊车的定位装置，所述装置包括：

计算单元，所述计算单元用于当车辆进入地下车库，循着规划好的全局规划路线行驶时，在k-1时刻，采集第一数量个第一类型标签图像，得到第一数量个第一类型标签相对于所述车辆的第一时刻位姿信息；

获取单元，所述获取单元用于获取k-1时刻车辆的航向角速率和速度；

所述计算单元还用于，根据所述k-1时刻车辆的航向角速率和速度，计算车辆在k时刻的位置和航向；

确定单元，所述确定单元用于根据车辆在k时刻的位置和航向,以及车辆在k-1时刻的位置和航向，确定车辆的第一位置增量和第一角度增量；

所述计算单元还用于，通过所述第一位置增量和所述第一角度增量，对第一数量个第一类型标签的第一时刻位姿信息进行修正，得到k时刻，第一数量个第一类型标签相对于所述车辆的第二时刻位姿信息；

所述计算单元还用于，在k时刻，采集第二数量个第一类型标签图像，得到第二数量个第一类型标签相对于所述车辆的第二时刻位姿信息；

曲线拟合单元，所述曲线拟合单元用于根据所述第一数量个、第二数量个第一类型标签相对于所述车辆的第二时刻位姿信息，对所述第一数量个第一类型标签、第二数量个第一类型标签经过聚类之后进行曲线拟合，得到第一局部路径；

判断单元，所述判断单元用于当所述车辆沿着所述第一局部路径行驶时，当k+n时刻，采集到第二类型标签的图像时，判断所述第二类型标签的图像中是否包括目标车位编号；

所述计算单元还用于，当包括目标车位编号时,经过欧拉变换，计算车辆相对于目标车位的第三时刻位姿信息；

所述计算单元还用于，根据k+n-1时刻的航向角速率和速度，计算车辆k+n时刻在车位坐标系的位置和航向；

处理单元，所述处理单元用于根据当前时刻的时间信息，对k+n时刻，所述车辆的位置、航向与所述第三时刻位姿信息进行同步处理；

融合单元，所述融合单元用于根据同步处理结果，通过滤波进行数据融合，得到车辆相对于目标车位的目标航向和目标位置。

第三方面，本发明提供了一种设备，包括存储器和处理器，所述存储器用于存储程序，所述处理器用于执行第一方面任一所述的方法。

第四方面，本发明提供了一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如第一方面任一所述的方法。

第五方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面任一所述的方法。

通过应用本发明实施例提供的地下车辆自动泊车的定位方法及装置，基于地下车库的标签，车辆上的imu和轮速计等低成本的传感器，可实现在地下车库等信号不好的地方的导航定位，为自动泊车提供了精确的相对位置和航向，也为量产自动泊车的落地提供可行的解决方案。

附图说明

图1为本发明实施例一提供地下车库自动泊车的定位方法流程示意图；

图2为本发明实施例提供的地下车库的标签示意图；

图3为本发明实施例二提供的地下车库自动泊车的定位装置结构示意图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

图1为本发明实施例一提供地下车库自动泊车的定位方法流程示意图。该方法应用在自动驾驶车辆在地下车库等信号不好或者没有信号的场地中，进行泊车的场景。该方法的执行主体是自动驾驶车辆控制单元(automatedvehiclecontrolunit，avcu),avcu为自动驾驶车辆的处理器，相当于车辆的“大脑”。如图1所示，该方法包括以下步骤：

步骤101，当车辆进入地下车库，循着规划好的全局规划路线行驶时，在k-1时刻，采集第一数量个第一类型标签图像，得到第一数量个第一类型标签相对于车辆的第一时刻位姿信息。

其中，在执行步骤101之前，车辆会进行初始化，车辆上电之后保持静止，采集惯性测量单元(inertialmeasurementunit,imu)传感器数据，计算2s加速度计输出数据的平均值得到车辆当前水平姿态角，2s陀螺仪输出数据平均值作为初始陀螺仪零偏值。

在车辆处于地下车库的入口时，车辆接收终端或服务器分配的目标车位信息，该目标车位信息包括目标车位的编号。

该终端可以是无人驾驶车辆所对应的终端，该终端上设置有app，可以通过随机分配或者用户选择，为车辆分配一个目标车位，并将目标车位的编号和位置信息发送给车辆。

该服务器可以是地下车库的管理系统，当自动驾驶车辆行驶至地下车库入口处时，可以与服务器进行通信，比如，车辆的avcu可以向服务器发送一个车位请求消息，服务器接收到车位请求消息后，向avcu返回包括目标车位编号和位置信息的响应消息。

此时，车辆根据当前的位置信息和目标车位的位置信息，进行路径规划，规划出全局规划路线。

当车辆循着全局规划路线行走时，实时的通过摄像头获取第一类型标签，获取的第一类型标签的数量为第一数量。其中，第一类型标签是航迹标签，参见图2中的小圆点。

获取到摄像头采集到的第一数量个航迹标签后，通过已训练的标签识别深度学习网络，可以分别计算出第一数量个航迹标签中心在相机坐标系下的位姿。根据预先标定的相机外参、内参及相机坐标系和车辆坐标系的转换关系，获得第一时刻位姿信息。第一时刻位姿信息包括航迹标签在k-1时刻的在车辆坐标系下的航向和位置。

步骤102，获取k-1时刻车辆的航向角速率和速度。

具体的，车辆上设置的imu，可以获取当前的航向角速率。车辆上的轮速计，可以实时的获取车辆的速度。

步骤103，根据k-1时刻车辆的航向角速率和速度，计算车辆在k时刻的位置和航向。

具体的，可以利用公式计算车辆在k时刻的位置和航向；

其中，为车辆在k时刻的位置和航向，px和py为位置、yaw为航向，为车辆在k-1时刻的位置和航向，v为k-1时刻的速度，yawk-1为k-1时刻的航向，groz为k-1时刻的航向角速率，δt为k-1时刻与k时刻的时间差值。

在一个示例中，首先，可以通过imu，获取到k-2时刻的航向角速率，通过轮速计获取到k-2时刻的速度。

然后，通过k-2时刻的航向角速率，k-2时刻的速度，获取k-1时刻的位置和航向。

最后，通过k-1时刻的位置和航向，k-1时刻的航向角速率和速度，计算k时刻的位置和航向。

步骤104，根据车辆在k时刻的位置和航向,以及车辆在k-1时刻的位置和航向，确定车辆的第一位置增量和第一角度增量。

具体的，可以利用公式计算第一航向角增量和第一位置增量；

其中，为k时刻的位置和航向，为k-1时刻的位置和航向；为第一位置增量和第一角度增量。

步骤105，通过第一位置增量和第一角度增量，对第一数量个第一类型标签的第一时刻位姿信息进行修正，得到k时刻，第一数量个第一类型标签相对于车辆的第二时刻位姿信息。

具体的，对获取的多个前一时刻和当前时刻的第一位置增量和第一角度增量进行欧拉变换，得到第一数量个第一类型标签相对于车辆的第二时刻位姿。

步骤106，在k时刻，采集第二数量个第一类型标签图像，得到第二数量个第一类型标签相对于车辆的第二时刻位姿信息。

具体的，与步骤101相同，此处不再赘述。

步骤107，根据第一数量个、第二数量个第一类型标签相对于车辆的第二时刻位姿信息，对第一数量个第一类型标签、第二数量个第一类型标签经过聚类之后进行曲线拟合，得到第一局部路径。

具体的，第一数量个第一类型标签和第二数量个第一类型标签可能存在部分重合，可以对其进行聚类后，剔除重合的标签，从而得到第三数量个第一类型标签，其中第三数量不大于第一数量与第二数量之和。

对第三数量个第一类型标签进行曲线拟合，从而得到第三数量个第一类型标签拟合后的第一局部路径。

随后，车辆可以按照第一局部路径，进行行驶。

由于每一时刻获取都会获取到新的第一类型标签，因此，车辆在行驶的过程中，实时的根据步骤101-步骤107的，进行局部路径的规划。本申请仅以第一局部路径为例进行说明，此处对于其它多个局部路径，不再进行赘述。

步骤108，车辆沿着第一局部路径行驶时，当k+n时刻，采集到第二类型标签的图像时，判断第二类型标签的图像中是否包括目标车位编号。

具体的，车辆在沿着局部路径进行行驶中，会实时的通过摄像头获取标签，当标签中包括第二类型标签时，该第二类型标签具有编号，车辆会将编号与之前服务器发送的目标车位的编号进行匹配。当匹配一致时，说明获取的多个第二类型标签中包括目标车位的车位标签。当不一致时，车辆继续行驶，直到找到目标车位的车位标签。

步骤109，当包括目标车位时,经过欧拉变换，计算车辆相对于目标车位的第三时刻位姿信息。

具体的，参见步骤101，计算第三时刻位姿信息的方法与步骤101相同，此处不再赘述。

步骤110，根据k+n-1时刻的航向角速率和速度，计算车辆k+n时刻在车位坐标系的位置和航向。

具体的，车辆可以利用imu，获取到k+n-1、k+n-2时刻的航向角速率，通过轮速计，获取到k+n-1、k+n-2时刻的速度。

然后，根据k+n-2时刻的航向角速率和速度，计算k+n-1时刻的位置和航向。

最后，根据k+n-1的航向角速率和速度，k+n-1时刻的位置和航向，通过步骤104下的公式，计算k+n时刻的位置和航向。

步骤111，根据当前时刻的时间信息，对k+n时刻，车辆的位置、航向与第三时刻位姿信息进行同步处理。

具体的，根据深度学习算法得到目标车位在当前时刻的第三时刻位姿信息带有时间戳，根据步骤104和步骤105下的公式得到目标车位在当前时刻的位置和航向也带有时间戳。通过时间戳进行数据同步。

步骤112，根据同步处理结果，通过滤波进行数据融合，得到车辆相对于目标车位的目标航向和目标位置。

具体的，通过滤波后，得到的目标航向和目标位置的精度更高。

可以理解的是，本申请中的imu可以更换为陀螺仪，在泊车入库过程中，会根据摄像头拍摄的图像，实时的检测车位标签和车位线，从而提供更准确，更稳定的航向。在数据融合时，也可以采用蒙特卡洛粒子滤波。

通过应用本发明实施例提供的地下车辆自动泊车的定位方法，基于地下车库的标签，车辆上的imu和轮速计等低成本的传感器，可实现在地下车库等信号不好的地方的导航定位，为自动泊车提供了精确的相对位置和航向，也为量产自动泊车的落地提供可行的解决方案。

图3为本发明实施例二提供的地下车库自动泊车的定位装置结构示意图。该装置应用在实施例一的方法中，如图3所示，该装置包括：计算单元301，获取单元302，确定单元303，曲线拟合单元304，判断单元305，处理单元306和融合单元307。

计算单元301用于当车辆进入地下车库，循着规划好的全局规划路线行驶时，在k-1时刻，采集第一数量个第一类型标签图像，得到第一数量个第一类型标签相对于车辆的第一时刻位姿信息；

获取单元302用于获取k-1时刻车辆的航向角速率和速度；

计算单元301还用于，根据k-1时刻车辆的航向角速率和速度，计算车辆在k时刻的位置和航向；

确定单元303用于根据车辆在k时刻的位置和航向,以及车辆在k-1时刻的位置和航向，确定车辆的第一位置增量和第一角度增量；

计算单元301还用于，通过第一位置增量和第一角度增量，对第一数量个第一类型标签的第一时刻位姿信息进行修正，得到k时刻，第一数量个第一类型标签相对于车辆的第二时刻位姿信息；

计算单元301还用于，在k时刻，采集第二数量个第一类型标签图像，得到第二数量个第一类型标签相对于车辆的第二时刻位姿信息；

曲线拟合单元304用于根据第一数量个、第二数量个第一类型标签相对于车辆的第二时刻位姿信息，对第一数量个第一类型标签、第二数量个第一类型标签经过聚类之后进行曲线拟合，得到第一局部路径；

判断单元305用于当车辆沿着第一局部路径行驶时，当k+n时刻，采集到第二类型标签的图像时，判断第二类型标签的图像中是否包括目标车位编号；

计算单元301还用于，当包括目标车位编号时,经过欧拉变换，计算车辆相对于目标车位的第三时刻位姿信息；

计算单元301还用于，根据k+n-1时刻的航向角速率和速度，计算车辆k+n时刻在车位坐标系的位置和航向；

处理单元306用于根据当前时刻的时间信息，对k+n时刻，车辆的位置、航向与第三时刻位姿信息进行同步处理；

融合单元307用于根据同步处理结果，通过滤波进行数据融合，得到车辆相对于目标车位的目标航向和目标位置。

每一个单元的具体作用与实施例一相对于，此处不再赘述。

通过应用本发明实施例提供的地下车辆自动泊车的定位装置，基于地下车库的标签，车辆上的imu和轮速计等低成本的传感器，可实现在地下车库等信号不好的地方的导航定位，为自动泊车提供了精确的相对位置和航向，也为量产自动泊车的落地提供可行的解决方案。

本发明实施例三提供了一种设备，包括存储器和处理器，存储器用于存储程序，存储器可通过总线与处理器连接。存储器可以是非易失存储器，例如硬盘驱动器和闪存，存储器中存储有软件程序和设备驱动程序。软件程序能够执行本发明实施例提供的上述方法的各种功能；设备驱动程序可以是网络和接口驱动程序。处理器用于执行软件程序，该软件程序被执行时，能够实现本发明实施例一提供的方法。

本发明实施例四提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行本发明实施例一提供的方法。

本发明实施例五提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现本发明实施例一提供的方法。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质中。

以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。