【技术实现步骤摘要】
本专利技术涉及智能驾驶导航定位，尤其涉及一种基于滑动窗口的融合定位方法。
技术介绍
1、智能驾驶车辆，作为现代交通技术的前沿领域，其定位精度和稳定性对于确保行车安全、实现高效导航至关重要。目前，智能驾驶车辆上的融合定位方法主要分为两大类。
2、第一类定位算法依赖于gnss/rtk(全球导航卫星系统/实时动态载波相位差分技术)和imu(惯性测量单元)等组合导航技术。这类算法在开阔环境下表现良好，但在某些特定场景中存在问题。尤其是在卫星信号受到严重遮挡的情境下(例如城区高楼间、高架桥下)，gnss/rtk信号容易发生跳变，导致车辆定位失效。这种失效可能会严重影响智能驾驶车辆的安全性和可靠性。
3、第二类定位算法则依赖于高精度地图。这种方法需要预先绘制场景的高精度地图，因此涉及额外的成本。不仅如此，该方法的性能高度依赖于地图的准确性和地图数据的更新程度。当实际环境发生改变时(如道路修建、交通标志重新划线等)，如果高精度地图未能及时更新，车端系统容易进行错误的匹配，进而导致车辆定位失效。这种对地图鲜度的严格要求限制了该方法的广泛应用和长期稳定性。
4、因此，现有的智能驾驶车辆融合定位方法在应对复杂多变的实际环境时存在局限性，迫切需要一种能够在各种场景下保持高精度和稳定性的改进型定位算法。这种算法应具备在卫星信号遮挡严重或地图数据不准确/过时的情况下，仍能保持可靠定位的能力，从而确保智能驾驶车辆在各种条件下的安全、有效运行。
技术实现思路
1、有鉴于此，本
2、本专利技术具体的技术方案如下：
3、一种基于滑动窗口的融合定位方法，包括以下步骤：
4、步骤1，数据预处理：将收集的传感器数据分别存入缓存队列中；
5、步骤2，状态初始化：构建滑窗状态队列，将当前状态存入滑窗状态缓存队列中；
6、步骤3，状态传递：当传感器数据缓存队列有新数据时，取出队首数据作为当前处理的数据；根据匀加速运动模型，对当前的车辆状态进行传播；根据传感器的测量模型，对滑窗内的状态和协方差进行更新；
7、步骤4，观测模型建立：根据相对位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，对滑窗内对应时刻的状态和协方差进行更新；
8、步骤5，状态更新：输出更新后的状态量和协方差；
9、步骤6，车辆状态输出：将更新后的状态量和协方差输出，完成车辆状态的估计。
10、具体地，步骤1中，传感器数据包括：imu的加速度和角速度数据、轮速计的左右轮速度数据、gnss数据和激光雷达点云数据。
11、具体地，步骤2中，使用gnss、轮速计和imu数据对系统状态中的全局位姿进行初始化。
12、具体地，步骤4中，当相对位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，根据相对位姿观测模型，对滑窗内对应时刻的状态和协方差进行更新。
13、具体地，步骤4中，当全局位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，根据全局位姿观测模型，对滑窗内对应时刻的状态和协方差进行更新。
14、具体地，步骤5中，若滑窗内大小超过预设值，根据时间戳对最早状态进行边缘化处理。
15、具体地，imu数据处理：当imu数据进入缓存队列时，将其取出并处理；将当前的角速度和线加速度状态量转换到imu坐标系下；与当前imu的测量值进行比较，得到关于位姿、速度、加速度状态量的残差和雅可比矩阵；更新车辆的状态量和协方差。。
16、具体地，轮速计数据处理：当轮速计数据进入缓存队列时，将其取出并处理；使用双轮差速模型和已知的线速度、角速度状态量来推算出左右轮的速度；将这些速度与测量值进行比较，计算残差并计算相对于角速度、线速度和轮径尺度的雅可比；更新车辆的当前状态量和协方差。
17、具体地，点云数据处理：当点云数据进入缓存队列时，将其取出并处理；对点云数据进行去畸变处理，消除畸变效应，提高测量精度；将处理后的点云送入lo模块以计算出当前帧的位姿；计算当前帧与滑窗内每一帧的相对位姿变换的测量值、残差和雅可比矩阵；根据相对位姿测量模型，更新滑窗内相应的状态量和协方差矩阵。
18、具体地，gnss数据处理：当gnss数据进入缓存队列时，将其取出并处理；解算出enu坐标系下的全局位姿；根据全局位姿观测模型，更新车辆的状态和协方差。
19、本专利技术的有益效果在于：
20、1.多传感器融合：该提案通过融合imu、轮速计、gnss和激光雷达等多种传感器信息，实现了更全面的车辆状态估计。这种多传感器融合的方法提高了定位的可靠性和鲁棒性。它不仅融合了gnss的全局信息，还融合了轮速、imu等相对信息。同时，该方法也融合了imu/轮速等运动模型和激光雷达的观测模型，进一步提高了车辆定位的准确性。
21、2.无需高精度地图：该提案的一个显著优点是无需依赖高精度地图进行车辆定位。这种方法增强了自动驾驶系统的适用性，使其能够在没有高精度地图的场景下使用。通过利用车载激光雷达构建局部地图，该算法实现了在各种驾驶场景下的稳定定位。
22、3.基于滑动窗口的框架：该提案采用了基于滑动窗口的框架，能够充分融合时序信息，进一步提高定位系统的准确性。通过在状态优化器中维护固定尺寸的历史状态窗口，该算法能够更好地处理时序数据，减少误差累积，提高定位精度。
23、4.鲁棒性增强：该提案通过融合多种传感器数据，降低了各种因素对定位精度的影响，增强了鲁棒性。无论是在gnss信号遮挡、多路径效应、建筑物反射等复杂环境中，还是在gnss跳变、轮胎打滑等特殊场景下，该算法都能实现更鲁棒的车辆定位。
24、5.实时性良好：由于采用了滑动窗口技术和多传感器融合策略，该提案能够快速处理大量数据，保证实时性。这对于自动驾驶车辆来说至关重要，因为它需要实时的定位信息来确保安全、有效的自动驾驶。
本文档来自技高网...【技术保护点】
1.一种基于滑动窗口的融合定位方法，其特征在于，包括以下步骤：
2.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤1中，传感器数据包括：IMU的加速度和角速度数据、轮速计的左右轮速度数据、GNSS数据和激光雷达点云数据。
3.如权利要求2所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤2中，使用GNSS、轮速计和IMU数据对系统状态中的全局位姿进行初始化。
4.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤4中，当相对位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，根据相对位姿观测模型，对滑窗内对应时刻的状态和协方差进行更新。
5.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤4中，当全局位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，根据全局位姿观测模型，对滑窗内对应时刻的状态和协方差进行更新。
6.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤5中，若滑窗内大小超过预设值，根据时间戳对最早
7.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，IMU数据处理：当IMU数据进入缓存队列时，将其取出并处理；将当前的角速度和线加速度状态量转换到IMU坐标系下；与当前IMU的测量值进行比较，得到关于位姿、速度、加速度状态量的残差和雅可比矩阵；更新车辆的状态量和协方差。
8.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，轮速计数据处理：当轮速计数据进入缓存队列时，将其取出并处理；使用双轮差速模型和已知的线速度、角速度状态量来推算出左右轮的速度；将这些速度与测量值进行比较，计算残差并计算相对于角速度、线速度和轮径尺度的雅可比；更新车辆的当前状态量和协方差。
9.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，点云数据处理：当点云数据进入缓存队列时，将其取出并处理；对点云数据进行去畸变处理，消除畸变效应，提高测量精度；将处理后的点云送入LO模块以计算出当前帧的位姿；计算当前帧与滑窗内每一帧的相对位姿变换的测量值、残差和雅可比矩阵；根据相对位姿测量模型，更新滑窗内相应的状态量和协方差矩阵。
10.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，GNSS数据处理：当GNSS数据进入缓存队列时，将其取出并处理；解算出ENU坐标系下的全局位姿；根据全局位姿观测模型，更新车辆的状态和协方差。
...【技术特征摘要】
1.一种基于滑动窗口的融合定位方法，其特征在于，包括以下步骤：
2.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤1中，传感器数据包括：imu的加速度和角速度数据、轮速计的左右轮速度数据、gnss数据和激光雷达点云数据。
3.如权利要求2所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤2中，使用gnss、轮速计和imu数据对系统状态中的全局位姿进行初始化。
4.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤4中，当相对位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，根据相对位姿观测模型，对滑窗内对应时刻的状态和协方差进行更新。
5.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤4中，当全局位姿观测数据的缓存队列有新数据时，循环取出队首数据作为当前处理的数据，根据全局位姿观测模型，对滑窗内对应时刻的状态和协方差进行更新。
6.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，所述步骤5中，若滑窗内大小超过预设值，根据时间戳对最早状态进行边缘化处理。
7.如权利要求1所述的基于滑动窗口的融合定位方法，其特征在于，imu数据处...
【专利技术属性】
技术研发人员：潘亮亮，赵季，张子潮，
申请(专利权)人：北京辉羲智能科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。