CN112269965B - 一种非完整约束条件下的连续曲率路径优化方法 - Google Patents

Abstract

本发明提供一种非完整约束条件下的连续曲率路径优化方法，该方法构造出来的平滑曲线满足非完整约束条件、曲率连续、存在解析解便于实时计算，有助于提高路径跟踪控制的精度。具体步骤包括：依据车辆转向能力约束确定可行驶的最大曲率；从原始路径上起始点开始按序选取三个路径点；构造两段参数化的三阶Bezier曲线，确定参数值以满足曲率连续和最大值受限双重约束条件；依据参数化方程计算平滑曲线上各点的曲率；对平滑曲线进行非均匀离散化，获取平滑后的目标路径点；重复上述步骤完成对全局路径点的平滑处理；检查平滑曲线与原路径点的拟合匹配情况，并与障碍地图进行碰撞检测。由此，最终得到的优化路径兼顾了无人车辆行驶控制的可通过性和平顺性。

Description

一种非完整约束条件下的连续曲率路径优化方法

技术领域

本发明属于自动驾驶或无人驾驶车辆路径规划与自动控制技术领域，具体涉及一种非完整约束条件下的连续曲率路径优化计算方法。

背景技术

对于无人驾驶车辆而言，目标路径如果按直线连接会给路径跟踪控制带来两个问题，其一，路径点构成的线段存在曲率跳变增大了控制量以及控制量的变化程度，系统能耗和冲击都会增大；其二，车辆行驶不是任意的路线都可以通过的，需要满足非完整约束条件，即最大曲率或最小转向半径受限，如果不满足该条件，会加大系统的跟踪误差，甚至出现车辆失稳，影响行驶安全。

针对目标路径点的平滑与优化处理方法，目前已有研究提出采用回旋曲线Clothoids或B-Spline样条曲线来构造，然而回旋曲线方程拟合缺乏闭环解析解，B样条曲线参数化函数过于复杂，曲率连续和曲率最大值约束难以同时满足。一些研究构造出来的平滑曲线也往往忽略或简化了车辆的行驶通过能力，缺乏后续的可行解检查和碰撞检测，无法确保车辆行驶的安全和平顺性。

发明内容

有鉴于此，本发明提出一种非完整约束条件下的连续曲率路径优化方法，该方法构造出来的平滑曲线满足非完整约束条件、曲率连续、存在解析解便于实时计算、能够实现可行解检查以及碰撞检测，采用该方法对无人驾驶车辆进行路径优化，提高了车辆行驶的安全性、平顺性，也有助于提高路径跟踪控制的精度，降低极限弯道下出现的车辆失稳的可能。

所述的非完整约束条件下的连续曲率路径优化方法，具体步骤包括：

步骤一：依据车辆行驶能力约束计算车辆可行驶路径的最大曲率，记为κ_max；

步骤二：令给定的期望路径包括n个离散路径点，依次为P₁,P₂…P_n，n≥3；在给定的期望路径上，从起始点开始选取三个路径点；

步骤三：在选取的路径点的连线上选取控制点位置以构造两条几何对称的Bezier曲线段；并计算两条几何对称的Bezier曲线段的特征长度，确定满足两条Bezier曲线连接点处曲率连续以及非完整约束条件时的曲线参数化方程；所述非完整约束条件指最大曲率值为κ_max；

步骤四：对步骤三构造的Bezier曲线段进行采样，得到一系列离散点作为目标路径点；

步骤五：在给定的期望路径上，按序依次选取三个路径点，对每次选取的三个路径点重复上述步骤三至步骤四，直到给定的期望路径中的所有离散路径点计算结束，得到平滑曲线；每次选取时去掉当前三个路径点中的第一个路径点，然后增加下一个路径点；

步骤六：检查步骤五得到的平滑曲线与期望路径中离散路径点的拟合匹配情况，当平滑曲线中出现往回折返的错误结果时，调整给定的期望路径中的路径点，然后返回至步骤一重新进行路径优化。

作为本发明的一种优选方式，还包括：

步骤七；对平滑曲线与已知的障碍地图进行障碍物的碰撞检测，若出现平滑曲线与障碍物碰撞的情况，则调整给定的期望路径中的路径点，然后返回至步骤一重新进行路径优化。

