[发明专利]一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统

权利要求书

1.一种基于五阶贝塞尔曲线的机器人路径平滑方法，其特征在于，包括如下步骤：

S1获取栅格地图信息以及连接起始点和目标点的折线路径；

S2根据折线路径设定多段五阶贝塞尔曲线，每段五阶贝塞尔曲线为五次多项式，以每段五阶贝塞尔曲线的六个控制点作为优化变量，多段五阶贝塞尔曲线的平滑性作为优化目标，建立线性等式约束条件和线性不等式约束条件；

S3采用二次规划算法进行求解，获得连接起始点和目标点的最优平滑路径。

2.根据权利要求1所述的一种基于五阶贝塞尔曲线的机器人路径平滑方法，其特征在于，所述栅格地图信息包括距离图和地图原点坐标。

3.根据权利要求2所述的一种基于五阶贝塞尔曲线的机器人路径平滑方法，其特征在于，所述距离图包含每个栅格与障碍物的距离信息。

4.根据权利要求1所述的一种基于五阶贝塞尔曲线的机器人路径平滑方法，其特征在于，所述接收连接起始点和目标点的折线路径后，还包括如下步骤：

S12判断折线路径是否为新生成，若是，则对路径点进行预处理，进入步骤S2；否则继续等待接收折线路径。

5.根据权利要求1所述的一种基于五阶贝塞尔曲线的机器人路径平滑方法，其特征在于，所述步骤S2，具体包括如下步骤：

根据折线路径设定多段五阶贝塞尔曲线，第i段五阶贝塞尔曲线方程表示为：

其中，参数u_i∈[0，1]，六个控制点包括路径起点p₀、终点p₅以及位于起点与终点之间从该起点至终点依次排布的四个中间控制点p₁、p₂、p₃和p₄；

贝塞尔曲线的两个端点S_i(0)、S_i(1)的位置、一阶导数、二阶导数与控制点满足如下关系式：

S_i(0)＝P₀ S_i(1)＝P₅

S′_i(0)＝5(P₁-P₀) S′_i(1)＝5(P₅-P₄)

S″_i(0)＝20(P₀-2P₁+P₂) S″_i(1)＝20(P₃-2P₄+P₅)

设计五阶贝塞尔曲线的优化变量为：

其中，共包含变量数为12×M，其中M为曲线分段数，状态由6个控制点组成：

第i段贝塞尔曲线可以改写为：S_i(u_i)＝Q^T(u_i)X_i，其中，

设计五阶贝塞尔曲线的平滑性作为优化目标：

其中k₁和k₂分别是二阶导和三阶导的权重系数，

建立线性等式约束条件：

S_i(1)＝S_i+1(0)

S′_i(1)＝S′_i+1(0)

S″_i(1)＝S″_i+1(0)

建立线性不等式约束条件：

其中，j＝0，1，...，5，R_i0，R_i1，R_i2，R_i3为矩形约束框的四个端点。

6.根据权利要求5所述的一种基于五阶贝塞尔曲线的机器人路径平滑方法，其特征在于，所述矩形约束框由所述折线路径中直线段进行不断膨胀到与障碍物相交为止生成。