CN110672097B - 一种基于激光雷达的室内定位跟踪方法、装置及系统 - Google Patents

一种基于激光雷达的室内定位跟踪方法、装置及系统 Download PDF

Info

Publication number
CN110672097B
CN110672097B CN201911148955.0A CN201911148955A CN110672097B CN 110672097 B CN110672097 B CN 110672097B CN 201911148955 A CN201911148955 A CN 201911148955A CN 110672097 B CN110672097 B CN 110672097B
Authority
CN
China
Prior art keywords
laser radar
data
hit
lidar
positioning
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.)
Active
Application number
CN201911148955.0A
Other languages
English (en)
Other versions
CN110672097A (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.)
Beijing Zhongke Shenzhi Technology Co ltd
Original Assignee
Beijing Zhongke Shenzhi 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 Beijing Zhongke Shenzhi Technology Co ltd filed Critical Beijing Zhongke Shenzhi Technology Co ltd
Priority to CN201911148955.0A priority Critical patent/CN110672097B/zh
Publication of CN110672097A publication Critical patent/CN110672097A/zh
Application granted granted Critical
Publication of CN110672097B publication Critical patent/CN110672097B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种基于激光雷达的室内定位跟踪方法、装置及系统，该方法包括：获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；对惯性测量单元IMU数据进行预处理；根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；对多个光传感器数据进行预处理；利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；对第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定被定位设备所在的位置。本发明提供的定位跟踪方法解决了现有技术中定位精度低，延迟高的问题。

Description

