CN112950781A - 特种场景的多传感器动态加权融合的点云地图构建方法 - Google Patents

特种场景的多传感器动态加权融合的点云地图构建方法 Download PDF

Info

Publication number
CN112950781A
CN112950781A CN202110296526.9A CN202110296526A CN112950781A CN 112950781 A CN112950781 A CN 112950781A CN 202110296526 A CN202110296526 A CN 202110296526A CN 112950781 A CN112950781 A CN 112950781A
Authority
CN
China
Prior art keywords
carrier
key frame
satellite positioning
pose
data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110296526.9A
Other languages
English (en)
Other versions
CN112950781B (zh
Inventor
熊会元
钱沛聪
马健
潘跃龙
刘羽
李同同
尹文成
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Nuclear Power Engineering Co Ltd
Sun Yat Sen University
Original Assignee
China Nuclear Power Engineering Co Ltd
Sun Yat Sen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Nuclear Power Engineering Co Ltd, Sun Yat Sen University filed Critical China Nuclear Power Engineering Co Ltd
Priority to CN202110296526.9A priority Critical patent/CN112950781B/zh
Publication of CN112950781A publication Critical patent/CN112950781A/zh
Application granted granted Critical
Publication of CN112950781B publication Critical patent/CN112950781B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Graphics (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Remote Sensing (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明提出了一种对最大后验概率目标函数中各传感器约束项进行动态加权以提高轨迹精度的方法，用于构建包含室外环境、室内环境和信号屏蔽区域的特种场景点云地图。在GNSS覆盖下，以载***置与卫星定位的欧氏距离最小化为目标构造卫星定位约束，并根据激光里程计与GNSS精度因子调整其权重来减小卫星定位误差对点云地图的影响；在GNSS盲区下，以载体姿态与惯性测量数据构成的预期重力方向与实际重力方向的夹角最小化为目标构造姿态约束，并根据载体加速度大小调整其权重，解决由于缺乏全局位置观测导致点云地图高程累积误差大的问题。以激光里程计为基础，使用位姿图优化方法整合所述约束并求解载***姿，拼接关键帧点云以准确生成所述特种场景点云地图。

Description

特种场景的多传感器动态加权融合的点云地图构建方法
技术领域
本发明涉及点云地图技术领域，具体涉及一种特种场景的多传感器动态加权融合的点云地图构建方法。
背景技术
精确的点云地图是无人***实现自主导航、执行高级别任务的前提，目前融合多传感器信息进行点云地图构建方法主要分为室外建图和室内建图两类。室外环境通过以GNSS(Global Navigation Satellite System，全球导航卫星***)定位信息作为载体的位置约束，使用普通最小二乘方法对GNSS定位和激光扫描帧间匹配得到的位置进行融合的方法，由于不同时空下的GNSS误差大小不一致，使用普通最小二乘法进行求解相当于将不同时空下的GNSS误差无差别引入***中，最终导致载***姿求解不准确、按载***姿拼接得到的点云地图局部细节不够精细化的问题。室内环境通过普通最小二乘融合UWB(UltraWide Band，超带宽定位)和激光雷达的建图方法，但在信号屏蔽的特种场景中UWB设备失效，在缺乏全局位置观测并且没有其他约束的条件下，将由于激光扫描帧间匹配误差累积导致无法建立准确的点云地图。另外，使用UWB还需要事先布设UWB基站，对于范围较大的场景，基站布设成本非常高。再者，UWB信号容易受到干扰，实用性较低。
针对信号屏蔽或室内环境的特种场景，现有的融合IMU(Inertial MeasurementUnit，惯性测量单元)和激光雷达的建图方法，通常仅使用IMU解算得到的载***姿信息作为激光扫描匹配的初始值或者使用IMU预积分对相邻时刻的载***姿进行约束的方法，没有充分利用IMU测量信息与重力方向之间的关系来对载体状态建立全局坐标系下的约束，最终导致以载***姿拼接而成的点云地图的累积误差无法被有效消除。
公开日为2020.07.10，公开号为：CN111402414A的中国发明专利：一种点云地图构建方法、装置、设备和存储介质，试图实现基于点云的只有不可运动物体的建图效果，避免动态物体导致的拖尾现象。但是该方案并未解决前述的技术问题。
发明内容
针对现有技术的局限，本发明提出一种特种场景的多传感器动态加权融合的点云地图构建方法，本发明采用的技术方案是：
一种特种场景的多传感器动态加权融合的点云地图构建方法，其通过设有激光雷达、卫星定位设备以及惯性测量单元的载体在预定的室内外环境中进行数据采集和激光扫描，根据激光扫描得到的点云构建所述载体移动轨迹范围内的点云地图；包括以下步骤：
S1，所述载体从室外启动，将起点处的载***姿以固定顶点的方式加入位姿图中，并以起点为坐标原点，以“东向-北向-天向”为基准方向，构建笛卡尔直角坐标系作为全局坐标系，后续卫星定位数据均转换到所述全局坐标系下；
S2，在所述载体运动距离大于设定阈值时，将激光雷达的扫描结果记为关键帧，将该关键帧对应的载***姿作为待求解顶点加入位姿图；根据激光雷达的帧间扫描配准结果来构造激光里程约束，并根据点云配准重合度来调整所述激光里程约束的动态权重；当载***于室外环境时，根据卫星定位结果与所述载***置的欧氏距离来构造卫星定位约束，并根据水平精度因子与激光里程信息来调整所述卫星定位约束的动态权重；当所述载***于室内环境时，根据惯性测量单元的数据并结合载体姿态计算预期重力方向与实际重力方向的夹角来构造姿态约束，并根据所述载体处于静止或匀速状态的置信度来调整所述姿态约束的动态权重；当所述载体从室内返回室外环境时，利用卫星定位结果对历史关键帧进行检索得到闭环候选关键帧，利用当前关键帧与以闭环候选关键帧为中心的局部子地图进行点云配准，根据配准结果来构造激光闭环约束，并根据点云配准重合度来调整所述激光闭环约束的动态权重；
S3，根据包括所述激光里程约束、卫星定位约束、姿态约束、激光闭环约束在内的约束项以及各约束项对应的动态权重，对各项约束相关的变量顶点进行连接，构建位姿图，对位姿图中的所有约束项进行求和得到关于载体轨迹的最大后验概率目标函数，使用非线性优化方法进行求解即对所述位姿图进行求解；
S4，根据所述位姿图的求解结果对所述历史关键帧点云进行拼接，生成点云地图。
相较于现有技术，本发明在GNSS覆盖下以最小化载***置与卫星定位之间的欧氏距离为目标构造卫星定位约束，并根据激光里程计与GNSS水平精度因子调整其权重来减小卫星定位误差对点云地图的影响；在GNSS盲区下以最小化载体姿态与惯性测量数据构成的预期重力方向与实际重力方向的夹角为目标构造姿态约束，并根据载体处于静止或匀速状态的置信水平调整其权重，解决由于缺乏全局位置观测导致点云地图高程累积误差大的问题。在激光里程计基础上，以位姿图方式对所述卫星定位约束、姿态约束以及激光里程约束和闭环约束进行整合，得到关于载体轨迹的最大后验概率目标函数。求解所述目标函数得到的载***姿对激光雷达关键帧点云拼接，得到累积误差小、与地理坐标系一致、局部细节清晰的室内外点云地图。本发明可用于包含室外环境、室内环境和信号屏蔽区域的特种场景的点云地图构建，并且在基于动态权重的卫星定位数据和惯性测量数据所构建的卫星定位约束和姿态约束条件下，实现了室内外点云地图与地理坐标系的对齐，有利于后续的融合定位方法实施。
作为一种优选方案，所述步骤S1中包括以下步骤：
S11，在室外环境中选择定位信号良好的位置作为所述载体的起点S0，运用所述卫星定位设备获取所述起点的经度λ0、纬度φ0、高度h0以及航向角ψ0，并将起点S0处的载***姿T0以固定顶点的形式加入到位姿图中，记起点处激光雷达扫描为关键帧V0；
S12，根据所述经度、纬度、高度以及航向角，以所述起点为原点，以“东-北-天”为基准方向，建立一个笛卡尔直角坐标系作为全局坐标系：先将所述起点的经度λ0、纬度φ0、高度h0转换到ECEF坐标系，得到起点S0所对应的ECEF坐标系坐标(ecefx0,ecefy0,ecefz0)，并计算经纬度所对应的旋转矩阵
Figure BDA0002984550230000031
Figure BDA0002984550230000032
S13，对于卫星定位设备后续输出的经度、纬度、高度信息(λi,φi,hi)，先将该坐标转换到ECEF坐标系得到(ecefxi,ecefyi,ecefzi)，再按下式计算得到对应的笛卡尔直角坐标系卫星定位坐标值：
Figure BDA0002984550230000033
进一步的，所述步骤S2在所述载体从所述起点出发到进入室内环境之前的过程中，循环以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S22，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi,i-1，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000041
进一步构造激光里程约束
Figure BDA0002984550230000042
以相对位姿变换ΔTi,i-1作为位姿增量，计算关键帧Vi位姿Ti的初始值，作为后续加入位姿图的顶点；
S23，以关键帧Vi对应的时间戳ti作为目标同步时间戳，对所述卫星定位数据进行插值，得到与关键帧Vi时间同步的卫星定位Pi及水平精度因子HDOPi，并根据激光里程计对插值同步后的卫星定位结果Pi进行置信度评估得到置信度估计bel(Pi)，基于所述的卫星定位Pi、水平精度因子HDOPi、置信度估计bel(Pi)构造卫星定位约束
Figure BDA0002984550230000043
其中，所述步骤S22包括以下步骤：
S221，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi,i-1；
S222，计算配准后的两帧点云之间的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000044
根据所述的载体相对位姿变换ΔTi,i-1构造激光里程约束
Figure BDA0002984550230000045
Figure BDA0002984550230000046
式中，L(·)函数表示把由旋转和平移组成的位姿数据T＝{R,t}映射到SE(3)流形空间的切空间中，得到对应的李代数
Figure BDA00029845502300000410
Figure BDA0002984550230000047
表示该激光里程约束的权重，由配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000048
表示：
Figure BDA0002984550230000049
S223，利用相对位姿变换ΔTi,i-1作为位姿增量，计算关键帧Vi的位姿Ti的初始值(Ti-1ΔTi,i-1)，并将位姿Ti作为后续加入到位姿图中与关键帧Vi-1对应的顶点相连接的顶点；
其中，所述步骤S23包括以下步骤：
S231，对所述卫星定位数据与关键帧Vi根据时间戳进行插值同步，过程如下：
取关键帧Vi对应的时间戳ti记为目标同步时间戳；检查卫星定位数据队列起始数据时间戳是否等于关键帧Vi对应的时间戳ti，是则认为该卫星定位数据与激光雷达关键帧Vi时间同步，取该卫星定位数据作为与关键帧Vi时间戳同步的卫星定位Pi，否则，对卫星定位数据队列进行检查：
如果数据队列起始数据时间戳
Figure BDA0002984550230000051
则本次插值同步失败；
如果数据队列起始数据时间戳
Figure BDA0002984550230000052
则该帧卫星定位数据时间不满足插值要求，将该数据从队列中弹出，对卫星定位数据队列进行检查；
如果卫星定位数据队列中第0和第1帧数据的时间戳均<ti，则将第0个数据从队列中弹出，对卫星定位数据队列进行检查；
如果数据队列第1个数据的时间戳
Figure BDA0002984550230000053
表明该卫星定位数据时间上较关键帧Vi对应的时间戳ti晚，本次插值同步失败；
卫星定位数据队列检查完毕后，如果卫星定位数据队列中数据量≥2，则满足插值同步条件，取卫星定位数据队列中第0和第1帧数据的时间戳
Figure BDA0002984550230000054
Figure BDA0002984550230000055
分别计算插值比例因子sf,sb，并根据插值比例因子对步骤S13所述的笛卡尔直角坐标系下的卫星定位信息进行插值，得到与关键帧Vi时间戳同步的卫星定位Pi及水平精度因子HDOPi：
Figure BDA0002984550230000056
Figure BDA0002984550230000057
Figure BDA0002984550230000058
Figure BDA0002984550230000059
其中，
Figure BDA00029845502300000510
表示卫星定位数据队列中第i帧数据，
Figure BDA00029845502300000511
与
Figure BDA00029845502300000512
表示对应的水平精度因子；
S232，根据S22所述的激光里程对插值同步后的卫星定位结果Pi进行置信度评估，过程如下：
以所述步骤S22中通过对激光雷达关键帧Vi的点云与激光雷达关键帧Vi-1的点云进行配准得到两个关键帧之间对应的载体相对位姿变换(ΔTi,i-1)-1作为载体从ti-1到ti的位姿增量估计；
以ti-1时刻的载***姿Ti-1作为基准，利用上述位姿增量估计ΔTi,i-1可以得到ti时刻对应的卫星定位预测
Figure BDA00029845502300000513
Figure BDA00029845502300000514
其中，(·)[x,y,z]表示取位姿数据中的位置信息；
根据ti时刻对应的卫星定位预测
Figure BDA0002984550230000061
与实际从GNSS/RTK接收机获取并经过时间同步的卫星定位Pi之间的欧式距离，对Pi进行置信度评估：
Figure BDA0002984550230000062
S233，根据经过插值同步后的卫星定位Pi，可构造关于关键帧Vi对应载***姿Ti的卫星定位约束
Figure BDA0002984550230000063
Figure BDA0002984550230000064
其中，
Figure BDA0002984550230000065
表示该卫星定位约束的权重，由经过插值同步后的水平精度因子HDOPi和基于激光里程的置信度评估bel(Pi)组成：
Figure BDA0002984550230000066
进一步的，所述步骤S2在所述载体在室内环境运行的过程中，循环以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S22，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi,i-1，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000067
进一步构造激光里程约束
Figure BDA0002984550230000068
以相对位姿变换ΔTi,i-1作为位姿增量，计算关键帧Vi位姿Ti的初始值，作为后续加入位姿图的顶点；
S24，以关键帧Vi对应的时间戳ti作为目标同步时间戳，对所述惯性测量单元采集到的惯性测量数据进行插值同步，得到与关键帧Vi时间同步的惯性测量数据Ii及I’i，并进一步根据惯性测量数据I’i估计载体处于静止或匀速运动的置信度bel(Ii)和设置标识位
Figure BDA0002984550230000069
基于所述的惯性测量数据Ii、静止或匀速状态标识位
Figure BDA00029845502300000610
置信度bel(Ii)构造姿态约束
Figure BDA00029845502300000611
所述步骤S24包括以下步骤：
S241，对惯性测量数据队列使用高斯权重插值以得到与关键帧Vi时间戳同步的惯性测量数据，过程如下：
取关键帧Vi对应的时间戳ti记为目标同步时间戳；
使用二分法在按时序排列的惯性测量数据队列中查找以目标同步时间戳ti为时间上界的惯性测量数据，记为Ik,并分别取前后各n帧惯性测量数据，构成候选插值惯性测量数据序列
Figure BDA0002984550230000071
使用高斯概率密度函数对候选插值惯性测量数据序列进行插值，得到与关键帧Vi时间戳同步的惯性测量数据Ii：
Figure BDA0002984550230000072
其中，wk表示惯性测量数据
Figure BDA0002984550230000073
的权重，根据该惯性测量数据时间戳按高斯概率密度函数计算获得：
Figure BDA0002984550230000074
式中，目标同步时间戳ti为该高斯分布均值，单个惯性测量周期σ作为高斯分布方差，
Figure BDA0002984550230000075
表示惯性测量数据队列中的第k帧数据的时间戳；
以ti为目标插值时间，取惯性测量数据
Figure BDA0002984550230000076
和
Figure BDA0002984550230000077
进行线性插值，得到另一个与关键帧Vi时间戳同步的惯性测量数据，记为I’i；
S242，根据惯性测量数据I’i估计载体处于静止或匀速运动的置信度bel(Ii)并设置标识位
Figure BDA0002984550230000078
Figure BDA0002984550230000079
Figure BDA00029845502300000710
式中，(·)[acc]函数表示从惯性测量数据中提取加速度计数据，G0表示当地的重力向量；
S243，根据插值同g步后的惯性测量数据Ii，利用重力方向构造关于关键帧Vi对应载***姿Ti的姿态约束
Figure BDA00029845502300000711
Figure BDA00029845502300000712
式中，(·)[r,p]函数表示从估计的载***姿数据中提取俯仰角和姿态角，(·)[acc]函数表示从惯性测量数据中提取加速度计数据，H(·)函数表示根据载体俯仰角、姿态角以及惯性测量加速度信息计算预估的重力方向，
Figure BDA00029845502300000713
表示对当地重力向量进行归一化得到实际的重力方向向量，
Figure BDA00029845502300000714
表示该项姿态约束的权重，由载体运动状态标识位
Figure BDA0002984550230000081
和置信度bel(Ii)组成：
Figure BDA0002984550230000082
进一步的，所述步骤S2在所述载体从室内环境返回到室外环境后，执行以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S25，以历史关键帧Vj为中心构建局部子地图，对当前关键帧Vi的点云与所述局部子地图进行闭环扫描匹配得到当前关键帧Vi与历史关键帧Vj对应的载体相对位姿变换ΔTij，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000083
构造激光闭环扫描约束
Figure BDA0002984550230000084
更进一步的，所述步骤S25包括以下步骤：
S251，利用当前时刻关键帧Vi对应的的卫星定位数据作为检索目标，对所有关键帧V所构成的KD-Tree进行搜索，检索出与当前时刻关键帧Vi对应的载***置欧氏距离最短的历史关键帧Vj；
S252，以历史关键帧Vj为中心，从历史关键帧序列分别向前后索引N个关键帧得到候选闭环关键帧序列[Vj-N,…,Vj,…,Vj+N]，并根据对应的载***姿序列[Tj-N,…,Tj,…,Tj+N]，对候选闭环关键帧序列中的点云进行如下坐标变换构建局部子地图：
Figure BDA0002984550230000085
式中，
Figure BDA0002984550230000086
表示局部子地图中的点，
Figure BDA0002984550230000087
表示候选闭环关键帧序列中关键帧Vj的点，{Rj,tj}分别表示旋转矩阵和位置平移量，共同组成关键帧Vj对应的载***姿Tj；
S253，将当前时刻关键帧Vi的点云与所述局部子地图进行点云配准，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000088
根据所述欧氏距离设置激光闭环标识位
Figure BDA0002984550230000089
Figure BDA00029845502300000810
S254，根据上述关键帧Vi的点云与所述局部子地图进行点云配准得到的当前关键帧Vi与历史关键帧Vj的对应的载体相对位姿观测ΔTji，构造关于载***姿Ti和Tj的激光闭环扫描约束
Figure BDA00029845502300000811
Figure BDA00029845502300000812
式中，L(·)函数表示把由旋转和平移组成的位姿数据T＝{R,t}映射到SE(3)流形空间，
Figure BDA0002984550230000091
表示该激光闭环扫描约束的权重，由配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000092
表示：
Figure BDA0002984550230000093
进一步的，所述步骤S3中包括以下步骤：
S31，以所述关键帧的位姿作为顶点依次连接，形成位姿图的主干部分；
S32，根据所述激光里程约束以及激光闭环约束的约束关系在所述位姿图中的各顶点之间进行连接，以不同粗细区分约束的权重大小；
S33，根据所述卫星定位约束以及姿态约束，对所述位姿图中的对应顶点进行连接，并以不同粗细区分约束的权重大小。
进一步的，所述步骤S3中还包括以下步骤：
S34，将所述位姿图中的各项约束进行求和，得到关于载体轨迹的最大后验概率目标函数，所述最大后验概率目标函数的待求变量为所有历史关键帧对应的载***姿；
S35，对所述目标函数进行求解获得所述位姿图中各顶点对应的载***姿T*。
进一步的，所述目标函数按以下公式表示：
Figure BDA0002984550230000094
进一步的，所述步骤S4中包括以下步骤：
S41，根据所述位姿图求解结果，获得各关键帧对应的载***姿Ti；
S42，对于所述关键帧Vi,(i＝0,1,…,m)点云Ci，将点云Ci中的各个点pn∈Ci,(n＝0,1,…k)转换到所述全局坐标系中实现点云的拼接，生成点云地图。
附图说明
图1为本发明实施例提供的特种场景的多传感器动态加权融合的点云地图构建方法的总逻辑示意图；
图2为本发明实施例提供的特种场景的多传感器动态加权融合的点云地图构建方法的步骤流程图；
图3为本发明实施例步骤S1的步骤流程图；
图4为本发明实施例步骤S2在所述载体从所述起点出发到进入室内环境之前的过程中循环的步骤流程图；
图5为本发明实施例步骤S22的步骤流程图；
图6为本发明实施例步骤S2在所述载体在室内环境运行的过程中循环的步骤流程图；
图7为本发明实施例步骤S2在所述载体从室内环境返回到室外环境后执行的步骤流程图；
图8为本发明实施例步骤S25的步骤流程图；
图9为本发明实施例步骤S3的步骤流程图；
图10为本发明实施例载体的活动轨迹示意图；
图11为本发明实施例例提供的特种场景的多传感器动态加权融合的点云地图构建方法生成的位姿图；
图12为本发明实施例步骤S4的步骤流程图；
图13为本发明实施例的特种场景的多传感器动态加权融合的点云地图构建方法生成的点云地图示例。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；
应当明确，所描述的实施例仅仅是本申请实施例一部分实施例，而不是全部的实施例。基于本申请实施例中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本申请实施例保护的范围。
在本申请实施例使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请实施例。在本申请实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。在本申请的描述中，需要理解的是，术语“第一”、“第二”、“第三”等仅用于区别类似的对象，而不必用于描述特定的顺序或先后次序，也不能理解为指示或暗示相对重要性。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。
此外，在本申请的描述中，除非另有说明，“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。以下结合附图和实施例对本发明做进一步的阐述。
为了解决现有技术的局限性，本实施例提供了一种技术方案，下面结合附图和实施例对本发明的技术方案做进一步的说明。
一种特种场景的多传感器动态加权融合的点云地图构建方法，其通过设有激光雷达、卫星定位设备以及惯性测量单元的载体在预定的室内外环境中进行数据采集和激光扫描，根据激光扫描得到的点云构建所述载体移动轨迹范围内的点云地图；
具体的，载体首先在室外启动，采集室外环境数据后进入信号屏蔽区的室内环境，最后从室内环境驶出，重新回到室外环境形成闭环。
请参考图1及图2，所述特种场景的多传感器动态加权融合的点云地图构建方法包括以下步骤：
S1，所述载体从室外启动，将起点处的载***姿以固定顶点的方式加入位姿图中，并以起点为坐标原点，以“东向-北向-天向”为基准方向，构建笛卡尔直角坐标系作为全局坐标系，后续卫星定位数据均转换到所述全局坐标系下；
S2，在所述载体运动距离大于设定阈值时，将激光雷达的扫描结果记为关键帧，将该关键帧对应的载***姿作为待求解顶点加入位姿图；根据激光雷达的帧间扫描配准结果来构造激光里程约束，并根据点云配准重合度来调整所述激光里程约束的动态权重；当载***于室外环境时，根据卫星定位结果与所述载***置的欧氏距离来构造卫星定位约束，并根据水平精度因子与激光里程信息来调整所述卫星定位约束的动态权重；当所述载***于室内环境时，根据惯性测量单元的数据并结合载体姿态计算预期重力方向与实际重力方向的夹角来构造姿态约束，并根据所述载体处于静止或匀速状态的置信度来调整所述姿态约束的动态权重；当所述载体从室内返回室外环境时，利用卫星定位结果对历史关键帧进行检索得到闭环候选关键帧，利用当前关键帧与以闭环候选关键帧为中心的局部子地图进行点云配准，根据配准结果来构造激光闭环约束，并根据点云配准重合度来调整所述激光闭环约束的动态权重；
S3，根据包括所述激光里程约束、卫星定位约束、姿态约束、激光闭环约束在内的约束项以及各约束项对应的动态权重，对各项约束相关的变量顶点进行连接，构建位姿图，对位姿图中的所有约束项进行求和得到关于载体轨迹的最大后验概率目标函数，使用非线性优化方法进行求解即对所述位姿图进行求解；
S4，根据所述位姿图的求解结果对所述历史关键帧点云进行拼接，生成点云地图。
相较于现有技术，本发明在GNSS覆盖下以最小化载***置与卫星定位之间的欧氏距离为目标构造卫星定位约束，并根据激光里程计与GNSS水平精度因子调整其权重来减小卫星定位误差对点云地图的影响；在GNSS盲区下以最小化载体姿态与惯性测量数据构成的预期重力方向与实际重力方向的夹角为目标构造姿态约束，并根据载体处于静止或匀速状态的置信水平调整其权重，解决由于缺乏全局位置观测导致点云地图高程累积误差大的问题。在激光里程计基础上，以位姿图方式对所述卫星定位约束、姿态约束以及激光里程约束和闭环约束进行整合，得到关于载体轨迹的最大后验概率目标函数。求解所述目标函数得到的载***姿对激光雷达关键帧点云拼接，得到累积误差小、与地理坐标系一致、局部细节清晰的室内外点云地图。本发明可用于包含室外环境、室内环境和信号屏蔽区域的特种场景的点云地图构建，并且在基于动态权重的卫星定位数据和惯性测量数据所构建的卫星定位约束和姿态约束条件下，实现了室内外点云地图与地理坐标系的对齐，有利于后续的融合定位方法实施。
具体的，所述约束项包括由所述卫星定位设备采集到的定位数据构建的卫星定位约束、由所述惯性测量单元采集到的惯性测量数据构建的姿态约束、对所述激光雷达激光扫描得到的点云进行配准构建的激光里程约束以及激光闭环约束。
所述历史关键帧，即相对于所述载体从室内环境返回室外环境时获得的关键帧，所对应的在所述载体从室外环境进入室内环境前在室外环境中获得的关键帧。
所述卫星定位设备指通过观测GNSS卫星获得坐标系内绝对定位坐标的测量设备。GNSS(Global Navigation Satellite System)是所有导航定位卫星的总称，凡是可以通过捕获跟踪其卫星信号实现定位的***，均可纳入GNSS***的范围，包括GPS、北斗卫星导航***(BDS)、GLONASS(格洛纳斯)卫星导航***以及GALILEO(伽利略)卫星导航***等。
所述卫星定位设备还可以结合RTK(Real-time kinematic，实时动态)载波相位差分技术，RTK是通过实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标的方法。
激光里程在于通过前后两帧激光雷达的数据计算出载体的相对运动距离以及旋转角度。
在SLAM(simultaneous localization and mapping，即时定位与地图构建)领域中，位姿表示世界坐标系到相机坐标系的变换，包括旋转与平移；位姿本质上是一种变换矩阵，特指世界坐标系与相机坐标系之间的变化关系。
位姿图(Pose Graph)即只考虑位姿，构建一个只有轨迹的图优化，而位姿节点之间的边，由两个关键帧之间通过特征匹配后得到的运动估计来给定初始值，一旦初始值完成，就不再优化路标点的位置，只关心采集设备位姿之间的联系。
在解决室外环境下由于GNSS/RTK误差引入目标函数的约束项中导致载***姿求解不准确，最终导致按载***姿拼接得到的点云地图无法严格对齐的技术问题上，本发明的实施例实际上可视为提出了一种基于动态权重的方法，通过对传感器观测所构建的约束项进行实时动态加权的方法以减小观测噪声对点云地图的影响，以GNSS为例，本实施例可以根据该时空下的可视卫星数作为卫星定位信息的置信度对卫星定位约束进行加权，由于该时空下的可视卫星数是动态变化的，因此所述卫星定位约束的权重也是动态变化的。
在解决特种场景中的信号屏蔽区域由于缺乏全局坐标的位置观测导致由于激光扫描帧间匹配的误差累积无法得到有效消除，最终点云地图高度方向累积误差大的问题，本发明提出采用惯性测量单元(Inertial Measurement Unit，IMU)数据构造载体姿态约束的方法，通过使用惯性测量单元对载体状态进行检测，当载体处于静止或者匀速的情况时，可根据惯性测量单元输出的加速度与重力方向的关系对载体姿态进行约束，以解决在信号屏蔽区域环境由于缺乏全局位置观测导致的点云地图在高度方向累积误差大的问题。
作为一种优选实施例，请参阅图3，所述步骤S1中包括以下步骤：
S11，在室外环境中选择定位信号良好的位置作为所述载体的起点S0，运用所述卫星定位设备获取所述起点的经度λ0、纬度φ0、高度h0以及航向角ψ0，并将起点S0处的载***姿T0以固定顶点的形式加入到位姿图中，记起点处激光雷达扫描为关键帧V0；
S12，根据所述经度、纬度、高度以及航向角，以所述起点为原点，以“东-北-天”为基准方向，建立一个笛卡尔直角坐标系作为全局坐标系：先将所述起点的经度λ0、纬度φ0、高度h0转换到ECEF坐标系，得到起点S0所对应的ECEF坐标系坐标(ecefx0,ecefy0,ecefz0)，并计算经纬度所对应的旋转矩阵
Figure BDA0002984550230000141
Figure BDA0002984550230000142
S13，对于卫星定位设备后续输出的经度、纬度、高度信息(λi,φi,hi)，先将该坐标转换到ECEF坐标系得到(ecefxi,ecefyi,ecefzi)，再按下式计算得到对应的笛卡尔直角坐标系卫星定位坐标值：
Figure BDA0002984550230000143
进一步的，请参阅图4，所述步骤S2在所述载体从所述起点出发到进入室内环境之前的过程中，循环以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S22，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi,i-1，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000144
进一步构造激光里程约束
Figure BDA0002984550230000145
以相对位姿变换ΔTi,i-1作为位姿增量，计算关键帧Vi位姿Ti的初始值，作为后续加入位姿图的顶点；
S23，以关键帧Vi对应的时间戳ti作为目标同步时间戳，对所述卫星定位数据进行插值，得到与关键帧Vi时间同步的卫星定位Pi及水平精度因子HDOPi，并根据激光里程计对插值同步后的卫星定位结果Pi进行置信度评估得到置信度估计bel(Pi)，基于所述的卫星定位Pi、水平精度因子HDOPi、置信度估计bel(Pi)构造卫星定位约束
Figure BDA0002984550230000146
其中，请参阅图5，所述步骤S22包括以下步骤：
S221，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi,i-1；
S222，计算配准后的两帧点云之间的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000151
根据所述的载体相对位姿变换ΔTi,i-1构造激光里程约束
Figure BDA0002984550230000152
Figure BDA0002984550230000153
式中，L(·)函数表示把由旋转和平移组成的位姿数据T＝{R,t}映射到SE(3)流形空间的切空间中，得到对应的李代数
Figure BDA00029845502300001512
Figure BDA0002984550230000154
表示该激光里程约束的权重，由配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000155
表示：
Figure BDA0002984550230000156
S223，利用相对位姿变换ΔTi,i-1作为位姿增量，计算关键帧Vi的位姿Ti的初始值(Ti-1ΔTi,i-1)，并将位姿Ti作为后续加入到位姿图中与关键帧Vi-1对应的顶点相连接的顶点；
其中，所述步骤S23包括以下步骤：
S231，对所述卫星定位数据与关键帧Vi根据时间戳进行插值同步，过程如下：
取关键帧Vi对应的时间戳ti记为目标同步时间戳；检查卫星定位数据队列起始数据时间戳是否等于关键帧Vi对应的时间戳ti，是则认为该卫星定位数据与激光雷达关键帧Vi时间同步，取该卫星定位数据作为与关键帧Vi时间戳同步的卫星定位Pi，否则，对卫星定位数据队列进行检查：
如果数据队列起始数据时间戳
Figure BDA0002984550230000157
则本次插值同步失败；
如果数据队列起始数据时间戳
Figure BDA0002984550230000158
则该帧卫星定位数据时间不满足插值要求，将该数据从队列中弹出，对卫星定位数据队列进行检查；
如果卫星定位数据队列中第0和第1帧数据的时间戳均<ti，则将第0个数据从队列中弹出，对卫星定位数据队列进行检查；
如果数据队列第1个数据的时间戳
Figure BDA0002984550230000159
表明该卫星定位数据时间上较关键帧Vi对应的时间戳ti晚，本次插值同步失败；
卫星定位数据队列检查完毕后，如果卫星定位数据队列中数据量≥2，则满足插值同步条件，取卫星定位数据队列中第0和第1帧数据的时间戳
Figure BDA00029845502300001510
Figure BDA00029845502300001511
分别计算插值比例因子sf,sb，并根据插值比例因子对步骤S13所述的笛卡尔直角坐标系下的卫星定位信息进行插值，得到与关键帧Vi时间戳同步的卫星定位Pi及水平精度因子HDOPi：
Figure BDA0002984550230000161
Figure BDA0002984550230000162
Figure BDA0002984550230000163
Figure BDA0002984550230000164
其中，
Figure BDA0002984550230000165
表示卫星定位数据队列中第i帧数据，
Figure BDA0002984550230000166
与
Figure BDA0002984550230000167
表示对应的水平精度因子；
S232，根据S22所述的激光里程对插值同步后的卫星定位结果Pi进行置信度评估，过程如下：
以所述步骤S22中通过对激光雷达关键帧Vi的点云与激光雷达关键帧Vi-1的点云进行配准得到两个关键帧之间对应的载体相对位姿变换(ΔTi,i-1)-1作为载体从ti-1到ti的位姿增量估计；
以ti-1时刻的载***姿Ti-1作为基准，利用上述位姿增量估计ΔTi,i-1可以得到ti时刻对应的卫星定位预测
Figure BDA0002984550230000168
Figure BDA0002984550230000169
其中，(·)[x,y,z]表示取位姿数据中的位置信息；
根据ti时刻对应的卫星定位预测
Figure BDA00029845502300001610
与实际从GNSS/RTK接收机获取并经过时间同步的卫星定位Pi之间的欧式距离，对Pi进行置信度评估：
Figure BDA00029845502300001611
S233，根据经过插值同步后的卫星定位Pi，可构造关于关键帧Vi对应载***姿Ti的卫星定位约束
Figure BDA00029845502300001612
Figure BDA00029845502300001613
其中，
Figure BDA00029845502300001614
表示该卫星定位约束的权重，由经过插值同步后的水平精度因子HDOPi和基于激光里程的置信度评估bel(Pi)组成：
Figure BDA00029845502300001615
进一步的，请参阅图6，所述步骤S2在所述载体在室内环境运行的过程中，循环以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S22，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi,i-1，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000171
进一步构造激光里程约束
Figure BDA0002984550230000172
以相对位姿变换ΔTi,i-1作为位姿增量，计算关键帧Vi位姿Ti的初始值，作为后续加入位姿图的顶点；
S24，以关键帧Vi对应的时间戳ti作为目标同步时间戳，对所述惯性测量单元采集到的惯性测量数据进行插值同步，得到与关键帧Vi时间同步的惯性测量数据Ii及I’i，并进一步根据惯性测量数据I’i估计载体处于静止或匀速运动的置信度bel(Ii)和设置标识位
Figure BDA0002984550230000173
基于所述的惯性测量数据Ii、静止或匀速状态标识位
Figure BDA0002984550230000174
置信度bel(Ii)构造姿态约束
Figure BDA0002984550230000175
所述步骤S24包括以下步骤：
S241，对惯性测量数据队列使用高斯权重插值以得到与关键帧Vi时间戳同步的惯性测量数据，过程如下：
取关键帧Vi对应的时间戳ti记为目标同步时间戳；
使用二分法在按时序排列的惯性测量数据队列中查找以目标同步时间戳ti为时间上界的惯性测量数据，记为Ik,并分别取前后各n帧惯性测量数据，构成候选插值惯性测量数据序列
Figure BDA0002984550230000176
使用高斯概率密度函数对候选插值惯性测量数据序列进行插值，得到与关键帧Vi时间戳同步的惯性测量数据Ii：
Figure BDA0002984550230000177
其中，wk表示惯性测量数据
Figure BDA0002984550230000178
的权重，根据该惯性测量数据时间戳按高斯概率密度函数计算获得：
Figure BDA0002984550230000179
式中，目标同步时间戳ti为该高斯分布均值，单个惯性测量周期σ作为高斯分布方差，
Figure BDA00029845502300001710
表示惯性测量数据队列中的第k帧数据的时间戳；
以ti为目标插值时间，取惯性测量数据
Figure BDA00029845502300001711
和
Figure BDA00029845502300001712
进行线性插值，得到另一个与关键帧Vi时间戳同步的惯性测量数据，记为I’i；
S242，根据惯性测量数据I’i估计载体处于静止或匀速运动的置信度bel(Ii)并设置标识位
Figure BDA0002984550230000181
Figure BDA0002984550230000182
Figure BDA0002984550230000183
式中，(·)[acc]函数表示从惯性测量数据中提取加速度计数据，G0表示当地的重力向量；
S243，根据插值同g步后的惯性测量数据Ii，利用重力方向构造关于关键帧Vi对应载***姿Ti的姿态约束
Figure BDA0002984550230000184
Figure BDA0002984550230000185
式中，(·)[r,p]函数表示从估计的载***姿数据中提取俯仰角和姿态角，(·)[acc]函数表示从惯性测量数据中提取加速度计数据，H(·)函数表示根据载体俯仰角、姿态角以及惯性测量加速度信息计算预估的重力方向，
Figure BDA0002984550230000186
表示对当地重力向量进行归一化得到实际的重力方向向量，
Figure BDA0002984550230000187
表示该项姿态约束的权重，由载体运动状态标识位
Figure BDA0002984550230000188
和置信度bel(Ii)组成：
Figure BDA0002984550230000189
具体的，在一种可选的实施例中，所述载体相对运动距离阈值Dist可以设为1米，即所述载体每运动1米即抽取一次关键帧；但是由于关键帧的抽取主要取决于激光雷达采集数据的计算结果，因此在时刻i中，所述卫星定位设备以及惯性测量单元不一定输出有数据，因此，可以通过线性差值的方式获得时刻i的数据。
在得到i时刻的惯性测量数据后，可以根据i时刻的惯性测量数据中角速度大小和比力大小来判断所述载体的运动状态，当角速度和比力小于预设的阈值时，可判断此时载体处于静止或匀速状态；当载体运动状态近似为匀速运动或静止状态时，姿态约束标志位FG有效；所述姿态约束Gi可以根据i时刻的惯性测量数据中加速度与重力方向的关系来构建，并以载体运动状态检测的置信度作为姿态约束的权重
Figure BDA00029845502300001810
越接近匀速运动或静止状态，权重
Figure BDA00029845502300001811
越大。
进一步的，请参阅图7，所述步骤S2在所述载体从室内环境返回到室外环境后，执行以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S25，以历史关键帧Vj为中心构建局部子地图，对当前关键帧Vi的点云与所述局部子地图进行闭环扫描匹配得到当前关键帧Vi与历史关键帧Vj对应的载体相对位姿变换ΔTij，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000191
构造激光闭环扫描约束
Figure BDA0002984550230000192
更进一步的，请参阅图8，所述步骤S25包括以下步骤：
S251，利用当前时刻关键帧Vi对应的的卫星定位数据作为检索目标，对所有关键帧V所构成的KD-Tree进行搜索，检索出与当前时刻关键帧Vi对应的载***置欧氏距离最短的历史关键帧Vj；
S252，以历史关键帧Vj为中心，从历史关键帧序列分别向前后索引N个关键帧得到候选闭环关键帧序列[Vj-N,…,Vj,…,Vj+N]，并根据对应的载***姿序列[Tj-N,…,Tj,…,Tj+N]，对候选闭环关键帧序列中的点云进行如下坐标变换构建局部子地图：
Figure BDA0002984550230000193
式中，
Figure BDA0002984550230000194
表示局部子地图中的点，
Figure BDA0002984550230000195
表示候选闭环关键帧序列中关键帧Vj的点，{Rj,tj}分别表示旋转矩阵和位置平移量，共同组成关键帧Vj对应的载***姿Tj；
S253，将当前时刻关键帧Vi的点云与所述局部子地图进行点云配准，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000196
根据所述欧氏距离设置激光闭环标识位
Figure BDA0002984550230000197
Figure BDA0002984550230000198
S254，根据上述关键帧Vi的点云与所述局部子地图进行点云配准得到的当前关键帧Vi与历史关键帧Vj的对应的载体相对位姿观测ΔTji，构造关于载***姿Ti和Tj的激光闭环扫描约束
Figure BDA0002984550230000199
Figure BDA00029845502300001910
式中，L(·)函数表示把由旋转和平移组成的位姿数据T＝{R,t}映射到SE(3)流形空间，
Figure BDA00029845502300001911
表示该激光闭环扫描约束的权重，由配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure BDA0002984550230000201
表示：
Figure BDA0002984550230000202
具体的，KD-Tree是K-Dimensional树的简称，是一种对K维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。主要应用于多维空间关键数据的搜索，如范围搜索和最近邻搜索等。
进一步的，请参阅图9，所述步骤S3中包括以下步骤：
S31，以所述关键帧的位姿作为顶点依次连接，形成位姿图的主干部分；
S32，根据所述激光里程约束以及激光闭环约束的约束关系在所述位姿图中的各顶点之间进行连接，以不同粗细区分约束的权重大小；
S33，根据所述卫星定位约束以及姿态约束，对所述位姿图中的对应顶点进行连接，并以不同粗细区分约束的权重大小。
具体的，载体在室外启动，采集室外环境数据后进入信号屏蔽区的室内环境，最后从室内环境驶出，重新回到室外环境形成闭环的轨迹如图10所示，其中顶点V2与顶点Vk之间表示为省略号。
请参阅图11，位姿图主要由顶点和边构成，顶点Vk为第k时刻下(即关键帧)对应的载***姿Tk；二元边Eij是位姿图中两个顶点{Vi,Vj}所对应的相对位姿观测，其中包括所述激光里程约束与激光闭环约束；一元边Eii是位姿图中顶点Vi的先验约束，其中包括所述卫星定位约束和姿态约束。图中顶点上的深色实线表示卫星定位约束，灰色实线表示姿态约束，连接顶点之间的黑色虚线表示激光里程约束，连接顶点之间的灰色虚线表示激光闭环约束，不同粗细表示不同的权重。
进一步的，请参阅图9，所述步骤S3中还包括以下步骤：
S34，将所述位姿图中的各项约束进行求和，得到关于载体轨迹的最大后验概率目标函数，所述最大后验概率目标函数的待求变量为所有历史关键帧对应的载***姿；
S35，对所述目标函数进行求解获得所述位姿图中各顶点对应的载***姿T*。
进一步的，所述目标函数按以下公式表示：
Figure BDA0002984550230000203
Figure BDA0002984550230000211
具体的，参数T表示各个关键帧所对应的载***姿，第一个求和项是GNSS约束误差项，第二个求和项是姿态约束误差项，第三个求和项是激光里程约束误差项，第四个求和项是闭环约束误差项。T*表示位姿图中所有顶点对应的载***姿，即待求解的参数。
上述目标函数本质上是一个非线性最小二乘模型，可以使用高斯-牛顿、列文伯格-马夸尔特等非线性优化方法进行求解以获取位姿图中各个顶点对应的载***姿T*。优化的目标是通过对参数T进行调整，使得整体的目标函数最小。
进一步的，请参阅图12，所述步骤S4中包括以下步骤：
S41，根据所述位姿图求解结果，获得各关键帧对应的载***姿Ti；
S42，对于所述关键帧Vi,(i＝0,1,…,m)点云Ci，将点云Ci中的各个点pn∈Ci,(n＝0,1,…k)转换到所述全局坐标系中实现点云的拼接，生成点云地图。
具体的，在所述步骤S42中，通过以下变换式将点pn转换到所述全局坐标系中：
Figure BDA0002984550230000212
其中，
Figure BDA0002984550230000213
表示转换到全局地图上的点，Ri和ti共同构成载***姿Ti，分别表示载体坐标系到全局坐标系的旋转以及平移变换。
最终本实施例生成的室内外点云地图请参阅图13，其中右上角为室内部分的局部放大。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，其通过设有激光雷达、卫星定位设备以及惯性测量单元的载体在预定的室内外环境中进行数据采集和激光扫描，根据激光扫描得到的点云构建所述载体移动轨迹范围内的点云地图；包括以下步骤：
S1，所述载体从室外启动，将起点处的载***姿以固定顶点的方式加入位姿图中，并以起点为坐标原点，以“东向-北向-天向”为基准方向，构建笛卡尔直角坐标系作为全局坐标系，后续卫星定位数据均转换到所述全局坐标系下；
S2，在所述载体运动距离大于设定阈值时，将激光雷达的扫描结果记为关键帧，将该关键帧对应的载***姿作为待求解顶点加入位姿图；根据激光雷达的帧间扫描配准结果来构造激光里程约束，并根据点云配准重合度来调整所述激光里程约束的动态权重；当载***于室外环境时，根据卫星定位结果与所述载***置的欧氏距离来构造卫星定位约束，并根据水平精度因子与激光里程信息来调整所述卫星定位约束的动态权重；当所述载***于室内环境时，根据惯性测量单元的数据并结合载体姿态计算预期重力方向与实际重力方向的夹角来构造姿态约束，并根据所述载体处于静止或匀速状态的置信度来调整所述姿态约束的动态权重；当所述载体从室内返回室外环境时，利用卫星定位结果对历史关键帧进行检索得到闭环候选关键帧，利用当前关键帧与以闭环候选关键帧为中心的局部子地图进行点云配准，根据配准结果来构造激光闭环约束，并根据点云配准重合度来调整所述激光闭环约束的动态权重；
S3，根据包括所述激光里程约束、卫星定位约束、姿态约束、激光闭环约束在内的约束项以及各约束项对应的动态权重，对各项约束相关的变量顶点进行连接，构建位姿图，对位姿图中的所有约束项进行求和得到关于载体轨迹的最大后验概率目标函数，使用非线性优化方法进行求解即对所述位姿图进行求解；
S4，根据所述位姿图的求解结果对所述历史关键帧点云进行拼接，生成点云地图。
2.根据权利要求1所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S1中包括以下步骤：
S11，在室外环境中选择定位信号良好的位置作为所述载体的起点S0，运用所述卫星定位设备获取所述起点的经度λ0、纬度φ0、高度h0以及航向角ψ0，并将起点S0处的载***姿T0以固定顶点的形式加入到位姿图中，记起点处激光雷达扫描为关键帧V0；
S12，根据所述经度、纬度、高度以及航向角，以所述起点为原点，以“东-北-天”为基准方向，建立一个笛卡尔直角坐标系作为全局坐标系：先将所述起点的经度λ0、纬度φ0、高度h0转换到ECEF坐标系，得到起点S0所对应的ECEF坐标系坐标(ecefx0，ecefy0，ecefz0)，并计算经纬度所对应的旋转矩阵
Figure FDA0002984550220000021
Figure FDA0002984550220000022
S13，对于卫星定位设备后续输出的经度、纬度、高度信息(λi，φi，hi)，先将该坐标转换到ECEF坐标系得到(ecefxi，ecefyi，ecefzi)，再按下式计算得到对应的笛卡尔直角坐标系卫星定位坐标值：
Figure FDA0002984550220000023
3.根据权利要求2所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S2在所述载体从所述起点出发到进入室内环境之前的过程中，循环以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S22，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi，i-1，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure FDA0002984550220000026
进一步构造激光里程约束
Figure FDA0002984550220000024
以相对位姿变换ΔTi，i-1作为位姿增量，计算关键帧Vi位姿Ti的初始值，作为后续加入位姿图的顶点；
S23，以关键帧Vi对应的时间戳ti作为目标同步时间戳，对所述卫星定位数据进行插值，得到与关键帧Vi时间同步的卫星定位Pi及水平精度因子HDOPi，并根据激光里程计对插值同步后的卫星定位结果Pi进行置信度评估得到置信度估计bel(Pi)，基于所述的卫星定位Pi、水平精度因子HDOPi、置信度估计bel(Pi)构造卫星定位约束
Figure FDA0002984550220000025
其中，所述步骤S22包括以下步骤：
S221，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi，i-1；
S222，计算配准后的两帧点云之间的有效匹配点对的平均欧氏距离
Figure FDA00029845502200000313
根据所述的载体相对位姿变换ΔTi，i-1构造激光里程约束
Figure FDA0002984550220000031
Figure FDA0002984550220000032
式中，L(·)函数表示把由旋转和平移组成的位姿数据T＝{R，t}映射到SE(3)流形空间的切空间中，得到对应的李代数
Figure FDA00029845502200000314
Figure FDA00029845502200000312
表示该激光里程约束的权重，由配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure FDA0002984550220000034
表示：
Figure FDA0002984550220000035
S223，利用相对位姿变换ΔTi，i-1作为位姿增量，计算关键帧Vi的位姿Ti的初始值(Ti-1ΔTi，i-1)，并将位姿Ti作为后续加入到位姿图中与关键帧Vi-1对应的顶点相连接的顶点；
其中，所述步骤S23包括以下步骤：
S231，对所述卫星定位数据与关键帧Vi根据时间戳进行插值同步，过程如下：
取关键帧Vi对应的时间戳ti记为目标同步时间戳；检查卫星定位数据队列起始数据时间戳是否等于关键帧Vi对应的时间戳ti，是则认为该卫星定位数据与激光雷达关键帧Vi时间同步，取该卫星定位数据作为与关键帧Vi时间戳同步的卫星定位Pi，否则，对卫星定位数据队列进行检查：
如果数据队列起始数据时间戳
Figure FDA0002984550220000036
则本次插值同步失败；
如果数据队列起始数据时间戳
Figure FDA0002984550220000037
则该帧卫星定位数据时间不满足插值要求，将该数据从队列中弹出，对卫星定位数据队列进行检查；
如果卫星定位数据队列中第0和第1帧数据的时间戳均＜ti，则将第0个数据从队列中弹出，对卫星定位数据队列进行检查；
如果数据队列第1个数据的时间戳
Figure FDA0002984550220000038
表明该卫星定位数据时间上较关键帧Vi对应的时间戳ti晚，本次插值同步失败；
卫星定位数据队列检查完毕后，如果卫星定位数据队列中数据量≥2，则满足插值同步条件，取卫星定位数据队列中第0和第1帧数据的时间戳
Figure FDA0002984550220000039
Figure FDA00029845502200000310
分别计算插值比例因子sf，sb，并根据插值比例因子对步骤S13所述的笛卡尔直角坐标系下的卫星定位信息进行插值，得到与关键帧Vi时间戳同步的卫星定位Pi及水平精度因子HDOPi：
Figure FDA00029845502200000311
Figure FDA0002984550220000041
Figure FDA0002984550220000042
Figure FDA0002984550220000043
其中，
Figure FDA0002984550220000044
表示卫星定位数据队列中第i帧数据，HDOPi gnss-deq与
Figure FDA0002984550220000045
表示对应的水平精度因子；
S232，根据S22所述的激光里程对插值同步后的卫星定位结果Pi进行置信度评估，过程如下：
以所述步骤S22中通过对激光雷达关键帧Vi的点云与激光雷达关键帧Vi-1的点云进行配准得到两个关键帧之间对应的载体相对位姿变换(ΔTi，i-1)-1作为载体从ti-1到ti的位姿增量估计；
以ti-1时刻的载***姿Ti-1作为基准，利用上述位姿增量估计ΔTi，i-1可以得到ti时刻对应的卫星定位预测
Figure FDA0002984550220000046
Figure FDA0002984550220000047
其中，(·)[x，y，z]表示取位姿数据中的位置信息；
根据ti时刻对应的卫星定位预测
Figure FDA0002984550220000048
与实际从GNSS/RTK接收机获取并经过时间同步的卫星定位Pi之间的欧式距离，对Pi进行置信度评估：
Figure FDA0002984550220000049
S233，根据经过插值同步后的卫星定位Pi，可构造关于关键帧Vi对应载***姿Ti的卫星定位约束
Figure FDA00029845502200000410
Figure FDA00029845502200000411
其中，
Figure FDA00029845502200000412
表示该卫星定位约束的权重，由经过插值同步后的水平精度因子HDOPi和基于激光里程的置信度评估bel(Pi)组成：
Figure FDA00029845502200000413
4.根据权利要求3所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S2在所述载体在室内环境运行的过程中，循环以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S22，对关键帧Vi的点云与关键帧Vi-1的点云进行点云配准，得到两个关键帧之间对应的载体相对位姿变换ΔTi，i-1，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure FDA0002984550220000051
进一步构造激光里程约束
Figure FDA0002984550220000052
以相对位姿变换ΔTi，i-1作为位姿增量，计算关键帧Vi位姿Ti的初始值，作为后续加入位姿图的顶点；
S24，以关键帧Vi对应的时间戳ti作为目标同步时间戳，对所述惯性测量单元采集到的惯性测量数据进行插值同步，得到与关键帧Vi时间同步的惯性测量数据Ii及I′i，并进一步根据惯性测量数据I′i估计载体处于静止或匀速运动的置信度bel(Ii)和设置标识位
Figure FDA0002984550220000053
基于所述的惯性测量数据Ii、静止或匀速状态标识位
Figure FDA0002984550220000054
置信度bel(Ii)构造姿态约束
Figure FDA0002984550220000055
所述步骤S24包括以下步骤：
S241，对惯性测量数据队列使用高斯权重插值以得到与关键帧Vi时间戳同步的惯性测量数据，过程如下：
取关键帧Vi对应的时间戳ti记为目标同步时间戳；
使用二分法在按时序排列的惯性测量数据队列中查找以目标同步时间戳ti为时间上界的惯性测量数据，记为Ik，并分别取前后各n帧惯性测量数据，构成候选插值惯性测量数据序列
Figure FDA0002984550220000056
使用高斯概率密度函数对候选插值惯性测量数据序列进行插值，得到与关键帧Vi时间戳同步的惯性测量数据Ii：
Figure FDA0002984550220000057
其中，wk表示惯性测量数据
Figure FDA0002984550220000058
的权重，根据该惯性测量数据时间戳按高斯概率密度函数计算获得：
Figure FDA0002984550220000059
式中，目标同步时间戳ti为该高斯分布均值，单个惯性测量周期σ作为高斯分布方差，
Figure FDA00029845502200000510
表示惯性测量数据队列中的第k帧数据的时间戳；
以ti为目标插值时间，取惯性测量数据
Figure FDA00029845502200000511
和
Figure FDA00029845502200000512
进行线性插值，得到另一个与关键帧Vi时间戳同步的惯性测量数据，记为I′i；
S242，根据惯性测量数据I′i估计载体处于静止或匀速运动的置信度bel(Ii)并设置标识位
Figure FDA0002984550220000061
Figure FDA0002984550220000062
Figure FDA0002984550220000063
式中，(·)[acc]函数表示从惯性测量数据中提取加速度计数据，G0表示当地的重力向量；
S243，根据插值同g步后的惯性测量数据Ii，利用重力方向构造关于关键帧Vi对应载***姿Ti的姿态约束
Figure FDA0002984550220000064
Figure FDA0002984550220000065
式中，(·)[r，p]函数表示从估计的载***姿数据中提取俯仰角和姿态角，(·)[acc]函数表示从惯性测量数据中提取加速度计数据，H(·)函数表示根据载体俯仰角、姿态角以及惯性测量加速度信息计算预估的重力方向，
Figure FDA0002984550220000066
表示对当地重力向量进行归一化得到实际的重力方向向量，
Figure FDA0002984550220000067
表示该项姿态约束的权重，由载体运动状态标识位
Figure FDA0002984550220000068
和置信度bel(Ii)组成：
Figure FDA0002984550220000069
5.根据权利要求4所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S2在所述载体从室内环境返回到室外环境后，执行以下步骤：
S21，根据预设的载体相对运动距离阈值Dist，从所述激光雷达采集到的数据中抽取关键帧Vi，i表示关键帧序号；
S25，以历史关键帧Vj为中心构建局部子地图，对当前关键帧Vi的点云与所述局部子地图进行闭环扫描匹配得到当前关键帧Vi与历史关键帧Vj对应的载体相对位姿变换ΔTij，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure FDA00029845502200000610
构造激光闭环扫描约束
Figure FDA00029845502200000611
6.根据权利要求5所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S25包括以下步骤：
S251，利用当前时刻关键帧Vi对应的的卫星定位数据作为检索目标，对所有关键帧V所构成的KD-Tree进行搜索，检索出与当前时刻关键帧Vi对应的载***置欧氏距离最短的历史关键帧Vj；
S252，以历史关键帧Vj为中心，从历史关键帧序列分别向前后索引N个关键帧得到候选闭环关键帧序列[Vj-N，…，Vj，…，Vj+N]，并根据对应的载***姿序列[Tj-N，…，Tj，…，Tj+N]，对候选闭环关键帧序列中的点云进行如下坐标变换构建局部子地图：
Figure FDA0002984550220000071
式中，
Figure FDA0002984550220000072
表示局部子地图中的点，
Figure FDA0002984550220000073
表示候选闭环关键帧序列中关键帧Vj的点，{Rj，tj}分别表示旋转矩阵和位置平移量，共同组成关键帧Vj对应的载***姿Tj；
S253，将当前时刻关键帧Vi的点云与所述局部子地图进行点云配准，并计算配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure FDA0002984550220000074
根据所述欧氏距离设置激光闭环标识位
Figure FDA0002984550220000075
Figure FDA0002984550220000076
S254，根据上述关键帧Vi的点云与所述局部子地图进行点云配准得到的当前关键帧Vi与历史关键帧Vj的对应的载体相对位姿观测ΔTji，构造关于载***姿Ti和Tj的激光闭环扫描约束
Figure FDA0002984550220000077
Figure FDA0002984550220000078
式中，L(·)函数表示把由旋转和平移组成的位姿数据T＝{R，t}映射到SE(3)流形空间，
Figure FDA0002984550220000079
表示该激光闭环扫描约束的权重，由配准后的两帧点云的有效匹配点对的平均欧氏距离
Figure FDA00029845502200000710
表示：
Figure FDA00029845502200000711
7.根据权利要求5或6所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S3中包括以下步骤：
S31，以所述关键帧的位姿作为顶点依次连接，形成位姿图的主干部分；
S32，根据所述激光里程约束以及激光闭环约束的约束关系在所述位姿图中的各顶点之间进行连接，以不同粗细区分约束的权重大小；
S33，根据所述卫星定位约束以及姿态约束，对所述位姿图中的对应顶点进行连接，并以不同粗细区分约束的权重大小。
8.根据权利要求7所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S3中还包括以下步骤：
S34，将所述位姿图中的各项约束进行求和，得到关于载体轨迹的最大后验概率目标函数，所述最大后验概率目标函数的待求变量为所有历史关键帧对应的载***姿；
S35，对所述目标函数进行求解获得所述位姿图中各顶点对应的载***姿T*。
9.根据权利要求7所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述目标函数按以下公式表示：
Figure FDA0002984550220000081
10.根据权利要求7所述的特种场景的多传感器动态加权融合的点云地图构建方法，其特征在于，所述步骤S4中包括以下步骤：
S41，根据所述位姿图求解结果，获得各关键帧对应的载***姿Ti；
S42，对于所述关键帧Vi，(i＝0，1，...，m)点云Ci，将点云Ci中的各个点pn∈Ci，(n＝0，1，...k)转换到所述全局坐标系中实现点云的拼接，生成点云地图。
CN202110296526.9A 2021-03-19 2021-03-19 特种场景的多传感器动态加权融合的点云地图构建方法 Active CN112950781B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110296526.9A CN112950781B (zh) 2021-03-19 2021-03-19 特种场景的多传感器动态加权融合的点云地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110296526.9A CN112950781B (zh) 2021-03-19 2021-03-19 特种场景的多传感器动态加权融合的点云地图构建方法

Publications (2)

Publication Number Publication Date
CN112950781A true CN112950781A (zh) 2021-06-11
CN112950781B CN112950781B (zh) 2023-04-25

Family

ID=76226933

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110296526.9A Active CN112950781B (zh) 2021-03-19 2021-03-19 特种场景的多传感器动态加权融合的点云地图构建方法

Country Status (1)

Country Link
CN (1) CN112950781B (zh)

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113340296A (zh) * 2021-06-21 2021-09-03 上海仙工智能科技有限公司 一种自动更新移动机器人地图的方法及装置
CN113516750A (zh) * 2021-06-30 2021-10-19 同济大学 三维点云地图构建方法、***、电子设备及存储介质
CN113538410A (zh) * 2021-08-06 2021-10-22 广东工业大学 一种基于3d激光雷达和uwb的室内slam建图方法
CN113534227A (zh) * 2021-07-26 2021-10-22 中国电子科技集团公司第五十四研究所 一种适用于复杂非合作场景的多传感器融合绝对定位方法
CN113628335A (zh) * 2021-07-28 2021-11-09 深圳优艾智合机器人科技有限公司 点云地图构建方法、装置及计算机可读存储介质
CN113819912A (zh) * 2021-09-30 2021-12-21 中科测试(深圳)有限责任公司 一种基于多传感器数据的高精度点云地图生成方法
CN113850864A (zh) * 2021-09-14 2021-12-28 中南大学 一种面向移动机器人的gnss/激光雷达回环检测方法
CN114838726A (zh) * 2022-04-20 2022-08-02 哈尔滨理工大学 一种基于多传感器数据融合的gps数据修正算法
CN115631314A (zh) * 2022-12-19 2023-01-20 中汽研（天津）汽车工程研究院有限公司 一种基于多特征和自适应关键帧的点云地图构建方法
CN116068604A (zh) * 2023-02-13 2023-05-05 腾讯科技（深圳）有限公司 融合定位方法、装置、计算机设备、存储介质及程序产品
CN116222544A (zh) * 2023-05-09 2023-06-06 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116772887A (zh) * 2023-08-25 2023-09-19 北京斯年智驾科技有限公司 一种车辆航向初始化方法、***、装置和可读存储介质
CN116929369A (zh) * 2023-07-24 2023-10-24 合肥中科深谷科技发展有限公司 一种基于位姿图更新的导航定位方法、装置及介质
CN117268373A (zh) * 2023-11-21 2023-12-22 武汉大学 一种多传感器信息融合的自主导航方法及***
WO2024001339A1 (zh) * 2022-07-01 2024-01-04 华为云计算技术有限公司 确定位姿的方法、装置以及计算设备
WO2024065852A1 (en) * 2022-09-30 2024-04-04 Intel Corporation Robot position error correction

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106599108A (zh) * 2016-11-30 2017-04-26 浙江大学 一种三维环境中多模态环境地图构建方法
KR20180087947A (ko) * 2017-01-26 2018-08-03 삼성전자주식회사 3차원의 포인트 클라우드를 이용한 모델링 방법 및 모델링 장치
US20190154449A1 (en) * 2016-07-22 2019-05-23 Regents Of The University Of Minnesota Square-root multi-state constraint kalman filter for vision-aided inertial navigation system
CN111210518A (zh) * 2020-01-15 2020-05-29 西安交通大学 基于视觉融合地标的拓扑地图生成方法
CN111815765A (zh) * 2020-07-21 2020-10-23 西北工业大学 一种基于异构数据融合的图像三维重建方法
CN112086010A (zh) * 2020-09-03 2020-12-15 中国第一汽车股份有限公司 地图生成方法、装置、设备及存储介质
CN112419501A (zh) * 2020-12-10 2021-02-26 中山大学 一种地空异构协同地图构建方法
CN112507056A (zh) * 2020-12-21 2021-03-16 华南理工大学 一种基于视觉语义信息的地图构建方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190154449A1 (en) * 2016-07-22 2019-05-23 Regents Of The University Of Minnesota Square-root multi-state constraint kalman filter for vision-aided inertial navigation system
CN106599108A (zh) * 2016-11-30 2017-04-26 浙江大学 一种三维环境中多模态环境地图构建方法
KR20180087947A (ko) * 2017-01-26 2018-08-03 삼성전자주식회사 3차원의 포인트 클라우드를 이용한 모델링 방법 및 모델링 장치
CN111210518A (zh) * 2020-01-15 2020-05-29 西安交通大学 基于视觉融合地标的拓扑地图生成方法
CN111815765A (zh) * 2020-07-21 2020-10-23 西北工业大学 一种基于异构数据融合的图像三维重建方法
CN112086010A (zh) * 2020-09-03 2020-12-15 中国第一汽车股份有限公司 地图生成方法、装置、设备及存储介质
CN112419501A (zh) * 2020-12-10 2021-02-26 中山大学 一种地空异构协同地图构建方法
CN112507056A (zh) * 2020-12-21 2021-03-16 华南理工大学 一种基于视觉语义信息的地图构建方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
胡向勇等: "基于关键帧的点云建图方法", 《热带地貌》 *
陈超等: "一种基于三维视觉的移动机器人定位与建图方法", 《现代电子技术》 *
马跃龙等: "一种基于深度相机的机器人室内导航点云地图生成方法", 《测绘工程》 *

Cited By (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113340296B (zh) * 2021-06-21 2024-04-09 上海仙工智能科技有限公司 一种自动更新移动机器人地图的方法及装置
CN113340296A (zh) * 2021-06-21 2021-09-03 上海仙工智能科技有限公司 一种自动更新移动机器人地图的方法及装置
CN113516750A (zh) * 2021-06-30 2021-10-19 同济大学 三维点云地图构建方法、***、电子设备及存储介质
CN113534227B (zh) * 2021-07-26 2022-07-01 中国电子科技集团公司第五十四研究所 一种适用于复杂非合作场景的多传感器融合绝对定位方法
CN113534227A (zh) * 2021-07-26 2021-10-22 中国电子科技集团公司第五十四研究所 一种适用于复杂非合作场景的多传感器融合绝对定位方法
CN113628335A (zh) * 2021-07-28 2021-11-09 深圳优艾智合机器人科技有限公司 点云地图构建方法、装置及计算机可读存储介质
CN113538410A (zh) * 2021-08-06 2021-10-22 广东工业大学 一种基于3d激光雷达和uwb的室内slam建图方法
CN113850864A (zh) * 2021-09-14 2021-12-28 中南大学 一种面向移动机器人的gnss/激光雷达回环检测方法
CN113850864B (zh) * 2021-09-14 2024-04-12 中南大学 一种面向室外移动机器人的gnss/lidar回环检测方法
CN113819912A (zh) * 2021-09-30 2021-12-21 中科测试(深圳)有限责任公司 一种基于多传感器数据的高精度点云地图生成方法
CN114838726A (zh) * 2022-04-20 2022-08-02 哈尔滨理工大学 一种基于多传感器数据融合的gps数据修正算法
WO2024001339A1 (zh) * 2022-07-01 2024-01-04 华为云计算技术有限公司 确定位姿的方法、装置以及计算设备
WO2024065852A1 (en) * 2022-09-30 2024-04-04 Intel Corporation Robot position error correction
CN115631314A (zh) * 2022-12-19 2023-01-20 中汽研（天津）汽车工程研究院有限公司 一种基于多特征和自适应关键帧的点云地图构建方法
CN115631314B (zh) * 2022-12-19 2023-06-09 中汽研（天津）汽车工程研究院有限公司 一种基于多特征和自适应关键帧的点云地图构建方法
CN116068604A (zh) * 2023-02-13 2023-05-05 腾讯科技（深圳）有限公司 融合定位方法、装置、计算机设备、存储介质及程序产品
CN116222544B (zh) * 2023-05-09 2023-08-04 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116222544A (zh) * 2023-05-09 2023-06-06 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116929369A (zh) * 2023-07-24 2023-10-24 合肥中科深谷科技发展有限公司 一种基于位姿图更新的导航定位方法、装置及介质
CN116772887B (zh) * 2023-08-25 2023-11-14 北京斯年智驾科技有限公司 一种车辆航向初始化方法、***、装置和可读存储介质
CN116772887A (zh) * 2023-08-25 2023-09-19 北京斯年智驾科技有限公司 一种车辆航向初始化方法、***、装置和可读存储介质
CN117268373A (zh) * 2023-11-21 2023-12-22 武汉大学 一种多传感器信息融合的自主导航方法及***
CN117268373B (zh) * 2023-11-21 2024-02-13 武汉大学 一种多传感器信息融合的自主导航方法及***

Also Published As

Publication number Publication date
CN112950781B (zh) 2023-04-25

Similar Documents

Publication Publication Date Title
CN112950781B (zh) 特种场景的多传感器动态加权融合的点云地图构建方法
CN110118549B (zh) 一种多源信息融合定位方法和装置
Maaref et al. Lane-level localization and mapping in GNSS-challenged environments by fusing lidar data and cellular pseudoranges
CN105579811B (zh) 用于外部混合照片制图的方法
WO2021248636A1 (zh) 一种自动驾驶对象探测和定位***及方法
JP6002126B2 (ja) 画像ベースの測位のための方法および装置
CN110044354A (zh) 一种双目视觉室内定位与建图方法及装置
US12019173B2 (en) Lane-level navigation system for ground vehicles with lidar and cellular signals
CN110617821B (zh) 定位方法、装置及存储介质
JP2019532433A (ja) リアルタイムオンラインエゴモーション推定を有するレーザスキャナ
JP2020067439A (ja) 移動***置推定システムおよび移動***置推定方法
JP2012185202A (ja) 局所地図生成装置、グローバル地図生成装置、及びプログラム
WO2019046962A1 (en) METHOD AND SYSTEM FOR TARGET POSITIONING AND CARD UPDATE
CN112129281A (zh) 一种基于局部邻域地图的高精度图像导航定位方法
CN110232683A (zh) 一种基于无人机点云的滑坡检测方法
Dill et al. Seamless indoor-outdoor navigation for unmanned multi-sensor aerial platforms
CN115183762A (zh) 一种机场仓库内外建图方法、***、电子设备及介质
CN113838129B (zh) 一种获得位姿信息的方法、装置以及***
CN115639570A (zh) 融合激光强度和点云几何特征的机器人定位和建图方法
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
Singh et al. Ubiquitous hybrid tracking techniques for augmented reality applications
Zhang et al. Online ground multitarget geolocation based on 3-D map construction using a UAV platform
Grejner-Brzezinska et al. From Mobile Mapping to Telegeoinformatics
Zahedian et al. Localization of autonomous vehicles: proof of concept for a computer vision approach
CN113850864B (zh) 一种面向室外移动机器人的gnss/lidar回环检测方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant