(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210744154.6 (22)申请日 2022.06.28 (71)申请人 中山大学 地址 510275 广东省广州市海珠区新港西 路135号 (72)发明人 熊会元　赖凯煌　朱桃宏　 (74)专利代理 机构 广州粤高专利商标代理有限 公司 44102 专利代理师 黄志铖 (51)Int.Cl. G01S 17/89(2020.01) G01S 17/86(2020.01) G06T 7/73(2017.01) G06V 10/762(2022.01) (54)发明名称 一种基于人工信标的无人车辆激光雷达建 图与定位方法 (57)摘要 本发明涉及一种基于人工信标的无人车辆 激光雷达建图与定位方法， 包括如下步骤： 步骤 一： 制作和布置信标； 步骤二： 无人车辆通过激光 雷达获取信标的位置和编号信息； 步骤三： 结合 车载里程计信息和信标位置和编号信息构建位 姿图； 步骤四： 对位姿 图进行优化得到车辆定位 轨迹和信标地图。 通过位姿图优化对 车载里程计 信息和信标观测结果进行融合定位， 大幅修正里 程计的定位误差， 可实现无人车辆在隧道等卫星 信号弱、 环境特 征少的场景中的精准定位。 权利要求书3页 说明书8页 附图2页 CN 115015959 A 2022.09.06 CN 115015959 A 1.一种基于人工信标的无人车辆激光雷达建图与定位方法， 其特征在于， 包括如下步 骤： 步骤一： 制作和布置信标； 步骤二： 无 人车辆通过激光雷达获取信标的位置和编号信息； 步骤三： 结合车 载里程计信息和信标位置和编号信息构建位姿图； 步骤四： 对位姿图进行优化得到车辆 定位轨迹和信标地图。 2.根据权利要求1所述的一种基于人工信标的无人车辆激光雷达建图与定位方法， 其 特征在于， 信标为根据编 码长度l、 编 码最小汉明距离h和 编码复杂度c生 成的二进制编码图 案； 编码长度l对应编 码图案部 分的方块个数， 编码图案由d ×d个黑白方块组成， 所以l必须 为完全平方数， 每个方块代表一个码字， 黑色方块代表0， 白色方块代表1， 根据编码按照从 上到下、 从左到右的顺序生成编码图案 。 3.根据权利要求2所述的一种基于人工信标的无人车辆激光雷达建图与定位方法， 其 特征在于， 在步骤二中， 激光雷达获取信标的原始 点云数据， 通过信标感知算法对激光雷达 获取的原 始点云数据进行处 理， 实时获取环境中信标的位置和编号信息 。 4.根据权利要求3所述的一种基于人工信标的无人车辆激光雷达建图与定位方法， 其 特征在于， 通过信标感知算法对激光雷达获取的原 始点云数据进行处 理的具体流 程如下： S2.1： 对原 始点云进行 特征点提取， 对点云中的每 个点计算距离梯度： 其中， pi,m为第m条激光线束中的第 i个点， pi‑1,m和pi+1,m分别为pi,m在水平方向上的左右 邻域点； 若某个点的距离梯度大于设定的阈值， 则认为该点为边缘点； 若某边缘点的左右领域 点均不是边 缘点， 则认为该点 为特征点； S2.2： 通过层次聚类算法对特征点进行聚类， 将每个特征点作为一个类簇， 然后不断找 到距离最近的两个类簇并合并， 直到任意两个类簇之间的距离大于 设定的阈值ζ， 对每个类 簇， 分别从类簇内点数、 强度边界点个数和平面度三个条件进 行验证， 若 该类簇同时符合上 述的三个验证条件， 则认为该类簇中的点 为信标点云； S2.3:从原始点云中提取出信标点云后， 通过位姿估计算法对信标相对于激光雷达的 位置和姿态进行求 解， 得到信标的位姿矩阵 S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处， 对信标点云进行分 割等分为(d+4)2个方块； 根据白色物体的激光反射强度比黑色物体高的特性， 对编码部分 的第i个方块计算反射强度均值 将其与整个信标点云的反射强度均值 对比， 若 则 认为该方块对应的码字为1， 否则为0； 把每个方块对应的码字依次串联起来即得到该信标 点云的编码值， 计算该编码值与编码库中的所有编码之间的汉明距离， 若汉明距离最小值 不大于设定的阈值， 则将该编码对应的编号作为编码信息 。 5.根据权利要求4所述的一种基于人工信标的无人车辆激光雷达建图与定位方法， 其 特征在于， 在步骤S2.2中， 类簇内点数、 强度边界点个数和平面度的验证方法具体为： 类簇内点数 ： 将属于该类簇内的原始点云填充回去 ， 类簇内点数N应满足权　利　要　求　书 1/3 页 2 CN 115015959 A 2其中， η＝4表示激光扫描到每个方块的最少点数， (d+4)2表示构 成信标图案的方块个数， M为扫描到信标的激光线束， D为信标与激光雷达之间的距离， θ＝ 0.1°为激光雷达的水平分辨 率； 强度边界点个数： 对类簇中的每 个点计算强度梯度 其中， 为第m条激光线束中的第i个点的强度值， 和 分别为pi,m在水平方 向上的左右邻域 点的强度值； 若某个点的强度梯度大于设定的阈值τ， 则认为该点为强度边界点， 强度边界点个数Q 应大于等于2(d+2)； 平面度： 对类簇内的所有点用随机采样一致算法进行平面拟合， 离群点个数应小于类 簇内点数的4％ ‑6％。 6.根据权利要求4所述的一种基于人工信标的无人车辆激光雷达建图与定位方法， 其 特征在于， 在步骤S2.3中， 得到位姿矩阵的具体流 程为： S2.3.1： 在激光雷达坐标系 原点处设置一个边长为s、 厚度 为∈＝0.01m的投影目标， 通 过刚体变换矩阵 将信标点云投影到投影目标处， 定义投影损失如下： 其中， 为信标点云， 和 分别为 中第i个点经过 投影后的坐标， c( ·)为损 失计算函数； S2.3.2： 定义损伤计算 函数c(·)为: 其中， λ ≥0 S2.3.3:通过非线性优化库NLopt对优化问题 进行求解， 得到 最优的刚体变换矩阵 使得投影损失最小， 对该矩阵求逆即得到信标的位姿矩阵 7.根据权利要求4所述的一种基于人工信标的无人车辆激光雷达建图与定位方法， 其 特征在于， 在步骤三中， 里程计信息记录的是两个时间步之间的车辆位姿变化， 记为ot； 设 定步骤二中车辆运行的步长为T， 则车辆的运动轨迹可以表 示为X＝{x0,…,xT}， 其中xt∈SE (3)， 表示车辆在第t个时间步的位姿， ot为里程计测得的xt与xt‑1之间的相对位姿； 信标设定有N个， 每个信标的编号各不相同， 记为L＝{l1,l2,…,lN}， 其中， 表示 编号为i的信标的三维坐标； 车辆在第t个时间步观测到Kt个信标， 信标相对于车辆的位置 记为 其中 信标的编号记为 其中权　利　要　求　书 2/3 页 3 CN 115015959 A 3