CN112764412A - 同步定位与建图优化方法 - Google Patents

同步定位与建图优化方法 Download PDF

Info

Publication number
CN112764412A
CN112764412A CN201910997510.3A CN201910997510A CN112764412A CN 112764412 A CN112764412 A CN 112764412A CN 201910997510 A CN201910997510 A CN 201910997510A CN 112764412 A CN112764412 A CN 112764412A
Authority
CN
China
Prior art keywords
time point
tracked
unit
point cloud
mapping
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
CN201910997510.3A
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.)
Automotive Research and Testing Center
Original Assignee
Automotive Research and Testing Center
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 Automotive Research and Testing Center filed Critical Automotive Research and Testing Center
Priority to CN201910997510.3A priority Critical patent/CN112764412A/zh
Publication of CN112764412A publication Critical patent/CN112764412A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

一种同步定位与建图优化方法，包含：持续感测待定位物件的周围环境，以获得在当前与前一单位时间点的第一感测数据；对于每一单位时间点的第一感测数据，根据所述第一感测数据，获得多个物件；自所述当前单位时间点的物件筛选出多个待追踪物件；对于每一待追踪物件，根据所述待追踪物件，与所述前一单位时间点中对应所述待追踪物件的待比对物件，将所述待追踪物件分类为动态物件或静态物件；根据分类结果，滤除所感测到的相关于所述周围环境的第二感测数据中对应于每一动态物件的数据部分；及根据经滤除的所述第二感测数据定位出所述待定位物件，以避免因动态物件的不确定性导致特征匹配时产生误判，进而提高定位精确度。

Description

同步定位与建图优化方法
技术领域
本发明涉及一种同步定位与建图方法，特别是涉及一种基于物件追踪的同步定位与建图优化方法。
背景技术
现有的定位技术通常使用高精度GPS导航定位模块来进行定位。然而在环境较复杂且遮蔽物多或是天气不好的情境下，GPS的定位准确度则会受影响。故根据环境特征进行自我定位的同步定位与建图(Simultaneous Localization and Mapping，简称SLAM)方法技术已普遍从室内的扫地机器人演变到为室外的自驾车所用。
现有的SLAM技术是通过感测器(如，光学镜头、激光光达(LiDAR，Light detectionand ranging)、彩色相机或深度相机)进行周围环境的感测，并将环境状况记录于地图当中，而在建立地图的同时，根据环境特征在地图上定位待定位物件。然而，现有的SLAM技术在进行特征与地图的匹配过程中，会因动态物体存在的不确定性导致匹配时产生误判或杂讯等情形，进而影响定位精确度。
发明内容
本发明的目的在于提供一种提高定位精确度的同步定位与建图优化方法。
本发明同步定位与建图优化方法，适用于定位待定位物件，并借由设置于待定位物件上的定位系统来实施，所述定位系统包含感测单元、同步定位与建图单元及电连接所述感测单元与所述同步定位与建图单元的物件分类单元，所述同步定位与建图优化方法包含以下步骤：
(A)通过所述感测单元持续感测所述待定位物件的周围环境，以获得在当前单位时间点与前一单位时间点的相关于所述周围环境的第一感测数据；
(B)对于每一单位时间点所获得的第一感测数据，通过所述物件分类单元，根据所述第一感测数据，获得多个物件；
(C)对于所述当前单位时间点所对应的物件，通过所述物件分类单元自所述物件筛选出多个待追踪物件；
(D)对于所述当前单位时间点所对应的每一待追踪物件，通过所述物件分类单元根据所述待追踪物件，与所述前一单位时间点中对应所述待追踪物件的待比对物件，将所述待追踪物件分类为动态物件或静态物件；
(E)通过所述物件分类单元将步骤(D)的分类结果传送至所述同步定位与建图单元；及
(F)通过所述同步定位与建图单元，根据步骤(D)的分类结果，滤除所述同步定位与建图单元所感测到的相关于所述周围环境的第二感测数据中对应于每一动态物件的数据部分；及
(G)通过所述同步定位与建图单元，根据经滤除的所述第二感测数据，利用同步定位与建图演算法，来定位出所述待定位物件。
本发明同步定位与建图优化方法，所述感测单元为光达模块，其中：
在步骤(A)中，通过所述光达模块持续扫描所述待定位物件的周围环境，以获得在所述当前单位时间点与所述前一单位时间点的相关于所述周围环境的点云数据，以分别作为所述第一感测数据；及
在步骤(B)中，对于每一单位时间点所获得的点云数据，通过所述物件分类单元将所述点云数据进行点云分群以获得多个不同的点云群组，并将每一点云群组标记为物件，以获得所述物件。
本发明同步定位与建图优化方法，在步骤(F)中，所述同步定位与建图单元所感测到的相关于所述周围环境的第二感测数据即为所述光达模块在所述当前单位时间点所获得的点云数据，所述同步定位与建图单元是直接滤除所述第二感测数据中对应于每一动态物件的点云群组。
本发明同步定位与建图优化方法，在步骤(C)中，所述物件分类单元是筛选出对应尺寸位于预设范围内的所述待追踪物件。
本发明同步定位与建图优化方法，步骤(D)包含以下子步骤：
(D-1)对于每一待比对物件，通过所述物件分类单元根据所述待比对物件所对应的点云群组，获得所述待比对物件的比对位置；
(D-2)对于每一待追踪物件，通过所述物件分类单元根据所述待追踪物件所对应的点云群组，获得所述待追踪物件的当前位置；
(D-3)对于每一待追踪物件，根据所述待追踪物件的当前位置、相关于所述待定位物件在所述当前单位时间点的移动的位置与指向角度，及相关于所述待定位物件在所述前一单位时间点的移动的位置与指向角度，估测所述待追踪物件在所述前一单位时间点的前一位置；及
(D-4)对于每一待追踪物件，根据所述待追踪物件的前一位置，及对应所述待追踪物件的待比对物件的比对位置，将所述待追踪物件分类为所述动态物件或所述静态物件。
本发明同步定位与建图优化方法，在步骤(A)中，每一单位时间点所获得的点云数据是所述光达模块在自第一个扫描时间点至第N个扫描时间点的预设期间内所扫描到的点云数据，在步骤(B)前，还包含以下步骤：
(H)对于每一单位时间点所获得的点云数据，根据相关于所述待定位物件自所述第一个扫描时间点至所述第N个扫描时间点的每一时间点的移动的位置与指向角度，校正所述点云数据中在第二个扫描时间点至所述第N个扫描时间点所扫描到的点云数据，以获得校正后的点云数据；
其中，在步骤(B)中，每一单位时间点所获得的点云数据是包含所述第一个扫描时间点所扫描到的点云数据及经步骤(H)校正后的点云数据。
本发明同步定位与建图优化方法，所述同步定位与建图单元包括用于拍摄所述待定位物件的周围环境，以获得相关于所述待定位物件的周围环境的第二感测数据的影像拍摄模块，在步骤(E)前，还包含以下步骤：
(I)对于每一被分类为动态物件的待追踪物件，通过所述物件分类单元根据所述待追踪物件所对应的点云群组，获得所述待追踪物件的物件中心座标；及
(J)对于每一被分类为动态物件的待追踪物件，根据相关于所述点云数据的点云座标系与所述第二感测数据的像素座标系的座标系转换的一座标转换参数组，将所述待追踪物件的物件中心座标转换为对应于所述第二感测数据的像素座标系的另一物件中心座标；
其中，在步骤(E)中，所述物件分类单元所传送的分类结果包含每一被分类为动态物件的待追踪物件的另一物件中心座标。
本发明的有益效果在于：通过所述物件分类单元将每一待追踪物件分类为动态物件或静态物件，并通过所述同步定位与建图单元滤除所述同步定位与建图单元所感测到的相关于所述周围环境的第二感测数据中对应于每一动态物件的数据部分，接着根据经滤除的所述第二感测数据利用所述同步定位与建图演算法，来定位出所述待定位物件，以避免因动态物件的不确定性导致特征匹配时产生误判，进而提高定位精确度。
附图说明
本发明的其他的特征及功效，将于参照图式的实施方式中清楚地呈现，其中：
图1是一方块图，说明实施本发明同步定位与建图优化方法的一第一实施例的定位系统；
图2是一流程图，说明本发明同步定位与建图优化方法的该第一实施例；
图3是一流程图，说明一物件分类单元如何校正每一单位时间点所获得的点云数据；
图4是一流程图，说明该物件分类单元如何将一待追踪物件分类为一动态物件或一静态物件；及
图5是一流程图，说明本发明同步定位与建图优化方法的一第二实施例。
具体实施方式
在本发明被详细描述前，应当注意在以下的说明内容中，类似的组件是以相同的编号来表示。
参阅图1，本发明同步定位与建图优化方法的一第一实施例，适用于定位一待定位物件，并通过一设置于待定位物件上的定位系统1来实施。该定位系统1包含一感测单元11、一同步定位与建图单元12及一电连接该感测单元11与该同步定位与建图单元12的物件分类单元13。
该感测单元11用于持续感测该待定位物件的一周围环境，以获得在不同单位时间点的相关于该周围环境的第一感测数据。在本实施例中，该感测单元11为一如光达感测器的光达模块。该光达模块用于持续扫描该待定位物件的该周围环境，以获得在不同单位时间点的相关于该周围环境的点云数据，以分别作为所述第一感测数据，每一单位时间点所获得的点云数据是该光达模块在一自一第一个扫描时间点至一第N个扫描时间点的预设期间内所扫描到的点云数据。
该同步定位与建图单元12包含一感测模块121，及一电连接该感测模块121的定位与建图模块122。该感测模块121用于持续感测该待定位物件的该周围环境以获得在不同单位时间点的相关于该周围环境的第二感测数据。该定位与建图模块122用于根据该感测模块121所获得的第二感测数据，利用一同步定位与建图演算法来建立相关于该周围环境的地图与定位该待定位物件。在本第一实施例中，该同步定位与建图单元12的感测模块121即为该光达模块，而该第二感测数据即为该点云数据。
该物件分类单元13用于将该当前单位时间点的第一感测数据所对应的每一待追踪物件分类为一动态物件或一静态物件。在本实施例中，该物件分类单元13与该定位与建图模块122的实施态样可整合为一具运算能力的处理器，该处理器可借由执行一物件追踪与分类程序及一同步定位与建图程序以实施本发明同步定位与建图优化方法的一第一实施例。
参阅图1与图2，本发明同步定位与建图优化方法的一第一实施例包含以下步骤。
在步骤21中，该感测单元11持续感测该待定位物件的该周围环境，以获得在一当前单位时间点与前一单位时间点的相关于该周围环境的第一感测数据。在本实施例中，该感测单元11即为该光达模块，该光达模块持续扫描该待定位物件的一周围环境，以获得在该当前单位时间点与该前一单位时间点的相关于该周围环境的点云数据，以分别作为所述第一感测数据。
在步骤22中，对于每一单位时间点所获得的点云数据，该物件分类单元13根据相关于该待定位物件自该第一个扫描时间点至该第N个扫描时间点的每一时间点的移动的一位置，与相关于该待定位物件自该第一个扫描时间点至一第N个扫描时间点的每一时间点的移动的一指向角度(Heading)，校正该点云数据中自一第二个扫描时间点至该第N个扫描时间点所扫描到的点云数据，以获得校正后的点云数据。其中，每一第一扫描时间点即为所对应的单位时间点，因此，在每一第一扫描时间点所扫描到的点云数据无须进行校正。
参阅图1与图3，值得特别说明的是，步骤22包含以下子步骤。
在子步骤221中，对于每一单位时间点所获得的点云数据，该物件分类单元13根据相关于该待定位物件自该第一个扫描时间点至该第N个扫描时间点的每一时间点的移动的该位置，与该指向角度，获得自该第二个扫描时间点至该第N个扫描时间点的每一时间点所对应的一平移校正矩阵与一旋转校正矩阵。值得一提的是，该物件分类单元13先根据第一个扫描时间点的位置与该第二个扫描时间点的位置计算出从该第一个扫描时间点到该第二个扫描时间点该待追踪物件所对应的位移(也就是说，设置于该待追踪物件上的该光达模块所对应的位移)，再根据该光达模块从该第一个扫描时间点到该第二个扫描时间点所对应的位移获得该第二个扫描时间点所对应的该平移校正矩阵，依此类推直到该物件分类单元13先根据第N-1个扫描时间点的位置与该第N个扫描时间点的位置计算出从该第N-1个扫描时间点到该第N个扫描时间点该光达模块所对应的位移，再根据该光达模块从该第N-1个扫描时间点到该第N个扫描时间点所对应的位移获得该第N个扫描时间点所对应的该平移校正矩阵，此外，该物件分类单元13根据该第一个扫描时间点的指向角度与该第二个扫描时间点的指向角度计算出从该第一个扫描时间点到该第二个扫描时间点该待追踪物件所对应的转向角度(也就是说，设置于该待追踪物件上的该光达模块所对应的转向角度)，再根据该光达模块从该第一个扫描时间点到该第二个扫描时间点所对应的转向角度获得该第二个扫描时间点所对应的该旋转校正矩阵，依此类推直到该物件分类单元13根据该第N-1个扫描时间点的指向角度与该第N个扫描时间点的指向角度计算出从该第N-1个扫描时间点到该第N个扫描时间点该待追踪物件所对应的转向角度，再根据该光达模块从该第N-1个扫描时间点到该第N个扫描时间点所对应的转向角度获得该第N个扫描时间点所对应的该旋转校正矩阵。此外，当该待定位物件为一自驾车时，该自驾车自该第一个扫描时间点至该第N个扫描时间点的每一时间点的该位置，与该指向角度(也就是说，车头方向)是可借由该自驾车的速率与转角讯号利用现有的自行车模型(Bicycle Model)而获得。
在子步骤222中，对于每一单位时间点所获得的点云数据，该物件分类单元13根据自该第二个扫描时间点至该第N个扫描时间点的每一时间点所对应的该平移校正矩阵与该旋转校正矩阵，校正该点云数据中自一第二个扫描时间点至该第N个扫描时间点所扫描到的点云数据，以获得校正后的点云数据。值得一提的是，该物件分类单元13是根据该第二个扫描时间点所对应的该平移校正矩阵与该旋转校正矩阵，校正该第二个扫描时间点所扫描到的点云数据，并根据该第二个扫描时间点所对应的该平移校正矩阵与该旋转校正矩阵与该第三个扫描时间点所对应的该平移校正矩阵与该旋转校正矩阵，校正该第三个扫描时间点所扫描到的点云数据，依此类推，该物件分类单元13根据该第二个扫描时间点至该第N个扫描时间点所对应的该平移校正矩阵与该旋转校正矩阵，校正该第N个扫描时间点所扫描到的点云数据。
值得特别说明的是，当该待定位物件的移动速度大于等于一预设速度阈值时，该光达模块在相邻两扫描时间点间的位移也会较大，因而可经由步骤22的校正来避免因该光达模块的扫描推迟而造成每一单位时间点所获得的点云数据有失真的情形，若该待定位物件的移动速度小于该预设速度阈值，则相邻两扫描时间点间的位移也会较小，故该光达模块的扫描推迟对每一单位时间点所获得的点云数据的正确性的影响不大，因此，在该待定位物件的移动速度小于该预设速度阈值的情形下，步骤22可以被省略。
在步骤23中，对于每一单位时间点所获得的点云数据(第一感测数据)，该物件分类单元13是借由将该点云数据进行点云分群以获得多个不同的点云群组，并将每一点云群组标记为一物件，以获得每一第一感测数据所对应的多个物件。其中每一单位时间点所获得的点云数据，是包含该第一扫描时间点所扫描到的点云数据及经步骤22的校正后的点云数据。
在步骤24中，对于该当前单位时间点所对应的所述物件，该物件分类单元13自所述物件筛选出多个待追踪物件。在本实施例中，该物件分类单元13是筛选出对应有尺寸位于一预设范围内的所述待追踪物件。
在步骤25中，对于该当前单位时间点所对应的每一待追踪物件，该物件分类单元13根据该待追踪物件，与该前一单位时间点中对应该待追踪物件的待比对物件，将该待追踪物件分类为一动态物件或一静态物件。
参阅图1与图4，值得一提的是，步骤25还包含以下子步骤。
在子步骤251中，对于每一待比对物件，该物件分类单元13根据该待比对物件所对应的点云群组，获得该待比对物件的一比对位置。
在子步骤252中，对于每一待追踪物件，该物件分类单元13根据该待追踪物件所对应的点云群组，获得该待追踪物件的一当前位置。
在子步骤253中，对于每一待追踪物件，该物件分类单元13根据该待追踪物件的该当前位置、相关于该待定位物件在该当前单位时间点的移动的一位置与一指向角度，及相关于该待定位物件在该前一单位时间点的移动的一位置与一指向角度，估测该待追踪物件在该前一单位时间点的前一位置。其中，该待定位物件(也就是说，自驾车)在该当前单位时间点及该前一单位时间点的该位置及该指向角度是可借由该自驾车的速率与转角讯号利用现有的自行车模型(Bicycle Model)而获得。值得特别说明的是，该物件分类单元13是先根据该前一单位时间点的该位置与该当前单位时间点的该位置计算出从该前一单位时间点到该当前单位时间点该待定位物件所对应的位移，再根据该待定位物件从该前一单位时间点到该当前单位时间点所对应的位移获得该当前单位时间点所对应的该平移校正矩阵，并根据该前一单位时间点的该指向角度与该当前单位时间点的该指向角度计算出从该前一单位时间点到该当前单位时间点该待定位物件所对应的转向角度，再根据该待定位物件从该前一单位时间点到该当前单位时间点所对应的转向角度获得在该当前单位时间点该待定位物件所对应的该旋转校正矩阵。接着，该物件分类单元13根据该待定位物件的该当前位置、该当前单位时间点所对应的该平移校正矩阵与该旋转校正矩阵，估测该待追踪物件在该前一单位时间点的该前一位置。
在子步骤254中，对于每一待追踪物件，该物件分类单元13根据该待追踪物件的该前一位置，及对应该待追踪物件的待比对物件的该比对位置，将该待追踪物件分类为该动态物件或该静态物件。值得一提的是，当该待追踪物件的该前一位置与对应该待追踪物件的待比对物件的该比对位置间的距离小于等于一预设距离阈值时，该待追踪物件即被分类为该静态物件；当该待追踪物件的该前一位置与对应该待追踪物件的待比对物件的该比对位置间的距离大于该预设距离阈值时，该待追踪物件即被分类为该动态物件。
在步骤26中，该物件分类单元13将步骤25的分类结果传送至该同步定位与建图单元12。在本实施例中，该分类结果指示出每一被分类为动态物件的待追踪物件。此外，由于在本实施例中，该同步定位与建图单元12的感测模块121即为该光达模块，故该物件分类单元13的分类结果中所指示出的该待追踪物件可以是该待追踪物件所对应的点云群组，也可以是该物件分类单元13根据该待追踪物件所对应的点云群组而获得的对应于该待追踪物件的一物件中心座标。
在步骤27中，该同步定位与建图单元12根据步骤25的分类结果，滤除该同步定位与建图单元所感测到的相关于该周围环境的第二感测数据中对应于每一动态物件的数据部分。
在步骤28中，该同步定位与建图单元12，根据经滤除的该第二感测数据，利用一同步定位与建图演算法，来定位出该待定位物件。借由该同步定位与建图单元12不考虑被分类为动态物件的待追踪物件来进行该待定位物件的定位，可避免因动态物件的不确定性导致特征匹配时产生误判，进而提高定位精确度。
参阅图1与图5，本发明同步定位与建图优化方法的一第二实施例大致上是与该第一实施例相同，相同处不再赘言，其中不同处在于，在该第二实施例中，该同步定位与建图单元12的感测模块121为一如深度相机的影像拍摄模块，该感测模块121同步于该感测单元11在不同单位时间点获得所述第二感测数据，而该感测模块121所获得的第二感测数据即为相关于该待定位物件的该周围环境的影像数据，因而该物件分类单元13所传送的分类结果中所指示出的每一待追踪物件须经过转换处理，以使该同步定位与建图单元12能从该影像数据识别出对应于该分类结果中所指示出的每一待追踪物件的影像部分，故在该第二实施例更进一步包含一步骤35及一步骤36。
在步骤35中，对于每一被分类为动态物件的待追踪物件，该物件分类单元13根据该待追踪物件所对应的点云群组，获得该待追踪物件的一物件中心座标。
在步骤36中，对于每一被分类为动态物件的待追踪物件，该物件分类单元13根据相关于该点云数据的一点云座标系与该第二感测数据(也就是说，该影像数据)的像素座标系的座标系转换的一座标转换参数组，将该待追踪物件的该物件中心座标转换为一对应于该第二感测数据的像素座标系的另一物件中心座标。值得一提的是，该座标转换参数组包含一外部参数矩阵及一内部参数矩阵，该外部参数矩阵是根据该感测单元11的架设位置与该感测模块121的架设位置利用以下公式(1)而获得。该内部参数矩阵是根据该感测模块121的以像素为单位的焦距距离，及影像中心座标而获得。
Figure BDA0002240231380000101
其中，
Figure BDA0002240231380000102
为该感测模块121的架设位置，
Figure BDA0002240231380000103
为该感测单元11的架设位置，Rroll×Rpitch×Ryaw为该外部参数矩阵的三轴旋转矩阵，
Figure BDA0002240231380000104
为该外部参数矩阵的平移矩阵。
在步骤37中，该物件分类单元13将步骤34的分类结果传送至该同步定位与建图单元12。其中，所传送的分类结果包含每一被分类为动态物件的待追踪物件的另一物件中心座标。
在步骤38中，该同步定位与建图单元12根据步骤34的分类结果，滤除该同步定位与建图单元所感测到的相关于该周围环境的影像数据中对应于每一动态物件的数据部分。值得一提的是，第二感测数据即为该影像拍摄模块在该当前单位时间点所获得的影像数据，此外，在本第二实施例中，由于该分类结果所指示出的该待追踪物件为该待追踪物件在该影像数据中的该另一物件中心座标，故该同步定位与建图单元12即可识别出每一动态的待追踪物件的影像部分，而不考虑对应于动态的待追踪物件的影像部分来进行定位。
在步骤39中，该同步定位与建图单元12，根据经滤除的该影像数据，利用该同步定位与建图演算法，来定位出该待定位物件。
综上所述，本发明同步定位与建图优化方法，借由该物件分类单元13将每一待追踪物件分类为一动态物件或一静态物件，以使该同步定位与建图单元12不考虑被分类为动态物件的待追踪物件，来定位出该待定位物件，以避免因动态物件的不确定性导致特征匹配时产生误判，进而提高定位精确度，故确实能达成本发明的目的。
以上所述者，仅为本发明的实施例而已，当不能以此限定本发明实施的范围，即凡依本发明权利要求书及说明书内容所作的简单的等效变化与修饰，皆仍属本发明的范围。

