CN114648561A - 旋转式三维激光雷达的点云匹配方法及系统 - Google Patents

旋转式三维激光雷达的点云匹配方法及系统 Download PDF

Info

Publication number
CN114648561A
CN114648561A CN202210267742.5A CN202210267742A CN114648561A CN 114648561 A CN114648561 A CN 114648561A CN 202210267742 A CN202210267742 A CN 202210267742A CN 114648561 A CN114648561 A CN 114648561A
Authority
CN
China
Prior art keywords
point
frame
radar
point cloud
cloud 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.)
Pending
Application number
CN202210267742.5A
Other languages
English (en)
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.)
Hunan Yunjiangna Micro Information Technology Co ltd
Original Assignee
Hunan Yunjiangna Micro Information Technology Co ltd
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 Hunan Yunjiangna Micro Information Technology Co ltd filed Critical Hunan Yunjiangna Micro Information Technology Co ltd
Priority to CN202210267742.5A priority Critical patent/CN114648561A/zh
Publication of CN114648561A publication Critical patent/CN114648561A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • 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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Operations Research (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明提供一种旋转式三维激光雷达的点云匹配方法及系统，载体行进，旋转式三维激光雷达旋转且扫描四周环境，步骤(PA)：判断环境角度变化值是否小于角度阈值，若判断结果为是，则判定环境位置点Mi,j,k为第一特征点，否则，判定环境位置点Mi,j,k为第二特征点；步骤(PB)：比较第i+1帧点云数据中各个雷达环与第i帧点云数据中第jb个雷达环的相似度；步骤(PC)：比较第i+1帧点云数据、第i帧点云数据，得到第i+1帧点云数据、第i帧点云数据的对应雷达环中环境位置点的对应关系；步骤(A)：构建误差方程，求解误差方程。

Description

旋转式三维激光雷达的点云匹配方法及系统
技术领域
本发明涉及一种激光雷达点云特征提取及其配准方法，具体涉及一种旋转式三维激光雷达的点云匹配方法及系统。
背景技术
随着驾驶辅助系统(AID)及无人驾驶技术的快速发展，三维激光雷达作为其重要的环境感知传感器得以快速发展并产生了大量的成熟产品。激光雷达作为一种大数据量环境感知模块现已被广泛应用于家用扫地机器人及自动驾驶的地图创建、车道感知等。所有激光雷达产生的点云都是相对于其自身局部坐标系的位置，要获得该位置在全局坐标系的表达需要进行位姿参数的求解，该参数求解的核心技术是三维点云的匹配，而匹配的关键点是点云特征的提取及使用方法。
三维激光点云特征提取及其应用的方法，吸引了广大的科技工作者及爱好者投身其中，并且产生了众多有益的成果。这些特征提取及点云匹配成果大多将三维激光点云作为一种无序点云来进行处理，因而要较为完整体表达这些点云的特性，需要引入大量的额外参数(法向量，匹域统计特性等)，并且一般需要考虑较大局部空间尺度以保证特征表达的完备性。当满足以上要求时，使得每一个点其对应特征的维度会远高于其位置表达的维度，这极大的增加了运算成本及存储成本。而对应于目前主流的机械激光雷达而言，其更趋近于有序点云的特征，再使用上述点云特征来进行匹配处理，在保证实时性下往往需要高性能的运算单元，而这些高性能运算单元会带来更高空间、散热、供电等要求，这会变相增大一个系统的复杂度。
发明内容
本发明要解决的问题是针对现有三维激光雷达中点云匹配时运算复杂度高、严重影响匹配的实时性的问题，提供一种旋转式三维激光雷达的点云匹配方法及系统。
本发明提供一种旋转式三维激光雷达的点云匹配方法，所述旋转式三维激光雷达设置在载体上，所述旋转式三维激光雷达具有在高度方向上间隔设置的J个雷达线，其特征在于，载体行进，旋转式三维激光雷达旋转且扫描四周环境，定义第i个采样周期得到的旋转式三维激光雷达的点云数据为第i帧点云数据，第i帧点云数据中由第j个雷达线扫描的各个环境位置点的数据构成第i帧点云数据的第j个雷达环，定义环境位置点Mi,j,k为第i帧点云数据中由第j个雷达线扫描的第k个环境位置点，所述点云数据为带有时间戳信息和雷达线序号的点云数据；
步骤(A)：根据第i帧点云数据判断环境位置点Mi,j,k对应的水平方向上的环境角度变化值θ1(i,j,k)是否在预设角度范围内，若判断结果为是，则判定环境位置点Mi,j,k为第一特征点，否则，判定环境位置点Mi,j,k为第二特征点，j＝1,2,…,J，k＝1,2,…,K，K为每个雷达线在每个采样周期中扫描的环境位置点的个数；
步骤(B)：构建误差方程
Figure BDA0003553051880000011
求解误差方程，得到第i+1帧到i帧平移参数tri+1＝(dxi+1,dyi+1,dzi+1)T、偏航角ψ'i+1、翻滚角
Figure BDA0003553051880000012
俯仰角θ'i+1；
其中：
ψ′i+1、
Figure BDA0003553051880000013
θ'i+1分别为第i帧点云数据到第i+1帧点云数据变换的偏航角、翻滚角、俯仰角，偏航角ψ'i+1根据第i帧点云数据与第i+1帧点云数据中具有对应关系的环境位置点在雷达环中所在位置序号之差、旋转式三维激光雷达的转动参数计算得到；
EAi+1,u1为第u1个第一直线与坐标(xa'i+1,u1,ya'i+1,u1,za'i+1,u1)之间的距离，EBi+1,v1为第v1个第一平面与坐标(xb'i+1,v1,yb'i+1,v1,zb'i+1,v1)之间的距离；
第u1个第一直线为经过第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点、第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点的同位位置点的直线；u1＝1,2,…,U(i+1)，U(i+1)为与第i帧点云数据的雷达环具有对应关系的第i+1帧点云数据的雷达环中第一特征点的总数；
第v1个第一平面由第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点和次对应位置点、第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点的同位位置点形成；v1＝1,2,…,V(i+1)，V(i+1)为与第i帧点云数据的雷达环具有对应关系的第i+1帧点云数据的雷达环中第二特征点的总数；
Figure BDA0003553051880000021
坐标(xai+1,u1,yai+1,u1,zai+1,u1)为U(i+1)个第一特征点中第u1个第一特征点的坐标；
坐标(xbi+1,v1,ybi+1,v1,zbi+1,v1)为V(i+1)个第二特征点中第v1个第二特征点的坐标；
旋转矩阵
Figure BDA0003553051880000022
其中，四元数qi+1的表达式如下
Figure BDA0003553051880000023
步骤(C)：对于第i+1帧点云数据中的各个环境位置点的坐标(xi+1,j,k,yi+1,j,k,zi+1,j,k)，利用下式计算变换后坐标(x'i+1,j,k,y'i+1,j,k,z'i,j,k)
(x'i+1,j,k,y'i+1,j,k,z'i,j,k)T＝Ri+1×(xi+1,j,k,yi+1,j,k,zi+1,j,k)T+tri+1。
本发明中，每一帧点云数据中的数据很大，本发明中，根据环境角度变化值将环境特征点分为第一特征点、第二特征点，从而找到特征较为明显、且在环境范围内数量较少的第一特征点，从而将点云数据中所有点的匹配简化为对第一特征点的匹配，从而大大降低工作量，提高匹配速度。确定两帧点云数据中第一特征点的对应关系后，也即确定了各个第二特征点的对应关系。通过构建两帧中第一特征点的变换式、且构建两帧中第二特征点的变换式，可通过误差方程求解得到各个变换参数，即旋转矩阵和平移参数，从而可以得到两帧之间的变换关系，包括旋转矩阵和平移参数，从而可将第i+1帧的点云数据进行变换后统一到第i帧点云数据的坐标系中，从而得到新的一帧点云数据后，可将新的环境点的坐标加入已有坐标系中，从而可一直更新，获得雷达扫描范围内的周围环境的坐标。第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点的同位位置点即第i帧点云数据中与对应位置点在不同环上、同一列的位置点，例如对应位置点在第i帧第1个雷达环的第32个点，则同位位置点可取第i帧中除了第1个雷达环之外的其他雷达环的第32个点，作为第1个雷达环的第32个点同位位置点。
上述技术方案中，第i+1帧点云数据、第i帧点云数据的雷达环、环境位置点的对应关系确定方法采用如下步骤(PA)-步骤(PC)：
步骤(PA)：若环境位置点Mi,j,k为第一特征点，则令环境位置点Mi,j,k对应的第一参数值Pi,j,k＝1，否则，令Pi,j,k＝0，构建第一序列Ai,j＝{Pi,j,1,Pi,j,2,…,Pi,j,K}；
步骤(PB)：令jb为预设的雷达环序号，比较第i帧点云数据中各个雷达环与第i+1帧点云数据中第jb个雷达环的相似度，具体包括如下步骤(PB1)-步骤(PB5)：
步骤(PB1)：按照元素顺序，将序列Ai+1,jb的K个元素划分为K/L组，令s＝1,2,…,K/L，第s组中的L个元素进行逻辑或运算后得到元素Qi+1,jb,s，从而得到序列A’i+1,jb＝{Qi+1,jb,1,Qi+1,jb,2,…,Qi+1,jb,K/L}；
若Qi+1,jb,1＝1，则A”i+1,jb＝A’i+1,jb，若Qi+1,jb,1＝0，则通过序列A’i+1,jb中元素的循环顺序向前移位，将序列A’i+1,jb中位于Qi+1,jb,1之后的第一个不为0的元素作为序列中的第1个元素，得到序列A”i+1,jb，其中，Shift(i+1,jb)为序列A”i+1,jb相对于序列A’i+1,jb移位的个数，若Qi+1,jb,1＝1，则Shift(i+1,jb)＝0；
步骤(PB2)：按照元素顺序，将序列Ai,1的K个元素划分为K/L组，令s＝1,2,…,K/L，第s组中的L个元素进行逻辑或运算后得到元素Qi,1,s，从而得到序列A’i,1＝{Qi,1,1,Qi,1,2,…,Qi,1,K/L}；
步骤(PB3)：通过序列中元素的循环顺序向前移位，将序列A’i,1中每个值为1的元素分别作为序列中的第1个元素，得到BM1(i,1)个序列B”i,1,1、B”i,1,2、…、B”i,1,BM1(i,1)，BM1(i,1)为序列A’i,1中值为1的元素的数量，BM1(i,1)个序列中的每个序列均与序列A”i+1,jb进行比较，得到BM1(i,1)个相似度值，所述BM1(i,1)个相似度值中的最大值为第i+1帧第jb个雷达环与第i帧第1个雷达环的相似度，按照同样方法，求得第i+1帧第jb个雷达环与第i帧第2个雷达环的相似度、第i+1帧第jb个雷达环与第i帧第3个雷达环的相似度、……、第i+1帧第jb个雷达环与第i帧第J个雷达环的相似度；
步骤(PB4)：将第i+1帧第jb个雷达环与第i帧第1个雷达环的相似度、第i+1帧第jb个雷达环与第i帧第2个雷达环的相似度、……、第i+1帧第jb个雷达环与第i帧第J个雷达环的相似度的大小进行比较，确定最大值对应的是第i+1帧第jb个雷达环与第i帧第ja个雷达环的相似度值；
步骤(PB5)：计算将序列A’i,ja-1中的元素循环顺序向前移动Shift(i,ja)个位置得到的序列、将序列A’i+1,jb-1中的元素循环顺序向前移动Shift(i+1,jb)个位置得到的序列的相似度，且计算将序列A’i,ja+1中的元素循环顺序向前移动Shift(i,ja)个位置得到的序列、将序列A’i+1,jb+1中的元素循环顺序向前移动Shift(i+1,jb)个位置得到的序列的相似度，若本步骤(PB5)中计算得到的相似度均不小于预设相似度阈值或不小于第i+1帧第jb个雷达环与第i帧第ja个雷达环的相似度，则确定第i帧点云数据的第j个雷达环与第i+1帧点云数据的第j+jb-ja个雷达环对应，其中，Shift(i,ja)为序列B”i,ja,1、B”i,ja,2、…、B”i,ja,BM1(i,ja)中与序列A”i+1,jb相似度最高的序列相对于序列A’i,ja的移位个数；
步骤(PC)：比较第i+1帧点云数据、第i帧点云数据，得到第i+1帧点云数据、第i帧点云数据的对应雷达环中环境位置点的对应关系，具体包括如下步骤(PC1)-(PC6)：
步骤(PC1)：将集合Ci+1,jb中元素循环顺序向前移动Shift(i+1,jb)个位置得到集合C’i+1,jb，其中，集合Ci+1,jb＝{Mi+1,jb,1,Mi+1,jb,2,……,Mi+1,jb,K}；
将集合Ci,ja中元素循环顺序向前移动Shift(i,ja)个位置得到集合C’i,ja，其中，集合Ci,ja＝{Mi,ja,1,Mi,ja,2,……,Mi,ja,K}；
步骤(PC2)：将集合C’i+1,jb从第1个元素开始划分为K/L个子集合，且第一个子集合与第K/L个子集合相接，使得集合C’i+1,jb对应的K/L个子集合围成环状；若子集合中有至少一个环境位置点为第一特征点，则定义该子集合为第一特征集合，否则，定义该子集合为第二特征集合；
将集合C’i,ja从第1个元素开始划分为K/L个子集合，且第一个子集合与第K/L个子集合相接，使得集合C’i,ja对应的K/L个子集合围成环状；若子集合中有至少一个环境位置点为第一特征点，则定义该子集合为第一特征集合，否则，定义该子集合为第二特征集合；
步骤(PC3)：计算环境位置点Mi+1,jb,k对应的旋转式三维激光雷达的旋转角度θAi+1,jb,k，计算位于环境位置点Mi+1,jb,k所在的子集合一侧、且与该子集合最近的第一特征集合中第jba个环境位置点对应的旋转式三维激光雷达的旋转角度θBi+1,jb,k，计算位于环境位置点Mi+1,jb,k所在的子集合另一侧、且与该子集合最近的第一特征集合中第jbb个环境位置点对应的旋转式三维激光雷达的旋转角度θCi+1,jb,k；jba、jbb均为预设值；
计算环境位置点Mi,ja,k对应的旋转式三维激光雷达的旋转角度θAi,ja,k，计算位于环境位置点Mi,ja,k所在的子集合一侧、且与该子集合最近的第一特征集合中第jba个环境位置点对应的旋转式三维激光雷达的旋转角度θBi,ja,k，计算位于环境位置点Mi,ja,k所在的子集合另一侧、且与该子集合最近的第一特征集合中第jbb个环境位置点对应的旋转式三维激光雷达的旋转角度θCi,ja,k；jba、jbb均为预设值；
步骤(PC4)：计算比例Ri+1,jb,k，其中角度θAi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中，角度θBi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中，角度θCi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中；
计算比例Ri,ja,k，其中角度θAi,ja,k包含在比例Ri,ja,k的分子和/或分母中，角度θBi,ja,k包含在比例Ri,ja,k计算式的分子和/或分母中，角度θCi,ja,k包含在比例Ri,ja,k的分子和/或分母中；
步骤(PC5)：若环境位置点Mi+1,jb,k为第一特征点，则将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值最小的值所对应的第一特征点作为环境位置点Mi+1,jb,k的对应位置点，且将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值第二小的值所对应的第一特征点作为环境位置点Mi+1,jb,k的次对应位置点；
若环境位置点Mi+1,jb,k为第二特征点，则将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值最小的值所对应的第二特征点作为环境位置点Mi+1,jb,k的对应位置点，且将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值第二小的值所对应的第二特征点作为环境位置点Mi+1,jb,k的次对应位置点；依次类推，从而在环境位置点Mi,ja,1、Mi,ja,2、……、Mi,ja,k中找到环境位置点Mi+1,jb,1、Mi+1,jb,2、……、Mi+1,jb,K的对应位置点、次对应位置点，从而得到第i帧点云数据中第ja个雷达环的各个环境位置点与第i+1帧点云数据中第jb个雷达环的各个环境位置点的对应关系；步骤(PC6)：对于第i+1帧点云数据、第i帧点云数据的对应雷达环，重复步骤(PC1)-(PC5)，从而得到第i+1帧点云数据中、第i帧点云数据的对应雷达环的各个环境位置点的对应关系；优选地，所述步骤(B)中，第i帧点云数据与第i+1帧点云数据中具有对应关系的环境位置点在雷达环中所在位置序号之差利用Shift(i+1,jb)、Shift(i,ja)计算。
申请人研究时发现，每个雷达环中采集的环境位置点个数较多，而且雷达环数的对应关系也需要进行确定。本发明中，构建第一序列，将每个雷达环的各个点分为多组，且每组中的L个元素进行逻辑或运算，即若该组中有第一特征点，则该组的逻辑值为1，否则逻辑值为0，从而进一步减少两帧中比对的工作量，即仅需比对逻辑值为1的组即可。而且，本申请中通过将序列中元素循环向前移位，从而比较第i帧中对应一个雷达环的序列、第i+1帧中对应一个雷达环的序列是否相似，从而根据相似度较快地得到雷达环的对应关系。确定两帧数据的环数对应关系后，需进一步确定第i帧、第i+1帧相应点的对应关系。通过比较与第一特征点相应的比例值的关系，从而确定两帧中相应环中各个第一特征点的对应关系。为了进一步减小计算误差，确定了一帧中某个点在另一帧中的对应点、次对应点。
上述技术方案中，所述步骤(PC4)中：
Figure BDA0003553051880000051
且
Figure BDA0003553051880000052
或
Figure BDA0003553051880000053
且
Figure BDA0003553051880000054
或
Figure BDA0003553051880000055
且
Figure BDA0003553051880000056
本发明中，比较两帧点云数据中的点的比例关系，从而可以方便确定两帧中的第一特征点的对应关系。
上述技术方案中，所述步骤(PA)中，利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000057
其中，m为长度，2≤m≤15；
优选地，
Figure BDA0003553051880000058
优选地，m＝5。
本发明中，根据上述计算方法，可以通过求均值消除误差，使得环境角度变化值的计算更准确。
上述技术方案中，所述步骤(PA)中：
若|αi+ωi,j,k|＜45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000059
若|αi+ωi,j,k|≥45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA00035530518800000510
其中，m为预设个数，2≤m≤15，角度αi＝2×cos-1(wi)，雷达与水平面之间夹角ωi,j,k，转轴角为
Figure BDA0003553051880000061
ψi+1、
Figure BDA0003553051880000062
θi+1分别为第i帧点云数据对应的偏航角、翻滚角、俯仰角；
优选地，若|αi+ωi,j,k|＜45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000063
优选地，若|αi+ωi,j,k|≥45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000064
本发明中，申请人在研究时发现，如果雷达并不是水平扫描，而是斜向扫描，则环境角度变化值的计算可能存在较大偏差，使得某个环境位置点可能为误判为错误的特征点类别。本发明中，通过角度关系，可以确定雷达是否为斜向扫描，若为斜向扫描，则进行投影后再计算环境角度变化值，从而使得对于特征点的判断更为准确。
上述技术方案中，测量旋转式三维激光雷达旋转角度、旋转轴轴向，所述点云数据为根据旋转式三维激光雷达旋转角度、旋转轴轴向数据校正后得到的各个环境位置点的三维坐标。
上述技术方案中，L的范围为5-15；优选地，L＝10。
上述技术方案中，所述预设角度范围为[90°-θ0，90°+θ0]；优选地，θ0≤30°。
本发明还提供一种旋转式三维激光雷达的点云匹配系统，包括行进的载体，所述载体上固定设置有旋转式三维激光雷达、惯性测量单元，所述旋转式三维激光雷达具有J个雷达环，每个雷达环具有K个激光线，其特征在于，还包括控制单元，所述控制单元被配置或编程为用于执行权利要求1-8中任一项所述的旋转式三维激光雷达的点云匹配方法的步骤。
进一步地，所述载体为人，所述旋转式三维激光雷达由人手持。
本发明具有的优点和积极效果是：本发明的特征表达简洁稳定而有效，基于该特征表达的匹配方法实时性明显提高。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明的系统结构示意图；
图2是本发明的方法步骤示意图；
图3是本发明实施例的点云数据清洗的步骤(P1)-(P4)的方法示意框图；
图4(a)、图4(b)分别是本发明实施例的去除异常点前、去除异常点后的激光环成图的俯视图；
图5(a)、图5(b)分别是未加载旋转载体信息、加载旋转载体信息的点云线示意图；
图6是本发明实施例的点云有序化示意图；
图7是本发明实施例的点云线平滑度差异向量构造示意图，显示环境位置点的三维坐标；
图8是本发明实施例的平滑度计算示意图；
图9是本发明实施例的旋转点云特征退化示意图；
图10(a)、图10(b)分别是本发明实施例的扫描线、对应的局部特征二值图；
图11是本发明实施例的有向环状二进制特征雷达环示意图；
具体实施方式
下面将结合本申请的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本发明充分利用三维激光点云的有序性及其局部线性特征而创造一种简洁有效的局部及全局特征表达，并基于这种特征表达设计其匹配算法。以使得三维激光雷达点云匹配更为高效。
现有的一般点云特征多考虑为无序点云、存在数据特征维度过高、表达复杂等问题、使用于点云匹配时运算复杂度高，严重影响匹配的实时性。本发明充分利用旋转三维激光雷达的结构特性对其产生点云进行排序，并根据其局部特性将点云划分为角点(本实施例中为第一特征点)和平面点(本实施例中为第二特征点)，根据该特性对三维激光点云线进行有向环状二进制编码，该编码可以有效表达三维激光雷达点云的全局特性。根据平面点、角点及环状二进制编码设计了三维激光雷达点云匹配方法。本发明的特征表达简洁稳定而有效，基于该特征表达的匹配方法实时性明显提高。可以实时运行于嵌入式设备上，从而为基于激光雷达的ADAS(高级驾驶辅助系统Advanced Driver Assistance System)、自动驾驶及室内外SLAM等提供技术支撑。
本发明产生点云硬件设备如图1所示。10为旋转三维激光雷达，101为旋转三维激光雷达的旋转轴、102为旋转三维激光雷达的旋转结构。该特征表达及匹配方法利用旋转多线三维激光雷达各线激光点云点和该点一定范围内点之间的关系，实现点云线在其延展方向上的平滑度估计，归一化的平滑度值将激光点云划分为平面点和角点两类特征点。旋转激光雷达点云线经平滑域分割后利用平面点和角点特征的分布特性生成其特征标识编码。设激光雷达载体运动在瞬时情况下逼近匀速模型或者匀加速模型则其最近邻点云线之间必然存在相似点云特征编码，利用该特性可以一次性锁定两帧点云对应点，从而避免在点云匹配过程中进行点云对应点重复查找，有效提高点云匹配的精度及速度。同时本发明还具有运算复杂度低、对阈值不敏感环境适应性强等优点，方便于模块化集成到各类算法之中，在激光SLAM(即时建图与定位，Simultaneous Localization And Mapping)、自动驾驶、激光测绘等领域具有广阔的应用前景。本发明可以实时运行于嵌入式设备上，可以为基于激光雷达的ADAS、自动驾驶及室内外SLAM等提供技术支撑。
实施例1
本发明提供一种旋转式三维激光雷达的点云匹配方法，所述旋转式三维激光雷达10设置在载体上，所述旋转式三维激光雷达10具有在高度方向上间隔设置的J个雷达线，载体行进，旋转式三维激光雷达10旋转且扫描四周环境。定义第i个采样周期得到的旋转式三维激光雷达10的点云数据为第i帧点云数据，第i帧点云数据中由第j个雷达线扫描的各个环境位置点的数据构成第i帧点云数据的第j个雷达环，定义环境位置点Mi,j,k为第i帧点云数据中由第j个雷达线扫描的第k个环境位置点，所述点云数据为带有时间戳信息和雷达线序号的点云数据。
本实施例中，所述载体为人，所述旋转式三维激光雷达由人手持。
人在行进过程，每隔一段时间，旋转式三维激光雷达10转一圈(对应旋转角度为360°)采集的数据作为一帧数据。旋转式三维激光雷达10转一圈，每个雷达线采集32000个点。前一帧数据和后一帧数据进行匹配，将新加入的环境的坐标调整到原有坐标系下，即确定周围环境。雷达旋转一帧过程中，人也在步行，即雷达数据和人步行的数据结合，才能将一帧中采集时间不同的各个点统一到相同坐标系，才能得到一帧数据，称为点云校正。通过一个更高频次的运动传感器如imu来对瞬时运动进行估计，从而对点云的畸变进行补偿。该处理属于现有技术，本领域技术人员可以理解。
本发明分为两部分，如图2所示。一部分为关于三维激光雷达的点云特征表达方法：具体包含四个方面的内容，分别为原始激光雷达点云的数据清洗方法、三维激光雷达点云线平滑度计算法方法、特征值的标准化及归一化方法及三维激光点云特征编码方法。另一部分为基于第一部分特征表达的点云匹配方法：该方法具体包含三个方面的内容，分别为基于点云特征编码的点云全局粗对应查找方法、基于平面点/角点分布的近邻点精对应查找方法和基于前两条对应信息的位姿变换求解方法。
图2中箭头的方向表示功能模块实现需要经历的步骤。发明内容的第二部分点云匹配依赖于第一部分的特征表达，因而两个模块之间使用箭头连接。但是第二部分并非只利用第一部分的特征编码，还包括第一部分的中间数据。严格来说特征表达产生了三类特征，局部特征分类的平面点和角点以及全局特征表达的有向环状二进制编码。
三维激光雷达点云数据清洗方法的主要目的是进行三维激光雷达点云数据有序化及去除干扰点。本申请的三维激光雷达具有有序化点云数据，但是如果本申请的三维激光雷达的安装载体在运动时不平稳(例如人员手持)，则还需要考虑激光雷达安装载体的旋转机械特性。如图3所示，在数据清洗步骤中，对所获得原始点云数据进行处理，生成一帧有序点云数据，以方便后期处理。数据清洗步骤包括求取环数信息(步骤P1)、获得扫描时序、去除异常点(步骤P2)、添加旋转轴向及旋转角度信息(步骤P3)、点云有序化(步骤P4)。步骤P1-P4均为已有技术，本领域技术人员可以理解。
雷达原始点云数据即为由雷达测距信息转换得到的三维坐标数据，由雷达厂商提供。
步骤(P1)求环数信息
求取环数信息有两种方法：
1)通过雷达驱动原始点云所附加的环数信息得到；
2)通过求取当前点坐标所属局部坐标的垂向角度值，以及雷达结构说明书激光头发射角度来估计出当前点所属环数(在添加载体旋转信息前进行处理)。
当激光雷达生产商未提供环数数据时，需要使用第二种方法，自行解算法点云所属环数。垂向角度值指：以激光雷达中心为原点，则每一个点和中心点的连线和水平面之间会形成一个夹角。激光雷达硬件厂商，会给出激光头排列角度的机械硬件说明，当求得和水平面的夹角，接近或等于该技术档的某一个角度时，可以推测为该点由该角度上的激光头发出。
步骤(P2)去除异常点
根据激光雷达的扫描次序和结构排布将原始点云进行结构化处理，以利于同一帧点云中目标点近邻索引。在本步骤中，去除点云中的空值和极大极小值以保证数据在一个合理范围内。去除旋转三维激光雷达旋转机械结构造成的遮挡点云，以避免边界判断，以误差边界作为特征值进行处理。
异常点分为两类，一类为环境中反射率过低而解析为异常值的点，另一类为被旋转机械机构遮挡的部分。这两类点都应该在采样时予以去除。去除前后单激光点云线示意如图4(a)、图4(b)所示。
反射率由激光雷达本身可以测量得到，是由激光雷达厂商提供的，本领域技术人员可以理解。
图4(a)、图4(b)表示原始点云采样值域的俯视示意图。如图4(a)所示，假设环境恰为环形并且空间无遮挡，则如其激光雷达测量线应该为一个环形。如图4(b)所示，在旋转激光雷达中因为加入了旋转机械结构，该旋转机械结构遮挡一部激光采样，故其采样在同样空间下在静止时表现为一个弧形。被遮挡的点也会有测量数据，该值的明显特征是点到雷达中心点的距离很小，这些值不是环境数据，可能是跟着一起运动的人或者物体，会对环境测量造成干扰。
在激光雷达运动中，其局部坐标系下一直持续一个固定方位和距离的点，都认为是异常点。将经验估计的与持有人员距离在一定范围内的点都定为异常点，例如该范围设定为1.5米。
判断为异常点的数据，在粗匹配阶段(步骤B)其位置为空，认为该数据对应的环境位置点是平面点(即为第二特征点)，即每个雷达环中数据的个数保持不变，并不删除异常点。
步骤(P3)添加旋转载体信息
测量旋转式三维激光雷达10旋转角度。所述点云数据为根据旋转式三维激光雷达10旋转角度、旋转轴数据校正后得到的各个环境位置点的三维坐标。
图3中添加旋转载体信息主要是根据旋转机构的编码器信息获得当前点绕静止时刻指定轴的旋转情况。
假设激光雷达旋转轴严格对齐激光雷达的X轴或者Y轴。若是假设测量前后环境为一面水平墙壁，并且激光雷达X轴垂直于墙壁，激光雷达在旋转机构带动下恰好绕X轴匀速旋转，则观察并对比静止与旋转情况在墙面上可以观察到如图5(a)、图5(b)所示图形。旋转载体信息是对雷达产品进行旋转的载体的自带功能。
同一帧中，对于各个不同时间采集的环境位置点，本设备中存在旋转问题，使得同一帧中采集的环境位置点的坐标不在同一个坐标系。加入旋转载体信息，将同一帧中采集的各个环境位置点的坐标统一在同一个坐标系，使得每一帧各个环境位置点坐标的原点相同。
旋转载体信息包括了电机的旋转角度、旋转轴向方向等。旋转轴抖动情况由激光雷达的机械转动结构内部的激光传感器给出。根据激光雷达中的高精度光栅码盘，可以获取旋转角度。
当未加旋转载体信息时，点云在墙面形成近似直线的点云线，如图5(a)所示。加载旋转信息后，其点云线回到原点时转过角度变大，电机旋转后，激光头转一圈无法再打在同一个位置，而在水平激光雷达情况下的上方或者下方，即偏离初始位置，即形成如图5(b)所示的不规则曲线(以雷达点云中线为例并且假设该点云线与雷达水平面平行)。
加旋转载体信息，即根据电机旋转角度、旋转轴轴向信息，将雷达的点云数据进行校正。旋转载体信息的校正是为了将激光点云统一载体坐标系下，并未改变激光平面与地面的夹角。校正后一帧点云作为一个整体进行处理，即每一帧点云数据的翻滚角、俯仰角、偏航角相同。
旋转轴向由硬件决定是一个已知量，在计算中作为一个已经参数引入，角度数据由编码器提供。激光雷达固件由手持端模块电机传动，而相对于手持端转过的角度。图1中，旋转三维激光雷达的旋转轴由旋转结构102的内部电机带动，旋转轴角度即旋转轴101相对于旋转结构102的转过角度，由旋转结构102的内置编码器进行测量得到。
步骤(P4)点云数据有序化
点云数据有序化具体为将每一帧激光雷达采集的点云映射为以激光雷达水平扫描点数为宽、以雷达线序号为高的二维浮点图像，以方便进行梯度等特征提取。以16线激光雷达为例，其示意图如图6所示。水平扫描点数和雷达线序号为整数，但是其测距信息为浮点数。
如图6所示，点云的每一个点都被映射至一个类图片格式的像素点，其中每一个元素值为一个三维数组，该三维数组包含加载旋转信息后点云三维空间坐标值。其二元索引(x，y)坐标分别代表当前点在扫描帧中方位信息，x、y分别表示环数(即j值)、水平扫描点数(即k值)。对于任意一个点,其近邻必然在索引坐标(x，y)[±1,±1]范围内。该特性可以方便后期的点云线平滑度计算。
可以通过激光雷达驱动读取点云包的顺序获得点云的扫描时序。驱动每读取一个包都会给该包打上一个时间戳。而该包中的点是严格按照扫描顺序获得的，对这些点采取时间插值即可获得每个点的时间序列，即如果打时间戳速度无法跟上读取包的速度，则没打标签的包可通过其前后的有标签的包平均一下，该参数将作为后期有序化的重要参数之一。
步骤(PA)：
根据第i帧点云数据判断环境位置点Mi,j,k对应的水平方向上的环境角度变化值θ1(i,j,k)是否在预设角度范围内，若判断结果为是，则判定环境位置点Mi,j,k为第一特征点，否则，判定环境位置点Mi,j,k为第二特征点，j＝1,2,…,J，k＝1,2,…,K，K为每个雷达线在每个采样周期中扫描的环境位置点的个数；若环境位置点Mi,j,k为第一特征点，则令环境位置点Mi,j,k对应的第一参数值Pi,j,k＝1，否则，令Pi,j,k＝0，构建第一序列Ai,j＝{Pi,j,1,Pi,j,2,…,Pi,j,K}。
所述预设角度范围可为[90°-θ0，90°+θ0]。优选地，θ0＝30°。因为本申请中，计算的环境角度变化值θ1(i,j,k)是多个值的平均，并且雷达的测量本身存在误差，即使是在城市环境，转角位置也不是绝对的直角，还有大量钝角的拐角，因此将预设角度范围取90°±30，可以适应大部分的情况。
本步骤中，判断水平方向上的环境是否有起伏，即是否有起伏形成凸起、凹陷(例如墙角、墙壁转角位置形成的的凸起或凹陷)，或未形成凸起、凹陷且较为平缓。若判断有起伏形成凸起、凹陷且在设定的预设角度范围内，则判定为第一特征点，而平面上的点、凸起凹陷程度较小的点判定为第二特征点。
利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000101
其中，m为第一数量，2≤m≤15。优选地，m＝5。预设角度范围角度阈值优选为90°±30°(60°、120°可包含在该范围内)，也可根据实际环境设置其他角度阈值。
该步骤中，利用目标点和其近邻之间的相对距离和角度关系来确定激光雷达线平滑度，该处理方法创新之处在于局部特征表达不受环境尺度关系影响，避免了特征值取值范围不确定的问题，更有利于方法的推广和利用。公式中，“.”表示点积。
三维旋转激光雷达点云线平滑度计算具体为一种通过局部点云线走势不同判断点环境曲折程度方法。其具体实施为。
根据所属环数、点云线宽度及高度之间差异对点云进行平滑域确定。其确定依据为“保证划分水平线的角度域是其垂直分辨率的整数倍”。以16线激光雷达为例，其垂直视野为30°划分为16线则其垂直分辨率为30/15＝2°。而水平分辨率为360/2000＝0.18°因而当取10～11个点时，其水平线角度域为1.8°～1.98°接近为一倍2°(即可取m＝5)。当其垂直分辨率较高时可以采用较高的倍率如32线64线激雷达。
同时对整个点云进行分析的计算度和难度是很高的，因而采取平滑域逐个分析的方法。平滑域取太小成为了单个点无法得到有效的点云结构信息，取太大计算复杂度变高。折中的方法是取即包含一定的环数，又包含一定的列数为好。当前点上下各取一环，左右宽度等于环数宽度的小正方块是个比较好的选择。可以在使用中调整包含的环数、列数。
根据确定的平滑域进行平滑值计算。其具体操作为依次取当前点(Mi+1,j,k)的两侧半域(一侧为Mi+1,j,k-1、Mi+1,j,k-2、……、Mi+1,j,k-s，另一侧为Mi+1,j,k+1、Mi+1,j,k+2、……、Mi+1,j,k+s)内对应的点组成差异向量值。求取两个差异值之间的向量差，并进行均值求取。其差异向量构造如图7所示。
图7中黑心实点需要求取平滑度的点，灰色点为其平滑域内的点。图中所示两个箭头指向的点为其对称平滑域点，则取该两点和当前点可以构造到一对差异向量。并且可以求得该对向量之间角度值，该θ值即为当前平滑度的一个平滑因子。平滑域是指以该点为中心，要取来用于分析当前点平滑情况的点云小块大小。利用第1-11个点计算第6个点的角度，利用第2-12个点计算第7个点的角度，依次类推。
根据B的差异向量及平滑因子求取方法，依次求出该点平滑域内的所有差异向量及平滑因子，并根据该点平滑域内所有平滑因子个数求取平滑因子平均值，该平均值即为该点的平滑度估计值。
根据上述步骤求出当前帧内所有点的平滑度值。当点云属于有序图像边缘不足半个平滑域时(即点接近边界)，不再求取该平滑度值，将该点为作离群点给予保留。保留可以用于增加地图完整性，对后期将地图用于建模或者用于定位有利。
三维旋转激光雷达点云特征值标准化及归一化具体实施为：当计算出平滑度(即环境角度变化值θ1(i,j,k))以后，只需要选择一个分割域值(即角度阈值θ0)，即可对将不同平滑度的点划分为两类。由图8可知θ1(i,j,k)的取值范围为0-180度。角度越趋近于180度则其局部特征越平滑。而θ1(i,j,k)的值越趋近于0度则其局部特征越曲折。若是不求取θ1(i,j,k)值，而是直接使用cosθ1(i,j,k)值来表征特征，则其取值为[-1,1]，其正负分界的零点为90度。0度指环境中是尖角部分。
可表述为：
Figure BDA0003553051880000111
Figure BDA0003553051880000112
本步骤中，判断环境位置点Mi,j,k对应的水平方向上的环境角度变化值θ1(i,j,k)<θ0是否成立，或判断cosθ1(i,j,k)＞cosθ0是否成立，若判断上述两个式子中任一个式子成立，则判定点Mi,j,k在水平方向上为第一特征点，否则，判定点Mi,j,k在水平方向上为第二特征点。
在平滑度估计完成后，设定一个合适的阈值对所有平滑度估计的激光雷达点进行二分类即可获得平面点及角点。假设所处环境为室内或者建筑扫描，则可以简单将平滑度角度小于90度(即平滑度值大于0的点)作为角点，如此可以简单的为该有序点云实现一个二值化图像的映射。以图9所示场景为例假设雷达为水平扫描，则在该环境下点云局部会生成如图10(a)、图10(b)所示的图像特征。
根据平滑度计算所得值的分布情况自动设定划分平面点与角点的阈值。将点云线的每一个点归类到平面点或者角点。将旋转激光雷达点云螺旋线进行n等分(n为大于1并且小云点云线点云数量的正整数)，根据每一分区中角点或者平面点占比情况，将该区置为0或者1(0或者1代表当前局部空间外表面的变化剧烈程度)，从而生成一个由0或者1构成的有向环状二进制雷达环(其方向为扫描方向)。该雷达环代表当前点去线空间弯曲情况，可以广泛用于点云查找及分割中。
本发明中，在使用梯度下降类优化算法中，一个好的估计值，以降低运算时间，提高结果精度，所有计算的估计值在优化时会作为初始值进行处理，在该初始值上进行最终的结果计算。估计得越精准结果越好。
步骤(PB)：比较两帧点云数据的相似度
令jb为预设的雷达环序号。可根据实际需求确定jb的值，例如jb可为1或J。优选jb＝J/2或接近J/2的值。通过这样设置，即使后一帧相对于前一帧上下晃动略大，也可快速找到对应的雷达环。
比较第i帧点云数据中各个雷达环与第i+1帧点云数据中第jb个雷达环的相似度，具体包括如下步骤(PB1)-步骤(PB5)：
步骤(PB1)：按照元素顺序，将序列Ai+1,jb的K个元素划分为K/L组，令s＝1,2,…,K/L，第s组中的L个元素进行逻辑或运算后得到元素Qi+1,jb,s，从而得到序列A’i+1,jb＝{Qi+1,jb,1,Qi+1,jb,2,…,Qi+1,jb,K/L}；
若Qi+1,jb,1＝0，则通过序列A’i+1,jb中元素的循环顺序向前移位，将序列A’i+1,jb中位于Qi+1,jb,1之后的第一个不为0的元素作为序列中的第1个元素，得到序列A”i+1,jb，且得到Shift(i+1,jb)为序列A”i+1,jb相对于序列A’i+1,jb移位的个数，若Qi+1,jb,1＝1，则Shift(i+1,jb)＝0。
例如，若Ai+1,jb＝{0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0}，令L＝5，则A”i+1,jb＝A’i+1,jb＝{1,0,1,0,1}。
L的范围为5-15；优选地，L＝10。对三维激光雷达点云线进行特征维度划分，这里划分标准为其每一维度(每个组)包含点云数不能低于平滑度估计平滑域点云个数。以16线激光雷达为例。在平滑度估计中平滑域为10-11个点。这里取10个点为维度划分最小单位，则任一扫描雷达线可以划分200个维度区间(16线激光雷达每一个雷达线包含2000个激光点)。维度最小单位可以进行动态调整，只要低于点云分辨率即可。将该特征维度按指定方向排列。在16线激光雷达中，取X轴顺时针或者逆时针方向。若是以黑色代表存在角点，以白色代表不存在角点，并且令其首尾相接，则其在表现上为一个黑白相间的圆环。其示意图如图11所示。
本申请中，突出、内凹在算法中可以认为变化剧烈程度是一样的。图11展示了一个三维激光扫描线的二维顺时针特征编码。
步骤(PB2)：按照元素顺序，将序列Ai,1的K个元素划分为K/L组，令s＝1,2,…,K/L，第s组中的L个元素进行逻辑或运算后得到元素Qi,1,s，从而得到序列A’i,1＝{Qi,1,1,Qi,1,2,…,Qi,1,K/L}。
步骤(PB3)：通过序列中元素的循环顺序向前移位，将序列A’i,1中每个值为1的元素分别作为序列中的第1个元素，得到BM1(i,1)个序列B”i,1,1、B”i,1,2、…、B”i,1,BM1(i,1)，BM1(i,1)为序列A’i,1中值为1的元素的数量，BM1(i,1)个序列中的每个序列均与序列A”i+1,jb进行比较，得到BM1(i,1)个相似度值，所述BM1(i,1)个相似度值中的最大值为第i+1帧第jb个雷达环与第i帧第1个雷达环的相似度，按照同样方法，求得第i+1帧第jb个雷达环与第i帧第2个雷达环的相似度、第i+1帧第jb个雷达环与第i帧第3个雷达环的相似度、……、第i+1帧第jb个雷达环与第i帧第J个雷达环的相似度。
若A’i,1＝＝{0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0}，令L＝5，则A’i,1＝{1,0,1,0,0}。由于A’i,1有2个元素为1，因此BM1(i,1)＝2，即序列B”i,1,1＝{1,0,1,0,0}，B”i,1,2＝{1,0,0,1,0}。
步骤(PB4)：将第i+1帧第jb个雷达环与第i帧第1个雷达环的相似度、第i+1帧第jb个雷达环与第i帧第2个雷达环的相似度、……、第i+1帧第jb个雷达环与第i帧第J个雷达环的相似度的大小进行比较，确定最大值对应的是第i+1帧第jb个雷达环与第i帧第ja个雷达环的相似度；
步骤(PB5)：计算将序列A’i,ja-1中的元素循环顺序向前移动Shift(i,ja)个位置得到的序列、将序列A’i+1,jb-1中的元素循环顺序向前移动Shift(i+1,jb)个位置得到的序列的相似度，且计算将序列A’i,ja+1中的元素循环顺序向前移动Shift(i,ja)个位置得到的序列、将序列A’i+1,jb+1中的元素循环顺序向前移动Shift(i+1,jb)个位置得到的序列的相似度，若本步骤(PB5)中计算得到的相似度均不小于预设相似度阈值或不小于第i+1帧第jb个雷达环与第i帧第ja个雷达环的相似度，则确定第i帧点云数据的第j个雷达环与第i+1帧点云数据的第j+jb-ja个雷达环对应，其中，Shift(i,ja)为序列B”i,ja,1、B”i,ja,2、…、B”i,ja,BM1(i,ja)中与序列A”i+1,jb相似度最高的序列相对于序列A’i,ja的移位个数。将序列A’i,ja中每个值为1的元素分别作为序列中的第1个元素，得到BM1(i,ja)个序列B”i,ja,1、B”i,ja,2、…、B”i,ja,BM1(i,ja)，BM1(i,ja)为序列A’i,ja中值为1的元素的数量。
jb为预设常量，ja为变量。
所述预设相似度阈值不小于第i帧第jb个雷达环与第i+1帧第ja个雷达环的相似度，或所述预设相似度阈值不小于70％。
例如jb＝1，K＝16，则利用第i+1帧的第1-16个雷达环分别与第i帧的第1个雷达环进行对比，如果得到第i+1帧的第3个雷达环与第i帧的第1个雷达环相似度最高，则用第i+1帧的第4个雷达环和第i帧的第2个雷达环进行对比。如果得到的相似度不小于预设相似度阈值，则可以确定第i+1帧的第3、4、……、16个雷达环分别与第i帧的第1、2、……、14个雷达环相对应。
本申请中，相对应的两个雷达环指另一帧中与一帧中某个雷达环相似度最高的雷达环。由于在行进过程，新一帧雷达环会采集到新数据，因此，两个对应的雷达环的数据并不完全相同。
有向环状二进制雷达环的相似度估计。有向环状二进制编码是一种高维特征二进制特征向量，每一个点云线生成其特定的二进制雷达环，其相似度估计方法用以评估两束点云线的近似程度。具体实施为一种首端对齐一致性判断方法，两个雷达环首端对齐后，剪去尾端多余数据，作同或运算，返回真值的计数与整体雷达环长度的比值表征了两束点云线的接近程度。这里称其为精对应查找是因为相邻的点云线有可能会具有相似性较高的雷达环表达。
基于特征编码的全局点云粗对应查找方法其核心在于判断两个有向环状二进制编码的相似度，其具体方法为一种基于循环匹配相同元素统计方法。这里定义当前采集的点云为“当前帧”(第i+1帧)，前一时刻采集点云为“目标帧”(第i帧)，则基于特征编码的全局点云粗对应方法目标为在“目标帧”找到和“当前帧”每一条激光点云束特征最这接近激光点云线。其算法流程如下所示。
a)读入“目标帧”所有雷达环；
b)读入“当前帧”所有雷达环；
如果相应的角点在“目标帧”雷达环、“当前帧”雷达环的位置不同，则比对会认为两个环不同
c)对于“当前帧”每一个有向二进制雷达环，依编码方向将第一个值为1的维度移到最前端。该维度前面的值依次补充到当前雷达环后端，保证雷达环维度一致。
d)对于经过c)的某一有向二制雷达环，在“目标帧”中选定一条雷达环和其进行相似度判断。
e)对于d)选定的雷达环将其依编码方向将第一个值为1的维度移到最前端。该维度前面的值依次补充到当前雷达环后端，保证雷达环维度一致。并将该调整后雷达环与该经过c)的某一有向二制雷达环进行“与或”运算。其真值个数即为当前调整下“当前帧”指定特征环和“目标帧”指定环的相似度判断。
f)将d)选定的雷达环第二个值为1的维度移到最前端。该维度前面的值依次补充到当前雷达环后端，保证雷达环维度一致。并再次进行e)中的相似度判断。
g)重复e),f)直到所有d)选定的雷达环的真值维度都被遍历。对比所有相似度，取最大值作为经过c)的某一有向二制雷达环和“目标帧”中选定雷达环的最终相似度值。
h)重复d)到g)直到c)中的雷达环全部计算完。最终依“当前帧”和“目标帧”激光点相似度进行排序给出对应结果。排序上出现1对多情况时，采取环数最近法则，丢弃环数差异大的对应关系。
i)保存并输出“当前帧”和“目标帧”的激光点云线对应关系。
步骤(PC)：确定相对应雷达环的各个环境位置点的对应关系。比较第i+1帧点云数据、第i帧点云数据，得到第i+1帧点云数据、第i帧点云数据的对应雷达环中环境位置点的对应关系，具体包括如下步骤(PC1)-(PC6)：
步骤(PC1)：将集合Ci+1,jb中元素循环顺序向前移动Shift(i+1,jb)个位置得到集合C’i+1,jb，其中，集合Ci+1,jb＝{Mi+1,jb,1,Mi+1,jb,2,……,Mi+1,jb,K}；
将集合Ci,ja中元素循环顺序向前移动Shift(i,ja)个位置得到集合C’i,ja，其中，集合Ci,ja＝{Mi,ja,1,Mi,ja,2,……,Mi,ja,K}。
步骤(PC2)：将集合C’i+1,jb从第1个元素开始划分为K/L个子集合，且第一个子集合与第K/L个子集合相接，使得集合C’i+1,jb对应的K/L个子集合围成环状；若子集合中有至少一个环境位置点为第一特征点，则定义该子集合为第一特征集合，否则，定义该子集合为第二特征集合；
将集合C’i,ja从第1个元素开始划分为K/L个子集合，且第一个子集合与第K/L个子集合相接，使得集合C’i,ja对应的K/L个子集合围成环状；若子集合中有至少一个环境位置点为第一特征点，则定义该子集合为第一特征集合，否则，定义该子集合为第二特征集合。
步骤(PC3)：计算环境位置点Mi+1,jb,k对应的旋转式三维激光雷达10的旋转角度θAi+1,jb,k，计算位于环境位置点Mi+1,jb,k所在的子集合一侧、且与该子集合最近的第一特征集合中第jba个环境位置点对应的旋转式三维激光雷达10的旋转角度θBi+1,jb,k，计算位于环境位置点Mi+1,jb,k所在的子集合另一侧、且与该子集合最近的第一特征集合中第jbb个环境位置点对应的旋转式三维激光雷达10的旋转角度θCi+1,jb,k；jba、jbb均为预设值。
例如，集合C’i+1,jb的第1个子集合为第二特征集合，第2个子集合为第一特征集合，第2个子集合中第1个点(例如为第jb个雷达线扫描的第ka个点)为第一特征点，第3个子集合为第二特征集合，第4个子集合为第一特征集合，第K/L个子集合为第一特征集合。则位于环境位置点Mi+1,jb,ka所在的子集合(即第2个子集合)一侧、且与该子集合最近的第一特征集合为第K/L个子集合，位于环境位置点Mi+1,jb,ka所在的子集合(即第2个子集合)另一侧、且与该子集合最近的第一特征集合为第4个子集合，即θBi+1,jb,ka为第K/L个子集合的第jba个环境位置点对应的旋转式三维激光雷达10的旋转角度，θCi+1,jb,ka为第4个子集合的第jbb个环境位置点对应的旋转式三维激光雷达10的旋转角度。
计算环境位置点Mi,ja,k对应的旋转式三维激光雷达10的旋转角度θAi,ja,k，计算位于环境位置点Mi,ja,k所在的子集合一侧、且与该子集合最近的第一特征集合中第jba个环境位置点对应的旋转式三维激光雷达10的旋转角度θBi,ja,k，计算位于环境位置点Mi,ja,k所在的子集合另一侧、且与该子集合最近的第一特征集合中第jbb个环境位置点对应的旋转式三维激光雷达10的旋转角度θCi,ja,k；jba、jbb均为预设值。jba优选为1或L，jbb优选为1或L。
步骤(PC4)：计算比例Ri+1,jb,k，其中角度θAi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中，角度θBi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中，角度θCi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中。
计算比例Ri,ja,k，其中角度θAi,ja,k包含在比例Ri,ja,k的分子和/或分母中，角度θBi,ja,k包含在比例Ri,ja,k计算式的分子和/或分母中，角度θCi,ja,k包含在比例Ri,ja,k的分子和/或分母中。
步骤(PC5)：若环境位置点Mi+1,jb,k为第一特征点，则将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值最小的值所对应的第一特征点作为环境位置点Mi+1,jb,k的对应位置点，且将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值第二小的值所对应的第一特征点作为环境位置点Mi+1,jb,k的次对应位置点；
若环境位置点Mi+1,jb,k为第二特征点，则将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值最小的值所对应的第二特征点作为环境位置点Mi+1,jb,k的对应位置点，且将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值第二小的值所对应的第二特征点作为环境位置点Mi+1,jb,k的次对应位置点；依次类推，从而在环境位置点Mi,ja,1、Mi,ja,2、……、Mi,ja,k中找到环境位置点Mi+1,jb,1、Mi+1,jb,2、……、Mi+1,jb,K的对应位置点、次对应位置点，从而得到第i帧点云数据中第ja个雷达环的各个环境位置点与第i+1帧点云数据中第jb个雷达环的各个环境位置点的对应关系。
步骤(PC6)：对于第i+1帧点云数据、第i帧点云数据的对应雷达环，重复步骤(PC1)-(PC5)，从而得到第i+1帧点云数据中、第i帧点云数据的对应雷达环的各个环境位置点的对应关系；
优选地，所述步骤(B)中，第i帧点云数据与第i+1帧点云数据中具有对应关系的环境位置点在雷达环中所在位置序号之差利用Shift(i+1,jb)、Shift(i,ja)计算。
本步骤是基于平面点/角点分布的近邻点精对应查找步骤。该方法具体为当找到两个相似度最高雷达环以后，根据编码特征之间的比例关系，确定第i帧点云数据的平面点及角点对应于第i帧点云数据中对应雷达环的哪一些角点及平面点。从目标点云(指第i帧点云)来看这是一种一对多的关系，任何一个指定的目标点云的雷达环中的点，根据其所属的分类(平面点/角点)在当前帧(指第i+1帧点云)对应的雷达环中必有多个同类特征点(即同为第一特征点或同为第二特征点)作为其近邻。而其最近邻不是以当前变换估计下距离值作为判断，而是以目标点在指定特征编码段内的所属比例和当前点在相似编码段内的比例关系来确认，其编码最相近者为即为最近邻。当第i帧、第i+1帧存在两个(最多存在两个)相似度最近邻点时，任取其一。
基于特征的点云精对应关系查找具体实施为根据步骤(PB)的线对应关系，更进一步确定“当前帧”和“目标帧”近邻点对应关系，其具体步骤如下所示。
a.读取“目标帧”的点云及其特征属性。
b.读取“当前帧”的点云及其特征属性。
c.读取“当前帧”与“目标帧”的线对应关系(指之前求得的雷达环的对应关系)及其所属变换，所述变换指的是：在进行线对应时，目标特征环循环顺序向前或向后移动的位置个数，其本质是两个点云之间的旋转角度估计。
d.对于“当前帧”的任意一个点，根据其特征对应属性及有序点排序位置。计算其所属于两个角点之间的比例关系。说明：在环状特征编码中，任何一个角点其左右必存在角点块。设其左侧角点块对应的激光头旋转角度为A，右侧角点块对应的激光头旋转角度为B，当前角点对应的激光头旋转角度为C，则当前角点在两个角点之间的位姿关系使用比例表达为(C-A)/(B-A)或(B+C)/(A)，(A-C)/(A-B)。A指左侧角点块顺时针第一个点，或者逆时针第一个点都行，方向一致即可。C指右侧角点块顺时针第一个点，或者逆时针第一个点都行，方向一致即可。
当前角点对应的激光头旋转角度为C，则当前角点在两个角点之间的位姿关系使用比例表达为(C-A)/(B-A)
e.对于“当前帧”的任意一个点，根据d计算得比例关系，在“目标帧”所属变换下必然可以找到与d所述比例关系最接近的特征点，该特征点即为两帧点云之间特征最近邻对应点，同时保存第二近邻点以备后续使用。每一个角点块中可能存在多个角点，这些角点到左右角点块的比例不一定相同，取比例最接近的两点作为对应点。当角点块中存在多个角点时，和目标帧的特征对应点比例关系有最接近的点，与有相对接近的点。这里所谓第二近邻点即是比例接近度第二的点，如果比例一致(不常见)，则一致当中任取一点。
假设前后两帧相邻位置必然存在相似的特征分布。假设雷达在两帧之间恰好转过了一个特征维度，那么在进行雷达环对应时，当前帧或者目标帧需要向前或者向后移动雷达环才能使得两者对应。
f.重复d步骤到e步骤直到所有“目标帧”中的特征点，都对应到“当前帧”的特征点。
构建误差方程
Figure BDA0003553051880000161
求解误差方程，得到第i+1个平移参数tri+1＝(dxi+1,dyi+1,dzi+1)T、翻滚角
Figure BDA0003553051880000162
俯仰角θ'i+1；
其中：
EAi+1,u1为第u1个第一直线与坐标(xa'i+1,u1,ya'i+1,u1,za'i+1,u1)之间的距离，EBi+1,v1为第v1个第一平面与坐标(xb'i+1,v1,yb'i+1,v1,zb'i+1,v1)之间的距离；
第u1个第一直线为经过第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点、第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点的同位位置点的直线；u1＝1,2,…,U(i+1)，U(i+1)为第i+1帧点云数据中与第i帧点云数据的雷达环具有对应关系的各个雷达环中第一特征点的总数。例如，第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点为第i帧点云数据中由第UA个雷达线扫描的第UB个环境位置点UC，则点UC的同位位置点为第i帧点云数据中由除了第UA个雷达线之外其他任一个雷达线扫描的第UB个环境位置点。
第v1个第一平面由第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点和次对应位置点、第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点的同位位置点形成；v1＝1,2,…,V(i+1)，V(i+1)为第i+1帧点云数据中与第i帧点云数据的雷达环具有对应关系的各个雷达环中第二特征点的总数。例如，第i+1帧中第v1个第一特征点在第i帧点云数据中的对应位置点为第i帧点云数据中由第VA个雷达线扫描的第VB个环境位置点VC，则点VC的同位位置点为第i帧点云数据中由除了第VA个雷达线之外其他任一个雷达线扫描的第VB个环境位置点。
Figure BDA0003553051880000171
坐标(xai+1,u1,yai+1,u1,zai+1,u1)为U(i+1)个第一特征点中第u1个第一特征点的坐标；
坐标(xbi+1,v1,ybi+1,v1,zbi+1,v1)为V(i+1)个第二特征点中第v1个第二特征点的坐标；
旋转矩阵
Figure BDA0003553051880000172
其中，四元数qi+1的表达式如下
Figure BDA0003553051880000173
ψ′i+1、
Figure BDA0003553051880000174
θ'i+1分别为第i帧点云数据变换到第i+1帧点云数据的偏航角、翻滚角、俯仰角，偏航角ψ'i+1根据Shifti+1,jb、Shifti,ja、旋转式三维激光雷达10的转动参数计算得到，平移参数tri+1＝(dxi+1,dyi+1,dzi+1)T。
本步骤是基于特征查找的相邻帧位姿变换求解方法，是在步骤(PB)、步骤(PC)基础上的基于最小二乘实现的帧间变换求解。
使用点线距离，和点到平面距离，作为非线性优化的残差。并且因为使用了特征编码，所有的近邻查找都通过二进制雷达环特性来实现。因为无需采用传统点云匹配中多维树查找方法，该匹配方法可以进一步提高匹配速度。
相邻两帧(第i+1帧、第i帧)之间的位姿变换参数是一个六维向量
Figure BDA0003553051880000175
其中
Figure BDA0003553051880000176
ψ'i+1、θ'i+1表示两帧之间的旋转参数这里为欧拉角(包括翻滚角、俯仰角、偏航角)，而dxi+1、dyi+1、dzi+1表示两帧之间的平移向量，大部位姿变换求解的优化方法在位变换较小时，具有更好的稳定性，这里使用线对应关系先对变换的部分参数进行初步估计，再使用非线性优化方法进行更进一步精确求解。
在完成一一对应前经历了两个变换，其中“变换一”是对特征进行了水平或垂直映射，“变换二”是步骤(PB)中环状特征真值循环前移，当确定了两个激光点云线之间的对应后，本质上已经包含两了上述两个映射参数。其中“变换二”表征两个相邻帧之间偏航关系。而依据“变换二”的偏航及“变换一”的两个特征水平或者垂直映射之间的角度差，可以判断两帧之间变换参数的变化。
水平及垂直投射公式，其投射角度计算本身便已经包含了横滚与俯仰信息。当旋转轴轴向相对于IMU的坐标已知后，一般对齐于X轴或者Y轴，此时旋转轴相对于IMU转过角度必为俯仰、或者横滚。此时另一角度不变，其俯仰或者横滚加上旋转轴转过角度，即为当前的俯仰或者横滚角。即，若旋转轴轴向对齐于IMU的Y轴，则其翻滚角不变，俯仰角加减电机转过角度；若旋转轴轴向对齐于IMU的X轴，则其俯仰角不变，翻滚角加上或减去雷达的旋转机构的转过角度，从而分别得到第i帧和第i+1帧相对于IMU初始时刻的变换参数，分别对两个时刻参数使用四元数来表达，设分别为qi和qi+1，则两帧之间的变换为qi -1*qi+1(-1表示取逆)，即Ri+1为Ri与两帧之间的变换的乘积。qi+1的表达式可通过qi对应得到。投射角度计算本身便已经包含了横滚与俯仰信息”，即对横滚与俯仰如何通过IMU及手持端102的旋转角度得到。
基于点对应关系的精准变换估计方法其具体实施流程如下。
a.根据粗对应关系，对“当前帧”进行旋转变换，以进一步降低和“目标帧”之间的差异。
b.对“当前帧”的任意角点，在“目标帧”中寻找其对应点，以及该对应点相异环数上的同位同特征值点，依据“目标帧”中对应点及同位同特征值点可以构建一条直线，求取该“当前帧”点和所构直线之间的距离，以生成一个残差方程。
c.对于“当前帧”的任意平面点，在“目标帧”中寻找其对应点，及次对应点，并且在相异环数上同位区域任取一同特征值点。依据“目标帧”中对应点及次对应点及区域同特征值点可以构建一个平面，求取该“当前帧”点和所构平面之间的距离，以生成一个残差方程。可以理解为在平面上取不同线的三个点以构成一个平面。优化目标是使得点到平面的距离最小。
d.列举所有b，c方程，并令其残差最小，其本质为一个最小二乘优化问题，可以使用高斯牛顿法及列文伯格-马夸尔特方法进行求解，其求得参数即为相邻两帧之间的变换参数。具体请参《最优化导论》(Edwin K.P.Chong Stanislaw H.Zak著)第四版，第二部分，无约束优化问题，第9章。算法实现示例可参考Google开源非线性优化库ceres或者Gtsam.
经过前成的处理得到两个对应的数据集，分别为角点特征和角点特征的对应数据集和平面点特征和面点特征的对应数据集。将目标帧的角点特征点集定义为Tpc{}平面点特征点集Tps{}，将当前帧的角点特征点集定义为Cpc{}平面点特征点集定义为Cps{}，当前帧特征点和目标特征点已经在前述内容中对应完毕。优化求解的目标在于寻找到变换参数欧拉角这里定义为
Figure BDA0003553051880000181
ψ'i+1、θ'i+1和平移参数tri+1＝(dxi+1,dyi+1,dzi+1)T，使得当前帧经上式参数变换后，当前帧角点特征到目标帧角点特征构成的线距离最小，当前帧平面特征到目标帧平面特征构成的平面距离最小。
因为惯性测量单元存在累积偏差，所以在“精准变换估计”则“精准变换估计”构建误差方差会同时求解旋转的角度参数
Figure BDA0003553051880000182
ψ'i+1、θ'i+1和平移参数tri+1＝(dxi+1,dyi+1,dzi+1)T。一帧中，各个采样点具有相同的翻滚角、相同的俯仰角、相同的偏航角。
设两帧之间的旋转函数为R，R为由变量
Figure BDA0003553051880000191
ψ'i+1、θ'i+1组成的函数。
则当前帧到目标帧这间的变换可以表示为。
tp＝G(R,t)＝R×cp+t (0.1)
G(R,t)为关于R和t的函数，cp为当前帧的特征点。设当前帧变换后的点到目标帧对应特征的距离为残差e即距离值d有。
e＝d＝D(G(R,t)) (0.2)
根据上述两个公式有残差为
e＝d＝D(G(R,t))＝D(R×cp+t) (0.3)
R的表达式见公式(1.1)，D(G(R,t))的求解根据特征不同而不同，若是角点特征则为求到对应点组成线的距离，若是为平面特征则为求到对应点组成平面的距离。
每一个特征点产生一个公式1.4的残差方程。优化问题模型为
Figure BDA0003553051880000192
以牛顿法为例，需要求解该模型一阶导数(多维变量下为雅可比矩阵)即。
Figure BDA0003553051880000193
其中
Figure BDA0003553051880000194
同理可求得其它变量偏导。代入牛顿法并进行迭代，指定迭代次数，与终止条件，当达到条件时完成求解。步骤(C)：对于第i+1帧点云数据中的各个环境位置点的坐标(xi+1,j,k,yi+1,j,k,zi+1,j,k)，利用下式计算变换后坐标(x'i+1,j,k,y'i+1,j,k,z'i,j,k)T＝Ri+1×(xi+1,j,k,yi+1,j,k,zi+1,j,k)T+tri+1。
本发明与现有技术的比较：在本机同等硬件条件下，intel core i7-9750H2.6GHz*12,显卡：GeForce RTX 2060,内存16GB。PCL icp,匹配一次非滤波用时约为300ms，滤波系数0.15m情况下约为100ms。本方法，不滤波速度为约为20ms-35ms.对于采样率10hz的16激光雷达，支持5倍速率匹配数据处理。可以便捷进行嵌入式设备迁移并在Nvidia TX2及Xavier上进行了验证。在车规级机械激光雷达匹配上本算法具有明显优势。
本发明还提供一种旋转式三维激光雷达的点云匹配系统，包括行进的载体，所述载体上固定设置有旋转式三维激光雷达10、惯性测量单元，所述旋转式三维激光雷达10具有J个雷达环，每个雷达环具有K个激光线，其特征在于，还包括控制单元，所述控制单元被配置或编程为用于执行权利要求1-8中任一项所述的旋转式三维激光雷达的点云匹配方法的步骤。
实施例2
在实际使中会现雷达在偏转后，并不会总是水平扫描向环境特征，导致求得特征点退化情况出现。三维旋转激光雷达点云特征值标准化及归一化的另一个重要任务是将斜向扫描获得特征，映射至水平/垂直情况下，点云特征退化情况如图9所示。
本实施例2与实施例1的区别在于，所述步骤(PA)中：
若|αi+ωi,j,k|＜45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000201
若|αi+ωi,j,k|≥45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000202
其中，m为预设个数，2≤m≤15，角度αi＝2×cos-1(wi)，雷达与水平面之间夹角ωi,j,k，转轴角为
Figure BDA0003553051880000203
ψi+1、
Figure BDA0003553051880000204
θi+1分别为第i帧点云数据对应的偏航角、翻滚角、俯仰角。
本实施例中，利用IMU的重力加速度方向及旋转机械结构特性数据对所有倾斜扫描点云线进行水平投影，保证所有点云线目标点特征值都等同于水平扫描状态，以去除倾斜角度下一维点云线特征畸变的情况，防止出现异常特征值，同时将所有特征值进行归一化处理映射至[-1,1]区间，以方便于特征域值划分的自动化实现。
图9中两个激光雷达的雷达线扫描同一个环境，在旋转情况下因为雷达偏转，会产生完全不同的表达，当偏转角度越偏离水平，如图9所示θ2(即环境位置点对应的环境角度变化值)会越来越接近180度而误判为平面点，但是该点实际属于角点，为应对该情况利用IMU的重力测量信息将当前平滑度值映射至水平/垂直情况下。其计算规则为：
a)根据IMU俯仰及翻滚角和机械旋转角求得当前雷达和水平面的夹角(此处思路为根据三个角度组成的逆变换在表达为轴角时在不考虑偏航情况下，恰好等于雷达和水平面夹角。推导过程不展开)。运算过程如下：
设IMU姿态欧拉角为
Figure BDA0003553051880000205
θ、ψ分别称为翻滚角、俯仰角、偏航角，其等价四元数表达如下
Figure BDA0003553051880000206
上式中，q为由欧拉角获得的四元数。根据四元数的定义，可以得到当前载体IMU相于水平面的角度αi+1＝2×cos-1(wi+1)，又通过旋转载体信息可直接读取雷达的偏转角度(即雷达与水平面之间夹角，这里假设为ωi+1,j,k，则雷达相对于水平面的夹角即为αi+1+ωi+1,j,k。
校正后一帧点云作为一个整体进行处理，所以翻滚角、俯仰角、偏航角相同。角度域是相对于激光雷达坐标系而言的，加载旋转信息后，点云变换到旋转载体坐标系下。加载旋转信息后，雷达转动轴平行于竖直方向。
c)根据a)求得夹角和b)的决策，将平滑度估计角度投射到水平坐标系或者垂直坐标下。
公式中
Figure BDA0003553051880000207
为投射到水平面的差异向量，θ1(i,j,k)为投射后的平滑度估计值。差异向量表示一组向量，该组向量用于求取当前点和附近点的差异。
水平或者竖直投射是为了在雷达斜扫时获得更为准确的拐点，否则接近水平方向上的角点易被判断为平面点。
实施例3
本实施例3与实施例1的区别在于，利用下式求解θ1(i+1,j,k)：
Figure BDA0003553051880000211
目标点近邻选择充分考虑设备系统误差，目标点近邻根据距离中心点的远近对不同的点进行加权处理，保证平滑度估计更为准确表达实际三维空间变化情况。
实施例4
本实施例4与实施例2的区别在于，环境角度变化值θ1(i+1,j,k)的求解表达式不同。
若|αi+ωi,j,k|＜45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000212
若|αi+ωi,j,k|≥45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure BDA0003553051880000213
目标点近邻选择充分考虑设备系统误差，目标点近邻根据距离中心点的远近对不同的点进行加权处理，保证平滑度估计更为准确表达实际三维空间变化情况。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落入本申请所附权利要求所限定的范围。在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

