一种基于准均匀B样条曲线的泊车路径规划方法与流程

本发明涉及一种泊车路径规划方法，特别是涉及一种基于准均匀b样条曲线的泊车路径规划方法。

背景技术：

自动泊车是智能辅助驾驶系统的重要组成部分，主要是以提升车辆舒适性和安全性为主要目的。在自动泊车过程中，泊车路径规划是其中的核心工作之一，其目标在于从车辆起始位置到目标停车位之间规划出一条无碰撞，满足车辆运动学约束条件的车辆行驶轨迹。传统的圆弧直线式泊车轨迹虽然能够满足车辆运动学约束条件，但由其产生的泊车轨迹存在曲率不连续等问题，导致车辆在泊车过程中存在原地转向等情况，造成轮胎磨损等问题的出现，因此需要寻求更加合理的泊车轨迹生成方法。

b样条曲线由贝塞尔样条曲线拓展而成，是通过逼近由控制点决定的特征多边形而获得的曲线，在继承了后者的优点基础上，还具有精度更高，控制点局部修改特性，曲线阶数可控等优点。控制点坐标与所生成的样条曲线之间的具体数学关系包括三种：均匀b样条曲线，准均匀b样条曲线，和非均匀b样条曲线。然而现有的b样条曲线泊车路径规划方法中，如论文《基于滑模控制的自动泊车系统路径跟踪研究》和《基于滑模控制的自动泊车系统路径跟踪研究》都使用均匀b样条曲线来构建轨迹，均匀b样条曲线在使用过程中存在一个问题，就是生成的曲线的两端与控制点不接触。

此外另一个亟待解决的问题是当前优化问题的约束条件及求解参数数量仍然较多，优化求解计算速度慢。

技术实现要素：

本发明的目的正是针对上述泊车路径规划问题，提出了一种能够同时满足车辆几何碰撞约束、方向角平滑、曲率连续的使用准均匀b样条曲线泊车路径规划方法，实现平行泊车任务的单次路径规划。

首先根据泊车场景的几何尺寸适当的选择控制点的数量；然后基于泊车场景的相关几何参数给出控制点的初始坐标，并将其中要优化的控制点坐标作为优化问题的控制量；根据车辆停泊过程中的几何关系建立约束条件，以车辆曲率最大值尽量小作为目标进行优化求解。为了显性的建立车辆起始位置和目标泊车位置与样条曲线起始位置和终止位置之间的联系，本发明采用准均匀b样条曲线，准均匀b样条曲线的实际控制点在两个端点有n个重复度，n为样条曲线的阶数，则可以保证生成的曲线会经过两端的控制点，即保证生成的轨迹曲线的起始位置和终止位置与对应的控制点序列的起始点和终止点坐标是重合的，从而生成的轨迹曲线对应的车辆姿态在起始点和终止点附近均能够满足泊车要求，这样能够通过改变控制点的位置来改变准b样条曲线的端点位置，从而直接决定轨迹曲线起始和终止位置及状态。而均匀b样条曲线不满足这样的条件。

本发明充分利用准均匀b样条曲线的几何特性来构建泊车轨迹，通过合理针对所建立的优化问题进行控制点对应控制量的简化，从而减小控制量的数量，有效减少泊车轨迹规划过程中所建立的优化求解问题的约束函数的数量，提高优化求解过程的计算速度。

附图说明

图1是泊车场景及坐标系示意图

图2是左前端碰撞示意图

图3是右侧碰撞示意图

图4是右前方与泊车区域碰撞示意图

图5是右后方与泊车区域碰撞示意图

具体实施方式

1.根据泊车参数建立泊车场景坐标系，泊车参数包括停车位纵向距离、停车位横向距离、道路宽度、车辆长度、车辆宽度和轴距。需要用的参数及示例取值如下表所示。

表1泊车参数

根据上述泊车参数，可以建立如图1所示的泊车场景。本示例中，以车辆右后方所在的停车位端点作为原点，以车辆前进方向作为x轴正方向，以车辆左侧作为y轴正方向。所建立的泊车场景如图1所示。

2.基于上述参考系，以车辆后轴中心处作为参考点，可以定义出泊车起始位置和泊车终止位置。在示例中，相关取值如表2所示。

表2泊车起终点坐标示例

3.由于准均匀b样条曲线的构建是通过一些离散的控制点生成的，当控制点的坐标确定后，b样条曲线的样子就唯一确定；通过改变控制点的坐标位置，就可以改变b样条曲线在平面上的样子。这样，我们就通过优化b样条曲线控制点的坐标位置，来确保b样条曲线所对应的泊车路径能够符合我们的各种要求。

根据泊车起始位置和泊车终止位置的坐标，按照公式(1)计算出待优化控制点的初值。公式(1)实现优化控制点的分布符合特点：两端的控制点比较密集，中间的控制点比较稀疏。

控制点的数量则是根据经验来选取，本示例中，控制点数量选择为9，其初值如下表所示。

表3泊车控制点坐标初值

其中，xstart和ystart表示泊车起始点的坐标，xend和yend表示泊车终止点的坐标；n表示控制点的数量,pix表示第i个控制点的x坐标值，piy表示第i个控制点的y坐标；

4.根据泊车任务的要求，可以建立如下的优化问题：

其中，f0为目标函数，具体定义为控制点生成的泊车路径轨迹曲线的最大曲率，该轨迹曲线通过n个控制点生成；第n个控制点坐标pn＝(pnx,pny)。上述目标函数的自变量为控制点的坐标。

目标函数的优化目标是：整个泊车过程中车辆参考点移动行程的轨迹曲线的曲率最大值最小，因为曲率是变化的，不同位置不一样，我们关心的是曲率最大值有没有超过车辆物理结构的限制，也就是以整条曲线的曲率最大值作为目标函数，希望泊车轨迹曲线的最大曲率尽量小。

目标函数确保了车辆能够符合转向的几何约束条件，即：车辆能够以不小于最小转弯半径的转向方式实现泊车过程。由于车辆最小转弯半径(即曲率半径)和曲率之间是倒数关系，因而该条件就相当于车辆泊车过程中要通过的路径对应的最大曲率不大于车辆最小转弯半径对应的曲率。在这一条件下，车辆能够实现转弯，否则，车辆不能实现转弯。在满足这个条件的基础上，我们还希望泊车路径对应的最大曲率不要太大，这样对应的曲率半径也就不会太小，这样使得车辆能够容易实现转弯，也为车辆轨迹跟踪控制等环节减轻了控制精度要求较高的负担。

f1至fm为约束函数，具体定义为车辆在泊车过程中存在的各类几何碰撞约束条件，共m个；上述约束函数的自变量为控制点的坐标。

约束函数的定义确保了车辆在泊车过程中能够满足几何碰撞约束条件。为保证生成的轨迹曲线的曲率连续变化，使得车辆在跟踪轨迹曲线过程中方向盘转角能够连续变化，b样条曲线的种类定义为4次准均匀样条曲线，优选地也可以使用更高次数以满足更高的设计要求。

5.控制量简化

由于存在n个控制点，因此该优化问题对应的控制量为k＝2n个，为了简化优化问题的求解难度，我们将部分控制点的坐标进行固定，以减少n+2个优化控制量的数量。

表4控制点坐标固定方式

每个控制点有x和y坐标，因此n个控制点具有2n个控制量，被固定的控制点的坐标包括：第1,2,3个控制点的y坐标，第n-2,n-1,和n控制点的y坐标，第4,5,6至n-3控制点的x坐标，以及第1和n这两个控制的x坐标。共计n+2个，所以通过固定部分控制点，使得原优化问题的控制量被减小至k＝n-2个。

6.约束条件定义

基于前述控制量简化方式，只需定义如下4个约束条件即可保证车辆在泊车过程中不产生运动干涉碰撞。

(1)为避免车辆左前端超过道路宽度对应的范围，如图2所示，应建立如下约束条件：

y1≤lw+lk,x∈[0,xstart](3)

其中，y1表示车身左前端纵坐标，lw表示道路宽度，lk表示车位宽度，x为车辆参考点的坐标。

(2)为避免车辆右侧与泊车区域前方端点发生碰撞，如图3所示，应建立如下约束条件：

tan(θ)(lc-x3)+y3≥lk,x2≥lc∩x3≤lc(4)

其中，lc表示车位长度，x3,y3表示车身右后点坐标，θ表示车辆在泊车轨迹上的方向角，x2右前端横坐标；

(3)为避免车辆右前端与泊车区域前方发生碰撞，如图4所示，应建立如下约束条件：

x2≤lc,y2≤lk(5)

x2，y2表示车身右前端坐标；

(4)为避免车辆右后端与泊车区域侧方发生碰撞，如图5所示，应建立如下约束条件

0≤miny3,x∈[0,x0](6)

x0表示车辆参考点横坐标的最大值，该值不小于泊车起始位置车辆参考点的横坐标值。

因此，泊车任务对应的优化问题就可以被具体定义为如下形式的目标函数，表示生成的轨迹曲线的曲率的最大值，我们希望目标函数最小，也就是最大曲率能够尽量小：

k个控制量(k＝n-2)与坐标点之间的关系为：

控制量的优化取值范围取值范围为：

四个约束条件分别为：

车辆的方向角由控制点确定的轨迹曲线给出。轨迹曲线是一条准均匀b样条曲线，其次数确定为4次及以上，通过控制点所生成的准均匀b样条曲线的导数来计算所生成的轨迹各处的方向角及曲率。车身四周坐标mp按照如下公式给出。

其中，(x1,y1)(x2,y2)(x3,y3)(x4,y4)分别表示车辆左前方、右前方、右后方和左后方端点的平面坐标，m1,m2和m3分别为如下格式

其中，lk为车宽，lf为前悬，lr为后悬，l为轴距。