(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211343799.5 (22)申请日 2022.10.31 (71)申请人 北方工业大 学 地址 100144 北京市石景山区晋元庄路5号 (72)发明人 王庞伟　刘程　俞宏胜　张名芳　 叶荣盛　 (74)专利代理 机构 北京德崇智捷知识产权代理 有限公司 1 1467 专利代理师 房婉琼 (51)Int.Cl. G01C 21/30(2006.01) G01S 17/86(2020.01) G01S 19/48(2010.01) G06F 17/16(2006.01) G08G 1/01(2006.01) (54)发明名称 基于多传感器融合的智能网联汽车复合定 位方法及装置 (57)摘要 本发明提供了一种基于多传感器融合的智 能网联汽车复合定位方法及装置。 本发明利用车 联网下V2X通信的优势， 结合地平线AI芯片设计 了智能网联汽车复合定位方法及装置。 其中， 搭 载V2X通信单元的路侧定位平台可代替差分基准 站， 通过V2X同通信的方式完成修正量的发送至 车载平台。 同时， 本发明提出了基于无迹卡尔曼 滤波算法的多层融合方法， 能够在路侧端准确定 位智能网联汽 车。 通过与高精地图相匹配得到精 准的智能网联汽车定位偏差并通过路侧单元将 经纬度偏差量发送至车载单元， 修正车载GNSS定 位结果， 达到厘米级的定位精度， 提高了智 能网 联汽车复合定位系统的可靠性。 权利要求书5页 说明书14页 附图1页 CN 115540882 A 2022.12.30 CN 115540882 A 1.一种基于多传感器融合的智能网联汽车复合定位方法， 其特征在于， 该方法包括如 下步骤： 步骤1： 智能网联汽车状态定义 激光雷达与摄像机分别为单个局部滤波器， 定义每个局部滤波器定位智能网联汽车的 状态如下： X(t)＝[ δx, δ y, δvx, δvy]T                      (1) 其中， δx, δy为智能网联汽车横纵向位置偏差； δvx, δvy为智能网联汽车横纵向速度偏 差， 其标准状态方程如下： 其中， f(·)是具有离散形式的非线性状态转移函数； w(k)为零均值高斯白噪声， 假设 其协方差为Q(k)； B(t)u(t)为局部滤波器的控制输入， 为获取智能网联汽车的位置与速度 的观测值， 构建激光雷达与摄 像机的量测模型， 如下： 其中， ZC(k)表示摄像机的量测 模式， ZL(k)表示激光雷达的量测 模式； xm、 ym、 vxm与vym分 别表示由高精地图获取的智能网联汽车位置与 速度数据； xCamera、 yCamera、 vxCamera与vyCamera分 别表示由摄像机获取的智能网联汽车位置与速度数据； xLidar、 yLidar、 vxLidar与vyLidar分别表 示由激光雷达获取的智能网联汽车位置与速度数据； 步骤2： 基于无迹卡尔曼 滤波的智能网联汽车定位数据融合 初始化激光雷达与摄 像机代表的局部滤波器Xs， 如公式(4)所示。 其中， 表示局部滤波器Xs的初始均值； Ps(0)表示局部滤波器Xs的初始方差； 分别获取激光雷达与摄像机定位智能网联汽车的结果作为观测值， 通过无迹变换得到 k‑1时刻2n+1个σ 点的状态， 如公式(5)所示。 定义无迹变换采用的参数如公式(6)所示。 其中， 分别表示第0个和第s个σ 点的状态； α是正极小量； 是 通过矩阵Ps(k‑1)的下三角分解获得的平方根的第j列， 其向量维数为 n。权　利　要　求　书 1/5 页 2 CN 115540882 A 2在k时刻计算下一 步复合定位方法的预测结果， 如公式(7)所示。 通过复合定位方法预测的结果更新激光雷达与摄像机传感器的量测结果， 如公式(8) 所示。 更新方程中权 重值 定义如公式(9)所示。 其中， Hs(k)表示测量矩阵； 表示局部滤波器Xs第k次观测的中间变量； 表示局部滤波器Xs第k次观测结果； 表示局部滤波器Xs第k次观测 结果的协方差； Rs(k)表示系统噪声； 最后计算各局部滤波器的最优状态估计 和协方差矩阵 并将每个局部滤波 器的结果输入至补偿融合层， 如公式(10)所示。 其中， Ks(k)表示卡尔曼滤波增益； 步骤3： 智能网联汽车定位数据融合补偿 智能网联汽车安装的GNSS接收机实时获取经纬度数据， 并分别计算激光雷达、 摄像机 传感器与接收机的相对定位误差平均值， 将高精度地图下智能网联汽车的位置数据Pm＝ [xm,ym,vxm,vym]作为局部滤波器的参考， 分别与激光雷达与摄像机的观测结果做差， 获取路 侧定位平台 中单传感器定位智能网联汽车的偏差量， 如公式(1 1)所示。权　利　要　求　书 2/5 页 3 CN 115540882 A 3