作为本发明的一种优选方式，所述步骤一中：

车辆可行驶路径的最大曲率κ_max的计算公式为：

式中：L为车辆轴距或等效轴距，ψ为转向轮的最大转角，k_R为考虑了多轴分布式驱动车辆矢量扭矩控制下的复合转向修正系数，k_R∈[1，2]。

作为本发明的一种优选方式，所述步骤三中：

令两条几何对称的三阶Bezier曲线段中，令为构造第1条三阶Bezier曲线段所选取的控制点分别为B₀，B₁，B₂和B₃，为构造第2条三阶Bezier曲线段所选取的控制点分别为E₀，E₁，E₂和E₃；其中控制点B₃和E₃重合于两条曲线的连接处，控制点B₀，B₁和B₂共线位于线段|P₁P₂|上，控制点E₀，E₁和E₂共线位于线段|P₂P₃|上，控制点的选取几何参数满足下述条件：

d₁＝d₂＝d，β＝γ/2

式中：d₁为控制点B₀与取路径点P₂之间的连线长度，d₂为线段|P₂P₃|的长度；d为设计参数，β为线段|P₁P₂|与线段|B₂B₃|之间的夹角，γ为线段|P₁P₂|与线段|P₂P₃|之间的夹角。

作为本发明的一种优选方式，所述步骤三中：

满足两条Bezier曲线段连接点处曲率连续以及非完整约束条件时的曲线方程的特征长度为：

g_b＝g_e＝c₂c₃d

h_b＝h_e＝c₃d

k_b＝k_e＝[(6c₃cosβ)/(c₂+4)]d

式中：g_b＝|B₀B₁|，h_b＝|B₁B₂|，k_b＝|B₂B₃|，g_e＝|E₀E₁|，h_e＝|E₁E₂|，k_e＝|E₂E₃|；c₁、c₂和c₃为特征长度的计算参数，c₁＝7.2364，c₃＝(c₂+4)/(c₁+6)；

满足最大曲率约束条件下的设计参数d为：

式中：c₄＝(c₂+4)²/54c₃；

由上述特征长度构成的两段三次Bezier曲线参数化方程为：

作为本发明的一种优选方式，所述步骤四中：

对对步骤三构造的Bezier曲线段进行非均匀采样，采样步长依据平滑偏距来确定，具体方法为：

首先计算平滑偏距d_e：d_e＝|P₂B₃|＝|B₂B₃|tanβ＝k_btanβ；

令离散化采样步长为dt，步长dt的确定公式为：

将所确定的采样步长代入Bezier曲线参数化方程，即可获得一系列离散点坐标。

作为本发明的一种优选方式，所述步骤六中：

首先定义可行解标志位rb_flag取值为0或1，rb_flag＝0表示不存在往回折返的情况，rb_flag＝1表示存在往回折返；

令初始给的期望路径的离散路径点中，前三个路径点P₁，P₂，P₃，记为casel，中间路径点P_k，P_k+1，P_k+2，1＜k＜n-2，记为case2，最后三个路径点P_n-2，P_n-1，P_n，记为case3，可行解标志位rb_flag的判断准则为：

式中特征长度d_pre是指当前三个路径点的前面一组路径点构成的Bezier曲线特征长度；

当rb_flag＝1时，表明平滑后的曲线中出现往回折返的错误结果。

作为本发明的一种优选方式，所述步骤七中：

障碍物碰撞检测的具体步骤如下：

记录下平滑后曲线中所有Bezier曲线段顶点与原有路径点的首尾2个点，记为P₁，V₁，V₂，V₃，…V_k，P_n，其中P₁和P_n为原有路径点的首尾2个点，V₁，V₂，V₃，…V_k为所有Bezier曲线段的顶点；所述障碍物包括圆形障碍物和矩形障碍物；令当前进行障碍物碰撞检测的线段为目标线段；

对于圆形障碍物：

将目标线段与圆形障碍物之间的位置关系判断分解为圆形障碍物圆心处的点与目标线段之间的位置关系判断，令圆形障碍物圆心处的点为点P，目标线段为AB；

点P到目标线段AB的最短距离d_pc为：

