CN113625702B - 基于二次规划的无人车同时路径跟踪与避障方法 - Google Patents

Abstract

本发明公开了一种基于二次规划的无人车同时路径跟踪与避障方法，通过获取目标及障碍物在行驶过程中经过的多个离散点坐标，同时得到对应的离散时间点，通过曲线拟合方法分别拟合出目标行驶轨迹坐标与时间之间的关系以及障碍物行驶轨迹坐标与时间之间的关系，利用控制李雅普诺夫函数和控制障碍函数分别构建无人车本体运动模型的软约束和硬约束，利用软约束和硬约束构造二次规划，然后利用计算机编程，迭代求解得到无人车本体运动模型的实时控制输入量，从而达到跟踪目标轨迹的同时避障的目的。与现有技术相比，本发明不但跟踪范围大，准确度高，算法速度快，鲁棒性更高，而且能够在跟踪目标轨迹的同时实现准确避开障碍物的功能。

Description

基于二次规划的无人车同时路径跟踪与避障方法

技术领域

本发明涉及无人驾驶小车技术领域，特别涉及一种基于二次规划的无人车同时路径跟踪与避障方法，适用于在各种二维平面运动的陆地无人车、水面无人船、水下无人潜艇等的同时路径跟踪及避障。

背景技术

目前，无人车一般具有完善的环境感知系统，能够根据感知系统提供的环境信息进行路径的决策和规划，并控制车辆到达目的地。路径跟踪与避障是在路径规划后进行的控制环节，要求车辆能够按照规划出的路径行驶，同时能够避开障碍物以免发生碰撞。

现有无人车的路径跟踪算法有基于PID控制的路径跟踪、基于纯跟踪的路径跟踪、以及基于模型预测控制的路径跟踪等方法。基于PID控制的路径跟踪算法根据目标路径信息和当前车辆位姿信息，计算路径误差，调整车辆当前位姿，消除稳态误差，使车辆平稳运行。基于纯跟踪的方法，通过预先设定目标轨迹中的一个点，计算圆弧路径，根据车辆实时运动状态，对车辆前进方向和速度进行动态调整，能够实现精度较高的路径跟踪控制。基于模型预测控制的路径规划，其控制策略为根据系统k时刻的状态和控制输入，来预测k+1时刻的输出，主要实现模型的预测作用。以上方法虽然能够实现较为准确的路径跟踪，但是算法效率低，同时无法实现在路径跟踪的过程中实时动态避障。

发明内容

本发明的目的在于提供一种基于二次规划的无人车同时路径跟踪与避障方法，不但跟踪范围大，准确度高，算法速度快，鲁棒性更高，而且能够在跟踪目标轨迹的同时准确避障，解决了现有无人驾驶小车路径跟踪误差大，算法效率低，智能化水平低，无法在跟踪目标轨迹的同时准确避障的问题。

为解决上述技术问题，本发明的实施例提供如下方案：

一种基于二次规划的无人车同时路径跟踪与避障方法，包括以下步骤：

S1、获取目标及障碍物在行驶过程中的离散坐标点，同时获取对应的离散时间点；

S2、通过曲线拟合方法分别拟合出目标行驶轨迹坐标与时间的关系以及障碍物行驶轨迹坐标与时间的关系；

S3、计算无人车与目标之间的位置偏差以及无人车与障碍物之间的距离，并且以无人车与目标之间的位置偏差作为无人车本体运动模型中的状态量；其中，所述位置偏差包括坐标偏差和方向角偏差；

S4、根据无人车与目标之间的位置偏差以及无人车与障碍物之间的距离，利用控制李雅普诺夫函数和控制障碍函数(control barrier function)分别构建无人车本体运动模型的软约束和硬约束，利用软约束和硬约束构造二次规划，利用计算机编程迭代求解，得到无人车本体运动模型的控制输入量，实现跟踪目标轨迹的同时准确避开障碍物的目的。

优选地，所述步骤S1具体包括：

利用激光雷达传感器获取目标及障碍物在行驶过程中所处位置的x，y坐标，其中，不同时刻无人车本体、目标和障碍物的坐标分别为：

优选地，所述步骤S2具体包括：

分别拟合出目标和障碍物行驶轨迹x坐标、y坐标与时间t之间的关系曲线；

根据拟合出的上述曲线再拟合出目标和障碍物x坐标与y坐标之间的曲线；

对于目标方向角的求取，将目标的y坐标对时间t的导数和x坐标对时间t的导数相除，再进行反正切求得目标的方向角，然后将目标的方向角对时间t求导数得到目标的方向角速度；

对于障碍物方向角的求取，将障碍物的y坐标对时间t的导数和x坐标对时间t的导数相除，再进行反正切求得障碍物的方向角，然后将障碍物的方向角对时间t求导数得到障碍物的方向角速度。

优选地，所述步骤S3具体包括：

以无人车本体坐标系为参考坐标系，在二维惯性坐标系上，目标和障碍物到无人车本体的旋转矩阵为：

设无人车的控制输入量为：前进线速度u、方向角速度r、方向角Φ，则无人车本体运动模型为:

得到目标到无人车本体坐标系的偏差为：

将-e_x，e_y，作为无人车本体运动模型的状态量，其中/>的表示如下：

其中α为正常数，

优选地，所述步骤S4具体包括：

以无人车本体运动模型的控制输入量为前进线速度u和方向角速度r，状态量为-e_x，e_y，建立无人车本体运动模型如下：

其中z表示无人车与障碍物之间的距离；

利用控制李雅普诺夫函数和control barrier function分别构建软约束和硬约束，其中，硬约束是指系统必须满足的约束条件，软约束是指与硬约束不冲突时需要满足的约束；

通过选择李雅普诺夫函数:

构建软约束:

其中，k₁>0.5、ε为正常数；

然后选择control barrier function：

h(x,z)＝z-0.5≥0

构建硬约束：

u_min≤u≤u_max

r_min≤r≤r_max

其中，z为无人车与障碍物之间的距离,u_min,u_max,r_min,r_max分别为无人车本体前进线速度和角速度的最小值和最大值；

构建二次规划代价函数：

其中H_acc,F_acc为任意选择的代价函数，δ_sc为松弛系数；

构建完软约束和硬约束之后，利用软约束和硬约束构造二次规划，在计算机中编程迭代求解得到无人车本体运动模型的控制输入量，实现无人车在跟踪目标轨迹的同时准确避开障碍物的目的。

本发明实施例提供的技术方案带来的有益效果至少包括：

本发明通过获取目标及障碍物在行驶过程中经过的多个离散点坐标，同时得到对应的离散时间点，通过曲线拟合方法分别拟合出目标行驶轨迹坐标与时间之间的关系以及障碍物行驶轨迹坐标与时间之间的关系，利用控制李雅普诺夫函数和control barrierfunction分别构建无人车本体运动模型的软约束和硬约束，利用软约束和硬约束构造二次规划，利用计算机编程，迭代求解得到无人车本体运动模型的控制输入量，从而达到跟踪目标轨迹的同时准确避障的目的。与现有技术相比，本发明不但跟踪范围大，准确度高，算法速度快，鲁棒性更高，而且能够在跟踪目标轨迹的同时实现准确避开障碍物，解决了现有无人驾驶小车路径跟踪误差大，算法效率低，智能化水平低，无法在跟踪目标轨迹的同时准确避障的问题。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种基于二次规划的无人车同时路径跟踪与避障方法的流程图；

图2是本发明实施例中无人车的示意图；

图3a-图3b是本发明实施例中坐标转换示意图；

图4a-图4b分别是直线运动障碍物和曲线运动障碍物的情况下无人车同时路径跟踪及避障方法仿真示意图；

图5a-图5b分别是直线运动障碍物和曲线运动障碍物的情况下无人车与障碍物之间距离变化示意图。

图6a-图6f分别是直线运动障碍物和曲线运动障碍物的情况下无人车与目标前进线速度、角速度和角度变化示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

本发明的实施例提供了一种基于二次规划的无人车同时路径跟踪与避障方法，参考图1所示，该方法包括以下步骤：

S1、获取目标及障碍物在行驶过程中的离散坐标点，同时获取对应的离散时间点；

S2、通过曲线拟合方法分别拟合出目标行驶轨迹坐标与时间的关系以及障碍物行驶轨迹坐标与时间的关系；

S3、计算无人车与目标之间的位置偏差以及无人车与障碍物之间的距离，并且以无人车与目标之间的位置偏差作为无人车本体运动模型中的状态量；其中，所述位置偏差包括坐标偏差和方向角偏差；

S4、根据无人车与目标之间的位置偏差以及无人车与障碍物之间的距离，利用控制李雅普诺夫函数和Control Barrier Function分别构建无人车本体运动模型的软约束和硬约束，利用软约束和硬约束构造二次规划，利用计算机编程迭代求解，得到无人车本体运动模型的控制输入量，实现跟踪目标轨迹的同时避开障碍物的目的。

