(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211077055.3 (22)申请日 2022.09.05 (71)申请人 广东工业大 学 地址 510062 广东省广州市越秀区东 风东 路729号 (72)发明人 孟伟　邹志键　陈浩冬　臧家瑶　 杨远林　 (74)专利代理 机构 广东广信君达律师事务所 44329 专利代理师 廖朗皓 (51)Int.Cl. G01C 21/00(2006.01) G01S 17/86(2020.01) G06T 17/00(2006.01) (54)发明名称 一种激光雷达与加速度传感器结合的无人 车建图方法 (57)摘要 本发明公开了一种激光雷达与加速度传感 器结合的无人车建图方法， 通过在无人车上安装 激光雷达和加速度传感器， 采集周围环境信息与 无人车自身位姿信息； 将无人车的位姿信息转换 到世界坐标系下， 通过无人车位姿的离散信息构 建里程计数学模型； 对里程计模型进行粒子滤 波， 将预测估计位姿与测量得到的位姿进行计 算， 求出噪声较小的无人车位姿， 基于此生成的 栅格地图； 在生成的栅格地图下插入帧数点云数 据， 使用四叉树搜索算法找到局部最优匹配帧， 再将得到的局部最优匹配帧进行一次点云对地 图匹配得到更准确的全局匹配帧， 利用这些全局 匹配帧构建更精确的栅格地图。 本方法可增强对 长廊环境以及相似环境的建图效果， 进而提高建 图的精度。 权利要求书3页 说明书7页 附图3页 CN 115540850 A 2022.12.30 CN 115540850 A 1.一种基于加速度传感器与激光雷达融合构建地图的方法， 其特征在于， 包括以下步 骤： 步骤1， 首先在无人车的正前方安装加速度传感器， 用于测量无人车的位姿； 激光雷达 安装在无 人车的顶部， 用于获取 雷达点云数据； 将无人车放在待建图的道路上， 启动无人车， 无人车在行进的过程中， 激光雷达与加速 度传感器实时获取点云数据以及速度数据； 步骤2， 根据采集到的速度数据， 计算得到无人车在激光雷达坐标系下的位姿信息， 然 后利用坐标变换矩阵将以激光雷达坐标系下的位姿信息转换到世界坐标系下； 在世界坐标 系下， 建立里程计模型： 步骤2.1， 加速度传感器采集到的当前时刻i下无人车的速度数据根据下式计算在激光 雷达坐标系下的无 人车的位姿： Sx＝Sx(i‑1)+Vx*Δt+ax*Δt2 上式中， Vx表示无人车投影到激光雷达坐标系x方向上行驶 的速度， ax表示无人车投影 到x方向上行驶的加速度， Δt表示测出Vx和ax的瞬时时间间隔， Sx(i‑1)表示i‑1时刻下投影到 x轴的坐标， Sx表示i时刻下投影到x轴的坐标； i时刻下投影到y轴的坐标为Sy， 则有 从而得到 激光雷达坐标系下i时刻无 人车的位姿(Sx,Sy,Sθ)； 将得到的Sx,Sy,Sθ通过坐标变换从激光雷达坐标系下转 化到世界坐标系下： 无人车在雷达坐标系下的位姿S以(Sx,Sy,Sθ)表示， 那么它在世界坐标系的位姿为U＝ (Ux,Uy,Uθ)T： 矩阵TU为正交旋转矩阵 其中 表示无人车的姿态角度， Ux表示无人 车在世界坐 标系下的横坐 标， Uy表示无人车在世界坐 标系下的纵坐 标， Uθ表示无人车在世界 坐标系下与横坐标的夹角； 步骤2.2， 采样周期内无 人车运动的路程长度Δs和旋转过的角度Δθ 可以表示 为： 式子中Δsr为无人车的右动力轮在采样间隔Δt时刻下走过的路程， Δsl为左动力轮在 采样间隔Δt时刻下走过的路程； 因此根据递推可以得到无人车从k时刻到k+1时刻估计位 姿数学表达式， 即里程计模型如下： 其中Zx(k+1),Zy(k+1),Zθ(k+1)表示估计k+1时刻下无人车在世界坐标系下的横坐标、 纵坐标 以及与横坐标之间的夹角； Ux(k),Uy(k),Uθ(k)表示k时刻无人车在世界坐标系下的位姿； 步骤3， 对里程计模型进行 粒子滤波处 理， 具体步骤为以下几步：权　利　要　求　书 1/3 页 2 CN 115540850 A 2步骤3.1， 通过式1可得到无人车在k时刻世界坐标系下的位姿 估计k时刻下 无人车的估计位姿Zk， 根据式2和式3， Zk可以表示 为 步骤3.2， 根据下面式子， 利用粒子滤波预测环境信息变化时， 在k时刻无人车估计位姿 的条件下， 得到无 人车测量位姿的概 率p(Uk|Zk)： 其中 表示无人车在k时刻抽取的第i个粒子， 一共抽取N个粒子， 狄拉克函数用σ(.)表 示， 代表了归一 化粒子的重要权值， 取 步骤3.3， 根据步骤3.2得到的概率p(Uk|Zk)与步骤3.1得到的Uk和Zk， 通过以下公式计算 得到滤波后k时刻的无 人车位姿态Pk： Pk＝A*Uk+B*(Zk‑p(Uk|Zk)*Uk)                   (5) 上式中， A表示测量值的超参数， B表示估计值与测量概率处理过的测量值作差的超参 数； 相比Uk， 滤波处理后的Pk更接近无人车的实际位姿； Pk可以表示 为 步骤3.4， 重复步骤3.1， 3.2和步骤3.3， 生成各个时刻下滤波后的无人车位姿集合， 这 些位姿的集 合按运动的时间进行 连接构建出栅格地图； 步骤4， 构建全局地图 步骤4.1， 在生成的栅格地图插入设定好帧数的点云数据， 点云数据来自于步骤1雷达 采集的点云数据， 采用四叉树搜索算法， 以深度优先为原则， 对栅格地图进行简化， 通过引 入无人车位姿方向上激光扫描点所对应栅格点的占有概率值Mnearest， 降低搜索算法的时间 复杂度； Mnearest计算方式如式7， 上述过程的数 学表示如下： 式6中W代表搜索空间也即生成的栅格地图， Mnearest表示无人车位姿方 向上激光扫描点 所对应栅格点的占有概率值， ξ表示栅格， Tξ表示栅格的一个点云坐标， 其中下标ξ∈W表示 遍历栅格地图的所有栅格； K表示构建栅格 地图的总时刻， hk表示k时刻下Tξ所表示点云坐标 在栅格地图的权 重值， 参数a,b∈(0,1)， ξ*表示栅格地图搜索的局部最优匹配帧； 步骤4.2， 在使用四叉树搜索算法找到局部最优 匹配帧之后， 在得到的局部最优 匹配帧 插入步骤1激光雷达获取到的点云数据， 将点云数据与局部最优匹配 帧按采集时间顺序进权　利　要　求　书 2/3 页 3 CN 115540850 A 3