式中：当0＜r＜1时，点P到目标线段AB的投影点点C的坐标为：C＝(1-r)A+rB；

设圆形障碍物的半径为R，则碰撞检测的结果表述为：

对于矩形障碍物：

将目标线段与矩形障碍物之间的位置关系判断分解为目标线段与矩形障碍物对角线之间的位置关系判断，令目标线段为线段a，矩形障碍物的对角线为线段b，线段a的两个端点为a1，a2，线段b的两个端点为b1，b2，定义：

C1＝v×v1，C2＝v×v2，C3＝t×t1，C4＝t×t2

其中：v为端点a1到端点a2的向量，v1为端点a1到端点b1的向量，v2为端点a1到端点b2的向量；t为端点b1到端点b2的向量，t1为端点b1到端点a1的向量，t2为端点b1到端点a2的向量；则：

if C1*C2＜0&&C3*C4＜0，线段a，b相交，else不相交。

如果目标线段与矩形障碍物的两个对角线都不相交，则该目标线段为无碰撞线段。有益效果：

(1)采用该方法对无人驾驶车辆进行路径优化，不仅满足车辆行驶的非完整约束条件，其道路的曲率变化连续且存在曲率值的解析解(即能够计算得到优化后的曲线各处的曲率值)，能够为高精度路径跟踪控制提供精确的前馈道路信息。

(2)本发明提供的路径优化方法无需迭代求取数值解，计算量小；同时依据相关准则可实现非均匀采样离散化以节省有限的计算资源，便于工程实现，无需额外硬件成本，具有很高的实用价值，能够广泛应用于各类无人驾驶车辆路径规划与跟踪控制中。

(3)该路径优化处理方法能够实现可行解检查以及碰撞检测，能够提高无人车辆行驶的通过性、平顺性与安全性，降低极限弯道下出现的车辆失稳的可能。

附图说明

图1为本发明的路径优化计算流程图；

图2为两条几何对称的Bezier曲线段的构造及特征长度示意；

图3为原有路径点的首尾两个点和Bezier曲线段的顶点构成的线段与障碍物之间的相对位置关系；

图4为圆形障碍物碰撞检测时点与线段的位置关系及最短距离计算；

图5为矩形障碍物碰撞检测时线段与线段之间位置关系判断；

图6为路径点的优化结果；

图7为路径点调整最大曲率时的对比结果。

具体实施方式

下面结合附图和较佳实施例对本发明进行详细阐述，以使本发明的优点和特征能更容易被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。

本实施例提供一种非完整约束条件下的连续曲率路径优化方法，采用该方法进行路径优化能够提高车辆行驶的安全性、平顺性，也有助于提高路径跟踪控制的精度，降低极限弯道下出现的车辆失稳的可能。

下面以表1所示的初始离散路径点(期望路径)为例，对该路径优化方法进行详细说明。

表1.初始路径点

其中，No.表示路径点序号，n为路径点的个数，n大于等于3；Global_x1和Global_y1分别表示各离散路径点的横坐标和纵坐标，Property表示路径点的属性，其中0表示行驶起点，2表示行驶途经点，1表示行驶终点，3表示交叉路口点。

该路径优化方法的具体步骤如图1所示：

步骤一：首先依据车辆行驶能力约束通过下述公式(1)计算车辆可行驶路径的最大曲率κ_max：

式中：L为车辆轴距或等效轴距，ψ为车辆转向轮的最大转角，k_R为考虑了多轴分布式驱动车辆矢量扭矩控制下的复合转向修正系数，可依据试验数据而定，一般k_R∈[1，2]。

本例中，以多轴分布式驱动车辆为例，其等效轴距L为0.6m，复合转向修正系数k_R取1.44，由此计算得到车辆的最大曲率约为1，即最小转向半径约为1m。

步骤二：令初始离散路径点从起始点到行驶终点依次为：P₁，P₂，P₃，P₄，…P_n-2，P_n-1，P_n；从路径点的起始点开始按顺序每次选取三个路径点，然后构造两条几何对称的Bezier曲线段B(t)和E(t)。