一种基于激光雷达的室内定位跟踪方法、装置及系统
技术领域
本发明涉及虚拟现实和空间定位技术领域，特别涉及一种基于激光雷达的室内定位跟踪方法、装置及系统。
背景技术
室内定位跟踪系统用于测量刚体的6自由度，是虚拟现实世界中一个非常基础和重要的技术领域。它可以为用户提供一种沉浸感和交互性。而准确的跟踪将进一步强化这种体验，甚至会让用户产生“错觉”。用户认为自己是在虚拟环境中，通过跟踪系统在虚拟世界中无意识地进行交互。所以精确和低延迟对于任何虚拟现实系统都是至关重要的。六自由度是指刚体在三维空间中的运动自由度。具体地说，身体可以自由地改变位置，如向前/向后(喘振)、向上/向下(起伏)、在三个垂直轴上的左/右(摇摆)平移，以及通过围绕三个垂直轴旋转(通常称为俯仰、偏航和横摇)来改变方向。
现有常用的位置跟踪技术包括GPS定位、WIFI定位、红外光学定位等。
(1)全球定位系统(GPS)，全球定位系统(GPS)是一个全球导航卫星系统(GNSS)，在所有天气条件下提供位置和时间信息。GPS的空间部分由24颗GPS工作卫星组成，这些GPS工作卫星构成GPS卫星星座。导航卫星21颗，备份卫星3颗。GPS卫星不断地传送它们当前的时间和位置。GPS接收机对多颗卫星进行监测，求解方程，确定接收机的精确位置及其与实时的偏差。至少，四颗卫星必须与被定位设备相对，才能计算出四个未知量，其定位服务比较完备，而且免费，可谓是非常理想的室外定位系统。但是其缺点也相当明显：信号受建筑物影响较大，衰弱很大，定位精度相对较低，民用GPS定位精度在10米左右，不适合虚拟现实应用。
(2)WIFI定位，每个无线接入点AP都有一个MAC地址，并且假设AP在一段时间内不会移动，因此使用这些功能，WIFI也可以做一个粗略的定位。AP定位最常用和最广泛的定位技术是基于测量接收信号的强度(接收信号强度指示或RSSI)和“指纹识别”方法。用于定位Wi-Fi热点或无线接入点的典型参数包括SSID和接入点的MAC地址。但其定位精度不高，不适合虚拟现实应用。
(3)红外光学定位，在该方案中，跟踪目标包含大量隐藏的红外标记。红外摄像机采集图像并传输到计算机上，通过特定的算法对设备的三维模型进行拟合，最终得到位置信息。这类定位系统有着非常高的定位精度，如果使用帧率很高的摄像头的话，延迟也会非常微弱，能达到非常好的效果。它的缺点是造价非常昂贵，系统复杂，应用成本极高。
针对上述的问题，目前尚未提出有效的解决方案。
发明内容
为解决上述技术问题，本发明的目的在于提供一种基于激光雷达的室内定位跟踪方法、装置及系统以解决相关技术中精度低和延迟高的问题。
第一方面，本发明实施例提供一种基于激光雷达的室内定位跟踪方法，应用于室内定位跟踪系统的定位处理设备,所述室内定位跟踪系统还包括第一激光雷达、第二激光雷达和被定位设备，所述第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，所述被定位设备上设有惯性测量单元IMU和多个光传感器，所述光传感器用于接收所述红外激光扫描光束，所述室内定位跟踪方法包括：包括：
获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；
对所述惯性测量单元IMU数据进行预处理；
根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；
对所述多个光传感器数据进行预处理，得到命中图HitMap，所述命中图HitMap包括多个2D命中点，所述2D命中点用于表示哪个光传感器被第一激光雷达或第二激光雷达中的哪个激光雷达击中；
根据所述3D位置和与所述3D位置相对应的2D命中点，利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；
对所述第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定所述被定位设备所在的位置。
进一步地，所述惯性测量单元IMU数据包括加速度计的数据、陀螺仪数据和磁强计的数据，对所述惯性测量单元IMU数据进行预处理的步骤包括：
将所述加速度计的数据转换为加速度；
将所述陀螺仪数据转换成角速度；
将所述磁强计的数据转换成磁场强度。
进一步地，所述光传感器数据包括时间刻度，对所述多个光传感器数据进行预处理，得到命中图HitMap的步骤包括：
根据每个光传感器的时间刻度获取每个光传感器相对于第一激光雷达和第二激光雷达的角度；
获取每个光传感器所对应的激光雷达信号，将所述激光雷达信号投影到投影平面，获得所述激光雷达信号在投影平面的2D命中点；
通过每个光传感器在投影平面上的2D命中点构成命中图HitMap。
进一步地，对所述第一激光雷达和第二激光雷达的平移和旋转数据进行融合的步骤包括：
根据所述第一激光雷达和第二激光雷达平移和旋转结果的可信度，对结果进行加权平均，其中，被击中的光传感器点数表示权重，平移Tout和旋转Rout的输出表达式分别为
Tout＝αT0+(1-α)T1
Rout＝Slerp(R0,R1,α)
式中，T0,R0表示第一激光雷达的平移和旋转数据，T1,R1表示第二激光雷达的平移和旋转数据，Slerp()是四元数的球面线性插值，n0,n1分别是被第一激光雷达和第二激光雷达击中的光传感器点数，以及
Figure BDA0002284007810000031
根据平移Tout和旋转Rout的输出结果，确定所述被定位设备所在的位置。
第二方面，本发明实施例提供一种基于激光雷达的室内定位跟踪装置，应用于室内定位跟踪系统的定位处理设备,所述室内定位跟踪系统还包括第一激光雷达、第二激光雷达和被定位设备，所述第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，所述被定位设备上设有惯性测量单元IMU和多个光传感器，所述光传感器用于接收所述红外激光扫描光束，所述室内定位跟踪装置包括：
获取模块，用于获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；
第一处理模块，用于对所述惯性测量单元IMU数据进行预处理；
3D位置获取模块，用于根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；
命中图获取模块，用于对所述多个光传感器数据进行预处理，得到命中图HitMap，所述命中图HitMap包括多个2D命中点，所述2D命中点用于表示哪个光传感器被第一激光雷达或第二激光雷达中的哪个激光雷达击中；
第二处理模块，用于根据所述3D位置和与所述3D位置相对应的2D命中点，利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；
定位模块，用于对所述第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定所述被定位设备所在的位置。
进一步地，所述惯性测量单元IMU数据包括加速度计的数据、陀螺仪数据和磁强计的数据，所述第一处理模块包括：
加速度处理模块，用于将所述加速度计的数据转换为加速度；
角速度处理模块，用于将所述陀螺仪数据转换成角速度；
磁场强度处理模块，用于将所述磁强计的数据转换成磁场强度。
进一步地，所述光传感器数据包括时间刻度，所述命中图获取模块包括：
角度获取模块，用于根据每个光传感器的时间刻度获取每个光传感器相对于第一激光雷达和第二激光雷达的角度；
投影模块，用于获取每个光传感器所对应的激光雷达信号，将所述激光雷达信号投影到投影平面，获得所述激光雷达信号在投影平面的2D命中点；
命中图构建模块，用于通过每个光传感器在投影平面上的2D命中点构成命中图HitMap。
进一步地，所述定位模块包括：
融合模块，用于根据所述第一激光雷达和第二激光雷达平移和旋转结果的可信度，对结果进行加权平均，其中，被击中的光传感器点数表示权重，平移Tout和旋转Rout的输出表达式分别为
Tout＝αT0+(1-α)T1
Rout＝Slerp(R0,R1,α)
式中，T0,R0表示第一激光雷达的平移和旋转数据，T1,R1表示第二激光雷达的平移和旋转数据，Slerp()是四元数的球面线性插值，n0,n1分别是被第一激光雷达和第二激光雷达击中的光传感器点数，以及
Figure BDA0002284007810000041
位置确定模块，用于根据平移Tout和旋转Rout的输出结果，确定所述被定位设备所在的位置。
第三方面，本发明实施例提供一种基于激光雷达的室内定位跟踪系统，所述室内定位跟踪系统包括定位处理设备、第一激光雷达、第二激光雷达和被定位设备，所述第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，所述被定位设备上设有惯性测量单元IMU和多个光传感器，所述光传感器用于接收所述红外激光扫描光束，所述定位处理设备包括任意一种上述的室内定位跟踪装置。
第四方面，本发明实施例提供一种存储介质，所述存储介质包括存储的程序，其中，所述程序执行任意一种上述的方法。
第五方面，本发明实施例提供一种处理器，上述处理器用于运行程序，其中，所述程序运行时执行任意一种上述的方法。
第六方面，本发明实施例提供一种电子设备，包括：一个或多个处理器，存储器，显示装置以及一个或多个程序，其中，上述一个或多个程序被存储在上述存储器中，并且被配置为由上述一个或多个处理器执行，上述一个或多个程序包括用于执行任意一种上述的方法。
有益效果
在本发明实施例中，在获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据之后，可以对惯性测量单元IMU数据和多个光传感器数据进行预处理，得到被定位设备在定位空间的3D位置和与所述3D位置相对应的2D命中点，然后利用透视n点算法，将原始数据转换为被定位设备在现实世界中的姿态，从而实现被定位设备的高精度定位，解决了现有技术中定位精度低，延迟高的问题。且本发明提供的室内定位跟踪系统价格低廉，能够应用于VR领域。
附图说明
图1是根据本发明实施例的红外光经过单轴激光扫描线产生装置的示意图；
图2是根据本发明实施例的一种基于激光雷达的室内定位跟踪方法的流程图；
图3是根据本发明实施例的被定位设备经过刚性变换后获取2D命中点的示意图；
图4是根据本发明实施例的一种基于激光雷达的室内定位跟踪装置的示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
在本发明实施中，提供一种基于激光雷达的室内定位跟踪方法，应用于室内定位跟踪系统的定位处理设备,室内定位跟踪系统还包括第一激光雷达、第二激光雷达和被定位设备，第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，被定位设备上设有惯性测量单元IMU和多个光传感器，光传感器用于接收红外激光扫描光束。
由于激光雷达和被定位设备之间的光路可能被其他物体阻挡，因此，上述室内定位跟踪系统至少需要2个激光雷达，即第一激光雷达和第二激光雷达。为避免2个激光雷达的光路冲突问题，第二激光雷达仍需与前一座激光雷达同步，两个激光雷达需交错产生激光。在实际应用中我们使这2个激光雷达在主/从模式下工作，2个激光雷达之间的相位同步是通过无线通信实现的。
激光雷达作为定位基准，其基本原理是在空间产生两个相互垂直的红外激光扫描光束，每个激光雷达有两个相互垂直的结构，分别定义为X轴和Y轴。为了将信号分类为两轴，驱动两轴的电机需要彼此错开180°以提供理论上的180°扫描角度覆盖。激光雷达的扫描角速度和扫描相位是由控制逻辑和测量电路确定的，这意味着光传感器被扫描的时刻可以计算为电流传感器位置和激光雷达位于两个垂直方向的点之间的角度。
激光雷达由三相无刷电机、激光模块、单轴激光扫描线产生装置等部分组成，激光模块用于产生红外光，红外光线来自一堆固定的led以及一对有源激光发射器。每秒钟60次，led闪烁，然后两个旋转的单轴激光扫描线产生装置中的一个将光束扫过定位空间。同时，被定位设备可以是VR头戴式耳机或控制器，被定位设备上覆盖着检测闪光和红外激光扫描光束的小型光传感器。当闪光灯发生时，被定位设备只是开始计数(像秒表)，直到它“看到”它的光传感器中的哪一个被激光束击中，并利用光传感器存在于被定位设备上的关系，当红外激光扫描光束击中光传感器时，数学地计算它相对于激光雷达的确切位置。
如图1所示，单轴激光扫描线产生装置接收激光模块产生红外光，射入圆柱形旋转结构10。该结构由三相无刷电机驱动。在圆柱形旋转结构10的中心轴上放置反射器20，将点光反射为固定在圆柱形旋转结构10的圆柱表面上的柱面透镜30，并且该柱面透镜30将点光转换成平行于圆柱形旋转结构10轴的激光扫描线，扫掠整个定位空间。
图2是根据本发明实施例的一种基于激光雷达的室内定位跟踪方法的流程图，如图2所示，该方法包括如下步骤：
步骤S102，获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；
其中，惯性测量单元IMU数据可以包括时间戳、加速度计的数据、陀螺仪数据和磁强计的数据，光传感器数据可以包括光传感器ID,时间刻度和扫描轴(X1,Y1,X2,Y2)，其中，X1和Y1分别表示第一激光雷达的X轴和Y轴，X2,Y2分别表示第二激光雷达的X轴和Y轴，扫描轴表示光传感器被哪个激光雷达的哪个轴击中。
步骤S104，对惯性测量单元IMU数据进行预处理；
由于原始的惯性测量单元IMU数据不能直接用于姿态解算，需要对其进行预处理，对惯性测量单元IMU数据进行预处理，可以包括如下步骤：
将加速度计的数据转换为加速度，单位为mm/s2；
将陀螺仪数据转换成角速度，单位为弧度/秒radian/s；
将磁强计的数据转换成磁场强度，单位为μT。
其中，在IMU数据中，加速度和角速度可以用来估计姿态，特别是旋转。由于IMU的高采样率(1000Hz)和鲁棒性，IMU数据主要作为旋转数据源，结合陀螺仪的角速度，使设备旋转更新率高，延时低。
步骤S106，根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；
步骤S108，对多个光传感器数据进行预处理，得到命中图HitMap，命中图HitMap包括多个2D命中点，2D命中点用于表示哪个光传感器被第一激光雷达或第二激光雷达中的哪个激光雷达击中；
可选的，对所述多个光传感器数据进行预处理，得到命中图HitMap的步骤可以包括：
根据每个光传感器的时间刻度获取每个光传感器相对于第一激光雷达和第二激光雷达的角度；
获取每个光传感器所对应的激光雷达信号，将所述激光雷达信号投影到投影平面，获得所述激光雷达信号在投影平面的2D命中点；
通过每个光传感器在投影平面上的2D命中点构成命中图HitMap。
其中，命中图HitMap数据结构包含有关光传感器击中激光雷达状态的信息，激光雷达的两个轴聚合到命中图HitMap中，以X和Y角表示哪个光源被哪个激光雷达撞击，命中图HitMap的每个元素表示一个光传感器被某个激光雷达的两个轴击中。对于激光雷达来说，HitMap是设备光传感器的投影结果，它可以很容易地转换成图像。
具体的，当红外激光扫描光束击中光传感器时，由于自激光雷达扫描开始以来的旋转速率、时间(无线同步)和方向是已知的，如图3所示，假设被定位设备经过刚性变换[R(ω),t]，其中ω是旋转的轴-角表示，t是是平移分量。让vi,i∈[0,n]表示附着在被定位设备的光传感器的位置。对于每个光传感器，我们可以得到激光雷达信号(αi,βi)，它可以投影到一个平面上，并在此平面上获得一个2D命中点：
Figure BDA0002284007810000081
Figure BDA0002284007810000082
步骤S110，根据3D位置和与3D位置相对应的2D命中点，利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；
给定2D命中点和每个点相对于激光雷达的3D位置，就可以得到被定位设备相对于激光雷达的平移和旋转，这就是所谓的透视n点问题(PnP问题)，可以采用多种策略来解决透视n点问题，具体取决于命中的光传感器数量，从而在准确性和鲁棒性之间取得平衡。除PnP求解外，还可以将卡尔曼滤波(Kalman filtering)应用于跟踪结果中，减少了设备的抖动，提高了跟踪的平滑度，为时间偏差提供了未来的平移和旋转预测。
其中，PnP问题定义为：透视-n点是在给定一组n个3D点和它们在图像中相应的2D投影的情况下，估计标定相机的姿态的问题。相机姿态由6个自由度(DOF)组成，由相机相对于地球的旋转(旋转、俯仰和偏航)和3D平移组成，利用EPnP求解初始状态下的被定位设备姿态，需要3个或3个以上的透视点。对于结果HitMap，可以选择EPnP或迭代法求解位姿。后者是在Levenberg-Marquardt优化的基础上，利用前面的结果迭代求解，速度更快、精度更高。根据点的分布，迭代法需要的点比EPnP少。这有助于跟踪算法解决一些非常糟糕的姿态，其中很少有点被击中，非常适合虚拟现实应用。
在实际应用中，由于IMU有副作用，它们有零点漂移，误差随时间增加。此外，角速度积分不知道器件的初始方向，因此通过IMU产生的旋转模型与被定位设备的实际旋转有恒定的差异。因此，需要利用光传感器的数据对IMU数据进行修正，并求解，作为对IMU的反馈发送。
步骤S112，对第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定被定位设备所在的位置。
在双激光雷达系统中，由于测量误差和每个激光雷达命中的点数，每个激光雷达发射一个解算的设备姿态，而两个激光雷达的结果或多或少不同，所以我们应该把多个结果合并成一个。融合策略根据每个激光雷达结果的可信度，对结果进行加权平均。众所周知，得分越多，结果就越准确稳定，信心就越大。我们利用被击中的光传感器点数表示权重，平移Tout和旋转Rout的输出表达式分别为
Tout＝αT0+(1-α)T1
Rout＝Slerp(R0,R1,α)
式中，T0,R0表示第一激光雷达的平移和旋转数据，T1,R1表示第二激光雷达的平移和旋转数据，Slerp()是四元数的球面线性插值，n0,n1分别是被第一激光雷达和第二激光雷达击中的光传感器点数，以及
Figure BDA0002284007810000091
根据平移Tout和旋转Rout的输出结果，确定所述被定位设备所在的位置。
通过本发明上述实施例，在获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据之后，可以对惯性测量单元IMU数据和多个光传感器数据进行预处理，得到被定位设备在定位空间的3D位置和与所述3D位置相对应的2D命中点，然后利用透视n点算法，将原始数据转换为被定位设备在现实世界中的姿态，从而实现被定位设备的高精度定位，解决了现有技术中定位精度低，延迟高的问题。
在本发明实施例中，还提供一种基于激光雷达的室内定位跟踪装置，需要说明的是，本申请实施例的室内定位跟踪装置可以用于执行本申请实施例所提供的室内定位跟踪方法。以下对本申请实施例提供的室内定位跟踪装置进行介绍。
图4是根据本申请实施例提供一种基于激光雷达的室内定位跟踪装置的示意图。如图4所示，该装置应用于室内定位跟踪系统的定位处理设备,室内定位跟踪系统还包括第一激光雷达、第二激光雷达和被定位设备，第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，被定位设备上设有惯性测量单元IMU和多个光传感器，光传感器用于接收红外激光扫描光束。该装置包括：
获取模块202，用于获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；
第一处理模块204，用于对惯性测量单元IMU数据进行预处理；
3D位置获取模块206，用于根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；
命中图获取模块208，用于对多个光传感器数据进行预处理，得到命中图HitMap，命中图HitMap包括多个2D命中点，2D命中点用于表示哪个光传感器被第一激光雷达或第二激光雷达中的哪个激光雷达击中；
第二处理模块210，用于根据3D位置和与3D位置相对应的2D命中点，利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；
定位模块212，用于对第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定被定位设备所在的位置。
进一步地，所述惯性测量单元IMU数据包括加速度计的数据、陀螺仪数据和磁强计的数据，所述第一处理模块可以包括：
加速度处理模块，用于将所述加速度计的数据转换为加速度；
角速度处理模块，用于将所述陀螺仪数据转换成角速度；
磁场强度处理模块，用于将所述磁强计的数据转换成磁场强度。
进一步地，所述光传感器数据包括时间刻度，所述命中图获取模块可以包括：
角度获取模块，用于根据每个光传感器的时间刻度获取每个光传感器相对于第一激光雷达和第二激光雷达的角度；
投影模块，用于获取每个光传感器所对应的激光雷达信号，将所述激光雷达信号投影到投影平面，获得所述激光雷达信号在投影平面的2D命中点；
命中图构建模块，用于通过每个光传感器在投影平面上的2D命中点构成命中图HitMap。
进一步地，所述定位模块可以包括：
融合模块，用于根据所述第一激光雷达和第二激光雷达平移和旋转结果的可信度，对结果进行加权平均，其中，被击中的光传感器点数表示权重，平移Tout和旋转Rout的输出表达式分别为
Tout＝αT0+(1-α)T1
Rout＝Slerp(R0,R1,α)
式中，T0,R0表示第一激光雷达的平移和旋转数据，T1,R1表示第二激光雷达的平移和旋转数据，Slerp()是四元数的球面线性插值，n0,n1分别是被第一激光雷达和第二激光雷达击中的光传感器点数，以及
Figure BDA0002284007810000111
位置确定模块，用于根据平移Tout和旋转Rout的输出结果，确定所述被定位设备所在的位置。
在本发明实施中，还提供一种基于激光雷达的室内定位跟踪系统，室内定位跟踪系统包括定位处理设备、第一激光雷达、第二激光雷达和被定位设备，第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，被定位设备上设有惯性测量单元IMU和多个光传感器，光传感器用于接收红外激光扫描光束，定位处理设备包括任意一种上述的室内定位跟踪装置。
其中，所述定位处理设备可以是桌上型计算机、笔记本、掌上电脑等计算设备。
由于激光雷达和被定位设备之间的光路可能被其他物体阻挡，因此，上述室内定位跟踪系统至少需要2个激光雷达，即第一激光雷达和第二激光雷达。为避免2个激光雷达的光路冲突问题，第二激光雷达仍需与前一座激光雷达同步，两个激光雷达需交错产生激光。在实际应用中我们使这2个激光雷达在主/从模式下工作，2个激光雷达之间的相位同步是通过无线通信实现的。
激光雷达作为定位基准，其基本原理是在空间产生两个相互垂直的红外激光扫描光束，每个激光雷达有两个相互垂直的结构，分别定义为X轴和Y轴。为了将信号分类为两轴，驱动两轴的电机需要彼此错开180°以提供理论上的180°扫描角度覆盖。激光雷达的扫描角速度和扫描相位是由控制逻辑和测量电路确定的，这意味着光传感器被扫描的时刻可以计算为电流传感器位置和激光雷达位于两个垂直方向的点之间的角度。
激光雷达由三相无刷电机、激光模块、单轴激光扫描线产生装置等部分组成，激光模块用于产生红外光，红外光线来自一堆固定的led以及一对有源激光发射器。每秒钟60次，led闪烁，然后两个旋转的单轴激光扫描线产生装置中的一个将光束扫过定位空间。同时，被定位设备可以是VR头戴式耳机或控制器，被定位设备上覆盖着检测闪光和红外激光扫描光束的小型光传感器。当闪光灯发生时，被定位设备只是开始计数(像秒表)，直到它“看到”它的光传感器中的哪一个被激光束击中，并利用光传感器存在于被定位设备上的关系，当红外激光扫描光束击中光传感器时，数学地计算它相对于激光雷达的确切位置。
如图1所示，单轴激光扫描线产生装置接收激光模块产生红外光，射入圆柱形旋转结构10。该结构由三相无刷电机驱动。在圆柱形旋转结构10的中心轴上放置反射器20，将点光反射为固定在圆柱形旋转结构10的圆柱表面上的柱面透镜30，并且该柱面透镜30将点光转换成平行于圆柱形旋转结构10轴的激光扫描线，扫掠整个定位空间。
在本发明实施中，还提供一种存储介质，存储介质包括存储的程序，其中，程序执行任意一种上述的方法。
在本发明实施中，还提供一种处理器，上述处理器用于运行程序，其中，程序运行时执行任意一种上述的方法。
在本发明实施中，还提供一种电子设备，包括：一个或多个处理器，存储器，显示装置以及一个或多个程序，其中，上述一个或多个程序被存储在上述存储器中，并且被配置为由上述一个或多个处理器执行，上述一个或多个程序包括用于执行任意一种上述的方法。
在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
通过本发明上述实施例，在获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据之后，可以对惯性测量单元IMU数据和多个光传感器数据进行预处理，得到被定位设备在定位空间的3D位置和与所述3D位置相对应的2D命中点，然后利用透视n点算法，将原始数据转换为被定位设备在现实世界中的姿态，从而实现被定位设备的高精度定位，解决了现有技术中定位精度低，延迟高的问题。且本发明提供的室内定位跟踪系统价格低廉，能够应用于VR领域。在空间扩展上，本发明也是线性的，只需要简单地增加激光雷达的数量就可以实现更大空间的定位。
以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。本发明的范围由所附权利要求来限定。

