[发明专利]一种引导车辆历史轨迹曲线的获取方法及相关装置

权利要求书

1.一种引导车辆历史轨迹曲线的获取方法，其特征在于，用于机动车辆的横向预瞄跟踪控制，所述引导车辆历史轨迹曲线的获取方法包括：

获取所述机动车辆的引导车辆历史点集序列、所述机动车辆的当前车速、横摆角速度和质心侧偏角；

根据所述当前车速确定轨迹点更新间隔；

根据所述轨迹点更新间隔、当前车速、质心侧偏角和所述横摆角速度，计算获得所述机动车辆在所述轨迹点更新间隔内的坐标系变化量；所述坐标系变化量包括横坐标变化量、纵坐标变化量和航心角变化量；

根据所述坐标系变化量，计算获得坐标变换矩阵；

根据所述坐标变换矩阵，将所述引导车辆历史点集序列转换到所述机动车辆的当前坐标系下，以获得目标历史点集序列；

对所述目标历史点集序列进行拟合，以获得所述引导车辆历史轨迹曲线。

2.根据权利要求1所述的方法，其特征在于，所述根据所述当前车速确定轨迹点更新间隔包括：

根据所述当前车速查询预设数据库，以获得与所述当前车速对应的轨迹点更新间隔，所述预设数据库中存储有当前车速与轨迹点更新间隔的对应关系，所述轨迹点更新间隔与所述当前车速负相关。

3.根据权利要求1所述的方法，其特征在于，所述根据所述轨迹点更新间隔、当前车速、质心侧偏角和所述横摆角速度，计算获得所述机动车辆在所述轨迹点更新间隔内的坐标系变化量包括：

将所述轨迹点更新间隔、当前车速和所述横摆角速度，代入第一预设公式组，以计算获得所述机动车辆在所述轨迹点更新间隔内的坐标系变化率；

所述第一预设公式组包括：其中，dX表示所述横坐标变化量，dY表示所述纵坐标变化率，Psi表示所述航心角变化量，V_ego表示所述当前车速，Beta表示所述机动车辆的质心侧偏角，YawRt表示所述横摆角速度。

4.根据权利要求1所述的方法，其特征在于，所述根据所述坐标系变化量，计算获得坐标变换矩阵包括：

将所述坐标系变化量代入第二预设公式，以计算获得所述坐标变换矩阵；

所述第二预设公式包括：

其中，dX表示所述横坐标变化量，dY表示所述纵坐标变化率，Psi表示所述航心角变化量，T表示所述坐标变换矩阵。

5.根据权利要求1所述的方法，其特征在于，所述根据所述坐标变换矩阵，将所述引导车辆历史点集序列转换到所述机动车辆的当前坐标系下，以获得目标历史点集序列包括：

根据第三预设公式，将所述引导车辆历史点集序列转换到所述机动车辆的当前坐标系下；

所述第三预设公式包括：其中，x_t、y_t分别表示所述引导车辆在所述机动车辆的当前坐标系下的横坐标和纵坐标，x_t-1、y_t-1分别表示所述引导车辆在t-1时刻的坐标系下的横坐标和纵坐标；T表示所述坐标变换矩阵。

6.根据权利要求1所述的方法，其特征在于，所述对所述目标历史点集序列进行拟合包括：

对所述目标历史点集序列进行采样，对采样获得的采样点进行补点，并利用补点后的所有采样点进行三次曲线拟合，以获得所述引导车辆历史轨迹曲线；

或

利用所述目标历史点集序列进行线性拟合，以获得所述引导车辆历史轨迹曲线；

或

对所述目标历史点集序列进行采样，并利用所有采样点进行三次曲线拟合，以获得所述引导车辆历史轨迹曲线。