一种自动泊车路径规划方法及系统、泊车控制设备与流程

技术特征：
1.一种自动泊车路径规划方法，其特征在于，包括：步骤s1、周期性地获取当前车辆状态、车辆周围空闲车位在世界坐标系中的车位坐标信息；所述车辆状态包括车辆在世界坐标系中的车辆坐标信息及姿态；步骤s2、基于当前车辆状态采用多层级树状搜索方式进行泊车路径搜索，判定每一树节点所对应的车辆状态是否满足预设泊入条件，并根据判定结果获得与车位中位线带中任一条线相切的若干泊车路径；其中所述预设泊入条件为在树节点所对应的车辆状态下是否存在一条泊车路径与空闲车位的车位中位线带中任一条线相切；步骤s3、根据预设筛选规则对所述若干泊车路径进行筛选得到最优路径。2.根据权利要求1所述的自动泊车路径规划方法，其特征在于，所述步骤s2具体包括：基于当前车辆状态进行第一层搜索，第一层包括n个第一层树节点，每一第一层树节点表示车辆执行一种预设行车动作后的车辆状态，并判定每一第一层树节点所对应的车辆状态是否满足预设泊入条件；若某一第一层树节点所对应的车辆状态满足预设泊入条件，则根据判定结果获得该第一层树节点所对应的与车位中位线带中任一条线相切的泊车路径；若某一第一层树节点所对应的车辆状态不满足预设泊入条件，则基于该第一层树节点所对应的车辆状态进行第二层搜索，并判定每一第二层树节点所对应的车辆状态是否满足预设泊入条件，若某一第二层树节点所对应的车辆状态满足预设泊入条件，则根据判定结果获得该第二层树节点所对应的与车位中位线带中任一条线相切的泊车路径；其中：该第一层树节点对应n个第二层树节点，每一第二层树节点表示车辆执行一种预设行车动作后的车辆状态。3.根据权利要求2所述的自动泊车路径规划方法，其特征在于，所述步骤s2具体还包括：若第二层所有树节点所对应的车辆状态均不满足预设泊入条件，则基于第二层所有树节点所对应的车辆状态进行第三层搜索，并判定每一第三层树节点所对应的车辆状态是否满足预设泊入条件，若某一第三层树节点所对应的车辆状态满足预设泊入条件，则根据判定结果获得该第三层树节点所对应的与车位中位线带中任一条线相切的泊车路径；其中：该第二层树节点对应n个第三层树节点，每一第三层树节点表示车辆执行一种预设行车动作后的车辆状态。4.根据权利要求3所述的自动泊车路径规划方法，其特征在于，所述方法还包括：步骤s4、根据所述最优路径以及最优路径所对应的第一层树节点，或第一层树节点与第二层树节点，或第一层树节点、第二层树节点与第三层树节点所对应的预设行车动作生成当前周期的行车指令，并将该行车指令发送至车辆行车执行机构。5.根据权利要求4所述的自动泊车路径规划方法，其特征在于，所述步骤s4具体还包括：若上一周期的最优路径包括多段路径，并且，生成当前周期的最优路径时，车辆行车执行机构未执行完上一周期的最优路径所对应的行车指令，则根据当前周期的最优路径对应的树节点所对应的预设行车动作、以及上一周期未执行完的一段路径对应的树节点所对应的预设行车动作生成当前周期的行车指令，并将该行车指令发送至车辆行车执行机构。6.根据权利要求1所述的自动泊车路径规划方法，其特征在于，所述步骤s3具体包括：
获取可行驶区域在世界坐标系中的坐标信息；根据所述可行驶区域在世界坐标系中的坐标信息，判定所述若干泊车路径是否满足预设可行驶条件；其中，所述预设可行驶条件为车辆按泊车路径行驶时，车身的多个预设位置点均不超出可行驶区域；若存在至少一个泊车路径满足预设可行驶条件，则根据第一预设评分规则对满足预设可行驶条件的泊车路径进行评分，并将得分最高的泊车路径输出为最优路径；若不存在至少一个泊车路径满足预设可行驶条件，则根据第二预设评分规则对所有泊车路径进行评分，并将得分最高的泊车路径输出为最优路径。7.根据权利要求6所述的自动泊车路径规划方法，其特征在于，获取车辆周围空闲车位在世界坐标系中的车位坐标信息，具体包括：获取多路车载鱼眼摄像头的环境图像，对多路环境图像进行图像识别得到车辆周围空闲车位在摄像头坐标系中的车位坐标信息，并根据所述车辆周围空闲车位在摄像头坐标系中的车位坐标信息获得车辆周围空闲车位在世界坐标系中的车位坐标信息。8.根据权利要求6所述的自动泊车路径规划方法，其特征在于，判定所述若干泊车路径是否满足预设可行驶条件，具体包括：计算车身的i1个第一预设位置点与所述可行驶区域的边界的距离；若车身的i1个第一预设位置点与所述可行驶区域的边界的距离大于等于第一距离，则判定所述若干泊车路径是否满足预设可行驶条件；若车身的i1个第一预设位置点与所述可行驶区域的边界的距离小于第一距离，则计算i2个第二预设位置点与所述可行驶区域的边界的距离，并根据与所述i2个第二预设位置点与所述可行驶区域的边界的距离判定所述若干泊车路径是否满足预设可行驶条件；若车身的i1个第一预设位置点与所述可行驶区域的边界的距离小于第二距离，则计算i2个第二预设位置点、i2个第三预设位置点与所述可行驶区域的边界的距离，并根据与所述i2个第二预设位置点、i3个第三预设位置点与所述可行驶区域的边界的距离判定所述若干泊车路径是否满足预设可行驶条件；其中，第一距离＞第二距离，i1，i2，i3均为＞0的偶数。9.根据权利要求6所述的自动泊车路径规划方法，其特征在于，获取可行驶区域在世界坐标系中的坐标信息具体包括：根据所述多路车载鱼眼摄像头的环境图像获得车辆周围环境的全景图像，并对所述全景图像进行图像语义分割得到可行驶区域在全景图像坐标系中的坐标信息，并根据所述可行驶区域在全景图像坐标系中的坐标信息获得可行驶区域在世界坐标系中的坐标信息。10.根据权利要求6所述的自动泊车路径规划方法，其特征在于，所述第一预设评分规则如下表达式所示：v＝l
×
w
a
d
×
w
b
w
×
w
c
n
×
w
d
所述第二预设评分规则如下表达式所示：v＝l
×
w
a
d
×
w
b
w
×
w
c
n
×
w
d
d
l
×
w
e
d
a
×
w
f
其中，v表示路径得分，l表示路径长度，d表示路径多段线段的折返次数，w表示路径多段线段的方向盘转角之和，n表示车辆执行路径车身离可行驶区域边沿最近的距离，d
l
表示执行路径后车辆与车位的距离，d
a
表示执行路径后车辆与车位角度差，w
a
、w
b
、w
c
、w
d
、w
e
、w
f
分
别为预先设定的权重值。11.一种自动泊车路径规划系统，其特征在于，包括：信息获取单元，用于周期性地获取当前车辆状态、车辆周围空闲车位在世界坐标系中的车位坐标信息；所述车辆状态包括车辆在世界坐标系中的车辆坐标信息及姿态；路径搜索单元，用于基于当前车辆状态采用多层级树状搜索方式进行泊车路径搜索，判定每一树节点所对应的车辆状态是否满足预设泊入条件，并根据判定结果获得与车位中位线带中任一条线相切的若干泊车路径；其中所述预设泊入条件为在树节点所对应的车辆状态下是否存在一条泊车路径与空闲车位的车位中位线带中任一条线相切；以及路径筛选单元，用于根据预设筛选规则对所述若干泊车路径进行筛选得到最优路径。12.根据权利要求11所述的自动泊车路径规划系统，其特征在于，所述路径搜索单元具体包括：第一搜索单元，用于基于当前车辆状态进行第一层搜索，第一层包括n个第一层树节点，每一第一层树节点表示车辆执行一种预设行车动作后的车辆状态，并判定每一第一层树节点所对应的车辆状态是否满足预设泊入条件；第一路径获取单元，用于当某一第一层树节点所对应的车辆状态满足预设泊入条件，则根据判定结果获得该第一层树节点所对应的与车位中位线带中任一条线相切的泊车路径；第二搜索单元，用于当某一第一层树节点所对应的车辆状态不满足预设泊入条件，则基于该第一层树节点所对应的车辆状态进行第二层搜索，并判定每一第二层树节点所对应的车辆状态是否满足预设泊入条件，该第一层树节点对应n个第二层树节点，每一第二层树节点表示车辆执行一种预设行车动作后的车辆状态；以及第二路径获取单元，用于当某一第二层树节点所对应的车辆状态满足预设泊入条件，则根据判定结果获得该第二层树节点所对应的与车位中位线带中任一条线相切的泊车路径。13.根据权利要求12所述的自动泊车路径规划系统，其特征在于，所述路径搜索单元具体包括：第三路径获取单元，用于当第二层所有树节点所对应的车辆状态均不满足预设泊入条件时，基于第二层所有树节点所对应的车辆状态进行第三层搜索，并判定每一第三层树节点所对应的车辆状态是否满足预设泊入条件，若某一第三层树节点所对应的车辆状态满足预设泊入条件，则根据判定结果获得该第三层树节点所对应的与车位中位线带中任一条线相切的泊车路径；其中：该第二层树节点对应n个第三层树节点，每一第三层树节点表示车辆执行一种预设行车动作后的车辆状态。14.根据权利要求13所述的自动泊车路径规划系统，其特征在于，所述系统还包括：行车控制单元，用于根据所述最优路径以及最优路径所对应的第一层树节点，或第一层树节点与第二层树节点，或第一层树节点、第二层树节点与第三层树节点所对应的预设行车动作生成当前周期的行车指令，并将该行车指令发送至车辆行车执行机构。15.根据权利要求14所述的自动泊车路径规划系统，其特征在于，所述行车控制单元具体还用于：当上一周期的最优路径包括多段路径，并且，生成当前周期的最优路径时，车辆行车执
行机构未执行完上一周期的最优路径所对应的行车指令，则根据当前周期的最优路径对应的树节点所对应的预设行车动作、以及上一周期未执行完的一段路径对应的树节点所对应的预设行车动作生成当前周期的行车指令，并将该行车指令发送至车辆行车执行机构。16.根据权利要求11所述的自动泊车路径规划系统，其特征在于，所述路径筛选单元具体包括：行驶区域获取单元，用于获取可行驶区域在世界坐标系中的坐标信息；行驶判定单元，用于根据所述可行驶区域在世界坐标系中的坐标信息，判定所述若干泊车路径是否满足预设可行驶条件；其中，所述预设可行驶条件为车辆按泊车路径行驶时，车身的多个预设位置点均不超出可行驶区域；第一路径优化单元，用于当存在至少一个泊车路径满足预设可行驶条件时，根据第一预设评分规则对满足预设可行驶条件的泊车路径进行评分，并将得分最高的泊车路径输出为最优路径；以及第二路径优化单元，用于当不存在至少一个泊车路径满足预设可行驶条件时，根据第二预设评分规则对所有泊车路径进行评分，并将得分最高的泊车路径输出为最优路径。17.根据权利要求16所述的自动泊车路径规划系统，其特征在于，所述信息获取单元具体包括：车辆信息获取单元，用于获取当前车辆状态；以及车位信息获取单元，用于获取多路车载鱼眼摄像头的环境图像，对多路环境图像进行图像识别得到车辆周围空闲车位在摄像头坐标系中的车位坐标信息，并根据所述车辆周围空闲车位在摄像头坐标系中的车位坐标信息获得车辆周围空闲车位在世界坐标系中的车位坐标信息。18.根据权利要求16所述的自动泊车路径规划方法，其特征在于，所述行驶判定单元具体包括：距离计算单元，用于根据所述可行驶区域在世界坐标系中的坐标信息计算车身的i1个第一预设位置点与所述可行驶区域的边界的距离；第一判定单元，用于当车身的i1个第一预设位置点与所述可行驶区域的边界的距离大于等于第一距离时，判定所述若干泊车路径是否满足预设可行驶条件；第二判定单元，用于当车身的i1个第一预设位置点与所述可行驶区域的边界的距离小于第一距离时，计算i2个第二预设位置点与所述可行驶区域的边界的距离，并根据与所述i2个第二预设位置点与所述可行驶区域的边界的距离判定所述若干泊车路径是否满足预设可行驶条件；以及第三判定单元，用于当车身的i1个第一预设位置点与所述可行驶区域的边界的距离小于第二距离时，计算i2个第二预设位置点、i2个第三预设位置点与所述可行驶区域的边界的距离，并根据与所述i2个第二预设位置点、i3个第三预设位置点与所述可行驶区域的边界的距离判定所述若干泊车路径是否满足预设可行驶条件；其中，第一距离＞第二距离，i1，i2，i3均为＞0的偶数。19.根据权利要求16所述的自动泊车路径规划系统，其特征在于，所述行驶区域获取单元具体用于：根据所述多路车载鱼眼摄像头的环境图像获得车辆周围环境的全景图像，并对所述全
景图像进行图像语义分割得到可行驶区域在全景图像坐标系中的坐标信息，并根据所述可行驶区域在全景图像坐标系中的坐标信息获得可行驶区域在世界坐标系中的坐标信息。20.根据权利要求16所述的自动泊车路径规划系统，其特征在于，所述第一预设评分规则如下表达式所示：v＝l
×
w
a
d
×
w
b
w
×
w
c
n
×
w
d
所述第二预设评分规则如下表达式所示：v＝l
×
w
a
d
×
w
b
w
×
w
c
n
×
w
d
d
l
×
w
e
d
a
×
w
f
其中，v表示路径得分，l表示路径长度，d表示路径多段线段的折返次数，w表示路径多段线段的方向盘转角之和，n表示车辆执行路径车身离可行驶区域边沿最近的距离，d
l
表示执行路径后车辆与车位的距离，d
a
表示执行路径后车辆与车位角度差，w
a
、w
b
、w
c
、w
d
、w
e
、w
f
分别为预先设定的权重值。21.一种泊车控制设备，其特征在于，包括：根据权利要求11-20任一项所述的自动泊车路径规划系统；或者，存储器和处理器，所述存储器中存储有计算机可读指令，所述计算机可读指令被所述处理器执行时，使得所述处理器执行根据权利要求1-10中任一项所述自动泊车路径规划方法。

技术总结
本发明涉及一种自动泊车路径规划方法及其系统、泊车控制设备，所述方法包括：周期性地获取当前车辆状态、车辆周围空闲车位在世界坐标系中的车位坐标信息；所述车辆状态包括车辆在世界坐标系中的车辆坐标信息及姿态；基于当前车辆状态采用多层级树状搜索方式进行泊车路径搜索，判定每一树节点所对应的车辆状态是否满足预设泊入条件，并根据判定结果获得与车位中位线带中任一条线相切的若干泊车路径；其中所述预设泊入条件为在树节点所对应的车辆状态下是否存在一条泊车路径与空闲车位的车位中位线带中任一条线相切；根据预设筛选规则对所述若干泊车路径进行筛选得到最优路径。实施本发明，能够实现动态路径规划，提高自动泊车路径规划的灵活性、鲁棒性和适应力。鲁棒性和适应力。鲁棒性和适应力。


技术研发人员：黄辉 翁茂楠 王玉龙 李智 陈泽武 苏威霖
受保护的技术使用者：广州汽车集团股份有限公司
技术研发日：2020.04.28
技术公布日：2021/10/28