Claims (10)

1.一种基于激光雷达的室内定位跟踪方法，其特征在于，应用于室内定位跟踪系统的定位处理设备,所述室内定位跟踪系统还包括第一激光雷达、第二激光雷达和被定位设备，所述第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，所述被定位设备上设有惯性测量单元IMU和多个光传感器，所述光传感器用于接收所述红外激光扫描光束，所述室内定位跟踪方法包括：
获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；
对所述惯性测量单元IMU数据进行预处理；
根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；
对所述多个光传感器数据进行预处理，得到命中图HitMap，所述命中图HitMap包括多个2D命中点，所述2D命中点用于表示哪个光传感器被第一激光雷达或第二激光雷达中的哪个激光雷达击中；
根据所述3D位置和与所述3D位置相对应的2D命中点，利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；
对所述第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定所述被定位设备所在的位置。
2.根据权利要求1所述的基于激光雷达的室内定位跟踪方法，其特征在于，所述惯性测量单元IMU数据包括加速度计的数据、陀螺仪数据和磁强计的数据，对所述惯性测量单元IMU数据进行预处理的步骤包括：
将所述加速度计的数据转换为加速度；
将所述陀螺仪数据转换成角速度；
将所述磁强计的数据转换成磁场强度。
3.根据权利要求1所述的基于激光雷达的室内定位跟踪方法，其特征在于，所述光传感器数据包括时间刻度，对所述多个光传感器数据进行预处理，得到命中图HitMap的步骤包括：
根据每个光传感器的时间刻度获取每个光传感器相对于第一激光雷达和第二激光雷达的角度；
获取每个光传感器所对应的激光雷达信号，将所述激光雷达信号投影到投影平面，获得所述激光雷达信号在投影平面的2D命中点；
通过每个光传感器在投影平面上的2D命中点构成命中图HitMap。
4.根据权利要求1所述的基于激光雷达的室内定位跟踪方法，其特征在于，对所述第一激光雷达和第二激光雷达的平移和旋转数据进行融合的步骤包括：
根据所述第一激光雷达和第二激光雷达平移和旋转结果的可信度，对结果进行加权平均，其中，被击中的光传感器点数表示权重，平移Tout和旋转Rout的输出表达式分别为
Tout＝αT0+(1-α)T1
Rout＝Slerp(R0,R1,α)
式中，T0,R0表示第一激光雷达的平移和旋转数据，T1,R1表示第二激光雷达的平移和旋转数据，Slerp()是四元数的球面线性插值，n0,n1分别是被第一激光雷达和第二激光雷达击中的光传感器点数，以及
Figure FDA0002698131150000021
根据平移Tout和旋转Rout的输出结果，确定所述被定位设备所在的位置。
5.一种基于激光雷达的室内定位跟踪装置，其特征在于，应用于室内定位跟踪系统的定位处理设备,所述室内定位跟踪系统还包括第一激光雷达、第二激光雷达和被定位设备，所述第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，所述被定位设备上设有惯性测量单元IMU和多个光传感器，所述光传感器用于接收所述红外激光扫描光束，所述室内定位跟踪装置包括：
获取模块，用于获取时间戳同步的惯性测量单元IMU数据和多个光传感器数据；
第一处理模块，用于对所述惯性测量单元IMU数据进行预处理；
3D位置获取模块，用于根据预处理后的惯性测量单元IMU数据获取每个光传感器在设备坐标系中的3D位置；
命中图获取模块，用于对所述多个光传感器数据进行预处理，得到命中图HitMap，所述命中图HitMap包括多个2D命中点，所述2D命中点用于表示哪个光传感器被第一激光雷达或第二激光雷达中的哪个激光雷达击中；
第二处理模块，用于根据所述3D位置和与所述3D位置相对应的2D命中点，利用透视n点算法得到被定位设备分别相对于第一激光雷达和第二激光雷达的平移和旋转数据；
定位模块，用于对所述第一激光雷达和第二激光雷达的平移和旋转数据进行融合，从而确定所述被定位设备所在的位置。
6.根据权利要求5所述的基于激光雷达的室内定位跟踪装置，其特征在于，所述惯性测量单元IMU数据包括加速度计的数据、陀螺仪数据和磁强计的数据，所述第一处理模块包括：
加速度处理模块，用于将所述加速度计的数据转换为加速度；
角速度处理模块，用于将所述陀螺仪数据转换成角速度；
磁场强度处理模块，用于将所述磁强计的数据转换成磁场强度。
7.根据权利要求5所述的基于激光雷达的室内定位跟踪装置，其特征在于，所述光传感器数据包括时间刻度，所述命中图获取模块包括：
角度获取模块，用于根据每个光传感器的时间刻度获取每个光传感器相对于第一激光雷达和第二激光雷达的角度；
投影模块，用于获取每个光传感器所对应的激光雷达信号，将所述激光雷达信号投影到投影平面，获得所述激光雷达信号在投影平面的2D命中点；
命中图构建模块，用于通过每个光传感器在投影平面上的2D命中点构成命中图HitMap。
8.根据权利要求5所述的基于激光雷达的室内定位跟踪装置，其特征在于，所述定位模块包括：
融合模块，用于根据所述第一激光雷达和第二激光雷达平移和旋转结果的可信度，对结果进行加权平均，其中，被击中的光传感器点数表示权重，平移Tout和旋转Rout的输出表达式分别为
Tout＝αT0+(1-α)T1
Rout＝Slerp(R0,R1,α)
式中，T0,R0表示第一激光雷达的平移和旋转数据，T1,R1表示第二激光雷达的平移和旋转数据，Slerp()是四元数的球面线性插值，n0,n1分别是被第一激光雷达和第二激光雷达击中的光传感器点数，以及
Figure FDA0002698131150000031
位置确定模块，用于根据平移Tout和旋转Rout的输出结果，确定所述被定位设备所在的位置。
9.一种基于激光雷达的室内定位跟踪系统，其特征在于，所述室内定位跟踪系统包括定位处理设备、第一激光雷达、第二激光雷达和被定位设备，所述第一激光雷达和第二激光雷达同步且交错的向定位空间发送水平和垂直的红外激光扫描光束，所述被定位设备上设有惯性测量单元IMU和多个光传感器，所述光传感器用于接收所述红外激光扫描光束，所述定位处理设备包括权利要求5至8中任一项所述的室内定位跟踪装置。
10.根据权利要求9所述的基于激光雷达的室内定位跟踪系统，其特征在于，如果需要扩大定位空间范围，只需要简单地增加激光雷达的数量，多个激光雷达之间通过无线方式进行相位同步。
CN201911148955.0A 2019-11-25 2019-11-25 一种基于激光雷达的室内定位跟踪方法、装置及系统 Active CN110672097B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911148955.0A CN110672097B (zh) 2019-11-25 2019-11-25 一种基于激光雷达的室内定位跟踪方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911148955.0A CN110672097B (zh) 2019-11-25 2019-11-25 一种基于激光雷达的室内定位跟踪方法、装置及系统