第一次选取的三个路径点为P₁，P₂和P₃，选取控制点位置以构造两条几何对称的Bezier曲线段B(t)和E(t)，如图2所示；令两条几何对称的三阶Bezier曲线段B(t)和E(t)中，三阶Bezier曲线段B(t)的控制点分别为B₀，B₁，B₂和B₃，三阶Bezier曲线段E(t)的控制点分别为E₀，E₁，E₂和E₃；其中控制点B₃和E₃重合于两条曲线段的连接处，控制点B₀，B₁和B₂共线位于线段|P₁P₂|上，控制点E₀，E₁和E₂共线位于线段|P₂P₃|上，控制点的选取几何参数满足下述条件：

d₁＝d₂＝d，β＝γ/2

式中：d₁为控制点B₀与路径点P₂之间的连线长度，d₂为线段|P₂P₃|的长度；d为设计参数，d₁和d₂的长度选取为相等，这个相等的数记为d；β为线段|P₁P₂|与线段|B₂B₃|之间的夹角，γ为线段|P₁P₂|与线段|P₂P₃|之间的夹角。

几何对称的Bezier曲线的特征长度计算公式如下：

式中：两条Bezier曲线的特征长度包括：g_b、g_e、h_b、h_e、k_b和k_e；其中g_b＝|B₀B₁|(线段B₀B₁的长度)，h_b＝|B₁B₂|(线段B₁B₂的长度)，k_b＝|B₂B₃|(线段B₂B₃的长度)，g_e＝|E₀E₁|(线段E₀E₁的长度)，h_e＝|E₁E₂|(线段E₁E₂的长度)，k_e＝|E₂E₃|(线段E₂E₃的长度)，c₁、c₂和c₃为特征长度的计算参数，为设定值，依据该计算参数得到的Bezier曲线的特征长度，能够满足两条Bezier曲线段连接点处曲率连续。

满足上述几何特征长度关系的Bezier曲线段的曲率是连续的且单调变化的，曲率在B₀到B₃段单调递增，曲率在E₃到E₀段单调递减，在B₃和E₃的连接处曲率达到最大值，控制点B₃处的曲率计算公式为：

式中：c₄＝(c₂+4)²/54c₃；

为了使得构造的Bezier曲线满足非完整约束条件，需选取相应的设计参数d；满足最大曲率约束条件下的设计参数d_kmax使控制点B₃处的曲率为步骤一所计算的车辆可行驶路径的最大曲率κ_max，即：

则满足最大曲率约束条件下的设计参数d_kmax为：

由于设计参数为满足最大曲率约束条件下的设计参数，由此能够使确定的Bezier曲线的参数化方程满足非完整约束条件，即最大曲率值为κ_max。

在上述特征长度确定后，由上述特征长度构成的两段三阶Bezier曲线参数化方程如下：

由上述Bezier曲线参数化方程可求取Bezier曲线段各处的曲率；以三阶Bezier曲线段B(t)为例，其曲率的计算方程如下：

参数t∈[0，1] (5)

式中：φ(t)为参数化方程B(t)的横坐标，ω(t)为参数化方程B(t)的纵坐标，k(t)为关于参数t的曲率。上述参数化曲率计算公式的最大曲率在t＝1处，满足如下关系：

k_max＝k(t)，t＝1

步骤三：为节约机载设备的计算资源，对平滑后的曲线(即步骤二计算得到的Bezier曲线段)进行非均匀采样获取目标路径点(步骤二中确定了Bezier曲线的参数化方程，是个连续的方程，实际应用时需要进行离散点采样，构成一系列离散点作为最后平滑的目标路径点)，如图3所示；

本例中采用非均匀采样的方式获取目标路径点，采样步长依据平滑偏距来确定，具体方法为：

(301)定义平滑偏距d_e为路径点P₂和平滑后的曲线顶点B₃之间的欧拉距离，即d_e＝|P₂B₃|＝|B₂B₃|tanβ＝k_btanβ；

(302)从B₀到E₀这段曲线的离散化采样步长记为dt，用来线性划分两段三次Bezier曲线参数化方程的参数t∈[0，1]，即t＝0：dt：1；代入参数化方程即可获取离散点坐标，步长dt的确定按如下计算公式：

将依据平滑偏距确定的步长dt代入上述两段三次Bezier曲线参数化方程(即公式(4))，即可获取一个以上离散点；

