CN113324542A - 一种定位方法、装置、设备和存储介质 - Google Patents
一种定位方法、装置、设备和存储介质 Download PDFInfo
- Publication number
- CN113324542A CN113324542A CN202110632444.7A CN202110632444A CN113324542A CN 113324542 A CN113324542 A CN 113324542A CN 202110632444 A CN202110632444 A CN 202110632444A CN 113324542 A CN113324542 A CN 113324542A
- Authority
- CN
- China
- Prior art keywords
- change information
- determining
- laser radar
- pose
- initial
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 76
- 230000008859 change Effects 0.000 claims abstract description 360
- 238000005259 measurement Methods 0.000 claims abstract description 127
- 206010034719 Personality change Diseases 0.000 claims description 25
- 238000004364 calculation method Methods 0.000 claims description 21
- 238000006073 displacement reaction Methods 0.000 claims description 15
- 230000004927 fusion Effects 0.000 claims description 7
- 230000004807 localization Effects 0.000 claims 1
- 230000008569 process Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 7
- 230000003287 optical effect Effects 0.000 description 7
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000005457 optimization Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000004590 computer program Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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
-
- 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
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)
- Navigation (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明实施例公开了一种定位方法、装置、设备和存储介质,应用于电子设备,电子设备包括激光雷达,该方法包括:确定电子设备本体的初始位姿变化信息;根据激光雷达获取到的激光雷达数据,确定电子设备本体的测量位姿变化信息,并计算初始位姿变化信息和测量位姿变化信息的第一残差;根据激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;根据第一残差和第二残差确定联合残差,并基于所述联合残差确定目标位姿。通过初始位姿变化信息和测量位姿变化信息确定的第一残差以及当前激光雷达数据和障碍物数据确定的第二残差构建联合残差,并通过联合残差确定目标位姿,实现电子设备的快速准确定位。
Description
技术领域
本发明实施例涉及定位技术,尤其涉及一种定位方法、装置、设备和存储介质。
背景技术
近年来,在室内外场景中越来越多的机器人被应用去执行各种任务,机器人要实现自主导航并执行各种任务,需要非常准确地知道自己的位置。机器人的定位可以为机器人提供了实时的位姿信息,它对机器人至关重要,是实现避障、路径规划与决策和控制的基础。
现有技术中,可以基于角位移传感器或角位移传感器和惯性测量单元确定里程计信息,并通过里程计信息和自适应蒙特卡罗定位算法实现对机器人的定位。
在实现本发明的过程中,发现现有技术中至少存在以下技术问题:
1、角位移传感器或角位移传感器和惯性测量单元确定的里程计信息存在一定的累计误差,使用的时间越长,累计误差越大,得到的位姿信息误差也越大,这对于需要进行精确定位的机器人影响较大;
2、自适应蒙特卡罗定位算法收敛速度较慢,无法及时确定机器人的位姿信息;
3、自适应蒙特卡罗定位算法严重依赖当前观测信息和先验地图,一旦环境发生变化,定位的效果往往会变得很差。
发明内容
本发明提供一种定位方法、装置、设备和存储介质,以实现对电子设备的快速准确定位。
第一方面,本发明实施例提供了一种定位方法,应用于电子设备,所述电子设备包括激光雷达,所述方法包括:
确定电子设备本体的初始位姿变化信息;
根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
第二方面,本发明实施例还提供了一种定位装置,应用于电子设备,所述电子设备包括激光雷达,该装置包括:
初始位姿变化信息确定模块,用于确定电子设备本体的初始位姿变化信息;
第一残差计算模块,用于根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
第二残差确定模块,用于根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
目标位姿确定模块,用于根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
第三方面,本发明实施例还提供了一种电子设备,所述电子设备包括:一个或多个处理器;存储装置,用于存储一个或多个程序;激光雷达,用于获取激光雷达数据;当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如第一方面中任一所述的定位方法。
第四方面,本发明实施例还提供了一种包含计算机可执行指令的存储介质,所述计算机可执行指令在由计算机处理器执行时用于执行如第一方面中任一所述的定位方法。
上述发明中的实施例具有如下优点或有益效果:
本发明实施例提供一种定位方法,应用于电子设备,所述电子设备包括激光雷达,所述方法包括:确定电子设备本体的初始位姿变化信息;根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。上述技术方案,首先可以确定初始位姿变化信息,其次可以根据激光雷达数据确定测量姿态变化信息,进而可以由初始位姿变化信息和测量位姿变化信息确定第一残差,再次可以根据当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差,以使得第一残差和第二残差可以构建得到联合残差,调整目标位姿变化信息,以根据联合残差收敛确定目标位姿,实现对电子设备的快速准确定位。
附图说明
图1为本发明实施例提供的一种定位方法的流程图;
图2为本发明实施例提供的另一种定位方法的流程图;
图3为本发明实施例提供的一种定位方法中S210的流程图;
图4为本发明实施例提供的一种定位方法中S220的流程图;
图5为本发明实施例提供的另一种定位方法中S220的流程图;
图6为本发明实施例提供的一种定位方法中一种实现方式的流程图;
图7为本发明实施例提供的一种定位方法中另一种实现方式的流程图;
图8为本发明实施例提供的一种定位方法中又一种实现方式的流程图;
图9为本发明实施例提供的一种定位方法中又一种实现方式的流程图;
图10为本发明实施例提供的一种定位方法中又一种实现方式的流程图;
图11为本发明实施例提供的一种定位方法的实现方式的定位效果图;
图12为本发明实施例提供的一种定位装置的结构示意图;
图13为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅仅用于解释本发明,而非对本发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与本发明相关的部分而非全部结构。
在更加详细地讨论示例性实施例之前应当提到的是,一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理,但是其中的许多操作可以被并行地、并发地或者同时实施。此外,各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止,但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。此外,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
图1为本发明实施例提供的一种定位方法的流程图,适用于需要对电子设备进行快速准确定定位的情况。该方法可以由定位装置来执行,该装置可以由软件和/或硬件的方式来实现。可选的,该定位装置可配置于电子设备中,该电子设备可以是移动机器人等电子设备。如图1所述,该方法具体包括以下步骤:
S110、确定电子设备本体的初始位姿变化信息。
其中,电子设备可以包括移动机器人,移动机器人可以在不同的环境中进行移动,以实现各种功能,例如实现货运或者清扫等功能。
初始位姿变化信息可以为当前位置到历史位置的初始位姿变化信息,其中,历史位置可以为预设时间之前的位置;初始位姿变化信息也可以为当前帧到历史帧的初始位姿变化信息,其中,历史帧可以为上一帧。
当然,初始位姿变化信息可以包括初始位置变化信息和初始姿态变化信息,具体可以包括初始横轴数据变化信息、初始纵轴数据变化信息和初始角度变化信息,进而可以根据当前横轴数据、当前纵轴数据、历史横轴数据和历史纵轴数据确定初始位置变化信息,还可以根据当前角度数据和历史角度数据确定初始位姿变化信息。
具体地,电子设备可以运行在当前环境中,在电子设备的运行过程中,可以确定历史位置到当前位置时电子设备本体的初始位姿变化信息,还可以确定历史帧到当前帧时电子设备本体的初始位姿变化信息。
本发明实施例中,可以通过角位移传感器确定电子设备本体的第一位姿变化信息,通过惯性测量单元确定第二姿态变化信息,进而对第一位姿变化信息和第二姿态变化信息进行信息融合,以确定电子设备本体的初始位姿变化信息。
S120、根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差。
其中,激光雷达为以发射激光束探测电子设备的位置、速度等特征量的雷达系统。激光雷达可以向目标发射探测信号(激光束),然后将接收到的从目标反射回来的信号(目标回波)与发射信号进行比较,并进行提处理,以获得目标的激光雷达数据,如目标距离、方位、高度、速度、姿态、甚至形状等参数,从而对移动机器人等目标进行探测、跟踪和识别。当然,激光雷达可以装载在电子设备上,激光雷达和电子设备本体存在相对位姿关系,当获取到激光雷达的位姿信息后,可以根据该位姿信息和相对位姿关系确定电子设备本体的位姿信息。
具体地,激光雷达数据可以包括激光雷达的距离、方位、高度、速度和姿态,因此,可以根据当前激光雷达的距离、方位、高度、速度和姿态,以及历史激光雷达的距离、方位、高度、速度和姿态,确定激光雷达的测量位姿变化信息。当然,电子设备的存储器中可以存储有上述相对位姿关系,在确定激光雷达的测量位姿变化信息后,可以根据激光雷达的测量位姿变化信息以及相对位姿关系,对激光雷达的测量位姿变化信息进行转化,以得到电子设备本体的测量位姿变化信息。
在实际应用中,随着电子设备上所装载的激光雷达位姿的变换,可以得到不同的激光点云信息,即激光雷达数据。基于rangeflow2D(RF2O)激光里程计对不同激光点云信息进行匹配计算可以得到激光雷达的测量位姿变化信息,其中,RF2O是一种将光流法应用到激光雷达的方法,通过计算相邻两帧激光雷达数据的变换得到激光雷达的测量位姿变化信息,进而可以得到当前电子设备本体的测量位姿变化信息。
另外,初始位姿变化信息可以包括初始横轴数据变化信息、初始纵轴数据变化信息和初始角度变化信息,测量位姿变化信息也可以包括测量横轴数据变化信息、测量纵轴数据变化信息和测量角度变化信息。
进而,可以基于预设正比例关系以及初始横轴数据变化信息与测量横轴数据变化信息的第一差值、初始纵轴数据变化信息与测量纵轴数据变化信息的第二差值和初始角度变化信息与测量角度变化信息的第三差值确定第一残差,即第一差值、第二差值和第三差值越小,对应确定的第一残差越小,反之,第一差值、第二差值和第三差值越大,对应确定的第一残差也越大。
需要说明的是,预设正比例关系可以预先存储在电子设备的存储器中,并且本发明实施例中,对预设正比例关系的系数不做具体限定,可以根据实际电子设备的需求确定预设正比例关系中的系数。
本发明实施例中,在根据激光雷达数据确定激光雷达的测量位姿变化信息后,可以基于激光雷达的测量位姿变化信息以及激光雷达的测量位姿变化信息和电子设备本体的相对位姿关系,对激光雷达的测量位姿变化信息进行转化以得到电子设备本体的测量位姿变化信息,进而可以根据电子设备本体的初始位姿变化信息和测量位姿变化信息确定第一残差。
S130、根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差。
在实际应用中,电子设备的运行环境可能与预设地图所包含的地图信息存在出入,所以可以根据实际获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差。
其中,当前激光雷达数据可以为当前环境下电子设备可以获取到的激光雷达数据。预设地图可以为根据电子设备建图算法提前建立好的栅格地图,该栅格地图可以为二维地图,每个栅格可以表示现实空间中预设大小的区域。栅格地图可以包括障碍物占据、障碍物空闲和障碍物未知三种状态确定的三种栅格数值。
具体地,根据当前激光雷达数据以及栅格数值可以确定当前环境中任一栅格的估算值,进而可以根据当前雷达数据以及当前位置所处栅格的相邻栅格的栅格数值,确定电子设备当前所处栅格的估算值。进而可以根据预设残差计算公式、电子设备当前所处栅格的估算值和电子设备当前所处栅格的的真实栅格数值,确定第二残差。
本发明实施例中,可以根据当前激光雷达数据确定当前栅格的估算值,进而根据该估算值和当前栅格对应的真实栅格数值确定第二残差。
S140、根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
具体地,可以基于非线性优化算法搜寻当前环境,以确定目标位姿,在确定目标位姿的过程中可以同时计算所搜寻位置的联合残差,并且将联合残差最小的位置确定为目标位姿。
本发明实施例中,测量位姿变化信息和初始位姿变化信息确定第一残差,预设地图所包含的真实栅格数据和根据当前激光雷达数据确定的估算值确定第二残差,第一残差和第二残差进行联合构建的联合残差可以降低激光雷达和预设地图的精确度对确定目标位姿的影响。
本发明实施例提供的一种定位方法,应用于电子设备,所述电子设备包括激光雷达,所述方法包括:确定电子设备本体的初始位姿变化信息;根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。上述技术方案,首先可以确定初始位姿变化信息,其次可以根据激光雷达数据确定测量姿态变化信息,进而可以由初始位姿变化信息和测量位姿变化信息确定第一残差,再次可以根据当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差,以使得第一残差和第二残差可以构建得到联合残差,调整目标位姿变化信息,以基于联合残差收敛确定目标位姿,实现对电子设备的快速准确定位。
图2为本发明实施例提供的另一种定位方法的流程图。本实施例是在上述实施例的基础上进行具体化,其中与上述各实施例相同或相应的术语的解释在此不再赘述。参见图2,本发明实施例提供的一种定位方法包括:
S210、确定电子设备本体的初始位姿变化信息。
可选的,所述电子设备还包括:角位移传感器和惯性测量单元。
角位移传感器可以轮式编码器,轮式编码器可以通过检测电子设备轮子在一定时间内转过的弧度数来确定电子设备本体位姿的变化,进而通过对每个时刻内电子设备位姿变化的累计运算可以得到电子设备相对于其起点的实时位姿。轮式编码器的精度很大程度上取决于标定,但是在实际应用中很难精确的标定出轮式编码器的内参,因此其每次相对位姿的变换都存在一定的误差,时间越长,电子设备相对于其起点的实时位姿累计误差越大。
惯性测量单元可以用于测量物体三轴姿态角(或角速率)以及加速度。惯性测量单元可以包含三个单轴的加速度计和三个单轴的陀螺,加速度计用于检测物体在载体坐标系统独立三轴的加速度信号,而陀螺用于检测载体相对于导航坐标系的角速度信号,惯性测量单元可以测量物体在三维空间中的角速度和加速度,算角速度信号在单位时间内的积分运算可以得到惯性测量单元的姿态变化,计算加速度信号在单位时间内的二次积分可以得到惯性测量单元的位移变化。
图3为本发明实施例提供的一种定位方法中S210的流程图,相应地,S210具体可以包括:
S2110、根据所述角位移传感器确定所述电子设备本体的当前里程计信息和历史里程计信息,并基于所述当前里程计信息和所述历史里程计信息确定第一位姿变化信息。
其中,所述第一位姿变化信息包括第一位置变化信息和第一姿态变化信息。
当前里程计信息可以包括当前位姿和当前速度,其中当前位姿包括当前位置和当前姿态,当前速度包括当前线速度和当前角速度;历史里程计信息可以包括历史位姿和历史速度,其中历史位姿包括历史位置和历史姿态,历史速度包括历史线速度和历史角速度。
当然,当前里程计信息可以为电子设备所处当前位置时的里程计信息,历史里程计信息可以为电子设备在历史时刻或者历史位置等的里程计信息。
具体地,在确定当前里程计信息和历史里程计信息之后,可以根据当前位置和历史位置确定第一位置变化信息,还可以根据当前姿态和历史姿态确定第一姿态变化信息,进而可以根据第一位置变化信息和第一姿态变化信息确定第一位姿变化信息。
S2120、根据所述惯性测量单元确定所述电子设备本体的当前角速度和历史角速度,并基于所述当前角速度和所述历史角速度确定第二姿态变化信息。
其中,对惯性测量单元所获取到的角速度信号进行预设时间内的积分运算可以得到惯性测量单元的姿态变化信息,对惯性测量单元所获取到的加速度信号进行预设时间内的二次积分运算可以得到惯性测量单元的位置变化信息,由于二次积分会将误差放大,所以本发明实施例中可以舍弃根据惯性测量单元得到的位置变化信息。
具体地,可以确定当前角速度和历史角速度的平均角速度,并基于该平均角速度进行预设时间内的积分运算,可以得到预设时间内的第二姿态变化信息。
S2130、将所述第一位置变化信息确定为初始位置变化信息,对所述第一姿态变化信息和所述第二姿态变化信息进行信息融合,以确定初始姿态变化信息,并将所述初始位置变化信息和所述初始姿态变化信息确定为所述初始位姿变化信息。
具体地,可以计算第一姿态变化信息和第二姿态变化信息的平均姿态变化信息,并将平均姿态变化信息确定为初始姿态变化信息。
本发明实施例中,可以对角速度传感器获取到的第一姿态变化信息和惯性测量单元获取到的第二姿态变换信息进行信息融合,得到初始姿态变化信息,以提高初始姿态变化信息的准确度,进而可以将角速度传感器获取到的第一位置变化信息确定为初始位置变化信息,进一步根据初始姿态变化信息和初始位姿变化信息确定初始位姿变化信息,以提高初始位姿变化信息的准确度和精度。
S220、根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差。
图4为本发明实施例提供的一种定位方法中S220的流程图,一种实施方式中,根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,具体可以包括:
S2210、根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息。
一种实施方式中,当所述历史里程计信息包括历史时刻里程计信息,所述历史角速度包括历史时刻角速度时,所述初始位姿变化信息包括初始时刻位姿变化信息,相应地,S2210具体可以包括:
根据所述激光雷达获取到的当前激光雷达数据和历史时刻激光雷达数据,确定所述激光位姿变化信息。
具体地,根据当前激光雷达数据和历史时刻激光雷达数据确定的为一段时间内的激光位姿变化信息。
相应地,也可以确定相同时间段内的初始位姿变化信息,进一步以确定第一残差。
一种实施方式中,当所述历史里程计信息包括历史帧里程计信息,所述历史角速度包括历史帧角速度时,所述初始位姿变化信息包括初始帧位姿变化信息,相应地,S2210具体可以包括:
基于激光雷达里程计算法对所述激光雷达获取到的当前激光雷达数据和历史帧激光雷达数据进行计算,以确定所述激光位姿变化信息。
具体地,根据当前激光雷达数据和历史帧激光雷达数据确定的为一段帧内的激光位姿变化信息,本发明实施例中,可以根据当前激光雷达数据和上一帧激光雷达数据确定激光位姿变化信息。
相应地,也可以确定相同帧段内的初始位姿变化信息,本发明实施例中,可以确定当前帧和上一帧,即相邻两帧之间初始位姿变化信息,进一步以确定第一残差。
S2211、根据所述激光雷达和所述电子设备本体之间的位姿关系以及所述激光位姿变化信息,确定所述测量位姿变化信息。
激光雷达和电子设备本体之间存在预设的位姿关系,该位姿关系可以预存在电子设备的存储器中,且该位姿关系与电子设备自身相关,且后期不可改变。根据激光雷达可以构建激光雷达坐标系,根据电子设备本体可以构建系统坐标系,激光雷达坐标系和系统坐标系之间的位姿关系即为激光雷达和电子设备本体的位姿关系。
具体地,在激光雷达坐标系中确定激光雷达的激光位姿变化信息之后,可以基于位姿关系对激光位姿变化信息进行坐标系转化,进而确定电子设备本体的测量位姿变化信息。
图5为本发明实施例提供的另一种定位方法中S220的流程图,一种实施方式中,所述测量位姿变化信息包括测量位置变化信息和测量姿态变化信息,相应地,计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差,包括:
S2220、根据所述初始位置变化信息和所述测量位置变化信息确定第一差值。
其中,第一差值可以包括第一横轴差值和第一纵轴差值。
具体地,初始位姿变化信息可以包括初始位置变化信息,进而可以包括初始横轴数据变化信息和初始纵轴数据变化信息,测量位置变化信息可以包括测量横轴数据变化信息和测量纵轴数据变化信息。计算测量横轴数据变化信息和初始横轴数据变化信息的差值可以确定第一横轴差值,计算初始纵轴数据变化信息和测量纵轴数据变化信息的差值可以确定第一纵轴差值。进一步,第一横轴差值和第一纵轴差值可以确定第一差值。
S2221、根据所述初始姿态变化信息和所述测量姿态变化信息确定第二差值。
其中,初始姿态变化信息可以包括初始角度变化信息,测量姿态变化信息可以包括测量角度变化信息。
具体地,计算测量角度变化信息和初始角度变化信息的差值可以确定第二差值。
S2222、基于所述第一差值和所述第二差值确定所述第一残差。
具体地,可以对第一差值和第二差值进行求和,以确定第一残差。
S230、根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差。
一种实施方式中,所述预设地图包括栅格地图,栅格地图所包含各栅格的空间状态用于确定各所述栅格的数值,相应地,S230具体可以包括:
根据所述当前激光雷达数据和相邻栅格的数值确定各所述栅格的估算值;基于各所述估算值和预设残差计算公式,确定所述第二残差。
其中,空间状态包括占据、空闲和未知,各栅格的空间状态用于确定各所述栅格的数值,例如,占据状态可以确定栅格的数值为1,空闲状态可以确定栅格的数值为0,未知状态可以确定栅格的数值为-1。
具体地,当前激光雷达数据可以包括多个激光点,栅格地图包含各栅格的栅格数值。每个栅格在栅格地图中对应确定的位置。进而可以将当前激光雷达数据投影到栅格地图中,这样当前激光雷达数据所包含的多个激光点可以投影在对应的栅格内部。对激光点所在栅格的栅格数据和周围栅格的栅格数值进行线性插值,可以得到激光点在栅格地图的估算值,进而可以对所有激光点在栅格地图的估算值进行求和,以得到当前激光帧与栅格地图的匹配度。当匹配度越高时,第二残差越小,进而确定得到的电子设备本体的目标位姿越准确;当匹配度越低时,第二残差越大,进而确定得到的电子设备本体的目标位姿误差较大。
因此,预设残差计算公式可以为预设反比例计算公式,该预设反比例计算公式的系数在此不做具体限定,可以根据实际需求进行确定。本发明实施例中,在确定当前激光点的估算值之后,如果当前激光点的估算值越接近1,所有激光点的估算值之和越大,激光点与地图匹配度越好,基于预设反比例计算公式计算出第二残差就越小。
S240、根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
一种实施方式中,根据所述第一残差和所述第二残差确定联合残差,包括:
将所述第一残差和所述第二残差确定为预设二元一次表达式的第一表达项和第二表达项;基于所述预设二元一次表达式以及所述第一表达项和所述第二表达项,确定所述联合残差。
具体地,可以将预设二元一次表达式的数字项确定为0,且第一系数和第二系数在此不做具体限定,可以根据实际需求进行限定。
本发明实施例中,第一残差可以用公式X=f1(x1,y1,θ1)表示,其中,x1表示横轴数据的第一残差,y1表示纵轴数据的第一残差,θ1表示角度的第一残差;第二残差可以用公式Y=f2(x2,y2,θ2)表示,其中,x2表示横轴数据的第二残差,y2表示纵轴数据的第二残差,θ2表示角度的第二残差;进而可以确定联合残差Z=aX+bY=af1(x1,y1,θ1)+bf2(x2,y2,θ2),其中,a和b可以为可调节参数,表示第一残差和第二残差的权重。当然,在本发明实施例中,可调节参数的具体取值可以不做具体限定,可以根据实际需求进行确定。
一种实施方式中,基于所述联合残差确定目标位姿,包括:
基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,并计算所述联合残差;在所述联合残差最小时,基于初始位姿和确定的所述目标位姿变化信息,确定所述目标位姿。
具体地,可以持续调整电子设备的位姿变化信息,同时计算电子设备所处位姿的联合残差,并且在联合残差最小时,将该位姿变化信息确定为目标位姿变化信息,进而可以根据电子设备的初始位姿和目标位姿变化信息确定目标位姿,实现对电子设备的定位。
进一步地,基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,包括:
基于预设滑窗所包含的滑窗数据调整目标位姿变化信息其中,所述滑窗数据包括预设数量的初始位姿变化信息和预设数量的测量位姿变化信息;依次更新所述滑窗数据,直至确定所述目标位姿。
其中,预设滑窗可以包括预设帧的滑窗数据,进而可以包括预设帧的初始位姿变化信息和预设帧的测量位姿变化信息。
具体地,在优化过程中,随着帧的增多,对所有的帧全部优化求解是不可靠的也是没有必要的。为了避免这种情况,可以使用预设滑窗将待优化的滑窗数据限制在一定的数量来控制优化数据的规模。当然,本发明实施例中,滑窗数据的帧数在此不做具体限定,可以根据时间需求进行确定。
基于预设滑窗内每相邻两帧之间的约束构成的第一残差和每一帧与地图的约束构成的第二残差构造一个联合残差,来优化预设滑窗内每一帧的位姿信息。一旦需要优化下一帧滑窗数据时,滑窗会添加下一帧滑窗数据,同时删除最旧的一帧滑窗数据来保持优化帧的数量。进而可以得到电子设备当前帧被优化后的姿态信息即目标位姿,实现对电子设备的精确定位。另外,本发明实施例所采用的非线性优化算法可以提升对电子设备定位的速度。
另外,本发明实施例采用预设滑窗不仅仅可以使用预设地图来进行电子设备定位,还使用了激光里程计算法提供的帧间约束。这样即使当前环境发生变化,测量位姿变化信息与预设地图信息无法实现较好的匹配,本发明实施例还可以使用激光里程计算法提供的信息来对电子设备的初始姿态变化信息进行优化修正,从而保证一个较好的电子设备定位效果。
本发明实施例提供的一种定位方法包括:确定电子设备本体的初始位姿变化信息;根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。上述技术方案,首先可以确定初始位姿变化信息,其次可以根据激光雷达数据确定测量姿态变化信息,进而可以由初始位姿变化信息和测量位姿变化信息确定第一残差,再次可以根据当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差,以使得第一残差和第二残差可以构建得到联合残差,调整目标位姿变化信息,以基于联合残差收敛确定目标位姿,实现对电子设备的快速准确定位。
图6为本发明实施例提供的一种定位方法中一种实现方式的流程图,如图6所示,包括:
S610、根据所述角位移传感器确定所述电子设备本体的当前里程计信息和历史里程计信息,并基于所述当前里程计信息和所述历史里程计信息确定第一位姿变化信息。
其中,所述第一位姿变化信息包括第一位置变化信息和第一姿态变化信息。
S611、根据所述惯性测量单元确定所述电子设备本体的当前角速度和历史角速度,并基于所述当前角速度和所述历史角速度确定第二姿态变化信息。
S612、将所述第一位置变化信息确定为初始位置变化信息,对所述第一姿态变化信息和所述第二姿态变化信息进行信息融合,以确定初始姿态变化信息,并将所述初始位置变化信息和所述初始姿态变化信息确定为所述初始位姿变化信息。
S613、根据所述激光雷达获取到的激光雷达数据,确定所述激光雷达的激光位姿变化信息。
S614、根据所述激光雷达和所述电子设备本体之间的位姿关系以及所述激光位姿变化信息,确定所述测量位姿变化信息。
S615、根据所述初始位置变化信息和所述测量位置变化信息确定第一差值。
S616、根据所述初始姿态变化信息和所述测量姿态变化信息确定第二差值。
S617、基于所述第一差值和所述第二差值确定所述第一残差。
S618、根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差。
S619、将所述第一残差和所述第二残差确定为预设二元一次表达式的第一表达项和第二表达项。
S620、基于所述预设二元一次表达式以及所述第一表达项和所述第二表达项,确定所述联合残差。
S621、基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,并计算所述联合残差。
S622、在所述联合残差最小时,基于初始位姿和确定的所述目标位姿变化信息,确定所述目标位姿。
本发明实施例所提供的一种定位的实现方式,根据所述角位移传感器确定所述电子设备本体的当前里程计信息和历史里程计信息,并基于所述当前里程计信息和所述历史里程计信息确定第一位姿变化信息,其中,所述第一位姿变化信息包括第一位置变化信息和第一姿态变化信息;根据所述惯性测量单元确定所述电子设备本体的当前角速度和历史角速度,并基于所述当前角速度和所述历史角速度确定第二姿态变化信息;将所述第一位置变化信息确定为初始位置变化信息,对所述第一姿态变化信息和所述第二姿态变化信息进行信息融合,以确定初始姿态变化信息,并将所述初始位置变化信息和所述初始姿态变化信息确定为所述初始位姿变化信息;根据所述激光雷达获取到的激光雷达数据,确定所述激光雷达的激光位姿变化信息;根据所述激光雷达和所述电子设备本体之间的位姿关系以及所述激光位姿变化信息,确定所述测量位姿变化信息;根据所述初始位置变化信息和所述测量位置变化信息确定第一差值;根据所述初始姿态变化信息和所述测量姿态变化信息确定第二差值;基于所述第一差值和所述第二差值确定所述第一残差;根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;将所述第一残差和所述第二残差确定为预设二元一次表达式的第一表达项和第二表达项;基于所述预设二元一次表达式以及所述第一表达项和所述第二表达项,确定所述联合残差;基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,并计算所述联合残差;在所述联合残差最小时,基于初始位姿和确定的所述目标位姿变化信息,确定所述目标位姿。上述技术方案,首先可以确定初始位姿变化信息,其次可以根据激光雷达数据确定测量姿态变化信息,进而可以由初始位姿变化信息和测量位姿变化信息确定第一残差,再次可以根据当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差,以使得第一残差和第二残差可以构建得到联合残差,调整目标位姿变化信息,以使得联合残差收敛,并在联合残差收敛时根据确定的目标位姿变化信息和初始位姿确定目标位姿,实现对电子设备的快速准确定位。
图7为本发明实施例提供的一种定位方法中另一种实现方式的流程图,如图7所示,当所述历史里程计信息包括历史时刻里程计信息,所述历史角速度包括历史时刻角速度时,所述初始位姿变化信息包括初始时刻位姿变化信息,相应地,S613可以替换为:
S713、根据所述激光雷达获取到的当前激光雷达数据和历史时刻激光雷达数据,确定所述激光位姿变化信息。
图8为本发明实施例提供的一种定位方法中又一种实现方式的流程图,如图8所示,当所述历史里程计信息包括历史帧里程计信息,所述历史角速度包括历史帧角速度时,所述初始位姿变化信息包括初始帧位姿变化信息,相应地,S613可以替换为:
S813、基于激光雷达里程计算法对所述激光雷达获取到的当前激光雷达数据和历史帧激光雷达数据进行计算,以确定所述激光位姿变化信息。
图9为本发明实施例提供的一种定位方法中又一种实现方式的流程图,如图9所示,所述预设地图包括栅格地图,栅格地图所包含各栅格的空间状态用于确定各所述栅格的数值,相应地,S618可以替换为:
S918、根据所述当前激光雷达数据和相邻栅格的数值确定各所述栅格的估算值;基于各所述估算值和预设残差计算公式,确定所述第二残差。
图10为本发明实施例提供的一种定位方法中又一种实现方式的流程图,如图10所示,S621可以替换为:
S1021、基于预设滑窗所包含的滑窗数据调整目标位姿变化信息其中,所述滑窗数据包括预设数量的初始位姿变化信息和预设数量的测量位姿变化信息;依次更新所述滑窗数据,直至确定所述目标位姿。
图11为本发明实施例提供的一种定位方法的实现方式的定位效果图,如图11所示,在当前环境发生改变,即当前环境信息与栅格地图中对应位置的预设环境信息不同时,也可以实现电子设备的快速准确定位。
图12为本发明实施例提供的一种定位装置的结构示意图,该定位装置可配置于电子设备中。该装置与上述各实施例的定位方法属于同一个发明构思,在定位装置的实施例中未详尽描述的细节内容,可以参考上述定位方法的实施例。
该定位装置的具体结构如图12所示,包括:
初始位姿变化信息确定模块1210,用于确定电子设备本体的初始位姿变化信息;
第一残差计算模块1220,用于根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
第二残差确定模块1230,用于根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
目标位姿确定模块1240,用于根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
本发明实施例提供的一种定位装置,应用于电子设备,所述电子设备包括激光雷达,该装置通过确定电子设备本体的初始位姿变化信息;根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。上述技术方案,首先可以确定初始位姿变化信息,其次可以根据激光雷达数据确定测量姿态变化信息,进而可以由初始位姿变化信息和测量位姿变化信息确定第一残差,再次可以根据当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差,以使得第一残差和第二残差可以构建得到联合残差,调整目标位姿变化信息,以基于联合残差确定目标位姿,实现对电子设备的快速准确定位。
在上述实施例的基础上,第一残差计算模块1220,具体用于:
根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息;根据所述激光雷达和所述电子设备本体之间的位姿关系以及所述激光位姿变化信息,确定所述测量位姿变化信息。
一种实施方式中,当所述历史里程计信息包括历史时刻里程计信息,所述历史角速度包括历史时刻角速度时,所述初始位姿变化信息包括初始时刻位姿变化信息,相应地,根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息,包括:根据所述激光雷达获取到的当前激光雷达数据和历史时刻激光雷达数据,确定所述激光位姿变化信息。
另一种实施方式中,当所述历史里程计信息包括历史帧里程计信息,所述历史角速度包括历史帧角速度时,所述初始位姿变化信息包括初始帧位姿变化信息,相应地,根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息,包括:基于激光雷达里程计算法对所述激光雷达获取到的当前激光雷达数据和历史帧激光雷达数据进行计算,以确定所述激光位姿变化信息。
在上述实施例的基础上,所述电子设备还包括:角位移传感器和惯性测量单元,相应地,初始位姿变化信息确定模块1210,具体用于:根据所述角位移传感器确定所述电子设备本体的当前里程计信息和历史里程计信息,并基于所述当前里程计信息和所述历史里程计信息确定第一位姿变化信息,其中,所述第一位姿变化信息包括第一位置变化信息和第一姿态变化信息;根据所述惯性测量单元确定所述电子设备本体的当前角速度和历史角速度,并基于所述当前角速度和所述历史角速度确定第二姿态变化信息;将所述第一位置变化信息确定为初始位置变化信息,对所述第一姿态变化信息和所述第二姿态变化信息进行信息融合,以确定初始姿态变化信息,并将所述初始位置变化信息和所述初始姿态变化信息确定为所述初始位姿变化信息。
在上述实施例的基础上,所述测量位姿变化信息包括测量位置变化信息和测量姿态变化信息,相应地,第一残差计算模块1220,还具体用于:根据所述初始位置变化信息和所述测量位置变化信息确定第一差值;根据所述初始姿态变化信息和所述测量姿态变化信息确定第二差值;基于所述第一差值和所述第二差值确定所述第一残差。
在上述实施例的基础上,所述预设地图包括栅格地图,栅格地图所包含各栅格的空间状态用于确定各所述栅格的数值,相应地,第二残差确定模块1230,具体用于:根据所述当前激光雷达数据和相邻栅格的数值确定各所述栅格的估算值;基于各所述估算值和预设残差计算公式,确定所述第二残差。
在上述实施例的基础上,目标位姿确定模块1240,具体用于:将所述第一残差和所述第二残差确定为预设二元一次表达式的第一表达项和第二表达项;基于所述预设二元一次表达式以及所述第一表达项和所述第二表达项,确定所述联合残差;基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,并计算所述联合残差;在所述联合残差最小时,基于初始位姿和确定的所述目标位姿变化信息,确定所述目标位姿。
一种实施方式中,基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,包括:基于预设滑窗所包含的滑窗数据调整目标位姿变化信息其中,所述滑窗数据包括预设数量的初始位姿变化信息和预设数量的测量位姿变化信息;依次更新所述滑窗数据,直至确定所述目标位姿。
本发明实施例所提供的定位装置可执行本发明任意实施例所提供的定位方法,具备执行定位方法相应的功能模块和有益效果。
值得注意的是,上述定位装置的实施例中,所包括的各个单元和模块只是按照功能逻辑进行划分的,但并不局限于上述的划分,只要能够实现相应的功能即可;另外,各功能单元的具体名称也只是为了便于相互区分,并不用于限制本发明的保护范围。
图13为本发明实施例提供的一种电子设备的结构示意图。图13示出了适于用来实现本发明实施方式的示例性电子设备13的框图。图13显示的电子设备13仅仅是一个示例,不应对本发明实施例的功能和使用范围带来任何限制。
如图13所示,电子设备13以通用计算电子设备的形式表现。电子设备13的组件可以包括但不限于:一个或者多个处理器或者处理单元16,系统存储器28,连接不同系统组件(包括系统存储器28和处理单元16)的总线18。
总线18表示几类总线结构中的一种或多种,包括存储器总线或者存储器控制器,外围总线,图形加速端口,处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说,这些体系结构包括但不限于工业标准体系结构(ISA)总线,微通道体系结构(MAC)总线,增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。
电子设备13典型地包括多种计算机系统可读介质。这些介质可以是任何能够被电子设备13访问的可用介质,包括易失性和非易失性介质,可移动的和不可移动的介质。
系统存储器28可以包括易失性存储器形式的计算机系统可读介质,例如随机存取存储器(RAM)30和/或高速缓存存储器32。电子设备13可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例,存储系统34可以用于读写不可移动的、非易失性磁介质(图13未显示,通常称为“硬盘驱动器”)。尽管图13中未示出,可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器,以及对可移动非易失性光盘(例如CD-ROM,DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下,每个驱动器可以通过一个或者多个数据介质接口与总线18相连。系统存储器28可以包括至少一个程序产品,该程序产品具有一组(例如至少一个)程序模块,这些程序模块被配置以执行本发明各实施例的功能。
具有一组(至少一个)程序模块42的程序/实用工具40,可以存储在例如系统存储器28中,这样的程序模块42包括但不限于操作系统、一个或者多个应用程序、其它程序模块以及程序数据,这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本发明所描述的实施例中的功能和/或方法。
电子设备13也可以与一个或多个外部设备14(例如键盘、指向设备、显示器24等)通信,还可与一个或者多个使得用户能与该电子设备13交互的设备通信,和/或与使得该电子设备13能与一个或多个其它计算设备进行通信的任何设备(例如网卡,调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口22进行。并且,电子设备13还可以通过网络适配器20与一个或者多个网络(例如局域网(LAN),广域网(WAN)和/或公共网络,例如因特网)通信。如图13所示,网络适配器20通过总线18与电子设备13的其它模块通信。应当明白,尽管图13中未示出,可以结合电子设备13使用其它硬件和/或软件模块,包括但不限于:微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。
处理单元16通过运行存储在系统存储器28中的程序,从而执行各种功能应用以及页面显示,例如实现本发实施例所提供的定位方法,包括:
确定电子设备本体的初始位姿变化信息;
根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
当然,本领域技术人员可以理解,处理器还可以实现本发明任意实施例所提供的定位方法的技术方案。
本发明实施例提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现例如本发实施例所提供的定位方法,该方法包括:
确定电子设备本体的初始位姿变化信息;
根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
本发明实施例的计算机存储介质,可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于:电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:无线、电线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码,程序设计语言包括面向对象的程序设计语言,诸如Java、Smalltalk、C++,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中,远程计算机可以通过任意种类的网络,包括局域网(LAN)或广域网(WAN),连接到用户计算机,或者,可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
本领域普通技术人员应该明白,上述的本发明的各模块或各步骤可以用通用的计算装置来实现,它们可以集中在单个计算装置上,或者分布在多个计算装置所组成的网络上,可选地,他们可以用计算机装置可执行的程序代码来实现,从而可以将它们存储在存储装置中由计算装置来执行,或者将它们分别制作成各个集成电路模块,或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样,本发明不限制于任何特定的硬件和软件的结合。
注意,上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解,本发明不限于这里的特定实施例,对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此,虽然通过以上实施例对本发明进行了较为详细的说明,但是本发明不仅仅限于以上实施例,在不脱离本发明构思的情况下,还可以包括更多其他等效实施例,而本发明的范围由所附的权利要求范围决定。
Claims (13)
1.一种定位方法,其特征在于,应用于电子设备,所述电子设备包括激光雷达,所述方法包括:
确定电子设备本体的初始位姿变化信息;
根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
2.根据权利要求1所述的定位方法,其特征在于,根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,包括:
根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息;
根据所述激光雷达和所述电子设备本体之间的位姿关系以及所述激光位姿变化信息,确定所述测量位姿变化信息。
3.根据权利要求2所述的定位方法,其特征在于,所述电子设备还包括:角位移传感器和惯性测量单元,
相应地,确定电子设备本体的初始位姿变化信息,包括:
根据所述角位移传感器确定所述电子设备本体的当前里程计信息和历史里程计信息,并基于所述当前里程计信息和所述历史里程计信息确定第一位姿变化信息,其中,所述第一位姿变化信息包括第一位置变化信息和第一姿态变化信息;
根据所述惯性测量单元确定所述电子设备本体的当前角速度和历史角速度,并基于所述当前角速度和所述历史角速度确定第二姿态变化信息;
将所述第一位置变化信息确定为初始位置变化信息,对所述第一姿态变化信息和所述第二姿态变化信息进行信息融合,以确定初始姿态变化信息,并将所述初始位置变化信息和所述初始姿态变化信息确定为所述初始位姿变化信息。
4.根据权利要求3所述的定位方法,其特征在于,当所述历史里程计信息包括历史时刻里程计信息,所述历史角速度包括历史时刻角速度时,所述初始位姿变化信息包括初始时刻位姿变化信息,
相应地,根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息,包括:
根据所述激光雷达获取到的当前激光雷达数据和历史时刻激光雷达数据,确定所述激光位姿变化信息。
5.根据权利要求3所述的定位方法,其特征在于,当所述历史里程计信息包括历史帧里程计信息,所述历史角速度包括历史帧角速度时,所述初始位姿变化信息包括初始帧位姿变化信息,
相应地,根据所述激光雷达数据确定所述激光雷达的激光位姿变化信息,包括:
基于激光雷达里程计算法对所述激光雷达获取到的当前激光雷达数据和历史帧激光雷达数据进行计算,以确定所述激光位姿变化信息。
6.根据权利要求3所述的定位方法,其特征在于,所述测量位姿变化信息包括测量位置变化信息和测量姿态变化信息,
相应地,计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差,包括:
根据所述初始位置变化信息和所述测量位置变化信息确定第一差值;
根据所述初始姿态变化信息和所述测量姿态变化信息确定第二差值;
基于所述第一差值和所述第二差值确定所述第一残差。
7.根据权利要求1所述的定位方法,其特征在于,所述预设地图包括栅格地图,栅格地图所包含各栅格的空间状态用于确定各所述栅格的数值,
相应地,根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差,包括:
根据所述当前激光雷达数据和相邻栅格的数值确定各所述栅格的估算值;
基于各所述估算值和预设残差计算公式,确定所述第二残差。
8.根据权利要求1所述的定位方法,其特征在于,根据所述第一残差和所述第二残差确定联合残差,包括:
将所述第一残差和所述第二残差确定为预设二元一次表达式的第一表达项和第二表达项;
基于所述预设二元一次表达式以及所述第一表达项和所述第二表达项,确定所述联合残差。
9.根据权利要求1所述的定位方法,其特征在于,基于所述联合残差确定目标位姿,包括:
基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,并计算所述联合残差;
在所述联合残差最小时,基于初始位姿和确定的所述目标位姿变化信息,确定所述目标位姿。
10.根据权利要求9所述的定位方法,其特征在于,基于所述初始位姿变化信息和所述测量位姿变化信息调整目标位姿变化信息,包括:
基于预设滑窗所包含的滑窗数据调整目标位姿变化信息其中,所述滑窗数据包括预设数量的初始位姿变化信息和预设数量的测量位姿变化信息;
依次更新所述滑窗数据,直至确定所述目标位姿。
11.一种定位装置,其特征在于,应用于电子设备,所述电子设备包括激光雷达,该装置包括:
初始位姿变化信息确定模块,用于确定电子设备本体的初始位姿变化信息;
第一残差计算模块,用于根据所述激光雷达获取到的激光雷达数据,确定所述电子设备本体的测量位姿变化信息,并计算所述初始位姿变化信息和所述测量位姿变化信息的第一残差;
第二残差确定模块,用于根据所述激光雷达获取到的当前激光雷达数据和预设地图所包含的障碍物数据确定第二残差;
目标位姿确定模块,用于根据所述第一残差和所述第二残差确定联合残差,并基于所述联合残差确定目标位姿。
12.一种电子设备,其特征在于,所述电子设备包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序;
激光雷达,用于获取激光雷达数据;
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1-10中任一所述的定位方法。
13.一种包含计算机可执行指令的存储介质,所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1-10中任一所述的定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110632444.7A CN113324542B (zh) | 2021-06-07 | 2021-06-07 | 一种定位方法、装置、设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110632444.7A CN113324542B (zh) | 2021-06-07 | 2021-06-07 | 一种定位方法、装置、设备和存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113324542A true CN113324542A (zh) | 2021-08-31 |
CN113324542B CN113324542B (zh) | 2024-04-12 |
Family
ID=77419982
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110632444.7A Active CN113324542B (zh) | 2021-06-07 | 2021-06-07 | 一种定位方法、装置、设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113324542B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114440928A (zh) * | 2022-01-27 | 2022-05-06 | 杭州申昊科技股份有限公司 | 激光雷达与里程计的联合标定方法、机器人、设备和介质 |
CN114485607A (zh) * | 2021-12-02 | 2022-05-13 | 陕西欧卡电子智能科技有限公司 | 一种运动轨迹的确定方法、作业设备、装置、存储介质 |
CN117471513A (zh) * | 2023-12-26 | 2024-01-30 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、定位装置、电子设备及存储介质 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111207740A (zh) * | 2020-01-13 | 2020-05-29 | 北京京东乾石科技有限公司 | 车辆定位的方法、装置、设备和计算机可读介质 |
CN111340851A (zh) * | 2020-05-19 | 2020-06-26 | 北京数字绿土科技有限公司 | 基于双目视觉与imu融合的slam方法 |
CN111337018A (zh) * | 2020-05-21 | 2020-06-26 | 上海高仙自动化科技发展有限公司 | 定位方法及装置、智能机器人及计算机可读存储介质 |
CN111595333A (zh) * | 2020-04-26 | 2020-08-28 | 武汉理工大学 | 视觉惯性激光数据融合的模块化无人车定位方法及系统 |
CN111797906A (zh) * | 2020-06-15 | 2020-10-20 | 北京三快在线科技有限公司 | 基于视觉和惯性里程定位的方法及装置 |
CN111895989A (zh) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | 一种机器人的定位方法、装置和电子设备 |
WO2020259248A1 (zh) * | 2019-06-28 | 2020-12-30 | Oppo广东移动通信有限公司 | 基于深度信息的位姿确定方法、装置、介质与电子设备 |
CN112197770A (zh) * | 2020-12-02 | 2021-01-08 | 北京欣奕华数字科技有限公司 | 一种机器人的定位方法及其定位装置 |
US20210080259A1 (en) * | 2019-09-12 | 2021-03-18 | Robert Bosch Gmbh | System and method for enhancing non-inertial tracking system with inertial constraints |
CN112781586A (zh) * | 2020-12-29 | 2021-05-11 | 上海商汤临港智能科技有限公司 | 一种位姿数据的确定方法、装置、电子设备及车辆 |
-
2021
- 2021-06-07 CN CN202110632444.7A patent/CN113324542B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2020259248A1 (zh) * | 2019-06-28 | 2020-12-30 | Oppo广东移动通信有限公司 | 基于深度信息的位姿确定方法、装置、介质与电子设备 |
US20210080259A1 (en) * | 2019-09-12 | 2021-03-18 | Robert Bosch Gmbh | System and method for enhancing non-inertial tracking system with inertial constraints |
CN111207740A (zh) * | 2020-01-13 | 2020-05-29 | 北京京东乾石科技有限公司 | 车辆定位的方法、装置、设备和计算机可读介质 |
CN111595333A (zh) * | 2020-04-26 | 2020-08-28 | 武汉理工大学 | 视觉惯性激光数据融合的模块化无人车定位方法及系统 |
CN111340851A (zh) * | 2020-05-19 | 2020-06-26 | 北京数字绿土科技有限公司 | 基于双目视觉与imu融合的slam方法 |
CN111337018A (zh) * | 2020-05-21 | 2020-06-26 | 上海高仙自动化科技发展有限公司 | 定位方法及装置、智能机器人及计算机可读存储介质 |
CN111797906A (zh) * | 2020-06-15 | 2020-10-20 | 北京三快在线科技有限公司 | 基于视觉和惯性里程定位的方法及装置 |
CN111895989A (zh) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | 一种机器人的定位方法、装置和电子设备 |
CN112197770A (zh) * | 2020-12-02 | 2021-01-08 | 北京欣奕华数字科技有限公司 | 一种机器人的定位方法及其定位装置 |
CN112781586A (zh) * | 2020-12-29 | 2021-05-11 | 上海商汤临港智能科技有限公司 | 一种位姿数据的确定方法、装置、电子设备及车辆 |
Non-Patent Citations (2)
Title |
---|
WAHAB KHAWAJA等: "UWB radar based beyond wall sensing and tracking for ambient assisted living", 《2017 14TH IEEE ANNUAL CONSUMER COMMUNICATIONS & NETWORKING CONFERENCE 》, 20 July 2017 (2017-07-20) * |
胡佳佳;周翟和;沈超;赵庆涛;: "一种自适应残差补偿算法在移动机器人姿态估计中的应用研究", 传感技术学报, no. 03, 15 March 2015 (2015-03-15) * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114485607A (zh) * | 2021-12-02 | 2022-05-13 | 陕西欧卡电子智能科技有限公司 | 一种运动轨迹的确定方法、作业设备、装置、存储介质 |
CN114485607B (zh) * | 2021-12-02 | 2023-11-10 | 陕西欧卡电子智能科技有限公司 | 一种运动轨迹的确定方法、作业设备、装置、存储介质 |
CN114440928A (zh) * | 2022-01-27 | 2022-05-06 | 杭州申昊科技股份有限公司 | 激光雷达与里程计的联合标定方法、机器人、设备和介质 |
CN117471513A (zh) * | 2023-12-26 | 2024-01-30 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、定位装置、电子设备及存储介质 |
CN117471513B (zh) * | 2023-12-26 | 2024-03-15 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、定位装置、电子设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN113324542B (zh) | 2024-04-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113324542B (zh) | 一种定位方法、装置、设备和存储介质 | |
CN110645974A (zh) | 一种融合多传感器的移动机器人室内地图构建方法 | |
JP6643883B2 (ja) | Gpsドリフトの処理方法、およびそのシステム | |
JP2022510418A (ja) | 時間同期処理方法、電子機器及び記憶媒体 | |
CN111338383B (zh) | 基于gaas的自主飞行方法及系统、存储介质 | |
US9002513B2 (en) | Estimating apparatus, estimating method, and computer product | |
CN110850807B (zh) | 一种奇异点规避方法、装置、设备及介质 | |
CN113108791A (zh) | 一种导航定位方法及导航定位设备 | |
CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
CN113655453A (zh) | 用于传感器标定的数据处理方法、装置及自动驾驶车辆 | |
CN113960622A (zh) | 融合激光雷达及imu传感器信息的实时定位方法及装置 | |
CN110988949A (zh) | 定位方法、定位装置、计算机可读存储介质与可移动设备 | |
CN113448326A (zh) | 机器人定位方法及装置、计算机存储介质、电子设备 | |
CN110989619B (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
CN112651535A (zh) | 局部路径规划方法、装置、存储介质、电子设备及车辆 | |
CN116358525A (zh) | 基于激光雷达的建图与定位方法、系统及工程车辆 | |
US11620846B2 (en) | Data processing method for multi-sensor fusion, positioning apparatus and virtual reality device | |
CN111427344A (zh) | 自主体轨迹冲突的解决方法、装置、设备和存储介质 | |
CN116626700A (zh) | 一种机器人定位方法、装置、电子设备和存储介质 | |
KR102506411B1 (ko) | 차량의 위치 및 자세 추정 방법, 장치 및 이를 위한 기록매체 | |
US20240134033A1 (en) | Method for determining a movement state of a rigid body | |
CN114199236A (zh) | 定位数据处理的方法、装置、电子设备和自动驾驶车辆 | |
Troll et al. | Indoor Localization of Quadcopters in Industrial Environment | |
CN113759384A (zh) | 一种传感器位姿转换关系确定方法、装置、设备和介质 | |
CN110849392A (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |