专利原文与附图
工程车辆位姿感知方法
A Pose Perception Method for Construction Vehicles
该专利读取RTK和IMU数据并进行均值滤波，根据RTK解状态和相邻位置变化选择直接输出或组合定位；在组合定位路径中，以IMU/轮速计模型预测位姿，并通过自适应扩展卡尔曼滤波输出横向位置、纵向位置和航向角。
专利摘要
本发明适用于车辆定位技术领域，公开了一种工程车辆位姿感知方法。针对RTK信号遮挡、测量突变和车辆振动对位姿数据的影响，该方法读取RTK和IMU数据并进行均值滤波，判断RTK的解状态和相邻位置变化；当RTK信息不满足设定条件时，以IMU/轮速计组合定位信息作为预测、RTK位姿信息作为观测，通过自适应扩展卡尔曼滤波计算车辆位姿。
- 工程车辆定位
- RTK
- IMU
- 轮速计
- 均值滤波
- 扩展卡尔曼滤波
- 自适应协方差
- 位姿感知
RTK遮挡、测量突变和车辆振动会影响工程车辆位姿数据
说明书指出，工程车辆作业速度和环境条件不同于乘用车，RTK信号可能因遮挡而中断，也可能在复杂环境中出现测量突变。
车辆作业振动会增加加速度和角速度数据中的噪声，因此位姿感知流程需要先处理传感器数据，再根据RTK信息状态选择输出路径。
S1—S2完成RTK、IMU读取和振动数据平滑
步骤S1通过串口通信读取RTK和IMU数据，并对IMU加速度计、陀螺仪的旋转、比例和偏移误差进行校准。
步骤S2采用移动平均滤波处理加速度和角速度数据；说明书以滤波前后曲线对比说明数据平滑变化。
图2对比滤波前后的数据曲线，保留时间轴、图例和原始纵轴刻度。
查看高清原图 ↗S3根据解状态和相邻位置变化选择直接输出或组合定位
步骤S3同时检查RTK状态以及当前、上一时刻横纵位置的变化，并以位置误差阈值作为判别条件。
满足条件时直接输出RTK测得数据；不满足条件时，将IMU/轮速计组合定位信息作为扩展卡尔曼滤波预测，将RTK位姿信息作为观测。
图1展示从RTK和IMU读取、均值滤波、RTK信息判别到IMU/轮速计状态方程及自适应扩展卡尔曼滤波的流程。
查看高清原图 ↗S4以轮速和IMU航向角速度推进平面位姿状态
系统状态向量由车辆横向位置、纵向位置和航向角组成；轮速编码器提供车辆速度，IMU提供航向角速度。
方法建立离散状态方程，并通过泰勒级数在预测状态附近线性化，得到状态雅可比矩阵和过程噪声矩阵。
S5以RTK位姿为观测完成扩展卡尔曼滤波更新
状态方程和过程噪声协方差用于计算先验误差协方差；RTK测得的横向位置、纵向位置和航向角构成观测数据。
随后依次计算卡尔曼增益、目标状态后验估计和误差协方差后验估计，得到车辆位姿信息。
残差和状态转移偏差用于更新观测噪声与过程噪声协方差
方法以实际测量值与状态估计之间的残差估计观测噪声协方差R，并通过遗忘因子保留历史估计。
测量值、预测值和状态转移函数之间的偏差用于估计过程噪声协方差Q，同样采用遗忘因子进行递推。
实施例分别对比位置轨迹和航向角变化
图3给出真实位置、IAEKF与AEKF的位置轨迹对比，并在局部区域放大三条轨迹的差异。
图4给出真实航向角、IAEKF与AEKF随时间变化的对比；说明书据此描述本发明方法的结果更接近真实值。下一时间步继续重复S1—S5。
图3对比真实、IAEKF和AEKF位置轨迹，并保留局部放大区域。
查看高清原图 ↗
图4对比真实、IAEKF和AEKF航向角随时间的变化。
查看高清原图 ↗第二层阅读
需要更快地把握方法链和核心机制？
AI简报不会替代上方的专利内容导读；它只在您主动打开时加载。引用专利
以下格式根据专利文献标识生成；使用前请按目标期刊或机构规范复核。
王永，姚宗伟，毕秋实，李学飞，刘昕晖，潘宝智. 工程车辆位姿感知方法: CN 119555062 A[P]. 2025-03-04.
王永，姚宗伟，毕秋实，李学飞，刘昕晖，潘宝智. (2025). 工程车辆位姿感知方法 (CN 119555062 A). China National Intellectual Property Administration.
[1] 王永，姚宗伟，毕秋实，李学飞，刘昕晖，潘宝智, “工程车辆位姿感知方法,” 中国专利 CN 119555062 A, 2025-03-04.