[发明专利]一种非完整约束条件下的连续曲率路径优化方法

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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