1.用于自动驾驶汽车位姿与速度同时跟随的运动预瞄方法,其特征在于,包括以下步骤:步骤1:生成目标曲线;
将目标轨迹点的坐标和各轨迹点处的目标车速映射到各个轨迹点的累计里程上;输入目标轨迹点坐标、各个目标轨迹点处的目标车速;输出目标位姿随累计里程变化的曲线以及目标车速随累计里程变化的曲线;
步骤2:跟随目标选取;
确定当前预瞄里程内需要跟随的目标,包括目标位姿和目标车速;输入目标位姿随累计里程变化的曲线以及目标车速随累计里程变化的曲线;输出当前预瞄里程内待跟随的目标曲线;
步骤3:优化变量初值求解;
求解位姿与速度同时预瞄所涉及的优化变量的初始值,包含转向运动优化变量初值求解和变速运动优化变量初值求解;输入待跟随的目标曲线;输出为优化变量的初始值,包括转向运动段数、各段转向运动的里程长度、各段转向运动的平均曲率的大小和符号、变速运动段数、各段变速运动的里程长度以及各段变速运动的平均加速度;
步骤4:位姿与速度同时预瞄;
确定汽车目标的纵向加速度和侧向加速度,保证汽车能同时满足预瞄里程处的位置、方向和速度要求;输入待跟随的目标曲线、优化变量的初始值以及汽车的运动状态,输出预瞄的纵向加速度和侧向加速度;
所述步骤4包括以下具体步骤:
步骤4.1:建立基元运动模型,将汽车行驶轨迹的曲率恒定不变的一段运动作为转向运动的基元运动,将汽车纵向加速度恒定不变的一段运动作为变速运动的基元运动,通过多段转向基元运动和变速基元运动的接续来构建预测模型;将各段转向基元运动的弧长和曲率以及各段变速基元运动的里程长度和纵向加速度作为优化方法的优化变量,各段终点处的位姿和车速作为因变量,进行迭代优化;单段基元运动的公式如下:直线基元运动,则有:
;
左转基元运动或右转基元运动,则有:
;
式中,表示单段转向基元运动预测的汽车位姿,表示预测过程的初始位姿,表示基元运动模型单段转向运动的曲率,表示基元运动模型单段转向运动的圆弧长度;
对于变速基元运动,公式如下:
;
式中,表示基元运动模型单段变速基元运动预测的车速,表示预测过程的初始车速,表示基元运动模型单段变速运动的纵向加速度,表示基元运动模型单段变速运动的里程长度;
通过单段基元运动的接续得到预测曲线,其在终点处的位姿和车速是关于初始位姿、初始车速、初始里程、转向运动段数、转向运动各段的曲率和弧长、变速运动段数、变速运动各段的纵向加速度和里程长度的函数,表达式如下:;
式中,表示终点处的累计里程,表示终点处的位姿,表示终点处的车速,表示预测过程的初始里程,表示基元运动模型第1段单段转向运动的曲率,表示基元运动模型第1段单段转向运动的圆弧长度,表示基元运动模型第1段单段变速运动的纵向加速度,表示基元运动模型第1段单段变速运动的里程长度,表示基元运动模型第2段单段运动的曲率,表示基元运动模型第2段单段运动的圆弧长度,表示基元运动模型第2段单段变速运动的纵向加速度,表示基元运动模型第2段单段变速运动的里程长度,表示基元运动模型第段单段运动的曲率,表示基元运动模型第段单段运动的圆弧长度,表示基元运动模型第段单段变速运动的纵向加速度,表示基元运动模型第段单段变速运动的里程长度;
步骤4.2:建立跟随评价函数,同时跟随目标位姿曲线和目标车速曲线,将预测轨迹与目标轨迹的位置偏差和方向偏差进行归一化,将预测车速与目标车速的偏差进行归一化,将归一化结果的平方和作为目标函数,通过优化算法求出使得目标函数最小的解;
将预测位姿曲线和车速曲线离散为若干个点,求这些点到目标轨迹的投影距离和投影点处的航向角,将投影距离作为预测轨迹与目标轨迹的位置偏差,将投影点处的航向角与预测轨迹点的航向角的偏差作为方向偏差,将预测车速曲线的离散点到目标车速曲线的投影距离作为预测车速曲线与目标车速曲线的速度偏差,得到目标函数公式如下:;
式中,表示取最小值的目标函数,表示预测位姿曲线和车速曲线上离散点的个数,表示位置偏差归一化系数,表示第个位置偏差,表示方向偏差归一化系数,表示第个方向偏差,表示速度偏差归一化系数,表示第个速度偏差;
步骤4.3:确定位姿与速度约束,考虑汽车机动能力的约束,预瞄轨迹的曲率应满足汽车的最小转弯半径的约束,预瞄的纵向加速度应不超过汽车最大的加速度和制动减速度;考虑汽车侧向稳定性的要求,预瞄的侧向加速度应小于汽车失稳的侧向加速度;考虑预测曲线在终点处的位姿和车速跟随要求,预测轨迹与目标轨迹在终点处的位置偏差和方向偏差应足够小,预测车速曲线与目标车速曲线在终点处的速度偏差应足够小,公式如下:;
式中,表示汽车的最小转弯半径,表示基元运动模型单段转向运动的侧向加速度,表示汽车可达到的最大侧向加速度,表示基元运动模型单段变速运动的纵向加速度,表示汽车可达到的最大制动减速度,表示汽车可达到的最大加速度,表示在终点处预测轨迹与目标轨迹的位置偏差,表示终点处的位置偏差阈值,表示在终点处预测轨迹与目标轨迹的方向偏差,表示终点处的方向偏差阈值,表示在终点处预测车速曲线与目标车速曲线的速度偏差,表示终点处的速度偏差阈值;
步骤4.4:选择优化算法进行求解,在约束条件范围内,迭代优化设计变量,求解目标函数取最小值时的一组设计变量;使用SQP序列二次规划方法进行求解,将设计变量中的基元运动模型第一段转向运动的预瞄曲率与车速的平方相乘,得到汽车的预瞄侧向加速度,将设计变量中的基元运动模型第一段变速运动的纵向加速度作为汽车的预瞄纵向加速度,预瞄结果公式如下:;
式中,表示预瞄侧向加速度,表示预瞄纵向加速度,表示基元运动模型第1段单段变速运动的纵向加速度。
2.根据权利要求1所述的用于自动驾驶汽车位姿与速度同时跟随的运动预瞄方法,其特征在于,根据目标轨迹点的坐标计算各个轨迹点处的累计里程,公式如下:;
式中,表示第个轨迹点处的累计里程,表示第个轨迹点的坐标,表示第个轨迹点的坐标;
对轨迹点进行多项式拟合,得到轨迹点构成曲线的函数,通过对函数求导得各个轨迹点处的目标方向,公式如下:;
式中,表示轨迹点处的目标方向,表示轨迹点处的函数的一阶导数值;
由上得到目标位姿随累计里程变化的曲线以及目标车速随累计里程变化的曲线,用向量表示如下:;
式中,表示目标位姿,表示累计里程,表示累计里程处的位置坐标,表示累计里程处的航向角,表示目标车速,表示累计里程处的目标车速。
3.根据权利要求2所述的用于自动驾驶汽车位姿与速度同时跟随的运动预瞄方法,其特征在于,在所述步骤2中,根据当前车速和预瞄时间确定预瞄里程,公式如下:;
式中,表示预瞄里程,表示当前车速,表示预瞄时间;
将汽车当前的位置坐标点向目标轨迹曲线投影,得到投影点的坐标,并计算当前汽车位置坐标点在目标轨迹曲线上的投影点所对应的累计里程,公式如下:;
式中,表示当前汽车位置坐标点在目标轨迹曲线上的投影点所对应的累计里程,表示当前汽车位置坐标点在目标轨迹曲线上的投影点坐标,表示目标轨迹点序列中,位于投影点之前且距离投影点最近的轨迹点坐标,表示目标轨迹点序列中,位于投影点之后且距离投影点最近的轨迹点坐标,表示第个轨迹点处的累计里程;
将当前汽车位置对应的里程与预瞄里程加和,并与目标轨迹点的累计里程进行比较,得到当前需要跟随的目标位姿和目标车速,公式如下:;
式中,表示待跟随目标曲线,表示待跟随目标的各里程点处的累计里程,表示待跟随位姿曲线上各点的位置坐标,表示待跟随位姿曲线上各点的航向角,表示待跟随车速曲线上各里程点处的目标车速。
4.根据权利要求3所述的用于自动驾驶汽车位姿与速度同时跟随的运动预瞄方法,其特征在于,所述步骤3包括以下具体步骤:步骤3.1:进行转向运动优化变量初值求解;
对需要跟随的轨迹点进行多项式拟合,得到轨迹点构成曲线的函数,根据曲率公式得到各个轨迹点处的曲率公式如下:;
式中,表示处的曲率,和分别表示函数在处的一阶导数值和二阶导数值;
设置曲率分段的门槛值,根据各个轨迹点处的曲率大小和符号,将目标轨迹分为若干个直线运动段、左转运动段和右转运动段,公式如下:;
式中,表示曲率分段的门槛值,表示第个直线运动段的集合,表示第个左转运动段的集合,表示第个右转运动段的集合;
转向运动段数为直线运动段数、左转运动段数和右转运动段数的总和;各段的里程长度为段内包含的轨迹点的最后一个点的累计里程减去第一个点的累计里程;各段的平均曲率为段内包含的各轨迹点的曲率平均值,公式如下:;
式中,表示转向运动的段数,分别表示直线运动段数、左转运动段数和右转运动段数;表示第段转向运动的里程长度,表示第段转向运动内包含的轨迹点的第一个点的累计里程,表示第段转向运动内包含的轨迹点的最后一个点的累计里程,表示第段转向运动的轨迹点个数,表示第段转向运动的平均曲率,表示第段转向运动中第个轨迹点处的曲率;
步骤3.2:进行变速运动优化变量初值求解;
根据目标车速曲线上各点的累计里程和目标车速,计算两个里程点内的目标加速度,假定汽车在两个里程点内的加速度不变,公式如下:;
式中,表示第个里程点处的目标加速度,表示第个里程点处的目标速度,表示第个里程点处的目标速度,表示第个里程点处的目标速度,表示第个里程点处的累计里程,表示第个里程点处的累计里程,表示第个里程点处的累计里程;
设置加速度分段的门槛值,根据各个轨迹点处的加速度大小和符号,将目标车速曲线分为若干个匀速运动段、加速运动段和减速运动段,公式如下:;
式中,表示加速度分段的门槛值,表示第个匀速运动段的集合,表示第个加速运动段的集合,表示第个减速运动段的集合,表示各里程点处的目标加速度;
变速运动段数为匀速运动段数、加速运动段数和减速运动段数的总和;各段的里程长度为段内包含的轨迹点的最后一个点的累计里程减去第一个点的累计里程;各段的平均加速度通过段内包含的第一个点和最后一个点的累计里程和目标车速来计算,公式如下:;
式中,表示变速运动的段数,分别表示匀速运动段数、加速运动段数和减速运动段数;表示第段变速运动的里程长度,表示第段变速运动内包含的第一个点的累计里程,表示第段变速运动内包含的最后一个点的累计里程,表示第段变速运动的平均加速度,表示第段变速运动内包含的第一个点的目标车速,表示第段变速运动内包含的最后一个点的目标车速;
综上,得到优化变量的初始值集合如下:
;
式中,表示优化变量的初始值集合,表示转向运动的段数,表示第段转向运动的里程长度,表示第段转向运动的平均曲率,表示变速运动的段数,表示第段变速运动的里程长度,表示第段变速运动的平均加速度。