Claims (7)

1.一种同步定位与建图优化方法，适用于定位待定位物件，并通过设置于所述待定位物件上的定位系统来实施，所述定位系统包含感测单元、同步定位与建图单元及电连接所述感测单元与所述同步定位与建图单元的物件分类单元，其特征在于：所述同步定位与建图优化方法包含以下步骤：
(A)通过所述感测单元持续感测所述待定位物件的周围环境，以获得在当前单位时间点与前一单位时间点的相关于所述周围环境的第一感测数据；
(B)对于每一单位时间点所获得的第一感测数据，通过所述物件分类单元，根据所述第一感测数据，获得多个物件；
(C)对于所述当前单位时间点所对应的物件，通过所述物件分类单元自所述物件筛选出多个待追踪物件；
(D)对于所述当前单位时间点所对应的每一待追踪物件，通过所述物件分类单元根据所述待追踪物件，与所述前一单位时间点中对应所述待追踪物件的待比对物件，将所述待追踪物件分类为动态物件或静态物件；
(E)通过所述物件分类单元将步骤(D)的分类结果传送至所述同步定位与建图单元；及
(F)通过所述同步定位与建图单元，根据步骤(D)的分类结果，滤除所述同步定位与建图单元所感测到之相关于所述周围环境的第二感测数据中对应于每一动态物件的数据部分；及
(G)通过所述同步定位与建图单元，根据经滤除的所述第二感测数据，利用同步定位与建图演算法，来定位出所述待定位物件。
2.根据权利要求1所述的同步定位与建图优化方法，其特征在于：所述感测单元为光达模块，其中：
在步骤(A)中，通过所述光达模块持续扫描所述待定位物件的周围环境，以获得在所述当前单位时间点与所述前一单位时间点的相关于所述周围环境的点云数据，以分别作为所述第一感测数据；及
在步骤(B)中，对于每一单位时间点所获得的点云数据，通过所述物件分类单元将所述点云数据进行点云分群以获得多个不同的点云群组，并将每一点云群组标记为物件，以获得所述物件。
3.根据权利要求2所述的同步定位与建图优化方法，其特征在于：在步骤(F)中，所述同步定位与建图单元所感测到的相关于所述周围环境的第二感测数据即为所述光达模块在所述当前单位时间点所获得的点云数据，所述同步定位与建图单元是直接滤除所述第二感测数据中对应于每一动态物件的点云群组。
4.根据权利要求1所述的同步定位与建图优化方法，其特征在于：在步骤(C)中，所述物件分类单元是筛选出对应尺寸位于预设范围内的所述待追踪物件。
5.根据权利要求2所述的同步定位与建图优化方法，其特征在于：步骤(D)包含以下子步骤：
(D-1)对于每一待比对物件，通过所述物件分类单元根据所述待比对物件所对应的点云群组，获得所述待比对物件的比对位置；
(D-2)对于每一待追踪物件，通过所述物件分类单元根据所述待追踪物件所对应的点云群组，获得所述待追踪物件的当前位置；
(D-3)对于每一待追踪物件，根据所述待追踪物件的当前位置、相关于所述待定位物件在所述当前单位时间点的移动的位置与指向角度，及相关于所述待定位物件在所述前一单位时间点的移动的位置与指向角度，估测所述待追踪物件在所述前一单位时间点的前一位置；及
(D-4)对于每一待追踪物件，根据所述待追踪物件的前一位置，及对应所述待追踪物件的待比对物件的比对位置，将所述待追踪物件分类为所述动态物件或所述静态物件。
6.根据权利要求1所述的同步定位与建图优化方法，其特征在于：在步骤(A)中，每一单位时间点所获得的点云数据是所述光达模块在自第一个扫描时间点至第N个扫描时间点的预设期间内所扫描到的点云数据，在步骤(B)前，还包含以下步骤：
(H)对于每一单位时间点所获得的点云数据，根据相关于所述待定位物件自所述第一个扫描时间点至所述第N个扫描时间点的每一时间点的移动的位置与指向角度，校正所述点云数据中在第二个扫描时间点至所述第N个扫描时间点所扫描到的点云数据，以获得校正后的点云数据；
其中，在步骤(B)中，每一单位时间点所获得的点云数据是包含所述第一个扫描时间点所扫描到的点云数据及经步骤(H)校正后的点云数据。
7.根据权利要求2所述的同步定位与建图优化方法，其特征在于：所述同步定位与建图单元包括用于拍摄所述待定位物件的周围环境，以获得相关于所述待定位物件的周围环境的第二感测数据的影像拍摄模块，在步骤(E)前，还包含以下步骤：
(I)对于每一被分类为动态物件的待追踪物件，通过所述物件分类单元根据所述待追踪物件所对应的点云群组，获得所述待追踪物件的物件中心座标；及
(J)对于每一被分类为动态物件的待追踪物件，根据相关于所述点云数据的点云座标系与所述第二感测数据的像素座标系的座标系转换的一座标转换参数组，将所述待追踪物件的物件中心座标转换为对应于所述第二感测数据的像素座标系的另一物件中心座标；
其中，在步骤(E)中，所述物件分类单元所传送的分类结果包含每一被分类为动态物件的待追踪物件的另一物件中心座标。
CN201910997510.3A 2019-10-21 2019-10-21 同步定位与建图优化方法 Pending CN112764412A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910997510.3A CN112764412A (zh) 2019-10-21 2019-10-21 同步定位与建图优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910997510.3A CN112764412A (zh) 2019-10-21 2019-10-21 同步定位与建图优化方法

Publications (1)

Publication Number Publication Date
CN112764412A true CN112764412A (zh) 2021-05-07

Family

ID=75691505

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910997510.3A Pending CN112764412A (zh) 2019-10-21 2019-10-21 同步定位与建图优化方法

Country Status (1)

Country Link
CN (1) CN112764412A (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101750060A (zh) * 2008-12-22 2010-06-23 财团法人工业技术研究院 利用感测元件的定位与检测的系统与方法
CN105869136A (zh) * 2015-01-22 2016-08-17 北京雷动云合智能技术有限公司 一种基于多摄像机的协作式视觉slam方法
CN109814572A (zh) * 2019-02-20 2019-05-28 广州市山丘智能科技有限公司 移动机器人定位建图方法、装置、移动机器人和存储介质
CN110084272A (zh) * 2019-03-26 2019-08-02 哈尔滨工业大学（深圳） 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法
TW202108982A (zh) * 2019-08-28 2021-03-01 財團法人車輛研究測試中心 同步定位與建圖優化方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101750060A (zh) * 2008-12-22 2010-06-23 财团法人工业技术研究院 利用感测元件的定位与检测的系统与方法
CN105869136A (zh) * 2015-01-22 2016-08-17 北京雷动云合智能技术有限公司 一种基于多摄像机的协作式视觉slam方法
CN109814572A (zh) * 2019-02-20 2019-05-28 广州市山丘智能科技有限公司 移动机器人定位建图方法、装置、移动机器人和存储介质
CN110084272A (zh) * 2019-03-26 2019-08-02 哈尔滨工业大学（深圳） 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法
TW202108982A (zh) * 2019-08-28 2021-03-01 財團法人車輛研究測試中心 同步定位與建圖優化方法

Similar Documents

Publication Publication Date Title
JP6825569B2 (ja) 信号処理装置、信号処理方法、およびプログラム
CN108020825B (zh) 激光雷达、激光摄像头、视频摄像头的融合标定系统及方法
CN111368706A (zh) 基于毫米波雷达和机器视觉的数据融合动态车辆检测方法
CN103065323B (zh) 一种基于单应性变换矩阵的分段空间对准方法
CN110246177B (zh) 一种基于视觉的自动测波方法
CN111708038A (zh) 基于姿态传感器和gnss的无人船激光雷达点云数据校正方法
CN110570449B (zh) 一种基于毫米波雷达与视觉slam的定位与建图方法
CN112233188B (zh) 一种激光雷达和全景相机的数据融合系统的标定方法
CN108868268A (zh) 基于点到面距离和互相关熵配准的无人车位姿估计方法
JP2006252473A (ja) 障害物検出装置、キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム
WO2019208101A1 (ja) 位置推定装置
CN103886107A (zh) 基于天花板图像信息的机器人定位与地图构建系统
CN109801336B (zh) 基于可见光及红外光视觉的机载目标定位系统及方法
JP2022045947A5 (zh)
JP2001091649A (ja) 合成開口レーダ画像精密幾何補正用地上基準点装置
TWI736960B (zh) 同步定位與建圖優化方法
CN112764412A (zh) 同步定位与建图优化方法
CN115792912A (zh) 一种弱观测条件下基于视觉与毫米波雷达融合的水面无人艇环境感知方法及系统
CN114973037B (zh) 一种无人机智能检测与同步定位多目标的方法
US11774593B2 (en) Method of simultaneous localization and mapping
CN112837343B (zh) 基于相机阵列的低空无人机防控光电预警识别方法及系统
WO2022133986A1 (en) Accuracy estimation method and system
CN113763457B (zh) 落差地形的标定方法、装置、电子设备和存储介质
JP7092741B2 (ja) 自己位置推定方法
CN114494039A (zh) 一种水下高光谱推扫图像几何校正的方法

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