Publications (2)

Publication Number Publication Date
CN110672097A CN110672097A (zh) 2020-01-10
CN110672097B true CN110672097B (zh) 2020-11-06

Family

ID=69088015

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911148955.0A Active CN110672097B (zh) 2019-11-25 2019-11-25 一种基于激光雷达的室内定位跟踪方法、装置及系统

Country Status (1)

Country Link
CN (1) CN110672097B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111337946B (zh) * 2020-04-23 2023-06-06 湖南云箭格纳微信息科技有限公司 一种旋转式全视野激光雷达扫描系统
CN111783679A (zh) * 2020-07-04 2020-10-16 北京中科深智科技有限公司 一种基于摄像头和imu数据混合的实时全身动捕系统、方法
CN113175927B (zh) * 2021-04-28 2024-09-20 北京中科深智科技有限公司 一种基于雷达和imu的姿态动态估计方法及系统
CN113591015B (zh) * 2021-07-30 2024-07-23 北京小狗吸尘器集团股份有限公司 时间延迟的计算方法、装置、存储介质及电子设备
CN113759357B (zh) * 2021-09-07 2023-11-21 四川启睿克科技有限公司 一种智慧家庭中精确定位人员位置的方法及系统
CN114485658A (zh) * 2021-12-08 2022-05-13 上海智能网联汽车技术中心有限公司 一种面向路侧感知系统精度评估的装置及方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108333579A (zh) * 2018-02-08 2018-07-27 高强 一种基于Vive Lighthouse的光感设备密集部署的系统及方法
CN209527982U (zh) * 2019-02-02 2019-10-25 河南黄烨科技有限公司 基于自适应频谱技术的多人在线式vr/ar/mr一体化头盔

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2015219059A (ja) * 2014-05-15 2015-12-07 多摩川精機株式会社 位置検出装置及び方法
CN104013387B (zh) * 2014-06-12 2016-01-27 清华大学 一种太赫兹快速断层成像系统及方法
CN107037880A (zh) * 2017-03-02 2017-08-11 深圳前海极客船长网络科技有限公司 基于虚拟现实技术的空间定位定姿系统及其方法
US20180365900A1 (en) * 2017-06-20 2018-12-20 Immerex Inc. Mixed Reality Head Mounted Display Device
CN107340522B (zh) * 2017-07-10 2020-04-17 浙江国自机器人技术有限公司 一种激光雷达定位的方法、装置及系统
US20190033460A1 (en) * 2017-07-27 2019-01-31 GM Global Technology Operations LLC Apparatus for increase field of view for lidar detector and illuminator
CN109407073B (zh) * 2017-08-15 2020-03-10 百度在线网络技术（北京）有限公司 反射值地图构建方法和装置
CN109344922A (zh) * 2018-09-06 2019-02-15 闫维新 一种具备运动捕捉功能的舞蹈动作评测系统
CN109341706B (zh) * 2018-10-17 2020-07-03 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108333579A (zh) * 2018-02-08 2018-07-27 高强 一种基于Vive Lighthouse的光感设备密集部署的系统及方法
CN209527982U (zh) * 2019-02-02 2019-10-25 河南黄烨科技有限公司 基于自适应频谱技术的多人在线式vr/ar/mr一体化头盔