Claims (10)

1.一种旋转式三维激光雷达的点云匹配方法，所述旋转式三维激光雷达(10)设置在载体上，所述旋转式三维激光雷达(10)具有在高度方向上间隔设置的J个雷达线，其特征在于，载体行进时，旋转式三维激光雷达(10)旋转且扫描四周环境，定义第i个采样周期得到的旋转式三维激光雷达(10)的点云数据为第i帧点云数据，第i帧点云数据中由第j个雷达线扫描的各个环境位置点的数据构成第i帧点云数据的第j个雷达环，定义环境位置点Mi,j,k为第i帧点云数据中由第j个雷达线扫描的第k个环境位置点，所述点云数据为带有时间戳信息和雷达线序号的点云数据；
步骤(A)：根据第i帧点云数据判断环境位置点Mi,j,k对应的水平方向上的环境角度变化值θ1(i,j,k)是否在预设角度范围内，若判断结果为是，则判定环境位置点Mi,j,k为第一特征点，否则，判定环境位置点Mi,j,k为第二特征点，j＝1,2,…,J，k＝1,2,…,K，K为每个雷达线在每个采样周期中扫描的环境位置点的个数；
步骤(B)：构建误差方程
Figure FDA0003553051870000011
求解误差方程，得到i+1帧到i帧平移参数tri+1＝(dxi+1,dyi+1,dzi+1)T、偏航角ψ'i+1、翻滚角
Figure FDA0003553051870000012
俯仰角θ'i+1；
其中：
ψ′i+1、
Figure FDA0003553051870000013
θ'i+1分别为第i帧点云数据到第i+1帧点云数据变换的偏航角、翻滚角、俯仰角，偏航角ψ'i+1根据第i帧点云数据与第i+1帧点云数据中具有对应关系的环境位置点在雷达环中所在位置序号之差、旋转式三维激光雷达(10)的转动参数计算得到；
EAi+1,u1为第u1个第一直线与坐标(xa'i+1,u1,ya'i+1,u1,za'i+1,u1)之间的距离，EBi+1,v1为第v1个第一平面与坐标(xb'i+1,v1,yb'i+1,v1,zb'i+1,v1)之间的距离；
第u1个第一直线为经过第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点、第i+1帧中第u1个第一特征点在第i帧点云数据中的对应位置点的同位位置点的直线；u1＝1,2,…,U(i+1)，U(i+1)为与第i帧点云数据的雷达环具有对应关系的第i+1帧点云数据的雷达环中第一特征点的总数；
第v1个第一平面由第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点和次对应位置点、第i+1帧中第v1个第二特征点在第i帧点云数据中的对应位置点的同位位置点形成；v1＝1,2,…,V(i+1)，V(i+1)为与第i帧点云数据的雷达环具有对应关系的第i+1帧点云数据的雷达环中第二特征点的总数；
Figure FDA0003553051870000021
坐标(xai+1,u1,yai+1,u1,zai+1,u1)为U(i+1)个第一特征点中第u1个第一特征点的坐标；
坐标(xbi+1,v1,ybi+1,v1,zbi+1,v1)为V(i+1)个第二特征点中第v1个第二特征点的坐标；
旋转矩阵
Figure FDA0003553051870000022
其中，四元数qi+1的表达式如下
Figure FDA0003553051870000023
步骤(C)：对于第i+1帧点云数据中的各个环境位置点的坐标(xi+1,j,k,yi+1,j,k,zi+1,j,k)，利用下式计算变换后坐标(x'i+1,j,k,y'i+1,j,k,z'i,j,k)
(x'i+1,j,k,y'i+1,j,k,z'i,j,k)T＝Ri+1×(xi+1,j,k,yi+1,j,k,zi+1,j,k)T+tri+1。
2.根据权利要求1所述的旋转式三维激光雷达的点云匹配方法，其特征在于，第i+1帧点云数据、第i帧点云数据的雷达环、环境位置点的对应关系确定方法采用如下步骤(PA)-步骤(PC)：
步骤(PA)：若环境位置点Mi,j,k为第一特征点，则令环境位置点Mi,j,k对应的第一参数值Pi,j,k＝1，否则，令Pi,j,k＝0，构建第一序列Ai,j＝{Pi,j,1,Pi,j,2,…,Pi,j,K}；
步骤(PB)：令jb为预设的雷达环序号，比较第i帧点云数据中各个雷达环与第i+1帧点云数据中第jb个雷达环的相似度，具体包括如下步骤(PB1)-步骤(PB5)：
步骤(PB1)：按照元素顺序，将序列Ai+1,jb的K个元素划分为K/L组，令s＝1,2,…,K/L，第s组中的L个元素进行逻辑或运算后得到元素Qi+1,jb,s，从而得到序列A’i+1,jb＝{Qi+1,jb,1,Qi+1,jb,2,…,Qi+1,jb,K/L}；
若Qi+1,jb,1＝1，则A”i+1,jb＝A’i+1,jb，若Qi+1,jb,1＝0，则通过序列A’i+1,jb中元素的循环顺序向前移位，将序列A’i+1,jb中位于Qi+1,jb,1之后的第一个不为0的元素作为序列中的第1个元素，得到序列A”i+1,jb，其中，Shift(i+1,jb)为序列A”i+1,jb相对于序列A’i+1,jb移位的个数，若Qi+1,jb,1＝1，则Shift(i+1,jb)＝0；
步骤(PB2)：按照元素顺序，将序列Ai,1的K个元素划分为K/L组，令s＝1,2,…,K/L，第s组中的L个元素进行逻辑或运算后得到元素Qi,1,s，从而得到序列A’i,1＝{Qi,1,1,Qi,1,2,…,Qi,1,K/L}；
步骤(PB3)：通过序列中元素的循环顺序向前移位，将序列A’i,1中每个值为1的元素分别作为序列中的第1个元素，得到BM1(i,1)个序列B”i,1,1、B”i,1,2、…、B”i,1,BM1(i,1)，BM1(i,1)为序列A’i,1中值为1的元素的数量，BM1(i,1)个序列中的每个序列均与序列A”i+1,jb进行比较，得到BM1(i,1)个相似度值，所述BM1(i,1)个相似度值中的最大值为第i+1帧第jb个雷达环与第i帧第1个雷达环的相似度，按照同样方法，求得第i+1帧第jb个雷达环与第i帧第2个雷达环的相似度、第i+1帧第jb个雷达环与第i帧第3个雷达环的相似度、……、第i+1帧第jb个雷达环与第i帧第J个雷达环的相似度；
步骤(PB4)：将第i+1帧第jb个雷达环与第i帧第1个雷达环的相似度、第i+1帧第jb个雷达环与第i帧第2个雷达环的相似度、……、第i+1帧第jb个雷达环与第i帧第J个雷达环的相似度的大小进行比较，确定最大值对应的是第i+1帧第jb个雷达环与第i帧第ja个雷达环的相似度值；
步骤(PB5)：计算将序列A’i,ja-1中的元素循环顺序向前移动Shift(i,ja)个位置得到的序列、将序列A’i+1,jb-1中的元素循环顺序向前移动Shift(i+1,jb)个位置得到的序列的相似度，且计算将序列A’i,ja+1中的元素循环顺序向前移动Shift(i,ja)个位置得到的序列、将序列A’i+1,jb+1中的元素循环顺序向前移动Shift(i+1,jb)个位置得到的序列的相似度，若本步骤(PB5)中计算得到的相似度均不小于预设相似度阈值或不小于第i+1帧第jb个雷达环与第i帧第ja个雷达环的相似度，则确定第i帧点云数据的第j个雷达环与第i+1帧点云数据的第j+jb-ja个雷达环对应，其中，Shift(i,ja)为序列B”i,ja,1、B”i,ja,2、…、B”i,ja,BM1(i,ja)中与序列A”i+1,jb相似度最高的序列相对于序列A’i,ja的移位个数；
步骤(PC)：比较第i+1帧点云数据、第i帧点云数据，得到第i+1帧点云数据、第i帧点云数据的对应雷达环中环境位置点的对应关系，具体包括如下步骤(PC1)-(PC6)：
步骤(PC1)：将集合Ci+1,jb中元素循环顺序向前移动Shift(i+1,jb)个位置得到集合C’i+1,jb，其中，集合Ci+1,jb＝{Mi+1,jb,1,Mi+1,jb,2,……,Mi+1,jb,K}；
将集合Ci,ja中元素循环顺序向前移动Shift(i,ja)个位置得到集合C’i,ja，其中，集合Ci,ja＝{Mi,ja,1,Mi,ja,2,……,Mi,ja,K}；
步骤(PC2)：将集合C’i+1,jb从第1个元素开始划分为K/L个子集合，且第一个子集合与第K/L个子集合相接，使得集合C’i+1,jb对应的K/L个子集合围成环状；若子集合中有至少一个环境位置点为第一特征点，则定义该子集合为第一特征集合，否则，定义该子集合为第二特征集合；
将集合C’i,ja从第1个元素开始划分为K/L个子集合，且第一个子集合与第K/L个子集合相接，使得集合C’i,ja对应的K/L个子集合围成环状；若子集合中有至少一个环境位置点为第一特征点，则定义该子集合为第一特征集合，否则，定义该子集合为第二特征集合；
步骤(PC3)：计算环境位置点Mi+1,jb,k对应的旋转式三维激光雷达(10)的旋转角度θAi+1,jb,k，计算位于环境位置点Mi+1,jb,k所在的子集合一侧、且与该子集合最近的第一特征集合中第jba个环境位置点对应的旋转式三维激光雷达(10)的旋转角度θBi+1,jb,k，计算位于环境位置点Mi+1,jb,k所在的子集合另一侧、且与该子集合最近的第一特征集合中第jbb个环境位置点对应的旋转式三维激光雷达(10)的旋转角度θCi+1,jb,k；jba、jbb均为预设值；
计算环境位置点Mi,ja,k对应的旋转式三维激光雷达(10)的旋转角度θAi,ja,k，计算位于环境位置点Mi,ja,k所在的子集合一侧、且与该子集合最近的第一特征集合中第jba个环境位置点对应的旋转式三维激光雷达(10)的旋转角度θBi,ja,k，计算位于环境位置点Mi,ja,k所在的子集合另一侧、且与该子集合最近的第一特征集合中第jbb个环境位置点对应的旋转式三维激光雷达(10)的旋转角度θCi,ja,k；jba、jbb均为预设值；
步骤(PC4)：计算比例Ri+1,jb,k，其中角度θAi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中，角度θBi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中，角度θCi+1,jb,k包含在比例Ri+1,jb,k计算式的分子和/或分母中；
计算比例Ri,ja,k，其中角度θAi,ja,k包含在比例Ri,ja,k的分子和/或分母中，角度θBi,ja,k包含在比例Ri,ja,k计算式的分子和/或分母中，角度θCi,ja,k包含在比例Ri,ja,k的分子和/或分母中；
步骤(PC5)：若环境位置点Mi+1,jb,k为第一特征点，则将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值最小的值所对应的第一特征点作为环境位置点Mi+1,jb,k的对应位置点，且将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值第二小的值所对应的第一特征点作为环境位置点Mi+1,jb,k的次对应位置点；
若环境位置点Mi+1,jb,k为第二特征点，则将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值最小的值所对应的第二特征点作为环境位置点Mi+1,jb,k的对应位置点，且将Ri,ja,1、Ri,ja,2、……、Ri,ja,K中与Ri+1,jb,k差值第二小的值所对应的第二特征点作为环境位置点Mi+1,jb,k的次对应位置点；依次类推，从而在环境位置点Mi,ja,1、Mi,ja,2、……、Mi,ja,k中找到环境位置点Mi+1,jb,1、Mi+1,jb,2、……、Mi+1,jb,K的对应位置点、次对应位置点，从而得到第i帧点云数据中第ja个雷达环的各个环境位置点与第i+1帧点云数据中第jb个雷达环的各个环境位置点的对应关系；
步骤(PC6)：对于第i+1帧点云数据、第i帧点云数据的对应雷达环，重复步骤(PC1)-(PC5)，从而得到第i+1帧点云数据中、第i帧点云数据的对应雷达环的各个环境位置点的对应关系；
优选地，所述步骤(B)中，第i帧点云数据与第i+1帧点云数据中具有对应关系的环境位置点在雷达环中所在位置序号之差利用Shift(i+1,jb)、Shift(i,ja)计算。
3.根据权利要求1或2所述的旋转式三维激光雷达的点云匹配方法，其特征在于，所述步骤(PC4)中：
Figure FDA0003553051870000051
且
Figure FDA0003553051870000052
或
Figure FDA0003553051870000053
且
Figure FDA0003553051870000054
或
Figure FDA0003553051870000055
且
Figure FDA0003553051870000056
4.根据权利要求1或2所述的旋转式三维激光雷达的点云匹配方法，其特征在于，所述步骤(PA)中，利用下式计算环境角度变化值θ1(i,j,k)
Figure FDA0003553051870000057
其中，m为长度，2≤m≤15；
优选地，
Figure FDA0003553051870000058
优选地，m＝5。
5.根据权利要求1或2所述的旋转式三维激光雷达的点云匹配方法，其特征在于，所述步骤(PA)中：
若|αi+ωi,j,k|＜45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure FDA0003553051870000059
若|αi+ωi,j,k|≥45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure FDA00035530518700000510
其中，m为预设个数，2≤m≤15，角度αi＝2×cos-1(wi)，雷达与水平面之间夹角ωi,j,k，转轴角为
Figure FDA0003553051870000061
ψi+1、
Figure FDA0003553051870000062
θi+1分别为第i帧点云数据对应的偏航角、翻滚角、俯仰角；
优选地，若|αi+ωi,j,k|＜45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure FDA0003553051870000063
优选地，若|αi+ωi,j,k|≥45°，则利用下式计算环境角度变化值θ1(i,j,k)
Figure FDA0003553051870000064
6.根据权利要求1或2所述的旋转式三维激光雷达的点云匹配方法，其特征在于，测量旋转式三维激光雷达(10)旋转角度、旋转轴轴向，所述点云数据为根据旋转式三维激光雷达(10)旋转角度、旋转轴轴向数据校正后得到的各个环境位置点的三维坐标。
7.根据权利要求1所述的旋转式三维激光雷达的点云匹配方法，其特征在于，L的范围为5-15；优选地，L＝10。
8.根据权利要求1所述的旋转式三维激光雷达的点云匹配方法，其特征在于，所述预设角度范围为[90°-θ0，90°+θ0]，θ0为预设角度值；优选地，θ0≤30°。
9.一种旋转式三维激光雷达的点云匹配系统，包括行进的载体，所述载体上固定设置有旋转式三维激光雷达(10)、惯性测量单元，所述旋转式三维激光雷达(10)具有J个雷达环，每个雷达环具有K个激光线，其特征在于，还包括控制单元，所述控制单元被配置或编程为用于执行权利要求1-8中任一项所述的旋转式三维激光雷达的点云匹配方法的步骤。
10.根据权利要求9所述的旋转式三维激光雷达的点云匹配系统，其特征在于，所述载体为人，所述旋转式三维激光雷达由人手持。
CN202210267742.5A 2022-03-18 2022-03-18 旋转式三维激光雷达的点云匹配方法及系统 Pending CN114648561A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210267742.5A CN114648561A (zh) 2022-03-18 2022-03-18 旋转式三维激光雷达的点云匹配方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210267742.5A CN114648561A (zh) 2022-03-18 2022-03-18 旋转式三维激光雷达的点云匹配方法及系统

Publications (1)

Publication Number Publication Date
CN114648561A true CN114648561A (zh) 2022-06-21

Family

ID=81996171

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210267742.5A Pending CN114648561A (zh) 2022-03-18 2022-03-18 旋转式三维激光雷达的点云匹配方法及系统

Country Status (1)

Country Link
CN (1) CN114648561A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024077967A1 (zh) * 2022-10-13 2024-04-18 广东博智林机器人有限公司 3d激光扫描仪的控制方法及装置

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024077967A1 (zh) * 2022-10-13 2024-04-18 广东博智林机器人有限公司 3d激光扫描仪的控制方法及装置

Similar Documents

Publication Publication Date Title
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN110569704B (zh) 一种基于立体视觉的多策略自适应车道线检测方法
CN109961440B (zh) 一种基于深度图的三维激光雷达点云目标分割方法
CN110853075B (zh) 一种基于稠密点云与合成视图的视觉跟踪定位方法
Kang et al. Automatic targetless camera–lidar calibration by aligning edge with gaussian mixture model
CN112070770B (zh) 一种高精度三维地图与二维栅格地图同步构建方法
Strom et al. Graph-based segmentation for colored 3D laser point clouds
CN111781608B (zh) 一种基于fmcw激光雷达的运动目标检测方法及系统
CN111046776A (zh) 基于深度相机的移动机器人行进路径障碍物检测的方法
CN114419147A (zh) 一种救援机器人智能化远程人机交互控制方法及系统
CN107462897B (zh) 基于激光雷达的三维建图的方法
CN111123242B (zh) 一种基于激光雷达和相机的联合标定方法及计算机可读存储介质
Mandow et al. Fast range-independent spherical subsampling of 3D laser scanner points and data reduction performance evaluation for scene registration
CN112164117A (zh) 一种基于Kinect相机的V-SLAM位姿估算方法
CN111798453A (zh) 用于无人驾驶辅助定位的点云配准方法及其系统
CN113393524A (zh) 一种结合深度学习和轮廓点云重建的目标位姿估计方法
CN117710603B (zh) 一种直线几何结构约束下无人机图像三维建筑物建模方法
CN114648561A (zh) 旋转式三维激光雷达的点云匹配方法及系统
Tian et al. Lidar super-resolution based on segmentation and geometric analysis
Zhang et al. Accurate real-time SLAM based on two-step registration and multimodal loop detection
Withers et al. Modelling scene change for large-scale long term laser localisation
Elkhrachy Feature extraction of laser scan data based on geometric properties
CN117029870A (zh) 一种基于路面点云的激光里程计
Tseng et al. Computing location and orientation of polyhedral surfaces using a laser-based vision system
Andreasson et al. Non-iterative vision-based interpolation of 3D laser scans

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