CN112078594A - 一种智能泊车系统的曲率连续泊车路径规划装置和方法 - Google Patents

Abstract

本发明提供一种智能泊车系统的曲率连续泊车路径规划装置，包括：传感器组件和信号处理组件；所述传感器组件，设置在车辆周身，用于采集所述车辆本身位姿信息及所述车辆周边的环境信息；所述信号处理组件，根据不同所述传感器组件提供的所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的泊车路径规划。曲率连续的路径有利于车辆跟踪时，减小离心力的变化，增加了车辆的舒适性和稳定性，曲率无突变，无需停车转向。

Description

一种智能泊车系统的曲率连续泊车路径规划装置和方法

技术领域

本发明涉及一种智能泊车系统的泊车空间探测和路径生成装置和方法，特别涉及曲率连续的泊车路径的确定。

背景技术

随着城市汽车数量的不断增长,汽车保有量持续增多,停车空间也越来越狭窄,资源也日益紧张,这对经验不足的驾驶员是严峻的考验.自动泊车技术等辅助泊车功能的实现,通过倒车雷达和摄像头,根据车辆当前位置信息和泊车车位,生成一条可行的泊车路径.有效提升泊车的成功率,降低驾驶员泊车难度.

现有技术中的泊车路径生成算法通常都各有利弊.A-star,Dijkstra,RRT等经典的算法,在当前寻路和图的遍历上普遍应用,具有一定的高效性.不过环境复杂多变的情况下,计算效率较低,但生成的路径包含不需要的折点,离散的路径点不满足曲率光滑的特性；样条曲线和多项式螺旋线都是光滑的曲线,容易保证曲率连续性,有界性,都要具有良好的性质,但是,在给定边界条件后,使用数值手段求解多项式的待定系数,求解效率低,且在需要避障的条件下,约束条件不易刻画；人工智能法需要依靠大量的专家数据作为训练样本,增加了工程实现的难度.

中国专利CN201710321947.6，提供一种泊车路径生成方法及系统、车载终端，设定车辆在车位中的位置为0级节点，并在所述车位外围设定一包含所述车位且沿车位纵向延伸的预设区域；基于0级节点，在前进或后退方向上将方向盘向左侧和右侧分别打一定角度所获取的路径不与车位的两侧边界相交且不超出预设区域时，在所获取的路径上生成若干个二级节点；依次类推，生成n级节点；设定各级节点均为关键节点，任一关键节点与车辆所在位置的连线与车位的长边沿的角度为关键角度，选取关键角度在一定阈值内的任一关键节点作为起始节点，由该起始节点所依附的一级节点至该起始节点的生成路径即为泊车路径。本发明的泊车路径生成方法及系统、车载终端能够生成多条泊车路径。但是其产生的路径是供驾驶员参照的多条路径，而非车辆自主泊车的无碰撞连续路径。

发明内容

为了实现本发明以上发明目的，本发明是通过以下技术方案实现的：

一种智能泊车系统的曲率连续泊车路径规划装置，包括：传感器组件和信号处理组件；

所述传感器组件，设置在车辆周身，用于采集所述车辆当前的位姿信息(位置和朝向，即车辆定位信息和角度信息)及所述车辆周边的环境信息；

所述信号处理组件，根据不同所述传感器组件提供的所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的泊车路径规划。

进一步所述的曲率连续泊车路径规划装置，所述传感器组件至少包括定位传感器、多路高清摄像头和/或多路无线电定位探测器；

所述定位传感器，用于确定所述车辆的位姿信息(车目前的位置(X/Y)坐标和角度(转Z)值)；所述定位传感器包括：IMU、GPS、陀螺仪、加速度计和/或地磁传感器。

所述高清摄像头，用于确定所述环境信息中的视觉信息；所述高清摄像头包括：RGB摄像头、广角摄像头、鱼眼摄像头和/或鱼眼广角摄像头。

所述无线电定位探测器，用于确定所述环境信息中的点云信息(尤其是目标物的点云数据：空间信息和位置信息——包括车位线、车道线、中心车道线、障碍物(停滞车辆)和/或静止车辆边界线的类别和位置)。所述无线电定位探测器包括：超声波雷达、毫米波雷达和/或激光雷达。

进一步所述的曲率连续泊车路径规划装置，所述信号处理组件包括：泊车位定位模块和泊车路径生成模块；

所述泊车位定位模块，用以确定所述车辆当前的位姿信息、空泊车位的位置和角度以及所述车辆周边的环境信息、所述空泊车位周边的环境信息；

所述泊车路径生成模块，用以由所述空泊车位的位置和角度至所述车辆的当前所述位姿信息生成曲率连续的所述泊车路径规划。

进一步所述的曲率连续泊车路径规划装置，所述泊车路径生成模块包括：设置子模块、节点生成子模块和节点遍历子模块；

所述设置子模块，用以设定所述车辆前向(车头朝向为前向，车尾朝向为后向)周边无碰撞范围和所述空泊车位前向周边无碰撞范围；(一字泊车位的前向，指与现停滞车辆前朝向，相同的方向；垂直泊车位的前向，指倒车停车入位后，停滞后车辆前朝向的方向)

所述节点生成子模块，用以在所述空泊车位与所述车辆之间生成多级路径探测节点；

所述节点遍历子模块，用以在所述多级路径探测节点的空间里遍历，找出与所述车辆的位姿或位姿延长线上的曲线连接，并探寻出至少一条(多条)无碰撞可达路径。

进一步所述的曲率连续泊车路径规划装置，所述泊车路径生成模块还包括：回溯子模块；

所述回溯子模块，用以从所述车辆的位姿至所述空泊车位，对所述无碰撞可达路径进行验证，最后采用数值最优化的方式(惩罚函数)生成一条光滑的几何路径，确定为所述泊车路径规划。

进一步所述的曲率连续泊车路径规划装置，所述节点生成子模块进一步包括：0级节点生成单元、1级节点生成单元、2级节点生成单元、3级节点生成单元、n级节点生成单元；

0级节点生成单元，用于确定在所述空泊车位内切的长方形短边中心连线上所述空泊车位后向三等分点所在的位置,标记为0级节点；

1级节点生成单元，用于由所述0级节点所在的所述长方形短边中心连线上(一字车位：后向三等分点至车位顶部)或其前向的延伸线上(垂直车位：车位顶部以外7米左右长度)生成多个直线型节点，标记为1级节点；

2级节点生成单元，用于由在任一所述1级节点的前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上和范围内(路径曲线C变化范围0至±0.2，下同)生成多个曲线型节点，标记为2级节点；

3级节点生成单元，用于由在任一所述2级节点的前向左侧和后向左侧，或者，前向右侧和后向右侧的二个切线方向上和范围内生成多个所述曲线型节点，标记为3级节点；

以此类推，n级节点生成单元，用于在n-1级节点的基础上生成n级所述曲线型节点；2级节点(含)以上节点皆为曲线型节点。

从而在所述空泊车位与所述车辆当前位姿之间，生成多级路径探测节点阵列。

进一步所述的曲率连续泊车路径规划装置，所述曲线型节点，至少根据之前节点在前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上对应第一、第二、第三和第四象限内生成的欧拉螺旋曲线：C＝R*L；其中，C为螺旋线的参数，路径曲率C变化设定于0至±0.2之间的，L为该曲线的弧长,R是曲线的曲率半径(那么在曲线的起点处有:L＝0，R＝∞终点处:L＝ls，R＝R_min,R_min是车辆的最小转弯半径.),R的最小值为所述车辆的最小转弯半径。

值得一提的是，在实际使用中，很有可能只生成一个明确的(大概率的)方向/方位上的欧拉螺旋曲线。

进一步所述的曲率连续泊车路径规划装置，当选取设定(一定)长度的弧长(15cm～80cm)的螺旋线，得到所述欧拉螺旋曲线的路径方程表达式:

dL＝R*dβ

dx＝dL*cosβ

dy＝dL*sinβ

其中，β为所述欧拉螺旋曲线上任意点处的切点与x轴的夹角.该所述欧拉螺旋曲线上的离散点通过菲涅儿积分求出。

进一步所述的曲率连续泊车路径规划装置，所述曲率连续的泊车路径规划(无碰撞可达路径)至少由B样条路径、高阶光滑曲线路径、贝塞尔路径或螺旋线路径中任一构成。

进一步所述的曲率连续泊车路径规划装置，当所述传感器组件检测到所述曲率连续的泊车路径规划中有节点被障碍物占据，所述信号处理组件重新确定所述曲率连续的泊车路径规划。