然后针对第二次选取的三个路径点P₂，P₃和P₄重复以上计算过程(包括步骤二中的构造两条几何对称的Bezier曲线段、计算每条Bezier曲线段的曲率以及步骤三中的获取每条Bezier曲线段的离散点)；依次对每次选取的三个路径点重复以上计算过程，直到初始离散路径点中最后一组路径点计算结束；由此完成初始离散路径点中所有路径点的优化计算，然后记录下新的路径点P₁，V₁，V₂，V₃，…V_i，…，P_n以及所计算的每条Bezier曲线段的曲率值；其中V_i表示获取的第i个离散点。

步骤四：针对全局路径点(即初始离散路径点)P₁，P₂，P₃，P₄，…P_n-2，P_n-1，P_n，检查平滑后的曲线与原路径点的拟合匹配情况，调整路径点坐标以消除往回折返的错误结果，确保平滑处理的结果是可行解，具体为：

首先定义标志位rb_flag取值为0或1，rb_flag＝0表示不存在往回折返的情况，rb_flag＝1表示存在往回折返；

令初始离散路径点中，前三个路径点P₁，P₂，P₃，记为case1，中间路径点P_k，P_k+1，P_k+2，1＜k＜n-2，记为case2，最后三个路径点P_n-2，P_n-1，P_n，记为case3，可行解标志位rb_flag的判断准则为：

上式中特征长度d_pre是指当前三个路径点的前面一组路径点构成的Bezier曲线特征长度，例如特征长度d对应当前三个路径点为P_k，P_k+1，P_k+2，特征长度d_pre对应的三个路径点为P_k-1，P_k，P_k+1。

当rb_flag＝1时，表明以最大曲率k_max进行全局路径点平滑时出现了平滑后路径点连线往回折返的情况，此时需要调整初始路径点坐标使其满足可通行要求，调整初始路径点坐标后返回至步骤一重新进行路径优化。

步骤五：针对上述路径优化处理后的结果(即经上述步骤四后的处理结果)进行碰撞检测：

初始给的离散路径点是经过上层路径规划下发的或提前采集的任务点，均为无碰撞路径点，经过前述优化处理后的路径与原有路径偏离最大的点均为Bezier曲线段曲率最大的顶点，即B₃或E₃点；记录下平滑后曲线所有的Bezier曲线段顶点以及原有路径点的首尾2个点，记为P₁，V₁，V₂，V₃，…V_k，P_n，其中P₁和P_n为初始离散路径点的首尾两个点，V₁，V₂，V₃，…V_k为所有Bezier曲线段的顶点，鉴于大部分障碍物均可等效为圆形或矩形障碍物，本例中考虑线段与平滑处理后的路径上已知的圆形或矩形障碍物进行碰撞检测：

对于圆形障碍物：对于线段与圆形障碍物之间的位置关系判断可以分解为圆心处的点(点P)与线段AB(待检测的目标线段)的位置关系判断，考虑图4所示的三种情况，其中(a)为圆心处的点(点P)到线段的投影点(点C)位于该线段上，(b)为圆心处的点到线段的投影点(点C)位于该线段的右边延长线上，(c)为圆心处的点到线段的投影点(点C)位于该线段的左边延长线上。

依据向量内积可以求取点P到线段AB的最短距离d_pc：

定义：/>

当0＜r＜1时，投影点C的坐标为：C＝(1-r)A+rB。

设圆形障碍物的半径为R，基于上述点到线段的最短距离，碰撞检测的结果可以表述为：

对于矩形障碍物：基于前后线段的连续性，如果线段与矩形障碍物之间出现碰撞，那么一定会有线段与矩形的两条对角线相交的情况，因此线段与矩形障碍物之间的位置关系判断可以分解为待检测的目标线段与矩形障碍物对角线之间位置关系的判断；如图5所示，设有两个线段，分别为线段a和线段b，线段a的两个端点为a1，a2，线段b的两个端点为b1，b2，利用向量叉乘，定义：

C1＝v×v1，C2＝v×v2，C3＝t×t1，C4＝t×t2

