汽车列车的位姿确定方法、装置、电子设备和存储介质与流程

本公开涉及自动驾驶，尤其涉及一种汽车列车的位姿确定方法、装置、电子设备和存储介质。

背景技术：

1、随着车辆控制技术的发展，集成自动驾驶、辅助驾驶等功能的智能驾驶系统逐渐成熟，并投入各种汽车列车及挂车/拖斗的实际应用中。在此类车辆行驶的过程中，后方拖斗的位姿(位置及航向角)常用于评估车辆的碰撞风险，因此，提升拖斗位姿的估计精度对车辆安全行驶较为重要。

2、现有技术中，通常通过在汽车列车中各拖斗上安装传感器测量拖斗的位置及航向角，或者，通过运动学模型预测拖斗的位置及航向角。然而，拖斗在货物运输过程中，需要频繁装卸，传感器的存在不利于大规模部署与维护，并且，传感器增加了系统集成的成本；此外，模型预测方式容易存在累计误差，导致位姿准确性低。

技术实现思路

1、为了解决上述技术问题或者至少部分地解决上述技术问题，本公开实施例提供了一种汽车列车的位姿确定方法、装置、电子设备和存储介质，实现对模型推导位姿的优化，减少累计误差，提高位姿准确性，并且，无需在每个车辆上安装传感器，降低了成本。

2、第一方面，本公开实施例提供了一种汽车列车的位姿确定方法，该方法包括：

3、基于与目标汽车列车对应的运动学模型、所述目标汽车列车中牵引车在当前时刻的控制量、以及所述目标汽车列车中各轴心在当前时刻的初始位姿，确定各轴心在下一时刻的模型推导位姿；

4、基于所述牵引车的传感器有效数据确定各轴心在下一时刻的实际检测位姿；

5、基于各轴心在下一时刻的模型推导位姿、目标估计位姿以及实际检测位姿构建模型约束残差、几何约束残差以及检测残差，根据所述模型约束残差、所述几何约束残差以及所述检测残差确定累计残差；

6、以所述累计残差最小化为目标并对所述目标求解，得到各轴心在下一时刻的目标估计位姿，将所述目标估计位姿作为所述下一时刻的初始位姿。

7、第二方面，本公开实施例还提供了一种汽车列车的位姿确定装置，该装置包括：

8、模型推导模块，用于基于与目标汽车列车对应的运动学模型、所述目标汽车列车中牵引车在当前时刻的控制量、以及所述目标汽车列车中各轴心在当前时刻的初始位姿，确定各轴心在下一时刻的模型推导位姿；

9、传感器检测模块，用于基于所述牵引车的传感器有效数据确定各轴心在下一时刻的实际检测位姿；

10、残差构建模块，用于基于各轴心在下一时刻的模型推导位姿、目标估计位姿以及实际检测位姿构建模型约束残差、几何约束残差以及检测残差，根据所述模型约束残差、所述几何约束残差以及所述检测残差确定累计残差；

11、目标求解模块，用于以所述累计残差最小化为目标并对所述目标求解，得到各轴心在下一时刻的目标估计位姿，将所述目标估计位姿作为所述下一时刻的初始位姿。

12、第三方面，本公开实施例还提供了一种电子设备，所述电子设备包括：一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上所述的汽车列车的位姿确定方法。

13、第四方面，本公开实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上所述的汽车列车的位姿确定方法。

14、本公开实施例提供的一种汽车列车的位姿确定方法，通过运动学模型、牵引车在当前时刻的控制量以及各轴心在当前时刻的初始位姿，得到各轴心在下一时刻的模型推导位姿，并根据牵引车的传感器有效数据确定各轴心在下一时刻的实际检测位姿，从而根据下一时刻的模型推导位姿、目标估计位姿以及实际检测位姿构建模型约束残差、几何约束残差以及检测残差，进而得到累计残差，以累计残差最小化为目标进行求解，得到各轴心在下一时刻的目标估计位姿，并将其作为下一时刻的初始位姿，该方法通过结合传感器检测的位姿构建三种残差，实现了对模型推导位姿的优化，减少了位姿估计中的累计误差，并且，无需在每个车辆上安装传感器，降低了位姿估计成本。

技术特征：

1.一种汽车列车的位姿确定方法，其特征在于，所述方法包括：

2.根据权利要求1所述的方法，其特征在于，所述基于与目标汽车列车对应的运动学模型、所述目标汽车列车中牵引车在当前时刻的控制量、以及所述目标汽车列车中各轴心在当前时刻的初始位姿，确定各轴心在下一时刻的模型推导位姿，包括：

3.根据权利要求2所述的方法，其特征在于，所述获取所述牵引车在当前时刻的控制量，包括：

4.根据权利要求1所述的方法，其特征在于，所述基于各轴心在下一时刻的模型推导位姿、目标估计位姿以及实际检测位姿构建模型约束残差、几何约束残差以及检测残差，包括：

5.根据权利要求4所述的方法，其特征在于，所述基于各轴心在下一时刻的模型推导位姿以及目标估计位姿之间的差距，构建模型约束残差，满足如下公式：

6.根据权利要求4所述的方法，其特征在于，所述基于相邻轴心在下一时刻的目标估计位姿中的横坐标、纵坐标和航向角，以及相邻轴心中前一个轴心的牵引销距离和后一个轴心的轴距，构建几何约束残差，满足如下公式：

7.根据权利要求4所述的方法，其特征在于，所述基于各轴心在下一时刻的实际检测位姿以及目标估计位姿之间的差距，构建检测残差，满足如下公式：

8.一种汽车列车的位姿确定装置，其特征在于，包括：

9.一种电子设备，其特征在于，所述电子设备包括：

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的方法。

技术总结
本公开实施例公开了一种汽车列车的位姿确定方法、装置、电子设备和存储介质，该方法包括：通过运动学模型、牵引车在当前时刻的控制量以及各轴心在当前时刻的初始位姿，得到各轴心在下一时刻的模型推导位姿，并根据牵引车的传感器有效数据确定各轴心在下一时刻的实际检测位姿，从而根据下一时刻的模型推导位姿、目标估计位姿以及实际检测位姿构建模型约束残差、几何约束残差以及检测残差，进而得到累计残差，以累计残差最小化为目标进行求解，得到各轴心在下一时刻的目标估计位姿，并将其作为下一时刻的初始位姿，该方法通过构建三种残差实现了对模型推导位姿的优化，减少了位姿估计中的累计误差，降低了位姿估计成本。

技术研发人员：牛弼陛,刘凯,曹世卓,周小成
受保护的技术使用者：驭势科技（北京）有限公司
技术研发日：
技术公布日：2024/1/16