进一步所述的曲率连续泊车路径规划装置，所述空泊车位包括：平行泊车位、垂直泊车位或倾斜泊车位。

本发明还提供了一种智能泊车系统的曲率连续泊车路径车载终端，至少包括：传感器组件、信号处理组件和人机接口；

所述传感器组件，设置在车辆周身，用于采集所述车辆本身位姿信息及所述车辆周边的环境信息；

所述信号处理组件，根据不同所述传感器组件提供的所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的泊车路径规划；

所述人机接口，用于根据输入的所述曲率连续的所述泊车路径规划，显示于所述车载终端的显示屏，和/或输出给所述车辆的运动机构，用于执行所述曲率连续的所述泊车路径规划。

本发明还提供了一种确定曲率连续的泊车路径的方法，包括以下步骤：

采集所述车辆本身位姿信息及所述车辆周边的环境信息；

根据所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的泊车路径规划。

进一步所述的确定曲率连续的泊车路径的方法，根据所述车辆周边的环境信息，从中确定所述车辆当前的位姿信息(位置和朝向，即车辆定位信息和角度信息)、空泊车位的位置和角度以及所述车辆周边的环境信息、所述空泊车位周边的环境信息。

进一步所述的确定曲率连续的泊车路径的方法，根据所述车辆当前的所述位姿信息、空泊车位的位置和角度以及所述车辆周边的环境信息、所述空泊车位周边的环境信息，设定所述车辆周边无碰撞范围和上述空泊车位周边无碰撞范围；

在所述空泊车位与所述车辆之间生成多级路径探测节点；

在所述多级路径探测节点的空间里遍历，找出与所述车辆的位姿或位姿延长线上的曲线连接，并探寻出至少一条(多条)无碰撞可达路径。

进一步所述的确定曲率连续的泊车路径的方法，从所述车辆的位姿至所述空泊车位，对所述无碰撞可达路径进行验证，最后采用数值最优化(惩罚函数)的方式生成一条光滑的几何路径，确定为所述泊车路径规划。

进一步所述的确定曲率连续的泊车路径的方法，

确定在所述空泊车位的长方形短边中心连线上所述空泊车位后向三等分点所在的位置为0级节点；

由所述0级节点所在的所述长方形短边中心连线上(一字车位：后向三等分点至车位顶部)或其前向的延伸线上(垂直车位：车位顶部以外7米左右长度)生成多个直线型节点为1级节点；

由在任一所述1级节点的前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上和范围内(路径曲线C变化范围0至±0.2)生成多个曲线型节点，并标记为2级节点；

由在任一所述2级节点的前向左侧和后向左侧，或者，前向右侧和后向右侧的二个切线方向上和范围内生成多个曲线型节点为，并标记为3级节点；

在n-1级节点的基础上生成n级所述曲线型节点；

从而在所述空泊车位与所述车辆当前位姿之间，生成多级路径探测节点阵列。

2级节点(含)以上节点皆为曲线型节点。

进一步所述的确定曲率连续的泊车路径的方法，根据之前节点在前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上对应第一、第二、第三和第四象限内生成的欧拉螺旋曲线：C＝R*L；路径曲率C变化设定于0至±0.2之间的，

其中，C为螺旋线的参数,L为该曲线的弧长,R是曲线的曲率半径(那么在曲线的起点处有:L＝0，R＝∞终点处:L＝ls，R＝R_min,R_min是车辆的最小转弯半径.)，R的最小值为所述车辆的最小转弯半径。

进一步所述的确定曲率连续的泊车路径的方法，当选取设定长度的弧长的螺旋线，得到所述欧拉螺旋曲线的路径方程表达式:

dL＝R*dβ

dx＝dL*cosβ

dy＝dL*sinβ

其中，β为所述欧拉螺旋曲线上任意点处的切点与x轴的夹角.该所述欧拉螺旋曲线上的离散点通过菲涅儿积分求出。

进一步所述的确定曲率连续的泊车路径的方法，

当检测到所述曲率连续的泊车路径规划中有任意节点被障碍物占据，需重新确定所述曲率连续的泊车路径规划。

本发明及实施例解决技术问题的有益效果/进步效果如下任一：

1.本算法基于当前泊车路径规划生成了曲率跳变问题，提出了基于螺旋曲线的空间搜有的规划方法，能有效解决这个难题，保证了生成的无碰撞路径能够满足车身动力学约束，同时保证了曲率是连续光滑的；