Also Published As

Publication number Publication date
CN110672097A (zh) 2020-01-10

Similar Documents

Publication Publication Date Title
CN110672097B (zh) 一种基于激光雷达的室内定位跟踪方法、装置及系统
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
US11704833B2 (en) Monocular vision tracking method, apparatus and non-transitory computer-readable storage medium
Georgiev et al. Localization methods for a mobile robot in urban environments
US6409687B1 (en) Motion tracking system
US20190170865A1 (en) Surveying device, and calibration method and calibration program for surveying device
US20190025411A1 (en) Laser scanning system, laser scanning method, movable laser scanning system, and program
CN111427061A (zh) 一种机器人建图方法、装置，机器人及存储介质
CN108700946A (zh) 用于并行测距和建图的故障检测和恢复的系统和方法
JP2004264892A (ja) 運動検出装置及び通信装置
Morales et al. Design and development of a fast and precise low-cost 3D laser rangefinder
CN102575933A (zh) 地图图像综合数据库生成系统以及地图图像综合数据库生成程序
CN210952856U (zh) 一种基于激光雷达的室内定位跟踪装置及系统
CN110031880B (zh) 基于地理位置定位的高精度增强现实方法及设备
JP2014186004A (ja) 計測装置、方法及びプログラム
US20240087166A1 (en) Aligning multiple coordinate systems for informaton model rendering
CN112985394B (zh) 定位方法和装置、存储介质
CN109241233A (zh) 一种坐标匹配方法及装置
JP2021143861A (ja) 情報処理装置、情報処理方法及び情報処理システム
CN116518959A (zh) 基于uwb与3d视觉结合的空间摄像机的定位方法和装置
Calloway et al. Three tiered visual-inertial tracking and mapping for augmented reality in urban settings
CN106646349B (zh) 偏移角的确定方法、装置和系统以及偏移角的矫正系统
TW202319707A (zh) 混合型室內定位系統
WO2019221763A1 (en) Position and orientation tracking system, apparatus and method
JP5230354B2 (ja) 位置特定装置及び異動建物検出装置

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
CP02 Change in the address of a patent holder

Address after: 100000 room 311a, floor 3, building 4, courtyard 4, middle Yongchang Road, Beijing Economic and Technological Development Zone, Beijing

Patentee after: Beijing Zhongke Shenzhi Technology Co.,Ltd.

Address before: 303 platinum international building, block C, fortune World Building, 1 Hangfeng Road, Fengtai District, Beijing

Patentee before: Beijing Zhongke Shenzhi Technology Co.,Ltd.

CP02 Change in the address of a patent holder
CP03 Change of name, title or address

Address after: Room 911, 9th Floor, Block B, Xingdi Center, Building 2, No.10, Jiuxianqiao North Road, Jiangtai Township, Chaoyang District, Beijing, 100000

Patentee after: Beijing Zhongke Shenzhi Technology Co.,Ltd.

Country or region after: China

Address before: 100000 room 311a, floor 3, building 4, courtyard 4, middle Yongchang Road, Beijing Economic and Technological Development Zone, Beijing

Patentee before: Beijing Zhongke Shenzhi Technology Co.,Ltd.

Country or region before: China

CP03 Change of name, title or address