本发明通过获取目标及障碍物在行驶过程中经过的多个离散点坐标，同时得到对应的离散时间点，通过曲线拟合方法分别拟合出目标行驶轨迹坐标与时间之间的关系以及障碍物行驶轨迹坐标与时间之间的关系，利用控制李雅普诺夫函数和control barrierfunction分别构建无人车本体运动模型的软约束和硬约束，利用软约束和硬约束构造二次规划，然后利用计算机编程，迭代求解得到无人车本体运动模型的控制输入量，从而达到跟踪目标轨迹的同时避障的目的。与现有技术相比，本发明不但跟踪范围大，准确度高，算法速度快，鲁棒性更高，而且能够在跟踪目标轨迹的同时实现准确避开障碍物，解决了现有无人驾驶小车路径跟踪误差大，算法效率低，智能化水平低，无法在跟踪目标轨迹的同时准确避障的问题。

进一步地，所述步骤S1具体包括：

如图2所示，利用激光雷达传感器1获取t_N个不同时刻目标target及障碍物obstacle在行驶过程中所处位置的x，y坐标，其中，不同时刻无人车本体、目标和障碍物的坐标分别为：

本发明利用激光雷达传感器获取目标及障碍物运行轨迹坐标点，具有测量精度高且不易受光线干扰等优点，可靠性得到了很大提升。

进一步地，所述步骤S2具体包括：

分别拟合出目标和障碍物行驶轨迹x坐标、y坐标与时间t之间的关系曲线；

根据拟合出的上述曲线再拟合出目标和障碍物x坐标与y坐标之间的曲线；

对于目标方向角的求取，将目标的y坐标对时间t的导数和x坐标对时间t的导数相除，再进行反正切求得目标的方向角，然后将目标的方向角对时间t求导数得到目标的方向角速度；

对于障碍物方向角的求取，将障碍物的y坐标对时间t的导数和x坐标对时间t的导数相除，再进行反正切求得障碍物的方向角，然后将障碍物的方向角对时间t求导数得到障碍物的方向角速度。

进一步地，所述步骤S3具体包括：

以无人车本体坐标系作为参考坐标系，图3a-图3b为无人车本体坐标系与惯性坐标系转化关系，在二维平面上，目标和障碍物到无人车本体坐标系的旋转矩阵为：

设无人车的控制输入量为：前进线速度u、方向角速度r、方向角Φ，则无人车本体运动模型为:

得到目标到无人车本体坐标系的偏差为：

从上面的式子可以看到，e_x，e_y，e_Φ代表了无人车本体与目标轨迹之间的坐标偏差和方向角偏差，将它们作为无人车运动模型中状态量时会出现当e_x，e_Φ趋于0时，e_y处于不可控状态，因此，本发明将-e_x，e_y，作为无人车本体运动模型的状态量，其中/>的表示如下：

其中α为正常数，

可以实现，当无人车本体运动模型中状态量均趋于0时，此时意味着无人车本体准确的跟踪上了目标轨迹。

进一步地，所述步骤S4具体包括：

以无人车本体运动模型的控制输入量为前进线速度u和方向角速度r，状态量为-e_x，e_y，建立无人车本体运动模型如下：

其中z表示无人车与障碍物之间的距离；

利用控制李雅普诺夫函数和control barrier function分别构建软约束和硬约束，其中，硬约束是指系统必须满足的约束条件，软约束是指与硬约束不冲突时需要满足的约束；

通过选择李雅普诺夫函数:

构建软约束:

其中，k₁>0.5、ε为正常数；

然后选择控制障碍函数：

h(x,z)＝z-0.5≥0

构建硬约束：

u_min≤u≤u_max

r_min≤r≤r_max

其中，z为无人车与障碍物之间的距离，u_min,u_max,r_min,r_max分别为无人车本体前

进线速度和角速度的最小值和最大值；

构建二次规划代价函数：

其中H_acc,F_acc为任意选择的代价函数,δ_sc为松弛系数；

构建完软约束和硬约束之后，利用软约束和硬约束构造二次规划，在计算机中编程迭代求解得到无人车本体运动模型的控制输入量，实现无人车在跟踪目标轨迹的同时避开障碍物。同时，当时间t到达某个点时，无人车本体与目标轨迹的坐标偏差和方向角偏差均为0，从此刻开始，无人车本体准确地跟踪上了目标轨迹，即此时无人车本体运动轨迹与目标轨迹完全重合。

为了验证本发明方法的有效性，本发明的实施例进行了仿真实验。图4a-图4b分别是直线运动障碍物和曲线运动障碍物的情况下无人车同时路径跟踪及避障方法仿真示意图，图5a-图5b分别是直线运动障碍物和曲线运动障碍物的情况下无人车与障碍物之间距离变化示意图。从四幅示意图中可以看出，无人车本体在跟踪目标轨迹的过程中能够准确避开障碍物，同时，当时间t到达某个点时，无人车本体与目标轨迹的坐标偏差和方向角偏差均为0，从此时开始，无人车本体的位置和方向角准确地跟踪上了目标运动轨迹，即此时无人车本体运动轨迹与目标轨迹完全重合，这也验证了本发明的有效性。

