CN113252042B - 一种隧道内基于激光和uwb定位方法、装置 - Google Patents
一种隧道内基于激光和uwb定位方法、装置 Download PDFInfo
- Publication number
- CN113252042B CN113252042B CN202110537296.0A CN202110537296A CN113252042B CN 113252042 B CN113252042 B CN 113252042B CN 202110537296 A CN202110537296 A CN 202110537296A CN 113252042 B CN113252042 B CN 113252042B
- Authority
- CN
- China
- Prior art keywords
- map
- sub
- coordinate system
- movable platform
- uwb
- 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
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/20—Instruments for performing navigational calculations
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
Abstract
本申请涉及一种隧道内基于激光和UWB定位方法、装置,包括:获取关于隧道的地图,所述的地图上设有固定端UWB;获取所述的移动端UWB安装于所述的可移动平台的外参信息;获取所述的移动端UWB和所述的固定端UWB之间的距离信息,获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;通过激光传感器获取环境点云数据;基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿;根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。采用本方法能够提供定位的精确度。
Description
技术领域
本申请涉及隧道内定位技术领域,特别是涉及一种隧道内基于激光和UWB定位方法、装置。
背景技术
隧道场景作为典型特殊场景,具有形状狭长、隧道内特征单一、光照条件差、卫星信号弱的特点,目前的定位方法存在各种弊端:单一的激光定位,对隧道内环境特征单一的场景辨识能力差,定位不准确;视觉定位对光线的要求较高,隧道内光照条件较差,识别能力差;卫星定位精确度较低,并且在隧道中卫星定位信号差,出现无法定位的情况;超宽带(UWB,Ultra Wide Band)技术定位,若某个uwb信号丢失就无法准确的估计当前位姿。
现有的定位技术,难以满足在隧道内实时获取较高定位精度的需求。
发明内容
基于此,有必要针对上述技术问题,提供一种能够提供可移动平台定位准确度的隧道内基于激光和UWB定位方法、装置。
一种隧道内基于激光和UWB定位方法,应用于安装移动端UWB和激光传感器的可移动平台,所述方法包括:
获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系;
获取所述的移动端UWB安装于所述的可移动平台的外参信息;
获取所述的移动端UWB和所述的固定端UWB之间的距离信息,;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;
确定所述可移动平台所在的子地图;
同时通过激光传感器获取环境点云数据;根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;
基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿;
根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
在其中一个实施例中,所述地图根据隧道的线路属性分成多个子地图,包括如下步骤:所述地图根据隧道的直道和弯道分成多个子地图,若所述的子地图为直道的子地图,所述子地图的坐标系的第一坐标轴设定为与隧道方向平行;若所述的子地图为弯道的子地图,所述子地图的坐标系的第一坐标轴的方向与一个相邻的直道的子地图方向一致。
在其中一个实施例中,所述的构建第一约束函数,包括如下计算步骤:根据所述的移动端UWB安装于所述的可移动平台的外参信息,得到所述移动端UWB的坐标系到所述可移动平台的坐标系的外参转换矩阵;构建所述可移动平台的坐标系到所述子地图的坐标系的第一转换矩阵函数;根据所述的外参转换矩阵和第一转换矩阵函数,得到所述移动端UWB的坐标系到所述子地图的坐标系的第二转换矩阵函数,获得所述移动端UWB到所述子地图的坐标系的第二平移矩阵函数;获取至少一个所述固定端UWB的坐标系到所述子地图的坐标系的转换矩阵,得到所述固定端UWB的坐标系到所述子地图的坐标系的平移矩阵;根据所述距离信息接近于所述第二平移矩阵函数与所述固定端UWB的坐标系到所述的地图的坐标系平移矩阵的平移量之差,构建第一约束函数:
其中,ε1(κ)基于第k个固定端UWB关于可移动平台位姿的第一约束函数;mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示所述的可移动平台的坐标系,ub表示所述的移动端UWB的坐标系,表示外参转换矩阵,/>表示第一转换矩阵函数,()t表示平移矩阵,/>表示所述的第二平移矩阵函数,表示所述固定端UWB的坐标系到第j个子地图的坐标系平移矩阵,/>表示所述的移动端UWB与第k个所述的固定端UWB之间的距离。
在其中一个实施例中,所述的构建第二约束函数,包括如下步骤:根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构造第二约束函数:
其中,mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示可移动平台的坐标系,表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵,px表示激光传感器测得的第x个当前激光点在可移动平台的坐标系下坐标,/>表示与px最接近的第j个子地图的坐标点集中的点。
在其中一个实施例中,所述的构建第三约束函数,包括如下步骤:
根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿构建第三约束函数;
其中,mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示可移动平台的坐标系,表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵,bpre表示当前时刻可移动平台的坐标系的预测位姿,/>表示当前时刻从可移动平台的坐标系的预测位姿到第j个子地图的坐标系的转换矩阵。
在其中一个实施例中,所述基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿,包括如下步骤:根据第一约束函数、第二约束函数和第三约束函数,按照权重构建最优化问题:
其中,w1为第一权重,w2为第二权重,w3为第三权重;通过对最优化问题求解,得到从可移动平台的坐标系到所述子地图的坐标系的转换矩阵;根据从可移动平台的坐标系到所述子地图的坐标系的转换矩阵,计算可移动平台在所述子地图的坐标系的位姿。
一种隧道内基于激光和UWB定位装置,所述装置包括:
地图获取模块,用于获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系;
外参信息获取模块,用于获取所述的移动端UWB安装于所述的可移动平台的外参信息;
距离信息获取模块,用于获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;
环境点云数据获取模块,用于同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
初步位姿获取模块,用于获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;
第一位姿计算模块,基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿;
第二位姿计算模块,用于根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现以下步骤:
获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系;
获取所述的移动端UWB安装于所述的可移动平台的外参信息;获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;
同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;
基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿;
根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现以下步骤:
获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系;
获取所述的移动端UWB安装于所述的可移动平台的外参信息;获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;
同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;
基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿;
根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
相比于现有技术中的定位技术,本申请至少融合了激光和UWB两种传感器采集的数据,在卫星信号弱、光照条件差的隧道环境内极大地提高了定位的准确度。而且本申请根据隧道中的直到和弯道分成多个子地图,在子地图中,分别给予基于UWB采集数据的第一约束函数赋予观测权重,基于激光采集数据的第二约束函数赋予点云匹配约束权重,以及基于子地图的第三约束函数赋予初值权重,三者在预设权重下进行联合优化,本方案可调整预设权重以适应不同的子地图,进一步提高了在隧道内实时获取较高定位精度。
附图说明
图1为一个实施例中隧道内基于激光和UWB定位的应用环境图;
图2为一个实施例中隧道内基于激光和UWB定位方法的流程示意图;
图3为一个实施例中隧道内基于激光和UWB定位装置的结构框图;
图4为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
本申请提供的隧道内基于激光和UWB定位方法,可以应用于如图1所示的应用环境中。其中,在隧道100内,可移动平台200上安装移动端UWB 201和激光传感器202,在隧道100的两侧设置多个固定端UWB,隧道100的地图根据线路属性分为多个子地图,隧道100的地图包括子地图map1 101、子地图map2102和子地图map3 103,移动端UWB 201与固定端UWB采用UWB技术通信,可移动平台200上安装移动端计算机对数据进行处理。可移动平台200在隧道内行走的过程中,获取关于隧道的地图,所述的地图上设有固定端UWB;获取各个所述的子地图和所述的地图的位姿转换关系;获取所述的移动端UWB 201安装于所述的可移动平台200的外参信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;获取所述的移动端UWB 201和所述的固定端UWB之间的距离信息同时通过激光传感器201获取第一环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;获取可移动平台200在所述的地图中的预测位姿,并确定所述可移动平台200所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台200在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。其中,移动端计算机可以但不限于是各种定位导航模块、工控机、个人计算机、笔记本电脑、平板电脑等。
在本申请中,位姿表示位置和姿态。
在本申请中,IMU,全称Inertial Measurement Unit,惯性测量单元,主要用来检测和测量加速度与旋转运动的传感器。其原理是采用惯性定律实现的,这些传感器从超小型的的MEMS传感器,到测量精度非常高的激光陀螺。
在本申请中,UWB,全称Ultra Wide Band。移动端UWB表示安装在可移动平台上的超宽带设备。固定端UWB表示固定安装在隧道中的超宽带设备。UWB技术是一种无线载波通信技术,利用纳秒级的非正弦波窄脉冲传输数据,其所占的频谱范围很宽。UWB技术具有系统复杂度低,发射信号功率谱密度低,对信道衰落不敏感,截获能力低,定位精度高等优点。
在一个实施例中,如图2所示,提供了一种隧道内基于激光和UWB定位方法,包括以下步骤:
S110,获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的直道和弯道分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系。
其中,关于隧道的地图是已知的点云地图,隧道的地图的固定端UWB位置已知。
在应用过程中,将已构建的点云地图分解为若干个子地图,分解方式根据线路属性进行分解,子图与子图间有重叠区域,如下图所示,根据线路是否为直道和弯道进行区分,若为直道,将子图坐标系中的x方向始终设定为跟隧道(车主要行进方向)平行,若为弯道,则方向设定为其前一个直道方向一致。由于子图的坐标系与地图的坐标系之间的相对关系已知。
举例而言,隧道的地图如果存在一个弯道两个直道,则将地图划分成直道、弯道、直道的3个子地图,如果存在两个弯道三个直道,则将地图划分成直道、弯道、直道、弯道、直道5个子地图。S120,获取所述的移动端UWB安装于所述的可移动平台的外参信息。
其中,移动端UWB安装于所述的可移动平台的外参信息可通过现有技术中的外参标定的方式获得,或者通过人工输入的方式获得。外参信息可为移动端UWB的坐标到可移动平台坐标系的坐标的转换矩阵。其中,可移动平台,指可以自主移动的设备,包括但不限于自动引导车、机器人、自动驾驶汽车、无人机、智能家居。
S130,获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数。
其中,移动端UWB和固定端UWB,具有无线测距功能,其采用UWB技术进行通信。其中,其通过发送和接收具有纳秒或纳秒级一下的极窄脉冲来传输数据。UWB易受到其他障碍物干扰导致其观测数据消失或者噪声变大,于是本文提出了UWB与激光联合定位方法。采用UWB技术通信的设备时,移动端UWB和固定端UWB通信过程中,根据时间戳,能够计算出二者之间的距离。
S140,同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
其中,激光传感器在可移动平台移动过程中,实时扫描环境点云数据。
S150,获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图。
其中,可移动平台所在的子地图,可以根据上一时刻可移动平台在地图中的位姿和IMU积分获取当前可移动平台在地图的预测位姿,根据预测位姿确定可移动平台所在的子地图。当然,可移动平台所在的子地图,也可以通过三个固定端UWB确定当前可移动平台在地图的预测位姿,根据预测位姿确定可移动平台所在的子地图。
更具体地,根据上一时刻可移动平台在地图中的位姿和IMU积分获取当前可移动平台在地图中的预测位姿,阐述如下:
0时刻可以通过外部给定预测位姿,一般方法有手动定位,也可以采用全局传感器定位。当前时刻为i时刻,已知上一时刻的定位结果,即i-1时刻的定位结果,i-1时刻到i时刻的相对位姿可以通过IMU积分获取,进而可以获取到第i时刻的初始位姿,利用初始位姿及i时刻UWB观测数据,激光观测数据通过已有约束条件对i时刻位姿进行优化,从而得到更高精度的定位结果。S160,基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿。
其中,根据所述移动端UWB到地图坐标系的平移矩阵与所述固定端UWB到地图坐标系的平移矩阵的平移量之差接近于所述距离信息,以及根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集,以及根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿构造最优化函数并求解,得到所述可移动平台在子地图的坐标。
S170,根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
其中,在计算得到可移动平台在子地图中的位姿后,通过位姿转换关系,计算得到可移动平台位于所述地图中的当前位姿。
上述隧道内基于激光和UWB定位方法中,通过隧道的地图中固定端UWB的位置,激光传感器获得的环境点云数据,移动端UWB安装于所述的可移动平台的外参信息,移动端UWB和固定端UWB之间的距离信息,可移动平台所处的子地图,计算可移动平台在子地图中的位姿,然后根据子地图和隧道的地图的位姿转换关系,获得可移动平台在地图中的当前位姿,保证可移动平台在隧道内行走,即使有其它车道阻挡,也能够根据固定端UWB和定位与地图构建传感器二者结合,计算出可移动平台的准确位姿。
需要说明的是,假设所述的地图的坐标系以笛卡尔坐标系(X,Y,Z)的X轴、Y轴、Z轴三个方向为例。在隧道中,无法对某个方向进行有效约束,造成X方向上的约束缺失。故,所述隧道中预设固定端UWB。
在其中一个实施例中,所述的所述地图根据隧道的线路属性分成多个子地图,包括如下步骤:所述地图根据隧道的直道和弯道分成多个子地图,若所述的子地图为直道的子地图,所述子地图的坐标系的第一坐标轴设定为与隧道方向平行;若所述的子地图为弯道的子地图,所述子地图的坐标系的第一坐标轴的方向与一个相邻的直道的子地图方向一致。
在其中一个实施例中,所述的构建第一约束函数,包括如下计算步骤:根据所述的移动端UWB安装于所述的可移动平台的外参信息,得到所述移动端UWB的坐标系到所述可移动平台的坐标系的外参转换矩阵;构建所述可移动平台的坐标系到所述子地图的坐标系的第一转换矩阵函数;根据所述的外参转换矩阵和第一转换矩阵函数,得到所述移动端UWB的坐标系到所述子地图的坐标系的第二转换矩阵函数,获得所述移动端UWB到所述子地图的坐标系的第二平移矩阵函数;获取至少一个所述固定端UWB的坐标系到所述子地图的坐标系的转换矩阵,得到所述固定端UWB的坐标系到所述子地图的坐标系的平移矩阵;根据所述距离信息接近于所述第二平移矩阵函数与所述固定端UWB的坐标系到所述的地图的坐标系平移矩阵的平移量之差,构建第一约束函数:
其中,ε1(κ)基于第k个固定端UWB关于可移动平台位姿的第一约束函数;mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示所述的可移动平台的坐标系,ub表示所述的移动端UWB的坐标系,表示外参转换矩阵,/>表示第一转换矩阵函数,()t表示平移矩阵,/>表示所述的第二平移矩阵函数,表示所述固定端UWB的坐标系到第j个子地图的坐标系平移矩阵,/>表示所述的移动端UWB与第k个所述的固定端UWB之间的距离。
在其中一个实施例中,所述的构建第二约束函数,包括如下步骤:根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构造第二约束函数:
其中,mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示可移动平台的坐标系,表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵,px表示激光传感器测得的第x个当前激光点在可移动平台的坐标系下坐标,/>表示与px最接近的第j个子地图的坐标点集中的点。
在其中一个实施例中,所述的构建第三约束函数,包括如下步骤:根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿构建第三约束函数;
其中,mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示可移动平台的坐标系,表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵,bpre表示当前时刻可移动平台的坐标系的预测位姿,/>表示当前时刻从可移动平台的坐标系的预测位姿到第j个子地图的坐标系的转换矩阵。
其中,可移动平台在初始位置到下一时刻的位置可以通过IMU积分方式获取。在可移动平台获得前一时刻的位姿时,通过IMU积分可以获得下一时刻的位置。首先,IMU积分获取的是一个相对量,是上一次定位时刻和当前定位时刻的相对位姿,可以通过这个位姿算得当前时刻在地图或者子地图中的初值,也就是bpre。
由于,IMU积分方式的局限性,计算的得到的可移动平台在地图坐标系中坐标不准确,需要采取本申请隧道内基于激光和UWB定位方法,采用联合优化的方式对结果进行优化。
在其中一个实施例中,所述基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿,包括如下步骤:根据第一约束函数、第二约束函数和第三约束函数,按照权重构建最优化问题:
其中,w1为第一权重,w2为第二权重,w3为第三权重;通过对最优化问题求解,得到从可移动平台的坐标系到所述子地图的坐标系的转换矩阵;根据从可移动平台的坐标系到所述子地图的坐标系的转换矩阵,计算可移动平台在所述子地图的坐标系的位姿。
其中,第一权重、第二权重和第三权重分别对应UWB观测权重,激光点云匹配约束权重矩阵,初值权重矩阵。
其中,UWB观测权重可以根据当前观测到的固定端UWB个数给定,个数越多,所占权重越大;激光点云匹配约束权重矩阵通过可移动平台当前所处的子地图确定,例如,如图1所示,激光点云匹配约束权重矩阵为3*3矩阵,其对角线上的数值分别对应第k个子图中x、y、z方向上权重,对于直行道map1101,环境点云数据在y、z轴方向上的数值是可以观测到的,所以相应的在y、z方向上权重要设置大些,x方向上是不可观的,所以在x方向上权重尽可能设置小;对于弯道map2 102,x、y、z方向上权重可设置相同大小;对应直行到map3 103,同理,环境点云数据在y、z轴方向上的数值是可以观测到的,所以相应的在y、z方向上权重要设置大些,x方向上是不可观的,所以在x方向上权重尽可能设置小。其中,初值权重矩阵可根据IMU传感器性能进行给定。
应该理解的是,虽然图2的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,图2中的至少一部分步骤可以包括多个步骤或者多个阶段,这些步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中,如图3所示,提供了一种隧道内基于激光和UWB定位装置,包括:地图获取模块210、外参信息获取模块220、距离信息获取模块230、初步位姿获取模块240、环境点云数据获取模块250、第一位姿计算模块260和第二位姿计算模块270,其中:
地图获取模块210,用于获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系。
外参信息获取模块220,用于获取所述的移动端UWB安装于所述的可移动平台的外参信息。
距离信息获取模块230,用于获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数。
环境点云数据获取模块250,用于同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数。
初步位姿获取模块240,用于获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数。
第一位姿计算模块260,基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿。
第二位姿计算模块270,用于根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
在其中一个实施例中,所述初步位姿获取模块240,还用于所述地图根据隧道的直道和弯道分成多个子地图,若所述的子地图为直道的子地图,所述子地图的坐标系的第一坐标轴设定为与隧道方向平行;若所述的子地图为弯道的子地图,所述子地图的坐标系的第一坐标轴的方向与一个相邻的直道的子地图方向一致。
在其中一个实施例中,所述第一位姿计算模块260,包括:第一约束函数构建单元,用于根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;第二约束函数构建单元,用于根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;第三约束函数构建单元,,构建第三约束函数;联合优化处理单元,用于基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿。
在其中一个实施例中,所述的构建第一约束函数,包括如下计算步骤:根据所述的移动端UWB安装于所述的可移动平台的外参信息,得到所述移动端UWB的坐标系到所述可移动平台的坐标系的外参转换矩阵;构建所述可移动平台的坐标系到所述子地图的坐标系的第一转换矩阵函数;根据所述的外参转换矩阵和第一转换矩阵函数,得到所述移动端UWB的坐标系到所述子地图的坐标系的第二转换矩阵函数,获得所述移动端UWB到所述子地图的坐标系的第二平移矩阵函数;获取至少一个所述固定端UWB的坐标系到所述子地图的坐标系的转换矩阵,得到所述固定端UWB的坐标系到所述子地图的坐标系的平移矩阵;根据所述距离信息接近于所述第二平移矩阵函数与所述固定端UWB的坐标系到所述的地图的坐标系平移矩阵的平移量之差,构建第一约束函数:
其中,ε1(κ)基于第k个固定端UWB关于可移动平台位姿的第一约束函数;mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示所述的可移动平台的坐标系,ub表示所述的移动端UWB的坐标系,表示外参转换矩阵,/>表示第一转换矩阵函数,()t表示平移矩阵,/>表示所述的第二平移矩阵函数,表示所述固定端UWB的坐标系到第j个子地图的坐标系平移矩阵,/>表示所述的移动端UWB与第k个所述的固定端UWB之间的距离。
在其中一个实施例中,所述的构建第二约束函数,包括如下步骤:根据基于所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构造第二约束函数:
其中,mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示可移动平台的坐标系,表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵,px表示激光传感器测得的第x个当前激光点在可移动平台的坐标系下坐标,/>表示与px最接近的第j个子地图的坐标点集中的点。
在其中一个实施例中,所述的构建第三约束函数,包括如下步骤:根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿构建第三约束函数;
其中,mapj表示第j个子地图的坐标系,所述第j个子地图是所述可移动平台所在的子地图,b表示可移动平台的坐标系,表示从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵,bpre表示当前时刻可移动平台的坐标系,/>表示当前时刻从可移动平台的坐标系到第j个子地图的坐标系的转换矩阵。
在其中一个实施例中,所述第一位姿计算模块260,包括:最优化问题构建单元,用于根据第一约束函数、第二约束函数和第三约束函数,按照权重构建最优化问题:
其中,w1为第一权重,w2为第二权重,w3为第三权重;
求解单元,用于通过对最优化问题求解,得到从可移动平台的坐标系到所述子地图的坐标系的转换矩阵;矩阵转换子单元,用于根据从可移动平台的坐标系到所述子地图的坐标系的转换矩阵,计算可移动平台在所述子地图的坐标系的位姿。
关于隧道内基于激光和UWB定位装置的具体限定可以参见上文中对于隧道内基于激光和UWB定位方法的限定,在此不再赘述。上述隧道内基于激光和UWB定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是移动端计算机,其内部结构图可以如图4所示。该计算机设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信,无线方式可通过WIFI、运营商网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种隧道内基于激光和UWB定位方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解,图4中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,还提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现上述各方法实施例中的步骤。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-Only Memory,ROM)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(Random Access Memory,RAM)或外部高速缓冲存储器。作为说明而非局限,RAM可以是多种形式,比如静态随机存取存储器(Static Random Access Memory,SRAM)或动态随机存取存储器(Dynamic Random Access Memory,DRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (9)
1.一种隧道内基于激光和UWB定位方法,应用于安装移动端UWB和激光传感器的可移动平台,其特征在于,所述方法包括:
获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系;所述地图根据隧道的线路属性分成多个子地图包括:所述地图根据隧道的直道和弯道分成多个子地图;
获取所述的移动端UWB安装于所述的可移动平台的外参信息;
获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;
同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;
基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在所述子地图中的位姿;
根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
2.根据权利要求1所述的方法,其特征在于,所述的所述地图根据隧道的线路属性分成多个子地图,包括如下步骤:若所述的子地图为直道的子地图,所述子地图的坐标系的第一坐标轴设定为与隧道方向平行;若所述的子地图为弯道的子地图,所述子地图的坐标系的第一坐标轴的方向与一个相邻的直道的子地图方向一致。
3.根据权利要求1所述的方法,其特征在于,所述的构建第一约束函数,包括如下计算步骤:
根据所述的移动端UWB安装于所述的可移动平台的外参信息,得到所述移动端UWB的坐标系到所述可移动平台的坐标系的外参转换矩阵;
构建所述可移动平台的坐标系到所述子地图的坐标系的第一转换矩阵函数;
根据所述的外参转换矩阵和第一转换矩阵函数,得到所述移动端UWB的坐标系到所述子地图的坐标系的第二转换矩阵函数,获得所述移动端UWB到所述子地图的坐标系的第二平移矩阵函数;
获取至少一个所述固定端UWB的坐标系到所述子地图的坐标系的转换矩阵,得到所述固定端UWB的坐标系到所述子地图的坐标系的平移矩阵;
根据所述距离信息接近于所述第二平移矩阵函数与所述固定端UWB的坐标系到所述的地图的坐标系平移矩阵的平移量之差,构建第一约束函数:
7.一种隧道内基于激光和UWB定位装置,其特征在于,所述装置包括:
地图获取模块,用于获取关于隧道的地图,所述的地图上设有固定端UWB;其中,所述地图根据隧道的线路属性分成多个子地图;并获取各个所述的子地图和所述的地图的位姿转换关系;所述地图根据隧道的线路属性分成多个子地图包括:所述地图根据隧道的直道和弯道分成多个子地图;
外参信息获取模块,用于获取移动端UWB安装于可移动平台的外参信息;
距离信息获取模块,用于获取所述的移动端UWB和所述的固定端UWB之间的距离信息;根据所述的外参信息,以及所述距离信息接近于所述移动端UWB与所述固定端UWB的实际距离,构建第一约束函数;
环境点云数据获取模块,用于同时通过激光传感器获取环境点云数据;根据所述的环境点云数据接近于所述子地图的坐标系的坐标点集,构建第二约束函数;
初步位姿获取模块,用于获取可移动平台在所述的地图中的预测位姿,并确定所述可移动平台所在的子地图;根据所述可移动平台在所述子地图的预测位姿接近于所述可移动平台在所述子地图的实际位姿,构建第三约束函数;
第一位姿计算模块,基于所述的第一约束函数、所述的第二约束函数和所述的第三约束函数,按照权重进行联合优化处理,得到所述可移动平台在子地图中的位姿;
第二位姿计算模块,用于根据所述子地图在所述地图的位姿转换关系,以及所述可移动平台在子地图中的位姿,计算可移动平台位于所述地图中的当前位姿。
8.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述方法的步骤。
9.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110537296.0A CN113252042B (zh) | 2021-05-18 | 2021-05-18 | 一种隧道内基于激光和uwb定位方法、装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110537296.0A CN113252042B (zh) | 2021-05-18 | 2021-05-18 | 一种隧道内基于激光和uwb定位方法、装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113252042A CN113252042A (zh) | 2021-08-13 |
CN113252042B true CN113252042B (zh) | 2023-06-13 |
Family
ID=77183298
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110537296.0A Active CN113252042B (zh) | 2021-05-18 | 2021-05-18 | 一种隧道内基于激光和uwb定位方法、装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113252042B (zh) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2017139432A1 (en) * | 2016-02-09 | 2017-08-17 | 5D Robotics, Inc. | Ultra wide band radar localization |
CN107796397A (zh) * | 2017-09-14 | 2018-03-13 | 杭州迦智科技有限公司 | 一种机器人双目视觉定位方法、装置和存储介质 |
CN109275093A (zh) * | 2018-10-08 | 2019-01-25 | 高子庆 | 基于uwb定位与激光地图匹配的定位方法和移动终端 |
CN110645974A (zh) * | 2019-09-26 | 2020-01-03 | 西南科技大学 | 一种融合多传感器的移动机器人室内地图构建方法 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
US20160357193A1 (en) * | 2015-06-02 | 2016-12-08 | 5D Robotics, Inc. | Mobile ultra wide band constellations |
CN111735439B (zh) * | 2019-03-22 | 2022-09-30 | 北京京东乾石科技有限公司 | 地图构建方法、装置和计算机可读存储介质 |
CN211765598U (zh) * | 2019-06-28 | 2020-10-27 | 成都九壹通智能科技股份有限公司 | 基于地铁列车精确定位的追踪系统 |
CN110440711A (zh) * | 2019-08-15 | 2019-11-12 | 郑州联睿电子科技有限公司 | 一种电缆隧道三维激光扫描装置及其移动定位方法 |
CN110618434B (zh) * | 2019-10-30 | 2021-11-16 | 北京航空航天大学 | 一种基于激光雷达的隧道内定位系统及其定位方法 |
CN112268559B (zh) * | 2020-10-22 | 2023-03-28 | 中国人民解放军战略支援部队信息工程大学 | 复杂环境下融合slam技术的移动测量方法 |
-
2021
- 2021-05-18 CN CN202110537296.0A patent/CN113252042B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2017139432A1 (en) * | 2016-02-09 | 2017-08-17 | 5D Robotics, Inc. | Ultra wide band radar localization |
CN107796397A (zh) * | 2017-09-14 | 2018-03-13 | 杭州迦智科技有限公司 | 一种机器人双目视觉定位方法、装置和存储介质 |
CN109275093A (zh) * | 2018-10-08 | 2019-01-25 | 高子庆 | 基于uwb定位与激光地图匹配的定位方法和移动终端 |
CN110645974A (zh) * | 2019-09-26 | 2020-01-03 | 西南科技大学 | 一种融合多传感器的移动机器人室内地图构建方法 |
Non-Patent Citations (2)
Title |
---|
Jacobson A 等.Semi-supervised slam: Leveraging low-cost sensors on underground autonomous vehicles for position tracking.《2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)》.2018,第3970-3977页. * |
石潇杰 ; 李恒通 ; 石峰 ; 郭鑫 ; 高泽宇 ; .基于UWB与激光测距的地下铲运机井下定位系统.有色金属(矿山部分).2020,72(02),第81-83页. * |
Also Published As
Publication number | Publication date |
---|---|
CN113252042A (zh) | 2021-08-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109901139B (zh) | 激光雷达标定方法、装置、设备和存储介质 | |
CN108762245B (zh) | 数据融合方法以及相关设备 | |
CN107438752B (zh) | 定位方法、终端和服务器 | |
CN109975773B (zh) | 毫米波雷达标定方法、装置、设备和存储介质 | |
KR101457279B1 (ko) | 동시 무선 송신기 매핑 및 이동국 위치결정 | |
CN109425365A (zh) | 激光扫描设备标定的方法、装置、设备及存储介质 | |
CN105466421A (zh) | 面向可靠wifi连接的移动机器人自主巡航方法 | |
CN111290385A (zh) | 一种机器人路径规划方法、机器人、电子设备及存储介质 | |
CN113124880B (zh) | 一种基于两种传感器数据融合的建图及定位方法、装置 | |
CN105157697A (zh) | 基于光电扫描的室内移动机器人位姿测量系统及测量方法 | |
CN110501712B (zh) | 无人驾驶中用于确定位置姿态数据的方法、装置和设备 | |
WO2019226684A1 (en) | System and method for angle measurement | |
CN112462391A (zh) | 目标对象坐标位置确定方法、装置、计算机设备和介质 | |
KR102172145B1 (ko) | 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치 | |
CN114739400A (zh) | 一种基于uwb和imu信息融合的室内定位方法 | |
CN115164936A (zh) | 高精地图制作中用于点云拼接的全局位姿修正方法及设备 | |
CN114114368A (zh) | 车辆定位方法、系统、设备及存储介质 | |
CN113177980B (zh) | 用于自动驾驶的目标对象速度确定方法、装置及电子设备 | |
CN113916231A (zh) | 移动设备定位导航方法、装置、移动设备及存储介质 | |
CN113252042B (zh) | 一种隧道内基于激光和uwb定位方法、装置 | |
CN111694006A (zh) | 一种用于室内无人系统的导航传感系统 | |
CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
US9400181B2 (en) | Systems and methods for detection of magnetic and motion-based landmarks | |
CN112710343B (zh) | 一种基于rt的车载传感器性能测试方法 | |
KR101042953B1 (ko) | 음영지역 지리정보 검색 시스템 및 방법 |
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 |