2.曲率连续的路径有利于车辆跟踪时，减小离心力的变化，增加了车辆的舒适性和稳定性，曲率无突变，无需停车转向。水平车位泊车路径通常是由两段圆弧设计而组成的,也就是存在曲率突变，当车辆来不及降低速度在跟踪这一类路径时,车辆的行驶轨迹会偏离规划的路径,会触发重规划,造成泊车把数过多,体验差而且易导致自动泊车失败；

3.设计的算法同时也规避了搜索泊车路径出现不可避免的微小段问题,我们将30cm内出现停车换挡的短距路径称为微小段,这对乘车体验,线控要求都很不友好,在实车控制上会造成很大的跟踪误差,本算法中能保证垂直最短换挡距离在80cm以上,水平车位在50cm以上；

4.生成的路径无需再二次优化,减少计算复杂度；

5.尽量发挥轮胎的混动摩擦，减少和避免滑动摩擦，降低轮胎的损耗。

在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

为更进一步阐述本申请所采取的技术手段及取得的效果，下面结合附图及较佳实施例，对本申请的技术方案，进行清楚和完整的描述。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细说明：

图1为本发明第一实施例流程实现图；

图2为本发明第一实施例探测点曲线生成原理图；

图3为本发明第一实施例多级节点生成示意图；

图4为本发明第二实施例探测点生成示意图；

图5为本发明第二实施例搜寻探测点(路径)示意图；

图6为本发明第三实施例探测点生成示意图；

图7为本发明第三实施例搜寻探测点(路径)示意图；

图8为本发明第二实施例探寻区域示意图；

图9为本发明第三实施例探寻区域示意图；

图10为本发明第四实施例探寻区域示意图。

附图标记说明

0-根节点的位置，700-车辆(箭头朝向为车头方向)，800-车位外检测与车辆前向检测大概率共同探索相关路径的区域。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，以下说明和附图对于本发明是示例性的，并且不应被理解为限制本发明。以下说明描述了众多具体细节以方便对本发明理解。然而，在某些实例中，熟知的或常规的细节并未说明，以满足说明书简洁的要求。本发明的原理图请参考图1所示，本发明的具体判断系统及方法参见下述实施例：

第一实施例

在现有技术中，设计一条曲率连续的路径通常采用的办法是首先应用A-star生成一条从起点到终点的无碰撞路径，然后基于这段路径，采用数值最优化的方式生成一条光滑的几何路径，类似的方法有B样条路径，高阶光滑曲线路径，贝塞尔路径等。基于这些数值方法求解得到的泊车路径都是曲率连续的，但是有两点不能做到保障，第一，即使曲率连续，但是不一定能保证最小曲率半径能满足车身动力学特性；第二，求解后的路径不保证是无碰撞的。这两点对实际泊车工程应用是非常重要的，需要提出更加完善的算法进行改进。

1关于螺旋线设计：

如图2所示,由原点(0.0，0.00)生成的四个方向的螺旋线路径,若当前车身后轴中心位于原点(0.0，0.00),车头方向的yaw值为0,最小曲率半径为5m.则图中第一和第四象限路径曲率变化分别是0→-0.2和0→+0.2,第二和第三象限分别表示车后退时相应的路径(曲率变化分别在第二象限是0→-0.2,第三象限是0→+0.2).

这里,生成的欧拉螺旋线具有以下性质:

C＝R*L

其中c为螺旋线的参数,L为该曲线的弧长,R是曲线的曲率半径.那么在曲线的起点处有:L＝0，R＝∞终点处:L＝ls，R＝R_min,R_min是车辆的最小转弯半径.

当选取一定长度的弧长的螺旋线,那么,得到曲线的路径方程表达式:

dL＝R*dβ

dx＝dL*cosβ

dy＝dL*sinβ

其中β为曲线上任意点处的切点与x轴的夹角.该曲线上的离散点可以通过菲涅儿积分求出.

图2中第一象限以一汽车辆参数为参考,当规划速度的为2.5km/h(R档行驶,满足实际应用),那么设计的螺旋线弧长为80cm,曲率变化为0→-0.2(正负值只与左右方向相关,与曲线形状无关).

2搜索空间的构成

1)搜索流程:

①将泊车规定停车的终点(如图8所示)记为根节点，或0级节点.

②生成一条曲率半径从无穷大到车辆最小转弯半径的螺旋线,该条螺旋线的上的点和长度需要满足车辆最小转弯半径R的约束,方向盘最大转速约束,以及泊车速度在3km/h(可调整)附近等约束条件.后续称该螺旋线为欧拉螺旋线.

③从根节点，也就是0级节点开始,并且在该车位平行方向的直线生成多个一级节点，或1级节点；基于多个一级节点,依次在该节点上生成前进和后退,左侧和右侧的欧拉螺旋线，若能生成完整的螺旋线长度的无碰撞路径，在在螺旋线的末节点处方向延伸若干个圆弧型离散的节点,上述圆弧类型和欧拉螺旋线离散点统称为二级节点，或2级节点；基于二级节点中的圆弧类离散点,这些节点曲率半径都是车辆的最小转弯半径,在前进/后退,左侧/右侧方向上,将欧拉螺旋线实现翻转,拼接在上述二级节点上,生成若干个离散的三级节点，或3级节点,依此类推,生成n级节点；

④在整个节点生成的空间中搜索,将车辆的当前位置和遍历搜索空间的节点进行匹配,判断车辆的可行性,生成一系列的候选路径,按照惩罚函数的计算法则,选取最优的泊车路径.

2)搜索法则:

一般情形下的说明:如图3中黑色节点0代表根节点,也就是泊车终点后轴中心所在的位置(同图8中的节点0位置),由根节点延伸直线型子节点,如图中的十字星节点,为一级节点；

由一级节点生成二级节点:如节点1位置,首先生成前进,后退,左侧,右侧的(至少4条)螺旋线路径(如节点1位置附近的五角星标记的曲线),曲率由0→±0.2(正负值只与左/右方向相关),该曲线存在两种情况:若整段螺旋线不存在碰撞关系,那么在螺旋线的终点处的切线方向,继续以最小转弯半径延伸90度圆弧,生成若干个无碰撞弧状离散点,连接五角星标记节点的黑色线段即为弧状路径,该段路径曲率的值恒为0.2；若螺旋线上的节点在延伸中存在碰撞,即不再生成圆弧状子节点.图中所有五角星标记的节点,为二级节点；

由二级节点生成三级节点:仅选取圆弧状二级节点,如节点2位置,将参考的螺旋线实现翻转,生成与本节点切线方向前进和后退方向的螺旋线路径,如连接节点2的五角星标记曲线,曲率变化是-0.2→0,若该段螺旋线不存在碰撞关系,在连接节点2五角星标记曲线终点处,继续生成相对称方向的螺旋线路径,曲率变化为0→0.2,如图中的含圆圈的五角星标记段曲线,同理,若该绿色段螺旋线不存在碰撞关系,继续以最小转弯半径延伸90度圆弧,生成若干个弧状无碰撞离散点,不再赘述.图中的所有含圆圈的五角星标记节点,为三级节点，也称3级节点；

按照上述节点生成法则,直至节点达到一定数量或者无法再生成满足条件的下一级节点,在整个节点生成的空间中搜索,遍历生成的节点空间集,若空间中某一节点和车辆当前位置相连的路径是可达的,那么从该节点可以依次回溯到根节点,生成一系列候选路径,给出惩罚函数选取最优泊车路径。

实施例二

在实施例一基础上，如图4、5、8所示，车头朝向为前向，车尾朝向为后向。图8中0为根节点的位置(图8中矩形短边中心连线的某个三等分点位置，不强求精确的三等分点位置，附近位置也可，一般为停泊后车辆的后轴中心点位置)，800为车位外检测与车辆前向检测大概率共同探索相关路径的区域。一字泊车位的前向(图8中矩形短边中心连线的箭头指示方向)，设定为与现停滞车辆(预泊车的车辆700的箭头朝向为车头方向)前朝向，相同的方向。

车辆的传感器组件采集到周边环境信息后，车辆的信号处理组件确定本车和空泊车位的相关空间位姿，接着初始化根节点(0节点)，根据预设规定生成多组长度不同，曲率连续变化的螺旋路径。具体是由0节点生成直线型1级节点，对于一字停车位，1级节点的生成起点是0节点，其前向延伸至图8中矩形中的箭头位置，然后，依次由每一个1级节点按设定生成2级节点，由于车辆的传感器组件可以确定一字车位的车位线，从而确定车位的入口处，所以只在入口外侧方向生成探索方向的曲线型2级节点(在车位非出入口方向生成曲线节点没有意义)，继续在2级节点上生成3级节点，依次类推。图8中800位置是大部分的曲线型节点生成的位置之一。