如果将离障碍物半径0.5单位以内的区域视为危险区域，从图中可以看出，初始时刻无人车本体与障碍物之间距离小于0.5单位，即无人车本体位于障碍物的危险区域内，从初始时刻开始到某个时刻之后，无人车本体将不再进入障碍物的危险区域内。

图6a-图6f分别是直线运动障碍物和曲线运动障碍物的情况下无人车与目标前进线速度、角速度和角度变化示意图，从图中可以看出，从初始时刻开始，到某一时刻，无人车本体和目标前进线速度、角速度和角度完全相等，从此刻开始，无人车本体完全跟踪上了目标轨迹。同时也可以看到，在整个过程中，无人车本体前进线速度和角速度约束均得到了满足。这也验证了本发明的有效性。

与现有技术相比，本发明不但跟踪范围广，算法速度快，鲁棒性更高，同时跟踪精度高，能够准确避开一个及多个障碍物，可适用于平面行驶的无人车、水面无人船、水下无人潜艇等设备。

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

Claims (3)

1.一种基于二次规划的无人车同时路径跟踪与避障方法，其特征在于，包括以下步骤：

S1、获取目标及障碍物在行驶过程中的离散坐标点，同时获取对应的离散时间点；

S2、通过曲线拟合方法分别拟合出目标行驶轨迹坐标与时间的关系以及障碍物行驶轨迹坐标与时间的关系；

S3、计算无人车与目标之间的位置偏差以及无人车与障碍物之间的距离，并且以无人车与目标之间的位置偏差作为无人车本体运动模型中的状态量；其中，所述位置偏差包括坐标偏差和方向角偏差；

所述步骤S3具体包括：

以无人车本体坐标系为参考坐标系，在二维惯性坐标系上，目标和障碍物到无人车本体的旋转矩阵为：

设无人车的控制输入量为：前进线速度u、方向角速度r、方向角Φ，则无人车本体运动模型为:

得到目标到无人车本体坐标系的偏差为：

将-e_x，e_y，作为无人车本体运动模型的状态量，其中/>的表示如下：

其中α为正常数，

S4、根据无人车与目标之间的位置偏差以及无人车与障碍物之间的距离，利用控制李雅普诺夫函数和控制障碍函数分别构建无人车本体运动模型的软约束和硬约束，利用软约束和硬约束构造二次规划，利用计算机编程迭代求解，得到无人车本体运动模型的控制输入量，实现跟踪目标轨迹的同时避开障碍物；

所述步骤S4具体包括：

以无人车本体运动模型的控制输入量为前进线速度u和方向角速度r，状态量为-e_x，e_y，建立无人车本体运动模型如下：

其中z表示无人车与障碍物之间的距离；

利用控制李雅普诺夫函数和控制障碍函数分别构建软约束和硬约束，其中，硬约束是指系统必须满足的约束条件，软约束是指与硬约束不冲突时需要满足的约束；

通过选择李雅普诺夫函数:

构建软约束:

其中，k₁>0.5、ε为正常数；

然后选择控制障碍函数：

h(x,z)＝z-0.5≥0

构建硬约束：

u_min≤u≤u_max

r_min≤r≤r_max

其中，z为无人车与障碍物之间的距离，u_min,u_max,r_min,r_max分别为无人车本体前进线速度和角速度的最小值和最大值；

构建二次规划代价函数：

其中H_acc,F_acc为任意选择的代价函数，δ_sc为松弛系数；

构建完软约束和硬约束之后，利用软约束和硬约束构造二次规划，在计算机中编程迭代求解得到无人车本体运动模型的控制输入量，实现无人车在跟踪目标轨迹的同时避开障碍物的目的。

2.根据权利要求1所述的基于二次规划的无人车同时路径跟踪与避障方法，其特征在于，所述步骤S1具体包括：

利用激光雷达传感器获取目标及障碍物在行驶过程中所处位置的x，y坐标，其中，不同时刻无人车本体、目标和障碍物的坐标分别为：

3.根据权利要求1所述的基于二次规划的无人车同时路径跟踪与避障方法，其特征在于，所述步骤S2具体包括：

分别拟合出目标和障碍物行驶轨迹x坐标、y坐标与时间t之间的关系曲线；

根据拟合出的上述曲线再拟合出目标和障碍物x坐标与y坐标之间的曲线；

对于目标方向角的求取，将目标的y坐标对时间t的导数和x坐标对时间t的导数相除，再进行反正切求得目标的方向角，然后将目标的方向角对时间t求导数得到目标的方向角速度；

对于障碍物方向角的求取，将障碍物的y坐标对时间t的导数和x坐标对时间t的导数相除，再进行反正切求得障碍物的方向角，然后将障碍物的方向角对时间t求导数得到障碍物的方向角速度。