(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211006263.4 (22)申请日 2022.08.22 (71)申请人 南京邮电大 学 地址 210023 江苏省南京市栖霞区文苑路9 号 (72)发明人 谭彩铭　 (74)专利代理 机构 南京经纬专利商标代理有限 公司 32200 专利代理师 田凌涛 (51)Int.Cl. G01C 21/16(2006.01) G01C 21/18(2006.01) G01C 21/20(2006.01) G01C 25/00(2006.01) G01D 21/02(2006.01) (54)发明名称 一种基于车轮安装惯性测量单元的车辆定 位方法 (57)摘要 本发明公开了一种基于车轮安装惯性测量 单元的车辆定位方法， 将IMU 安装在车辆 车轮上， 基于已知IMU安装参数， 通过车辆、 车轮运动学约 束构建状态方程和观测方程， 利用卡尔曼滤波 器， 将车轮转角、 车轮旋转角速度和车身姿态作 为状态量进行估计， 进而获得车辆位姿与速度。 同时提供了 车轮IMU安装参数的估计方法， 首先， 在车轮‑IMU对准阶段， 获得IMU相对于车轮的初 步安装姿态； 然后， 将安装参数包含在卡尔曼滤 波器状态量中进行估计， 获得车轮IMU安装参数、 车辆位姿、 速度。 本发明提高了IMU相对于车轮的 安装参数估计精度， 并进一步提高了基于车轮安 装IMU的车辆 定位精度。 权利要求书5页 说明书14页 附图4页 CN 115451949 A 2022.12.09 CN 115451949 A 1.一种基于车轮安装惯性测量单元的车辆定位方法， 其特征在于： 基于安装在车辆车 轮预设位置的惯性测量单元， 针对惯性测量单元安装参数未知情况下， 执行步骤A ‑B， 获得 惯性测量单元安装参数， 以及车辆实时位置、 姿态和速度； 针对惯性测量单元安装参数已知 情况下， 执行步骤C， 获得 车辆实时位置、 姿态和速度： 步骤A： 基于惯性测量单元的输出数据， 采用预设姿态解算算法获得惯性测量单元相对 于车轮的初步 安装姿态 矩阵； 步骤B： 基于惯性测量单元相对于车轮的初步安装姿态矩阵， 将预设各类型车轮参数、 预设各类型车身姿态参数和预设惯性测量单元安装相关参数包含在状态量中， 将惯性测量 单元的输出数据以及车身零水平角包含在观测 量中， 结合卡尔曼滤波器， 获得惯性测量单 元安装参数， 以及车辆实时位置、 姿态和速度； 步骤C： 基于已知的惯性测量单元安装参数， 将预设各类型车轮参数、 预设各类型车身 姿态参数包含在状态量中， 将惯性测量单元的输出数据以及车身零水平角包含在观测量 中， 结合卡尔曼滤波器， 获得 车辆实时位置、 姿态、 速度。 2.根据权利要求1所述一种基于车轮安装惯性测量单元的车辆定位方法， 其特征在于： 所述步骤A中， 具体执 行以下步骤， 获得惯性测量单 元相对于车轮的初步 安装姿态 矩阵： 步骤A1： 采集车辆停止状态下预设时间段内惯性测量单元中三轴加速度计的输出数 据， 并对预设时间段内三轴加速度计的输出数据取平均值， 记为 采集车辆停止状态下预 设时间段内惯性测量单元中三轴陀螺仪的输出数据， 并对预设时间段内三轴陀螺仪的输出 数据取平均值， 记为陀螺零偏； 步骤A2： 采集车辆行驶状态下预设时间段内三轴陀螺仪的输出数据， 并将三轴陀螺仪 输出数据减去陀螺零偏， 进 而获得预设时间段内三轴陀螺 仪的输出 数据平均值， 记为 步骤A3： 基于步骤A1和步骤A2得到的平均值数据， 通过以下公式所示的姿态解算算法 获得惯性测量单 元相对于车轮的初步 安装姿态 矩阵 其中， 其中， 表示b系到h系的姿态变换矩阵， 即惯性测量单元相对于车轮 的初步安装姿态 矩阵； h系 为沿其z轴 根据右手定则转动β后与w系重合的坐标系， h系与w系的原点重合， h系 初始时刻其y轴为沿竖直方向指向上方； w系表示车轮坐标系， 其原点位于车轮中心， 其x轴 垂直于车轮转轴， 且由车轮中心指向惯性测量单元中心， z轴沿车轮转轴方向指向车身左 侧， y轴由右手坐标系规则确 定； h系和w系均相 对于车轮凝固； 惯性测量单元坐标系 为b系， 三个轴向分别沿着惯性测量单元 的三个敏感方向， 其原点位于惯性测量单元中心， 即三轴权　利　要　求　书 1/5 页 2 CN 115451949 A 2加速度计敏感中心； i系为惯性坐标系； 表示原坐标系 沿着该系j轴根据右手定则转动 所得坐标系到原坐标系的姿态变换矩阵， j表示x、 y或z轴； θh表示惯性测量单元在h系下的俯仰角， γh表示惯性测量单元在h系下的横滚角， ψh表示惯 性测量单 元在h系下的方位角。 3.根据权利要求2所述一种基于车轮安装惯性测量单元的车辆定位方法， 其特征在于： 所述步骤B中， 具体执行以下步骤， 获得惯性测量单元安装参数， 以及车辆实时位置、 姿态、 速度； 步骤B1： 针对包含预设各类型车轮参数、 预设各类型车身姿态参数和预设惯性测量单 元安装相关参数的状态量， 即状态量 构建状态方程； 针 对 包 含 惯 性 测 量 单 元 输 出 数 据 和 车 身 零 水 平 角 的 观 测 量 ，即 观 测 量 构建观测方程； 设定初值； 其中， α 为车轮转角， 为车轮角速度， 为车轮角加 速度； r为惯性测量单元中心与车轮 转轴之间的距 离， 即偏心距； γ、 θ和 ψ分别为车辆横滚角、 俯仰角和方位角； 为实测的惯性 测量单元 中三轴加速度 计的输出数据； 为实测的惯性测量单元 中三轴陀螺仪输出角速 度在h系下的投影中z轴的输出 数据； 步骤B2： 基于构 建的状态方程与观测方程， 利用卡尔曼滤波器， 迭代执行步骤B2.1至步 骤B2.4， 直到车辆定位结束迭代结束， 进而获得惯性测量单元安装参数和车辆实时位置、 姿 态、 速度： 步骤B2.1： 基于预设采样周期， 获得采样时刻tk惯性测量单元的输出数据， 其中惯性测 量单元的三轴陀螺 仪输出数据减去陀螺零偏； 步骤B2.2： 进行卡尔曼滤波时间更新， 具体为： 基于采样时刻tk‑1的状态量估计值， 通过 状态方程， 获得采样时刻tk的状态量预测值； 并基于采样时刻tk‑1的状态误差协方差阵估计 值以及状态方程获得采样时刻tk的状态误差协方差阵预测值； 步骤B2.3： 进行卡尔曼滤波测量更新， 具体为： 基于采样时刻tk的状态误差协方差阵预 测值、 观测噪声方差阵以及观测方程， 获得采样时刻tk的增益矩阵； 基于增益矩阵、 采样时 刻tk的状态量预测值、 采样时刻tk观测量数据以及观测方程， 获得采样时刻tk的状态量估计 值； 并且基于增益矩阵、 采样时刻tk的状态误差协方差阵预测值、 观测噪声方差阵以及观测 方程， 获得采样时刻tk的状态误差协方差阵估计值； 步骤B2.4： 基于采样时刻tk的状态量估计值， 获得采样时刻tk的车辆姿态 车辆位置 偏心距r与惯性测量单元相对于车 轮的安装姿态矩阵 其中R为车轮半径； 返回步骤B2.1， 进入下一采样时刻 迭代。 4.根据权利要求3所述一种基于车轮安装惯性测量单元的车辆定位方法， 其特征在于： 所述构建的状态方程、 观测方程如下 所示：权　利　要　求　书 2/5 页 3 CN 115451949 A 3