车辆的传感器组件采集，也能确定本车的位姿，从本车的后轴中心点出发，在800位置的区域(图中800区域非是对区域大小的限定)内查询无碰撞前进路径。即在生成的节点空间里遍历，查找出能从车身起始位置到该节点的无碰撞可达路径。最后回溯，产生完整的无碰撞泊车路径。

根据设置，该无碰撞的泊车路径，可以显示在车载终端上，也可以直接传输给车辆的执行机构，实施自主泊车。

实施例三

在实施例一基础上，如图6、7、9所示，预泊车的车辆的车头朝向为前向(图9中700车辆的箭头朝向为车头方向)，车尾朝向为后向。垂直泊车位的前向(图9中矩形短边中心连线的箭头指示方向)，设定为倒车停车入位后，停滞后车辆前朝向的方向。

图9中0为根节点的位置(图9中矩形短边中心连线的某个三等分点位置，不强求精确的三等分点位置，附近位置也可，一般为停泊后车辆的后轴中心点位置)，800为车位外检测与车辆前向检测大概率共同探索相关路径的区域。

车辆的传感器组件采集到周边环境信息后，车辆的信号处理组件确定本车和空泊车位的相关空间位姿，接着初始化根节点(0节点)，根据预设规定生成多组长度不同，曲率连续变化的螺旋路径。具体是由0节点生成直线型1级节点，对于垂直停车位，1级节点的生成起点是0节点，其前向延伸至图9中矩形中的箭头位置之外，之外的延伸长度可以设定；然后，依次由每一个1级节点按设定生成2级节点，由于车辆的传感器组件可以确定垂直车位的车位线和周边停泊车辆等，从而确定车位的入口处，所以只在入口外侧方向生成探索方向的曲线型2级节点(在车位非出入口方向生成曲线节点没有意义)，继续在2级节点上生成3级节点，依次类推。图9中800位置是大部分的曲线型节点生成的位置之一。

车辆的传感器组件采集，也能确定本车的位姿，从本车的后轴中心点出发，在800位置的区域(图中800区域非是对区域大小的限定)内查询无碰撞前进路径。即在生成的节点空间里遍历，查找出能从车身起始位置到该节点的无碰撞可达路径。最后回溯，产生完整的无碰撞泊车路径。

根据设置，该无碰撞的泊车路径，可以显示在车载终端上，也可以直接传输给车辆的执行机构，实施自主泊车。

实施例四

在实施例一基础上，如图10所示，预泊车的车辆的车头朝向为前向(图10中700车辆箭头朝向为车头方向)，车尾朝向为后向。斜列泊车位的前向(图10中内切矩形短边中心连线的箭头指示方向)，指倒车停车入位后，停滞后车辆前朝向的方向

图10中0为根节点的位置(图10中矩形短边中心连线的某个三等分点位置，不强求精确的三等分点位置，附近位置也可，一般为停泊后车辆的后轴中心点位置)，800为车位外检测与车辆前向检测大概率共同探索相关路径的区域。

车辆的传感器组件采集到周边环境信息后，车辆的信号处理组件确定本车和空泊车位的相关空间位姿，接着初始化根节点(0节点)，根据预设规定生成多组长度不同，曲率连续变化的螺旋路径。具体是由0节点生成直线型1级节点，对于斜列停车位，1级节点的生成起点是0节点，其前向延伸至图10中矩形中的箭头位置之外，之外的延伸长度可以设定；然后，依次由每一个1级节点按设定生成2级节点，由于车辆的传感器组件可以确定斜列车位的车位线和周边停泊车辆等，从而确定车位的入口处，所以只在入口外侧方向生成探索方向的曲线型2级节点(在车位非出入口方向生成曲线节点没有意义)，继续在2级节点上生成3级节点，依次类推。图10中800位置是大部分的曲线型节点生成的位置之一。

车辆的传感器组件采集，也能确定本车的位姿，从本车的后轴中心点出发，在800位置的区域(图中800区域非是对区域大小的限定)内查询无碰撞前进路径。即在生成的节点空间里遍历，查找出能从车身起始位置到该节点的无碰撞可达路径。最后回溯，产生完整的无碰撞泊车路径。

根据设置，该无碰撞的泊车路径，可以显示在车载终端上，也可以直接传输给车辆的执行机构，实施自主泊车。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (20)

1.一种智能泊车系统的曲率连续泊车路径规划装置，其特征在于，包括：传感器组件和信号处理组件；

所述传感器组件，设置在车辆周身，用于采集所述车辆当前的位姿信息及所述车辆周边的环境信息；

所述信号处理组件，根据不同所述传感器组件提供的所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的泊车路径规划。

2.如权利要求1所述的曲率连续泊车路径规划装置，其特征在于，所述传感器组件至少包括定位传感器、多路高清摄像头和/或多路无线电定位探测器；

所述定位传感器，用于确定所述车辆的位姿信息；

所述高清摄像头，用于确定所述环境信息中的视觉信息；

所述无线电定位探测器，用于确定所述环境信息中的点云信息。

3.如权利要求1所述的曲率连续泊车路径规划装置，其特征在于，所述信号处理组件包括：泊车位定位模块和泊车路径生成模块；

所述泊车位定位模块，用以确定所述车辆当前的位姿信息、空泊车位的位置和角度以及所述车辆周边的环境信息、所述空泊车位周边的环境信息；

所述泊车路径生成模块，用以由所述空泊车位的位置和角度至所述车辆的当前所述位姿信息生成曲率连续的所述泊车路径规划。

4.如权利要求3所述的曲率连续泊车路径规划装置，其特征在于，所述泊车路径生成模块包括：设置子模块、节点生成子模块和节点遍历子模块；

所述设置子模块，用以设定所述车辆前向周边无碰撞范围和所述空泊车位前向周边无碰撞范围；

所述节点生成子模块，用以在所述空泊车位与所述车辆之间生成多级路径探测节点；

所述节点遍历子模块，用以在所述多级路径探测节点的空间里遍历，找出与所述车辆的位姿或位姿延长线上的曲线连接，并探寻出至少一条无碰撞可达路径。

5.如权利要求4所述的曲率连续泊车路径规划装置，其特征在于，所述泊车路径生成模块还包括：回溯子模块；

所述回溯子模块，用以从所述车辆的位姿至所述空泊车位，对所述无碰撞可达路径进行验证，最后采用惩罚函数优化的方式生成一条光滑的几何路径，确定为所述泊车路径规划。

6.如权利要求4所述的曲率连续泊车路径规划装置，其特征在于，所述节点生成子模块进一步包括：0级节点生成单元、1级节点生成单元、2级节点生成单元、3级节点生成单元、n级节点生成单元；

0级节点生成单元，用于确定在所述空泊车位内切的长方形短边中心连线上所述空泊车位后向三等分点所在的位置，标记为0级节点；

1级节点生成单元，用于由所述0级节点所在的所述长方形短边中心连线上或其前向的延伸线上生成多个直线型节点，标记为1级节点；

2级节点生成单元，用于由在任一所述1级节点的前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上和范围内生成多个曲线型节点，标记为2级节点；

3级节点生成单元，用于由在任一所述2级节点的前向左侧和后向左侧，或者，前向右侧和后向右侧的二个切线方向上和范围内生成多个所述曲线型节点，标记为3级节点；

n级节点生成单元，用于在n-1级节点的基础上生成n级所述曲线型节点。

7.如权利要求6所述的曲率连续泊车路径规划装置，其特征在于，所述曲线型节点，至少根据之前节点在前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上对应第一、第二、第三和第四象限内生成的欧拉螺旋曲线：

C＝R*L；

其中，C为螺旋线的参数，路径曲率C变化设定于0至±0.2之间的，L为该曲线的弧长，R是曲线的曲率半径，R的最小值为所述车辆的最小转弯半径。

8.如权利要求7所述的曲率连续泊车路径规划装置，其特征在于，当选取设定长度的弧长的螺旋线，得到所述欧拉螺旋曲线的路径方程表达式：

dL＝R*dβ

dx＝dL*cosβ

dy＝dL*sinβ

其中，β为所述欧拉螺旋曲线上任意点处的切点与x轴的夹角.该所述欧拉螺旋曲线上的离散点通过菲涅儿积分求出。

9.如权利要求1所述的曲率连续泊车路径规划装置，其特征在于，所述曲率连续的泊车路径规划至少由B样条路径、高阶光滑曲线路径、贝塞尔路径或螺旋线路径中任一构成。

10.如权利要求1或9所述的曲率连续泊车路径规划装置，其特征在于，当所述传感器组件检测到所述曲率连续的所述泊车路径规划中有任意节点被障碍物占据，所述信号处理组件重新确定所述曲率连续的所述泊车路径规划。

11.如权利要求10所述的曲率连续泊车路径规划装置，其特征在于，所述空泊车位包括：平行泊车位、垂直泊车位或倾斜泊车位。

12.一种智能泊车系统的曲率连续泊车路径车载终端，其特征在于，至少包括：传感器组件、信号处理组件和人机接口；

所述传感器组件，设置在车辆周身，用于采集所述车辆本身位姿信息及所述车辆周边的环境信息；

所述信号处理组件，根据不同所述传感器组件提供的所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的泊车路径规划；

所述人机接口，用于根据输入的所述曲率连续的所述泊车路径规划，显示于所述车载终端的显示屏，和/或输出给所述车辆的运动机构，用于执行所述曲率连续的所述泊车路径规划。

13.一种确定曲率连续的泊车路径的方法，其特征在于，包括以下步骤：

采集车辆本身位姿信息及所述车辆周边的环境信息；

根据所述车辆周边的环境信息，从中确定空泊车位，并确定所述空泊车位与所述车辆的当前位姿间的曲率连续的所述泊车路径规划。

14.如权利要求13所述的确定曲率连续的泊车路径的方法，其特征在于，根据所述车辆周边的环境信息，从中确定所述车辆当前的位姿信息、空泊车位的位置和角度以及所述车辆周边的环境信息、所述空泊车位周边的环境信息。

15.如权利要求14所述的确定曲率连续的泊车路径的方法，其特征在于，根据所述车辆当前的所述位姿信息、空泊车位的位置和角度以及所述车辆周边的环境信息、所述空泊车位周边的环境信息，设定所述车辆周边无碰撞范围和上述空泊车位周边无碰撞范围；

在所述空泊车位与所述车辆之间生成多级路径探测节点；

在所述多级路径探测节点的空间里遍历，找出与所述车辆的位姿或位姿延长线上的曲线连接，并探寻出至少一条无碰撞可达路径。

16.如权利要求15所述的确定曲率连续的泊车路径的方法，其特征在于，从所述车辆的位姿至所述空泊车位，对所述无碰撞可达路径进行验证，最后采用惩罚函数优化的方式生成一条光滑的几何路径，确定为所述泊车路径规划。

17.如权利要求15所述的确定曲率连续的泊车路径的方法，其特征在于，

确定在所述空泊车位的长方形短边中心连线上所述空泊车位后向三等分点所在的位置为0级节点；

由所述0级节点所在的所述长方形短边中心连线上或其前向的延伸线上生成多个直线型节点为1级节点；

由在任一所述1级节点的前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上和范围内生成多个曲线型节点，并标记为2级节点；

由在任一所述2级节点的前向左侧和后向左侧，或者，前向右侧和后向右侧的二个切线方向上和范围内生成多个曲线型节点为，并标记为3级节点；

在n-1级节点的基础上生成n级所述曲线型节点。

18.如权利要求17所述的确定曲率连续的泊车路径的方法，其特征在于，根据之前节点在前向左侧、前向右侧、后向左侧、后向右侧的四个切线方向上对应第一、第二、第三和第四象限内生成的欧拉螺旋曲线：

C＝R*L；

其中，C为螺旋线的参数，路径曲率C变化设定于0至±0.2之间的，

L为该曲线的弧长，R是曲线的曲率半径，R的最小值为所述车辆的最小转弯半径。

19.如权利要求18所述的确定曲率连续的泊车路径的方法，其特征在于，当选取设定长度的弧长的螺旋线，得到所述欧拉螺旋曲线的路径方程表达式：

dL＝R*dβ

dx＝dL*cosβ

dy＝dL*sinβ

其中，β为所述欧拉螺旋曲线上任意点处的切点与x轴的夹角.该所述欧拉螺旋曲线上的离散点通过菲涅儿积分求出。

20.如权利要求13所述的确定曲率连续的泊车路径的方法，其特征在于，

当检测到所述曲率连续的所述泊车路径规划中有任意节点被障碍物占据，需重新确定所述曲率连续的所述泊车路径规划。

Images