其中：v为端点a1到端点a2的向量，v1为端点a1到端点b1的向量，v2为端点a1到端点b2的向量；t为端点b1到端点b2的向量，t1为端点b1到端点a1的向量，t2为端点b1到端点a2的向量。

如果数值C1和C2异号，表明b1，b2在线段a的两侧，同理，如果数值C3和C4异号，表明a1，a2在线段b的两侧，则有：

if C1*C2＜0&&C3*C4＜0，线段a，b相交，else不相交

基于此，如果目标线段与矩形障碍物的两条对角线都不存在相交，那么该线段记为无碰撞线段。

重复上述过程，完成对P₁，V₁，V₂，V₃，…V_k，P_n这些点构成的连续线段和障碍物之间的检测，即可完成整体的碰撞检测。

在进行碰撞检测时，若出现有与障碍物碰撞的线段，则需要调整初始路径点坐标使其满足可通行要求，调整初始路径点坐标后返回至步骤一重新进行路径优化。

通过上述计算可得到优化后的结果如图6所示，左侧的图是优化前后的路径点对比，右侧的图是路径累加弧长和对应的曲率值，可以看到曲率满足了最大值为1的约束，并且曲率是连续变化的，不存在跳变现象，优化结果不存在往回折返的情况，离散化步长dt＝0.5，与障碍占据网格进行碰撞检测结果显示无碰撞。

为了进一步说明计算过程，将全局最大可通过曲率设置为0.3，即κ_max＝0.3，经过上述步骤计算优化后的结果如图7的左侧所示，出现了往回折返的错误结果，说明原有路径点在最大曲率等于0.3时无法通过，因此需要调整曲率，右侧的图是κ_max＝1.0时的优化结果，左右两图的离散化步长dt＝0.2。

综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种非完整约束条件下的连续曲率路径优化方法，其特征在于，包括如下步骤：

步骤一：依据车辆行驶能力约束计算车辆可行驶路径的最大曲率，记为κ_max；

步骤二：令给定的期望路径包括n个离散路径点，依次为P₁，P₂…P_n，n≥3；在给定的期望路径上，从起始点开始选取三个路径点；

步骤三：在选取的路径点的连线上选取控制点位置以构造两条几何对称的Bezier曲线段；并计算两条几何对称的Bezier曲线段的特征长度，确定满足两条Bezier曲线连接点处曲率连续以及非完整约束条件时的曲线参数化方程；所述非完整约束条件指最大曲率值为κ_max；

步骤四：对步骤三构造的Bezier曲线段进行采样，得到一系列离散点作为目标路径点；

步骤五：在给定的期望路径上，按序依次选取三个路径点，对每次选取的三个路径点重复上述步骤三至步骤四，直到给定的期望路径中的所有离散路径点计算结束，得到平滑曲线；每次选取时去掉当前三个路径点中的第一个路径点，然后增加下一个路径点；

步骤六：检查步骤五得到的平滑曲线与期望路径中离散路径点的拟合匹配情况，当平滑曲线中出现往回折返的错误结果时，调整给定的期望路径中的路径点，然后返回至步骤一重新进行路径优化；

步骤七；对平滑曲线与已知的障碍地图进行障碍物的碰撞检测，若出现平滑曲线与障碍物碰撞的情况，则调整给定的期望路径中的路径点，然后返回至步骤一重新进行路径优化；

所述步骤一中：

车辆可行驶路径的最大曲率κ_max的计算公式为：

式中：L为车辆轴距或等效轴距，ψ为转向轮的最大转角，k_R为考虑了多轴分布式驱动车辆矢量扭矩控制下的复合转向修正系数，k_R∈[1,2]；

所述步骤三中：

令两条几何对称的三阶Bezier曲线段中，令为构造第1条三阶Bezier曲线段所选取的控制点分别为B₀，B₁，B₂和B₃，为构造第2条三阶Bezier曲线段所选取的控制点分别为E₀，E₁，E₂和E₃；其中控制点B₃和E₃重合于两条曲线的连接处，控制点B₀，B₁和B₂共线位于线段|P₁P₂|上，控制点E₀，E₁和E₂共线位于线段|P₂P₃|上，控制点的选取几何参数满足下述条件：

d₁＝d₂＝d，β＝γ/2

式中：d₁为控制点B₀与取路径点P₂之间的连线长度，d₂为线段|P₂P₃|的长度；d为设计参数，β为线段|P₁P₂|与线段|B₂B₃|之间的夹角，γ为线段|P₁P₂|与线段|P₂P₃|之间的夹角；

所述步骤三中：

满足两条Bezier曲线段连接点处曲率连续以及非完整约束条件时的曲线方程的特征长度为：

g_b＝g_e＝c₂c₃d

h_b＝h_e＝c₃d

k_b＝k_e＝[(6c₃ cosβ)/(c₂+4)]d

式中：g_b＝|B₀B₁|，h_b＝|S₁S₂|，k_b＝|B₂B₃|，g_e＝|E₀E₁|，h_e＝|E₁E₂|，k_e＝|E₂E₃|；c₁、c₂和c₃为特征长度的计算参数，c₁＝7.2364，c₃＝(c₂+4)/(c₁+6)；

满足最大曲率约束条件下的设计参数d为：

式中：c₄＝(c₂+4)²/54c₃；

由上述特征长度构成的两段三次Bezier曲线参数化方程为：

所述步骤四中：

对步骤三构造的Bezier曲线段进行非均匀采样，采样步长依据平滑偏距来确定，具体方法为：

首先计算平滑偏距d_e：d_e＝|P₂B₃|＝|B₂B₃|tanβ＝k_btanβ；

令离散化采样步长为dt，步长dt的确定公式为：

将所确定的采样步长代入Bezier曲线参数化方程，即可获得一系列离散点坐标。

2.根据权利要求1所述的非完整约束条件下连续曲率路径优化方法，其特征在于，所述步骤六中：

首先定义可行解标志位rb_flag取值为0或1，rb_flag＝0表示不存在往回折返的情况，rb_flag＝1表示存在往回折返；

令初始给的期望路径的离散路径点中，前三个路径点P₁，P₂，P₃，记为case1，中间路径点P_k，P_k+1，P_k+2，1＜k＜n-2，记为case2，最后三个路径点P_n-2，P_n-1，P_n，记为case3，可行解标志位rb_flag的判断准则为：

式中特征长度d_pre是指当前三个路径点的前面一组路径点构成的Bezier曲线特征长度；

当rb_flag＝1时，表明平滑后的曲线中出现往回折返的错误结果。

3.根据权利要求1所述的非完整约束条件下连续曲率路径优化方法，其特征在于，所述步骤七中：

障碍物碰撞检测的具体步骤如下：

记录下平滑后曲线中所有Bezier曲线段顶点与原有路径点的首尾2个点，记为P₁，V₁，V₂，V₃，…V_k，P_n，其中P₁和P_n为原有路径点的首尾2个点，V₁，V₂，V₃，…V_k为所有Bezier曲线段的顶点；所述障碍物包括圆形障碍物和矩形障碍物；令当前进行障碍物碰撞检测的线段为目标线段；

对于圆形障碍物：

将目标线段与圆形障碍物之间的位置关系判断分解为圆形障碍物圆心处的点与目标线段之间的位置关系判断，令圆形障碍物圆心处的点为点P，目标线段为AB；

点P到目标线段AB的最短距离d_pc为：

式中：当0＜r＜1时，点P到目标线段AB的投影点点C的坐标为：C＝(1-r)A+rB；

设圆形障碍物的半径为R，则碰撞检测的结果表述为：

对于矩形障碍物：

将目标线段与矩形障碍物之间的位置关系判断分解为目标线段与矩形障碍物对角线之间的位置关系判断，令目标线段为线段a，矩形障碍物的对角线为线段b，线段a的两个端点为a1，a2，线段b的两个端点为b1，b2，定义：

C1＝v×v1，C2＝v×v2，C3＝t×t1，C4＝t×t2

其中：v为端点a1到端点a2的向量，v1为端点a1到端点b1的向量，v2为端点a1到端点b2的向量；t为端点b1到端点b2的向量，t1为端点b1到端点a1的向量，t2为端点b1到端点a2的向量；则：

if C1*C2＜0&&C3*C4＜0，线段a，b相交，else不相交；

如果目标线段与矩形障碍物的两个对角线都不相交，则该目标线段为无碰撞线段。