CN116299383A - 多传感器融合综合管廊巡检小车定位方法 - Google Patents

多传感器融合综合管廊巡检小车定位方法 Download PDF

Info

Publication number
CN116299383A
CN116299383A CN202310142851.9A CN202310142851A CN116299383A CN 116299383 A CN116299383 A CN 116299383A CN 202310142851 A CN202310142851 A CN 202310142851A CN 116299383 A CN116299383 A CN 116299383A
Authority
CN
China
Prior art keywords
matrix
lora
positioning
inspection trolley
trolley
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
Application number
CN202310142851.9A
Other languages
English (en)
Other versions
CN116299383B (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 University of Civil Engineering and Architecture
Original Assignee
Beijing University of Civil Engineering and Architecture
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 University of Civil Engineering and Architecture filed Critical Beijing University of Civil Engineering and Architecture
Priority to CN202310142851.9A priority Critical patent/CN116299383B/zh
Publication of CN116299383A publication Critical patent/CN116299383A/zh
Application granted granted Critical
Publication of CN116299383B publication Critical patent/CN116299383B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
    • G01S11/06—Systems for determining distance or velocity not using reflection or reradiation using radio waves using intensity measurements
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/38—Services specially adapted for particular environments, situations or purposes for collecting sensor information
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Signal Processing (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本申请公开一种多传感器融合综合管廊巡检小车定位方法，包括：激光雷达、lora传感器、lora网关、lora服务器等设备。通过将激光雷达里程计获取的轨迹信息、激光点云加权投影法获取的二维场景信息与基于视觉特征提取算子提取的三维结构特征结合在一起，得到地下综合管廊的道路场景表征模型；在所述基础上，利用lora网络对巡检小车进行初步定位，再结合二维场景特征和三维结构，将定位精度一步步提高，实现从节点级到度量级的进步。最后为了减小累计算法所带来的漂移误差，利用线性卡尔曼滤波对激光雷达定位轨迹与lora数据进行融合，提高定位精度。

Description

多传感器融合综合管廊巡检小车定位方法
技术领域
本发明属于定位追踪技术领域，特别涉及一种多传感器融合综合管廊巡检小车定位方法。
背景技术
城市地下综合管廊是城市基础设施的重要组成部分。相比专用电力隧道，地下综合管廊的运作维护情况较为复杂。城市地下综合管廊将市政、电力、通讯、燃气、供水、排水等各种管线集于一体，在城市道路的地下空间建造一个集约化的隧道。在此环境下，常用的GPS定位便不再适用，不仅是因为地下GPS定位信号弱，还因为其设备价格高昂、功耗较大。排除常规的室外定位方法，室内定位的热点研究方向在于物联网。在物联网中，应用到的终端设备或网络设备都遵循少量原则，芯片所需的内存少，数据处理能力低，通信频次低占据宽带，当然所耗功率也随之降低。以lora(远距离无线电Long Range Radio)为代表的低功率广域网便满足其所有要求。lora网络部署所需设备成本低廉、能量消耗较小的优势，使它在物联网中得到了广泛的应用。
本发明旨在利用lora进行初步的室内定位，将位置数据与巡检小车上的激光雷达数据进行融合，来提高管廊内巡检小车的定位精度。
发明内容
本发明的目的在于提供一种多传感器融合综合管廊巡检小车定位方法，可实现有效的地下环境定位，节约了GPS设备带来的高昂价格与较大功耗；与常见的室内定位方法相比，利用lora网联技术可以实现低成本、低功耗的数据传输从而实现地下定位；将lora网络与激光雷达数据进行融合可有效地消除大量计算带来的漂移误差，提高了定位的精度。
为了达到上述目的，本发明提供了如下技术方案：
一种多传感器融合综合管廊巡检小车定位方法，所述巡检小车包括搭载lora模块的lora传感器，与lora网关、lora服务器共同组成一个lora网络环境，巡检小车顶部设有激光雷达和激光里程计，巡检小车内部设有中央处理器；
所述方法包括如下步骤：
S1、利用激光雷达与激光里程计对地下综合管廊进行道路场景表征建模，得到地下综合管廊的道路场景表征模型：
S1.1、利用基于LOAM算法的激光里程计计算巡检小车运行时激光雷达在每个节点的轨迹位姿对激光雷达获得的三维激光雷达点云中的3D局部形状特征如线特征、面特征进行提取，随后进行点云配准，得到前后帧点云的位姿关系，位姿关系与轨迹位姿共同组成了巡检小车的高精度轨迹；
S1.2、在巡检小车的内置中央处理器中利用基于距离的激光点云加权投影法将激光雷达获得的三维激光雷达点云转换成二维场景图像以及三维结构；
S1.3、在巡检小车的内置中央处理器中利用ORB视觉特征提取算子对步骤S1.2中得到的二维场景图像以及三维结构进行特征提取，得到地下综合管廊的二维场景特征以及三维结构特征；
将激光里程计获取的巡检小车的轨迹信息、激光点云加权投影法获取的二维场景特征与基于视觉特征提取算子提取的三维结构特征结合在一起，得到地下综合管廊的道路场景表征模型；
S2、通过内置搭载lora模块的lora传感器的巡检小车、lora网关以及lora服务器搭建lora网络环境，在此网络环境下结合步骤S1得到的地下综合管廊的道路场景表征模型中的激光里程计获取的巡检小车的轨迹信息对巡检小车进行粗定位，包括如下步骤：
S2.1、利用lora物理层协议与mac协议，建立一个以LoraWAN规范标准的满足物联网低功耗的通讯网络，为室内定位提供网络环境；
S2.2、利用基于信号接收强弱(RSSI)的方法来进行室内定位，获取巡检小车的坐标；
S2.3、利用步骤S2.2的lora室内定位通过公式5获取的巡检小车的坐标加上地下综合管廊的场景表征模型中激光里程计所获取的巡检小车的高精度轨迹信息，实现巡检小车的初步粗定位；
在巡检小车的内置中央处理器中利用三边定位算法来求得目标节点的位置；包括如下步骤：
首先根据RSSI测距方法计算出任意三个参考节点和目标节点之间的距离长度，接着画出以三个参考节点为圆心，所求距离为半径的圆，三个圆两两相交，由相交的区域来定位未知节点；
三个参考节点的坐标分别为O1(xa，ya)，O2(xb，yb)，O3(xc，yc)；目标节点P为待测的室内空间坐标，设其为(x,y),dA,dB,dC表示由RSSI测距方法求出的三个参考节点到点P的距离；将这些参数代入平面上两点间的距离公式，如下公式4：
Figure BDA0004088187450000031
解出公式4中的方程组即可得到点P坐标为：
Figure BDA0004088187450000032
公式4和公式5中，da,db,dc分别表示由RSSI测距方法求出的三个参考节点到点P的距离，单位为m；xa,ya,xb,yb,xc,yc分别为三个参考节点的坐标值，单位为m；x,y为点P的坐标值，单位为m；
S3、基于场景特征匹配的节点级定位：
在步骤S2的粗定位结束之后，地下综合管廊的道路场景表征模型提取出属于粗定位结果的二维场景特征；随着巡检小车的不断移动，道路场景表征模型也在不断发生变化；根据步骤S1.3使用的ORB视觉特征提取算子得到当前的二维场景特征；将粗定位结果的二维场景特征与当前的二维场景特征通过步骤S1.2使用的激光点云加权投影法生成三维激光雷达点云的二维场景图像，并利用视觉特征提取算子对二维场景图像进行特征提取；利用K最近邻算法，对ORB视觉特征提取算子进行海明距离的计算，公式如下：
Figure BDA0004088187450000041
公式6中，O1，O2分别为两个OBR描述子；O1i和O2i分别为两个描述子中的第i个元素；XOR表述对两个元素进行异或计算；n为描述子大小；Dh(O1,O2)为海明距离；
随后计算两帧图像之间的旋转平移矩阵来对上述的二维场景特征与道路场景表征模型中的场景特征进行匹配；旋转平移矩阵如下式所示：
Figure BDA0004088187450000042
公式7中，[u v]T为当前二维场景图像中的特征点位置；[x y]T为对应的地下综合管廊的道路场景表征模型中二维图像的特征点的位置；R为旋转矩阵；T为平移矩阵；0为零矩阵；
匹配过程中，选取匹配特征点数目最多的三个节点作为节点级定位结果，并在巡检小车的内置中央处理器中计算出相应的旋转平移矩阵；最后得到当前车辆在局部坐标系下的位姿Pcm；位姿Pcm计算公式为：
Pcm＝Pck·Pkm 公式8-1
公式8-1中，Pck为当前巡检小车与第k个数据采集点之间的旋转平移矩阵；Pkm为第k个数据采集点与第一个数据采集点之间的旋转平移矩阵；Pcm为当前巡检小车在局部坐标系下的位姿；
S4、基于点云及三维结构的度量级定位：
将步骤S3中得到的节点级定位结果与地下综合管廊的道路场景表征模型当中的三维结构进行结合，通过巡检小车的内置中央处理器计算当前激光雷达点云与节点级定位结果相对应的激光雷达点云之间的误差平方和来得到优化后的旋转平移矩阵，最优旋转平移矩阵如下式所示：
Figure BDA0004088187450000051
Figure BDA0004088187450000052
Figure BDA0004088187450000053
公式9～公式11中，(ak,bk)表示第k个初值的相对平移矩阵；P(ak,bk)定义第k个初值的概率；
Figure BDA0004088187450000054
为最优旋转平移矩阵；Tcm是由/>
Figure BDA0004088187450000055
组成的最优旋转平移矩阵；β为期望公式中的常数；ai表示第i个初值的最优旋转矩阵；bi表示第i个初值的最优平移矩阵；
最优旋转平移矩阵与距离最近的节点的相对旋转矩阵Pcm共同组成位姿优化结果P‘cm，如公式8-2所示；位姿优化结果作为快速ICP算法的初值，通过此算法进行点云匹配得到精确的度量级定位结果；
Pcm＝Pck·Pkm 公式8-1
Pcm′＝Pcm·Tcm 公式8-2
公式8-2中，P′cm为当前巡检小车在局部坐标系下的位姿优化结果；Pcm为当前巡检小车在局部坐标系下的位姿；Tcm是由
Figure BDA0004088187450000061
组成的最优旋转平移矩阵；
S5、利用线性卡尔曼滤波对激光雷达定位轨迹与lora定位数据进行融合，减小激光里程计以及上述一些其他算法所带来的累积误差，得到更精准，更稳定的轨迹信息；
S5.1、卡尔曼滤波状态量
Figure BDA0004088187450000066
的推导：
旋转平移矩阵由R、T矩阵组合而成，采用cosθ、sinθ、Tx、Ty来代替R、T；通过实时获取lora定位坐标与lora传感器的坐标，来更新激光坐标系与lora坐标系之间的旋转平移矩阵，如公式12所示：
Figure BDA0004088187450000062
在第k个采集点，用α、β代替cosθ、sinθ；确定状态量
Figure BDA0004088187450000065
如公式13所示：
Figure BDA0004088187450000063
公式12～公式13中，
Figure BDA0004088187450000064
为Lora定位坐标此处将位置作为主要的卡尔曼滤波状态量；[xk，yk]T为激光坐标系下lora传感器的轨迹坐标，单位为m；θ为激光雷达坐标系与Lora坐标系之间的夹角，用于坐标系之间的转换；Tx、Ty分别为上述旋转平移矩阵T的两个分量，利于RTK的计算；Xk(x)、Xk(y)分别是巡检小车在第k个采集点位置Xk的横坐标与纵坐标，单位为m；Xk-1(x)、Xk-1(y)分别是巡检小车在第k-1个采集点位置Xk-1的横坐标与纵坐标，单位为m；αk、βk、Tx,k、Ty,k均是激光雷达与RTK(实时差分定位技术)在第k个采集点的旋转平移矩阵的分量；
S5.2、状态转移矩阵Fk的推导：
由于前后帧采集点Xk和Xk-1满足：
Xk＝2Xk-1-Xk-2 公式14
因此前后帧状态
Figure BDA0004088187450000073
因满足如下关系式：
Figure BDA0004088187450000071
由此得到状态转移矩阵Fk为：
Figure BDA0004088187450000072
公式14～公式16中，Xk表示巡检小车在第k个采集点的位置，Xk-1、Xk-2表示巡检小车在第k-1、k-2个采集点的位置，单位为m，用于前后帧的迭代；Xk(x)、Xk(y)分别是巡检小车在第k个采集点位置Xk的横坐标与纵坐标，单位为m；Xk-1(x)、Xk-1(y)分别是巡检小车在第k-1个采集点位置Xk-1的横坐标与纵坐标，单位为m；
Xk-2(x)、Xk-2(y)分别是巡检小车在第k-2个采集点位置Xk-2的横坐标与纵坐标，单位为m；αk、βk、Tx,k、Ty,k均是激光雷达与RTK在第k个采集点的旋转平移矩阵的分量；αk-1、βk-1、Tx,k-1、Ty,k-1均是激光雷达与RTK在第k-1个采集点的旋转平移矩阵的分量；Fk为状态转移矩阵；
S5.3、测量矩阵
Figure BDA0004088187450000084
观测矩阵Hk的推导：
将第k个采集点的lora传感器坐标经过旋转平移得到的坐标即为当前小车位置坐标，第k个采集点的lora轨迹坐标即为当前小车位置坐标，将第k个采集点的激光雷达轨迹的坐标旋转平移后得到的坐标为第k个采集点的RTK轨迹坐标，基于上面三个方面，总结以下三个方程：
Figure BDA0004088187450000081
公式17中，Rk、Tk分别表示旋转以及平移矩阵；
Figure BDA0004088187450000082
表示第k个采集点的Lora传感器的坐标；/>
Figure BDA0004088187450000083
表示第k个采集点的Lora定位坐标；Xk表示巡检小车在第k个采集点的位置；
用αk、βk、Tx，k、Ty，k代替Rk和Tk，公式17转化为：
Figure BDA0004088187450000085
因此：
Figure BDA0004088187450000091
最后得到测量矩阵
Figure BDA00040881874500000915
观测矩阵Hk；
公式18～公式19中，
Figure BDA00040881874500000916
为卡尔曼滤波状态量；xk、yk表示第k个采集点的Lora接收器的横、纵坐标，单位为m；Lxk、Lyk表示第k个采集点的Lora定位坐标的横、纵坐标，单位为m；
Figure BDA00040881874500000917
为测量矩阵；Hk为观测矩阵；
S5.4、利用卡尔曼滤波的状态量
Figure BDA0004088187450000092
状态转移矩阵Fk、测量矩阵/>
Figure BDA0004088187450000093
观测矩阵Hk对巡检小车的位置进行更新迭代，最后计算得到巡检小车的精准定位；计算过程如下：
线性离散卡尔曼滤波状态量
Figure BDA0004088187450000094
的预测状态/>
Figure BDA0004088187450000095
与预测估计协方差矩阵Pk∣k-1分别为：
Figure BDA0004088187450000096
Figure BDA0004088187450000097
公式20～公式21中，Fk T为状态转移矩阵的转置矩阵；
Figure BDA0004088187450000098
Figure BDA0004088187450000099
为卡尔曼滤波状态量/>
Figure BDA00040881874500000910
的预测状态；Fk为步骤S5.2得到的状态转移矩阵；Bk是最优控制向量/>
Figure BDA00040881874500000911
作用在/>
Figure BDA00040881874500000912
上的控制矩阵，此处Bk＝0；/>
Figure BDA00040881874500000913
为控制向量；Pk∣k-1、Pk-1∣k-1为预测估计协方差矩阵；Qk是服从正态分布的协方差；其中：
Qk＝ws2·E8
ws为常数，E8为8阶单位矩阵；
通过以下步骤对巡检小车的状态进行更新：
S5.4.1、根据公式22更新状态的估计值：
Figure BDA0004088187450000101
公式22中，
Figure BDA0004088187450000102
为k-1到k时刻的状态量；Kk是最优卡尔曼增益，在步骤S5.4.2时进行计算；/>
Figure BDA0004088187450000103
为步骤S5.3得到的状态矩阵；Hk为步骤S5.3得到的观测矩阵；/>
Figure BDA0004088187450000104
为k时刻的状态量；
S5.4.2、根据公式23求解最优卡尔曼增益：
Figure BDA0004088187450000105
公式23中，Hk T为观测矩阵的转置矩阵；Kk是最优卡尔曼增益；Pk为k时刻的协方差矩阵；Hk为观测矩阵；Rgk为服从正态分布的协方差，其中：
Rgk＝vs2·E6
vs为常数，E6为6阶单位矩阵；
S5.4.3、更新协方差矩阵：
Figure BDA0004088187450000106
公式24中，I为单位矩阵；Kk为在步骤S5.4.2得到的最优卡尔曼增益；Hk为观测矩阵；
Figure BDA0004088187450000107
为k时刻的协方差矩阵；/>
Figure BDA0004088187450000108
为k-1至k时刻更新的协方差矩阵；
已知第一个采集点的Lora定位坐标
Figure BDA0004088187450000109
再根据第k个采集点的测量矩阵
Figure BDA00040881874500001010
便能够通过上述步骤计算得到/>
Figure BDA00040881874500001011
其中的Xk即为巡检小车当前的位置坐标，实现了激光雷达定位轨迹与lora定位数据的融合。
步骤S1.2中，预处理后，将点云投影至XOY平面，设激光点云P中的一点pi(x，y，z)其中x∈(-N1，N1)米，x∈(-N2，N2)米以及x∈(-Z1，Z2)米；将激光点云在平面XOY上沿x轴和y轴均匀地分为2N1行和2N2列的栅格；二维图像的大小为(2N1，2N2)像素，而图像中的每个像素值由公式1决定：
Figure BDA0004088187450000111
公式1中，I(i,j)为图像中的每个像素值；∑pij为第i行第j列栅格所包含的点云总数；Kij为第i行第j列栅格的投影权值，由第i行第j列栅格到Z轴的投影距离决定；如公式2所示：
Figure BDA0004088187450000112
公式2中，Kij为第i行第j列栅格的投影权值；x，y为第i行第j列栅格中任意一点的X轴与Y轴坐标，单位为m；N为一个常数，根据激光雷达的工作范围确定，此处N的范围为(-31，31)。
步骤S2.2中，首先在lora环境中放置多个参考节点，这些参考节点一般为拥有MAC地址的数据链路层设备，巡检小车作为目标节点与这些参考节点进行相互的通信，当目标节点接收到来自参考节点的信号后，通过巡检小车的内置中央处理器计算出接收信号的强度值，将所得的RSSI值代入公式3中计算出参考节点和目标节点的距离；
Figure BDA0004088187450000113
公式3中，A为距离1米时的RSSI值的绝对值，最佳范围在45-49之间，单位为dbm；RSSI为接收信号的强度值，单位为dBm；n为环境对信号的衰减系数，最佳范围在3.25-4.5之间；d为参考节点和目标节点的距离，单位为m；
目标节点和参考节点将数据发送给lora网关，lora网关再将定位结果d传输给巡检小车的内置中央处理器。
与现有技术相比，本发明的有益效果在于：
首先通过将激光里程计获取的轨迹信息、激光点云加权投影法获取的二维场景信息与基于视觉特征提取算子提取的三维结构特征结合在一起，得到地下综合管廊的道路场景表征模型；
进一步地利用lora网络与表征模型中的轨迹信息来实现巡检小车的粗定位；
再结合表征模型中的二维场景特征和三维结构，将定位精度一步步提高，实现从节点级到度量级的进步；
最后为了减小累计算法所带来的漂移误差，利用线性卡尔曼滤波对激光雷达定位轨迹与lora定位数据进行融合，提高定位精度。
与传统的GPS定位相比，本方法可实现有效的地下环境定位，节约了GPS设备带来的高昂价格与较大功耗；与常见的室内定位方法相比，利用lora网联技术可以实现低成本、低功耗的数据传输从而实现地下定位；将lora网络与激光雷达数据进行融合可有效地消除大量计算带来的漂移误差，提高了定位的精度。
附图说明
图1为本发明的多传感器融合综合管廊巡检小车定位方法流程图；
图2为三边定位示意图。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明。
一种多传感器融合综合管廊巡检小车定位方法，所述巡检小车包括搭载lora模块的lora传感器，与lora网关、lora服务器共同组成一个lora网络环境，巡检小车顶部设有激光雷达和激光里程计，巡检小车内部设有中央处理器。如图1所示，该方法包括如下步骤：
S1、利用激光雷达与激光里程计对地下综合管廊进行道路场景表征建模，得到地下综合管廊的道路场景表征模型：
S1.1、利用基于LOAM算法的激光里程计计算巡检小车运行时激光雷达在每个节点的轨迹位姿对激光雷达获得的三维激光雷达点云中的3D局部形状特征如线特征、面特征进行提取，随后进行点云配准，得到前后帧点云的位姿关系，位姿关系与轨迹位姿共同组成了巡检小车的高精度轨迹，有效改进了传统里程计对闭环检测和后端优化的不足。
S1.2、在巡检小车的内置中央处理器中利用基于距离的激光点云加权投影法将激光雷达获得的三维激光雷达点云转换成二维场景图像以及三维结构，方便视觉特征算子进行特征提取与匹配，可以很大程度上提高点云配准的效率。
本实施例中，使用典型的Velodyne VLP-16型号的激光雷达，在(-31m，31m)范围内进行激光点云的预处理。预处理后，将点云投影至XOY平面，设激光点云P中的一点pi(x，y，z)，其中x∈(-N1，N1)米，y∈(-N2，N2)米以及z∈(-Z1，Z2)米。将激光点云在平面XOY上沿x轴和y轴均匀地分为2N1行和2N2列的栅格。二维图像的大小为(2N1，2N2)像素，而图像中的每个像素值由公式1决定：
Figure BDA0004088187450000131
公式1中，I(i,j)为图像中的每个像素值；∑pij为第i行第j列栅格所包含的点云总数；Kij为第i行第j列栅格的投影权值，由第i行第j列栅格到Z轴的投影距离决定。如公式2所示：
Figure BDA0004088187450000132
公式2中，Kij为第i行第j列栅格的投影权值；x，y为第i行第j列栅格中任意一点的X轴与Y轴坐标，单位为m；N为一个常数，根据激光雷达的工作范围确定，此处N的范围为(-31，31)。
S1.3、在巡检小车的内置中央处理器中利用ORB视觉特征提取算子对步骤S1.2中得到的二维场景图像以及三维结构进行特征提取，得到地下综合管廊的二维场景特征以及三维结构特征。
由于三维激光雷达点云是对周围环境的一种表征，而从二维图像中所提取的视觉特征是对三维激光雷达点云的一种表征，因此在此处，将经过特征提取的二维图像加上本身的三维结构特征来共同表征三维结构，此方法可提高鲁棒性。
ORB算法首先采用FAST算法检测二维图像的特征点，即在特征点周围邻域内有足够多的像素点与特征点处于不同的区域内；然后利用图像矩为检测到的特征点加上信息方向信息，构成oFAST；最后采用BRIEF算法来计算每个特征点的描述子，即在特征点的周围邻域选取32个点对，与特征点数目Nx组成一个Nx×32的矩阵，由此便得到基于ORB局部特征描述的二维场景特征和三维结构特征。
激光里程计采用了LOAM算法，而LOAM算法作为一个比较成熟的SLAM算法，自身可提供较为稠密的点云地图与较为准确的里程计信息，而较为稠密的点云在投影到二维后经过增强可以用视觉方法做特征提取。以此得到激光里程计获取的轨迹信息、激光点云加权投影法获取的二维场景信息、基于视觉特征提取算子提取的三维结构特征，这三部分便是地下综合管廊的场景表征模型的三要素。
将激光里程计获取的巡检小车的轨迹信息、激光点云加权投影法获取的二维场景特征与基于视觉特征提取算子提取的三维结构特征结合在一起，得到地下综合管廊的道路场景表征模型。
S2、通过内置搭载lora模块的lora传感器的巡检小车、lora网关以及lora服务器搭建lora网络环境，在此网络环境下结合步骤S1得到的地下综合管廊的道路场景表征模型中的激光里程计获取的巡检小车的轨迹信息对巡检小车进行粗定位，包括如下步骤：
S2.1、利用lora物理层协议与mac协议，建立一个以LoraWAN规范标准的满足物联网低功耗的通讯网络，为室内定位提供网络环境。
Lora的网络架构分为内置lora模块的终端设备、lora网关以及lora服务器。此处的内置lora模块的终端设备即为搭载lora模块的巡检小车。在lora网络中，终端设备不需要在单次传输中传输太多字节，有效降低了功耗，延长电池的寿命。此外，地面工作人员可以通过用户终端连接lora服务器，以便实时查看数据及后续管理。
进一步的，巡检小车作为终端设备，配备有激光雷达等其他传感器，实现lora协议内的物理层、MAC层和应用层，遵守lora联盟的协议规范，实现巡检小车与用户终端之间点对点的远距离低功耗传输。单个或多个终端设备将信号传输给lora网关，lora网关支持对多终端信号的多信道、多调制收发，完成信息的中继。信号被整理收集之后由lora网关传输给lora云服务器，lora云服务器对数据包进行接收、读取、处理和应用，用户终端可以随时登陆服务器进行查看和管理。
S2.2、利用基于信号接收强弱(RSSI)的方法来进行室内定位，获取巡检小车的坐标。
首先在lora环境中放置多个参考节点，这些参考节点一般为拥有MAC地址的数据链路层设备，巡检小车作为目标节点与这些参考节点进行相互的通信，当目标节点接收到来自参考节点的信号后，通过巡检小车的内置中央处理器计算出接收信号的强度值，将所得的RSSI值代入公式3中便可计算出参考节点和目标节点的距离。
Figure BDA0004088187450000151
公式3中，A为距离1米时的RSSI值的绝对值，最佳范围在45-49之间，单位为dbm；RSSI为接收信号的强度值，单位为dBm；n为环境对信号的衰减系数，最佳范围在3.25-4.5之间；d为参考节点和目标节点的距离，单位为m。
目标节点和参考节点将数据发送给lora网关，lora网关再将定位结果d传输给巡检小车的内置中央处理器。
进一步的，在巡检小车的内置中央处理器中利用三边定位算法来求得目标节点的位置。
所述利用三边定位算法来求得目标节点的位置包括如下步骤：
首先根据RSSI测距方法计算出任意三个参考节点和目标节点之间的距离长度，接着画出以三个参考节点为圆心，所求距离为半径的圆，三个圆两两相交，由相交的区域来定位未知节点。
如图2所示，三个圆的圆心表示三个参考节点，坐标已知，分别为O1(xa，ya)，O2(xb，yb)，O3*xc，yc)。目标节点P为待测的室内空间坐标，设其为(x，y)，dA，dB，dC表示由RSSI测距方法求出的三个参考节点到点P的距离。将这些参数代入平面上两点间的距离公式，可列如下公式4：
Figure BDA0004088187450000161
解出公式4中的方程组即可得到点P坐标为：
Figure BDA0004088187450000162
公式4和公式5中，da,db,dc分别表示由RSSI测距方法求出的三个参考节点到点P的距离，单位为m；xa,ya,xb,yb,xc,yc分别为三个参考节点的坐标值，单位为m；x,y为点P的坐标值，单位为m。
S2.3、利用步骤S2.2的lora室内定位通过公式5获取的巡检小车的坐标加上地下综合管廊的场景表征模型中激光里程计所获取的巡检小车的高精度轨迹信息，实现巡检小车的初步粗定位。
S3、基于场景特征匹配的节点级定位：
在步骤S2的粗定位结束之后，地下综合管廊的道路场景表征模型提取出属于粗定位结果的二维场景特征。随着巡检小车的不断移动，道路场景表征模型也在不断发生变化。根据步骤S1.3使用的ORB视觉特征提取算子可得到当前的二维场景特征。将粗定位结果的二维场景特征与当前的二维场景特征通过步骤S1.2使用的激光点云加权投影法生成三维激光雷达点云的二维场景图像，并利用视觉特征提取算子对二维场景图像进行特征提取。利用K最近邻算法，对ORB视觉特征提取算子进行海明距离的计算，公式如下：
Figure BDA0004088187450000171
公式6中，O1，O2分别为两个OBR描述子；O1i和O2i分别为两个描述子中的第i个元素；XOR表述对两个元素进行异或计算；n为描述子大小；Dh(O1,O2)为海明距离。
随后计算两帧图像之间的旋转平移矩阵来对上述的二维场景特征与道路场景表征模型中的场景特征进行匹配。旋转平移矩阵如下式所示：
Figure BDA0004088187450000172
公式7中，[u v]T为当前二维场景图像中的特征点位置；[x y]T为对应的地下综合管廊的道路场景表征模型中二维图像的特征点的位置；R为旋转矩阵；T为平移矩阵；0为零矩阵。
匹配过程中，选取匹配特征点数目最多的三个节点作为节点级定位结果，并在巡检小车的内置中央处理器中计算出相应的旋转平移矩阵。最后得到当前车辆在局部坐标系下的位姿Pcm。位姿Pcm计算公式为：
Pcm＝Pck·Pkm 公式8-1
公式8-1中，Pck为当前巡检小车与第k个数据采集点之间的旋转平移矩阵；Pkm为第k个数据采集点与第一个数据采集点之间的旋转平移矩阵；Pcm为当前巡检小车在局部坐标系下的位姿。
在粗定位的基础上实现了节点级的定位，定位精度更加精确。
S4、基于点云及三维结构的度量级定位：
将步骤S3中得到的节点级定位结果与地下综合管廊的道路场景表征模型当中的三维结构进行结合，通过巡检小车的内置中央处理器计算当前激光雷达点云与节点级定位结果相对应的激光雷达点云之间的误差平方和来得到优化后的旋转平移矩阵，最优旋转平移矩阵如下式所示：
Figure BDA0004088187450000181
Figure BDA0004088187450000182
Figure BDA0004088187450000183
公式9～公式11中，(ak,bk)表示第k个初值的相对平移矩阵；P(ak,bk)定义第k个初值的概率；
Figure BDA0004088187450000184
为最优旋转平移矩阵；Tcm是由/>
Figure BDA0004088187450000185
组成的最优旋转平移矩阵；β为期望公式中的常数；ai表示第i个初值的最优旋转矩阵；bi表示第i个初值的最优平移矩阵。
最优旋转平移矩阵与距离最近的节点的相对旋转矩阵Pcm共同组成位姿优化结果P′cm，如公式8-2所示。位姿优化结果作为快速ICP算法的初值，通过此算法进行点云匹配得到精确的度量级定位结果。
Pcm＝Pck·Pkm 公式8-1
Pcm′＝Pcm·Tcm 公式8-2
公式8-2中，P′cm为当前巡检小车在局部坐标系下的位姿优化结果；Pcm为当前巡检小车在局部坐标系下的位姿；Tcm是由
Figure BDA0004088187450000194
组成的最优旋转平移矩阵。
S5、利用线性卡尔曼滤波对激光雷达定位轨迹与lora定位数据进行融合，减小激光里程计以及上述一些其他算法所带来的累积误差，得到更精准，更稳定的轨迹信息。
S5.1、卡尔曼滤波状态量
Figure BDA0004088187450000195
的推导：
旋转平移矩阵由R、T矩阵组合而成，由于本申请利用的是线性卡尔曼滤波，因此采用cosθ、sinθ、Tx、Ty来代替R、T。通过实时获取lora定位坐标与lora传感器的坐标，来更新激光坐标系与lora坐标系之间的旋转平移矩阵，如公式12所示：
Figure BDA0004088187450000191
在第k个采集点，用α、β代替cosθ、sinθ。确定状态量
Figure BDA0004088187450000196
如公式13所示：/>
Figure BDA0004088187450000192
公式12～公式13中，
Figure BDA0004088187450000193
为Lora定位坐标此处将位置作为主要的卡尔曼滤波状态量；[xk，yk]T为激光坐标系下lora传感器的轨迹坐标，单位为m；Xk(x)、Xk(y)分别是巡检小车在第k个采集点位置Xk的横坐标与纵坐标，单位为m；Xk-1(x)、Xk-1(y)分别是巡检小车在第k-1个采集点位置Xk-1的横坐标与纵坐标，单位为m；αk、βk、Tx,k、Ty,k均是激光雷达与RTK(实时差分定位技术)在第k个采集点的旋转平移矩阵的分量；θ为激光雷达坐标系与Lora坐标系之间的夹角，用于坐标系之间的转换；Tx、Ty分别是上述旋转平移矩阵T的两个分量，利于RTK的计算。
S5.2、状态转移矩阵Fk的推导：
由于前后帧采集点Xk和Xk-1满足：
Xk＝2Kk-1-Xk-2 公式14
因此前后帧状态
Figure BDA0004088187450000203
因满足如下关系式：
Figure BDA0004088187450000201
由此得到状态转移矩阵Fk为：
Figure BDA0004088187450000202
公式14～公式16中，Xk表示巡检小车在第k个采集点的位置，Xk-1、Xk-2表示巡检小车在第k-1、k-2个采集点的位置，单位为m，用于前后帧的迭代；Xk(x)、Xk(y)分别是巡检小车在第k个采集点位置Xk的横坐标与纵坐标，单位为m；Xk-1(x)、Xk-1(y)分别是巡检小车在第k-1个采集点位置Xk-1的横坐标与纵坐标，单位为m；Xk-2(x)、Xk-2(y)分别是巡检小车在第k-2个采集点位置Xk-2的横坐标与纵坐标，单位为m；αk、βk、Tx,k、Ty,k均是激光雷达与RTK在第k个采集点的旋转平移矩阵的分量；αk-1、βk-1、Tx,k-1、Ty,k-1均是激光雷达与RTK在第k-1个采集点的旋转平移矩阵的分量；Fk为状态转移矩阵。
S5.3、测量矩阵
Figure BDA0004088187450000214
观测矩阵Hk的推导：
将第k个采集点的lora传感器坐标经过旋转平移得到的坐标即为当前小车位置坐标，第k个采集点的lora轨迹坐标即为当前小车位置坐标，将第k个采集点的激光雷达轨迹的坐标旋转平移后得到的坐标为第k个采集点的RTK轨迹坐标，基于上面三个方面，可总结以下三个方程：
Figure BDA0004088187450000211
公式17中，Rk、Tk分别表示旋转以及平移矩阵；
Figure BDA0004088187450000212
表示第k个采集点的Lora传感器的坐标；/>
Figure BDA0004088187450000213
表示第k个采集点的Lora定位坐标；Xk表示巡检小车在第k个采集点的位置。
用αk、βk、Tx，k、Ty，k代替Rk和Tk，公式17可转化为：
Figure BDA0004088187450000221
因此：
Figure BDA0004088187450000222
最后得到测量矩阵
Figure BDA0004088187450000223
观测矩阵Hk。
公式18～公式19中，
Figure BDA0004088187450000224
为卡尔曼滤波状态量；xk、yk表示第k个采集点的Lora接收器的横、纵坐标，单位为m；Lxk、Lyk表示第k个采集点的Lora定位坐标的横、纵坐标，单位为m；
Figure BDA0004088187450000225
为测量矩阵；Hk为观测矩阵。/>
S5.4、利用卡尔曼滤波的状态量
Figure BDA0004088187450000226
状态转移矩阵Fk、测量矩阵/>
Figure BDA0004088187450000227
观测矩阵Hk对巡检小车的位置进行更新迭代，最后计算得到巡检小车的精准定位。计算过程如下：
线性离散卡尔曼滤波状态量
Figure BDA0004088187450000228
的预测状态/>
Figure BDA0004088187450000229
与预测估计协方差矩阵Pk∣k-1分别为：
Figure BDA00040881874500002210
Figure BDA00040881874500002211
公式20～公式21中，Fk T为状态转移矩阵的转置矩阵；
Figure BDA00040881874500002212
Figure BDA00040881874500002213
为卡尔曼滤波状态量/>
Figure BDA00040881874500002214
的预测状态；Fk为步骤S5.2得到的状态转移矩阵；Bk是最优控制向量/>
Figure BDA0004088187450000231
作用在/>
Figure BDA0004088187450000232
上的控制矩阵，此处Bk＝0；/>
Figure BDA0004088187450000233
为控制向量；Pk∣k-1、Pk-1∣k-1为预测估计协方差矩阵；Qk是服从正态分布的协方差，其中：
Qk＝ws2·E8
ws为常数，E8为8阶单位矩阵。
通过以下步骤对状态进行更新：
S5.4.1、根据公式22更新状态的估计值：
Figure BDA0004088187450000234
公式22中，
Figure BDA0004088187450000235
为k-1到k时刻的状态量；Kk是最优卡尔曼增益，在步骤S5.4.2时进行计算；/>
Figure BDA0004088187450000236
为步骤S5.3得到的状态矩阵；Hk为步骤S5.3得到的观测矩阵；/>
Figure BDA0004088187450000237
为k时刻的状态量。
S5.4.2、根据公式23求解最优卡尔曼增益：
Figure BDA0004088187450000238
公式23中，Hk T为观测矩阵的转置矩阵；Kk是最优卡尔曼增益；Pk为k时刻的协方差矩阵；Hk为观测矩阵；Rgk为服从正态分布的协方差，其中：
Rgk＝vs2·E6
vs为常数，E6为6阶单位矩阵。
S5.4.3、更新协方差矩阵：
Figure BDA0004088187450000239
公式24中，I为单位矩阵；Kk为在步骤S5.4.2得到的最优卡尔曼增益；Hk为观测矩阵；
Figure BDA00040881874500002310
为k时刻的协方差矩阵；/>
Figure BDA00040881874500002311
为k-1至k时刻更新的协方差矩阵。
已知第一个采集点的Lora定位坐标
Figure BDA00040881874500002312
再根据第k个采集点的测量矩阵/>
Figure BDA0004088187450000241
便能够通过上述步骤计算得到/>
Figure BDA0004088187450000242
其中的Xk即为巡检小车当前的位置坐标，实现了激光雷达定位轨迹与lora定位数据的融合。
实施例：
参照图1，一种多传感器融合综合管廊巡检小车定位方法，该定位方法具体步骤如下：
1、巡检小车在地下综合管廊行驶过程中，小车顶部的激光里程计会对激光雷达在每个节点的轨迹位姿进行记录，同样位于车顶的激光雷达在对周围环境进行扫描后，会得到激光点云图。激光里程计在记录下轨迹位姿的同时，也会对激光点云中的局部特征如线特征、面特征进行提取，随后进行点云匹配得到前后帧点云的位姿关系，激光里程计将轨迹位姿以及位姿关系在中央处理器中计算并储存，得到巡检小车的高精度轨迹。
2、激光雷达在扫描过后得到周围环境的点云图，将点云预处理并用加权投影法得到地下综合管廊场景的二维图像以及三维结构，利用ORB视觉特征提取算子对二维图像以及三维结构进行特征提取，得到地下综合管廊场景的二维场景特征以及三维结构特征。算法的计算都由小车内置中央处理器完成。值得一提的是，小车的高精度轨迹、地下综合管廊场景的二维场景特征以及三维结构特征，对小车的最终定位影响至关重要，因此为了方便理解，在此处将这三个部分组成一个概念叫做地下综合管廊的道路场景表征模型。
3、内置lora模块的巡检小车、分散布置在管廊各处的lora网关以及lora服务器三者共同构成了一个基础的lora网络架构。巡检小车作为一个目标节点在行驶时，会接收到管廊各处lora网关的信号，lora网关的位置是固定且已知的并且作为参考节点存在。小车根据信号的强弱可以计算出自身与周围参考节点的距离。在小车内置中央处理器中利用三边定位法得到小车的初步定位坐标。初步定位坐标结合地下综合管廊表征模型中的小车高精度轨迹，实现巡检小车的粗定位。
4、得到了粗定位结果后，再次利用ORB视觉提取算子得到粗定位结果的二维场景特征，与此同时，由于小车在不停地移动，因此对应的道路场景表征模型也在不断地变化，其中的二维场景特征也随之变化，将粗定位结果的二维场景特征与实时变化的表征模型当中的二维场景特征结合。利用加权投影法将两个二维场景特征转化为激光雷达点云的二维图像，再次利用视觉提取算子进行特征提取以及特征点的匹配，得到进一步的节点级定位。
5、节点级定位得到后，将激光雷达得到的点云图进行特征提取，得到基于节点级定位的三维结构特征，结合此刻地下综合管廊道路场景表征模型当中的三维结构特征，通过旋转平移矩阵来对节点级定位进行优化，得到更进一步的度量级定位。
6、可以看到，在上述的定位过程中，反复用到了加权投影法以及ORB视觉特征提取等方法，所以在最后用卡尔曼滤波对上述多传感器进行数据融合，从而来减小累计误差，得到更精确更稳定的定位结果。

Claims (3)

1.一种多传感器融合综合管廊巡检小车定位方法，其特征在于：所述巡检小车包括搭载lora模块的lora传感器，与lora网关、lora服务器共同组成一个lora网络环境，巡检小车顶部设有激光雷达和激光里程计，巡检小车内部设有中央处理器；
所述方法包括如下步骤：
S1、利用激光雷达与激光里程计对地下综合管廊进行道路场景表征建模，得到地下综合管廊的道路场景表征模型：
S1.1、利用基于LOAM算法的激光里程计计算巡检小车运行时激光雷达在每个节点的轨迹位姿对激光雷达获得的三维激光雷达点云中的3D局部形状特征如线特征、面特征进行提取，随后进行点云配准，得到前后帧点云的位姿关系，位姿关系与轨迹位姿共同组成了巡检小车的高精度轨迹；
S1.2、在巡检小车的内置中央处理器中利用基于距离的激光点云加权投影法将激光雷达获得的三维激光雷达点云转换成二维场景图像以及三维结构；
S1.3、在巡检小车的内置中央处理器中利用ORB视觉特征提取算子对步骤S1.2中得到的二维场景图像以及三维结构进行特征提取，得到地下综合管廊的二维场景特征以及三维结构特征；
将激光里程计获取的巡检小车的轨迹信息、激光点云加权投影法获取的二维场景特征与基于视觉特征提取算子提取的三维结构特征结合在一起，得到地下综合管廊的道路场景表征模型；
S2、通过内置搭载lora模块的lora传感器的巡检小车、lora网关以及lora服务器搭建lora网络环境，在此网络环境下结合步骤S1得到的地下综合管廊的道路场景表征模型中的激光里程计获取的巡检小车的轨迹信息对巡检小车进行粗定位，包括如下步骤：
S2.1、利用lora物理层协议与mac协议，建立一个以LoraWAN规范标准的满足物联网低功耗的通讯网络，为室内定位提供网络环境；
S2.2、利用基于信号接收强弱(RSSI)的方法来进行室内定位，获取巡检小车的坐标；
S2.3、利用步骤S2.2的lora室内定位通过公式5获取的巡检小车的坐标加上地下综合管廊的场景表征模型中激光里程计所获取的巡检小车的高精度轨迹信息，实现巡检小车的初步粗定位；
在巡检小车的内置中央处理器中利用三边定位算法来求得目标节点的位置；包括如下步骤：
首先根据RSSI测距方法计算出任意三个参考节点和目标节点之间的距离长度，接着画出以三个参考节点为圆心，所求距离为半径的圆，三个圆两两相交，由相交的区域来定位未知节点；
三个参考节点的坐标分别为O1(xa，ya)，O2(xb，yb)，O3(xc，yc)；目标节点P为待测的室内空间坐标，设其为(x,y),dA,dB,dC表示由RSSI测距方法求出的三个参考节点到点P的距离；将这些参数代入平面上两点间的距离公式，如下公式4：
Figure FDA0004088187440000021
解出公式4中的方程组即可得到点P坐标为：
Figure FDA0004088187440000022
公式4和公式5中，da,db,dc分别表示由RSSI测距方法求出的三个参考节点到点P的距离，单位为m；xa,ya,xb,yb,xc,yc分别为三个参考节点的坐标值，单位为m；x,y为点P的坐标值，单位为m；
S3、基于场景特征匹配的节点级定位：
在步骤S2的粗定位结束之后，地下综合管廊的道路场景表征模型提取出属于粗定位结果的二维场景特征；随着巡检小车的不断移动，道路场景表征模型也在不断发生变化；根据步骤S1.3使用的ORB视觉特征提取算子得到当前的二维场景特征；将粗定位结果的二维场景特征与当前的二维场景特征通过步骤S1.2使用的激光点云加权投影法生成三维激光雷达点云的二维场景图像，并利用视觉特征提取算子对二维场景图像进行特征提取；利用K最近邻算法，对ORB视觉特征提取算子进行海明距离的计算，公式如下：
Figure FDA0004088187440000031
公式6中，O1，O2分别为两个OBR描述子；O1i和O2i分别为两个描述子中的第i个元素；XOR表述对两个元素进行异或计算；n为描述子大小；Dh(O1,O2)为海明距离；
随后计算两帧图像之间的旋转平移矩阵来对上述的二维场景特征与道路场景表征模型中的场景特征进行匹配；旋转平移矩阵如下式所示：
Figure FDA0004088187440000032
公式7中，[u v]T为当前二维场景图像中的特征点位置；[x y]T为对应的地下综合管廊的道路场景表征模型中二维图像的特征点的位置；R为旋转矩阵；T为平移矩阵；0为零矩阵；
匹配过程中，选取匹配特征点数目最多的三个节点作为节点级定位结果，并在巡检小车的内置中央处理器中计算出相应的旋转平移矩阵；最后得到当前车辆在局部坐标系下的位姿Pcm；位姿Pcm计算公式为：
Pcm＝Pck·Pkm 公式8-1
公式8-1中，Pck为当前巡检小车与第k个数据采集点之间的旋转平移矩阵；Pkm为第k个数据采集点与第一个数据采集点之间的旋转平移矩阵；Pcm为当前巡检小车在局部坐标系下的位姿；
S4、基于点云及三维结构的度量级定位：
将步骤S3中得到的节点级定位结果与地下综合管廊的道路场景表征模型当中的三维结构进行结合，通过巡检小车的内置中央处理器计算当前激光雷达点云与节点级定位结果相对应的激光雷达点云之间的误差平方和来得到优化后的旋转平移矩阵，最优旋转平移矩阵如下式所示：
Figure FDA0004088187440000041
Figure FDA0004088187440000042
Figure FDA0004088187440000043
公式9～公式11中，(ak,bk)表示第k个初值的相对平移矩阵；P(ak,bk)定义第k个初值的概率；
Figure FDA0004088187440000044
为最优旋转平移矩阵；Tcm是由/>
Figure FDA0004088187440000045
组成的最优旋转平移矩阵；β为期望公式中的常数；ai表示第i个初值的最优旋转矩阵；bi表示第i个初值的最优平移矩阵；
最优旋转平移矩阵与距离最近的节点的相对旋转矩阵Pcm共同组成位姿优化结果P′cm，如公式8-2所示；位姿优化结果作为快速ICP算法的初值，通过此算法进行点云匹配得到精确的度量级定位结果；
Pcm＝Pck·Pkm 公式8-1
Pcm′＝Pcm·Tcm 公式8-2
公式8-2中，P‘cm为当前巡检小车在局部坐标系下的位姿优化结果；Pcm为当前巡检小车在局部坐标系下的位姿；Tcm是由
Figure FDA0004088187440000046
组成的最优旋转平移矩阵；
S5、利用线性卡尔曼滤波对激光雷达定位轨迹与lora定位数据进行融合，减小激光里程计以及上述一些其他算法所带来的累积误差，得到更精准，更稳定的轨迹信息；
S5.1、卡尔曼滤波状态量
Figure FDA0004088187440000051
的推导：
旋转平移矩阵由R、T矩阵组合而成，采用cosθ、sinθ、Tx、Ty来代替R、T；通过实时获取lora定位坐标与lora传感器的坐标，来更新激光坐标系与lora坐标系之间的旋转平移矩阵，如公式12所示：
Figure FDA0004088187440000052
在第k个采集点，用α、β代替cosθ、sinθ；确定状态量
Figure FDA0004088187440000055
如公式13所示：
Figure FDA0004088187440000053
公式12～公式13中，
Figure FDA0004088187440000054
为Lora定位坐标此处将位置作为主要的卡尔曼滤波状态量；[xk，yk]T为激光坐标系下lora传感器的轨迹坐标，单位为m；θ为激光雷达坐标系与Lora坐标系之间的夹角，用于坐标系之间的转换；Tx、Ty分别为上述旋转平移矩阵T的两个分量，利于RTK的计算；Xk(x)、Xk(y)分别是巡检小车在第k个采集点位置Xk的横坐标与纵坐标，单位为m；Xk-1(x)、Xk-1(y)分别是巡检小车在第k-1个采集点位置Xk-1的横坐标与纵坐标，单位为m；αk、βk、Tx,k、Ty,k均是激光雷达与RTK(实时差分定位技术)在第k个采集点的旋转平移矩阵的分量；
S5.2、状态转移矩阵Fk的推导：
由于前后帧采集点Xk和Xk-1满足：
Xk＝2Kk-1-Xk-2 公式14
因此前后帧状态
Figure FDA0004088187440000061
因满足如下关系式：
Figure FDA0004088187440000062
由此得到状态转移矩阵Fk为：
Figure FDA0004088187440000063
公式14～公式16中，Xk表示巡检小车在第k个采集点的位置，Xk-1、Xk-2表示巡检小车在第k-1、k-2个采集点的位置，单位为m，用于前后帧的迭代；Xk(x)、Xk(y)分别是巡检小车在第k个采集点位置Xk的横坐标与纵坐标，单位为m；Xk-1(x)、Xk-1(y)分别是巡检小车在第k-1个采集点位置Xk-1的横坐标与纵坐标，单位为m；Xk-2(x)、Xk-2(y)分别是巡检小车在第k-2个采集点位置Xk-2的横坐标与纵坐标，单位为m；αk、βk、Tx,k、Ty,k均是激光雷达与RTK在第k个采集点的旋转平移矩阵的分量；αk-1、βk-1、Tx,k-1、Ty,k-1均是激光雷达与RTK在第k-1个采集点的旋转平移矩阵的分量；Fk为状态转移矩阵；
S5.3、测量矩阵
Figure FDA0004088187440000071
观测矩阵Hk的推导：
将第k个采集点的lora传感器坐标经过旋转平移得到的坐标即为当前小车位置坐标，第k个采集点的lora轨迹坐标即为当前小车位置坐标，将第k个采集点的激光雷达轨迹的坐标旋转平移后得到的坐标为第k个采集点的RTK轨迹坐标，基于上面三个方面，总结以下三个方程：
Figure FDA0004088187440000072
公式17中，Rk、Tk分别表示旋转以及平移矩阵；
Figure FDA0004088187440000073
表示第k个采集点的Lora传感器的坐标；/>
Figure FDA0004088187440000074
表示第k个采集点的Lora定位坐标；Xk表示巡检小车在第k个采集点的位置；
用αk、βk、Tx，k、Ty，k代替Rk和Tk，公式17转化为：
Figure FDA0004088187440000075
因此：
Figure FDA0004088187440000081
最后得到测量矩阵
Figure FDA0004088187440000082
观测矩阵Hk；
公式18～公式19中，
Figure FDA0004088187440000083
为卡尔曼滤波状态量；xk、yk表示第k个采集点的Lora接收器的横、纵坐标，单位为m；Lxk、Lyk表示第k个采集点的Lora定位坐标的横、纵坐标，单位为m；/>
Figure FDA0004088187440000084
为测量矩阵；Hk为观测矩阵；
S5.4、利用卡尔曼滤波的状态量
Figure FDA0004088187440000085
状态转移矩阵Fk、测量矩阵/>
Figure FDA00040881874400000816
观测矩阵Hk对巡检小车的位置进行更新迭代，最后计算得到巡检小车的精准定位；计算过程如下：
线性离散卡尔曼滤波状态量
Figure FDA0004088187440000086
的预测状态/>
Figure FDA0004088187440000087
与预测估计协方差矩阵Pk∣k-1分别为：
Figure FDA0004088187440000088
Figure FDA0004088187440000089
公式20～公式21中，Fk T为状态转移矩阵的转置矩阵；
Figure FDA00040881874400000810
Figure FDA00040881874400000811
为卡尔曼滤波状态量/>
Figure FDA00040881874400000812
的预测状态；Fk为步骤S5.2得到的状态转移矩阵；Bk是最优控制向量/>
Figure FDA00040881874400000813
作用在/>
Figure FDA00040881874400000814
上的控制矩阵，此处Bk＝0；/>
Figure FDA00040881874400000815
为控制向量；Pk∣k-1、Pk-1∣k-1为预测估计协方差矩阵；Qk是服从正态分布的协方差；其中：
Qk＝ws2·E8
ws为常数，E8为8阶单位矩阵；
通过以下步骤对巡检小车的状态进行更新：
S5.4.1、根据公式22更新状态的估计值：
Figure FDA0004088187440000091
公式22中，
Figure FDA0004088187440000092
为k-1到k时刻的状态量；Kk是最优卡尔曼增益，在步骤S5.4.2时进行计算；/>
Figure FDA0004088187440000093
为步骤S5.3得到的状态矩阵；Hk为步骤S5.3得到的观测矩阵；/>
Figure FDA0004088187440000094
为k时刻的状态量；
S5.4.2、根据公式23求解最优卡尔曼增益：
Figure FDA0004088187440000095
公式23中，Hk T为观测矩阵的转置矩阵；Kk是最优卡尔曼增益；Pk为k时刻的协方差矩阵；Hk为观测矩阵；Rgk为服从正态分布的协方差，其中：
Rgk＝vs2·E6
vs为常数，E6为6阶单位矩阵；
S5.4.3、更新协方差矩阵：
Figure FDA0004088187440000096
公式24中，I为单位矩阵；Kk为在步骤S5.4.2得到的最优卡尔曼增益；Hk为观测矩阵；
Figure FDA0004088187440000097
为k时刻的协方差矩阵；/>
Figure FDA0004088187440000098
为k-1至k时刻更新的协方差矩阵；
已知第一个采集点的Lora定位坐标
Figure FDA0004088187440000099
再根据第k个采集点的测量矩阵/>
Figure FDA00040881874400000910
便能够通过上述步骤计算得到/>
Figure FDA00040881874400000911
其中的Xk即为巡检小车当前的位置坐标，实现了激光雷达定位轨迹与lora定位数据的融合。
2.如权利要求1所述的多传感器融合综合管廊巡检小车定位方法，其特征在于：步骤S1.2中，预处理后，将点云投影至XOY平面，设激光点云P中的一点pi(x，y，z)，其中x∈(-N1，N1)米，y∈(-N2，N2)米以及z∈(-Z1，Z2)米；将激光点云在平面XOY上沿x轴和y轴均匀地分为2N1行和2N2列的栅格；二维图像的大小为(2N1，2N2)像素，而图像中的每个像素值由公式1决定：
Figure FDA0004088187440000101
公式1中，I(i,j)为图像中的每个像素值；∑pij为第i行第j列栅格所包含的点云总数；Kij为第i行第j列栅格的投影权值，由第i行第j列栅格到Z轴的投影距离决定；如公式2所示：
Figure FDA0004088187440000102
公式2中，Kij为第i行第j列栅格的投影权值；x，y为第i行第j列栅格中任意一点的X轴与Y轴坐标，单位为m；N为一个常数，根据激光雷达的工作范围确定，此处N的范围为(-31，31)。
3.如权利要求1或2所述的多传感器融合综合管廊巡检小车定位方法，其特征在于：步骤S2.2中，首先在lora环境中放置多个参考节点，这些参考节点一般为拥有MAC地址的数据链路层设备，巡检小车作为目标节点与这些参考节点进行相互的通信，当目标节点接收到来自参考节点的信号后，通过巡检小车的内置中央处理器计算出接收信号的强度值，将所得的RSSI值代入公式3中计算出参考节点和目标节点的距离；
Figure FDA0004088187440000103
公式3中，A为距离1米时的RSSI值的绝对值，最佳范围在45-49之间，单位为dbm；RSSI为接收信号的强度值，单位为dBm；n为环境对信号的衰减系数，最佳范围在3.25-4.5之间；d为参考节点和目标节点的距离，单位为m；
目标节点和参考节点将数据发送给lora网关，lora网关再将定位结果d传输给巡检小车的内置中央处理器。
CN202310142851.9A 2023-02-14 2023-02-14 多传感器融合综合管廊巡检小车定位方法 Active CN116299383B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310142851.9A CN116299383B (zh) 2023-02-14 2023-02-14 多传感器融合综合管廊巡检小车定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310142851.9A CN116299383B (zh) 2023-02-14 2023-02-14 多传感器融合综合管廊巡检小车定位方法

Publications (2)

Publication Number Publication Date
CN116299383A true CN116299383A (zh) 2023-06-23
CN116299383B CN116299383B (zh) 2024-02-02

Family

ID=86784401

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310142851.9A Active CN116299383B (zh) 2023-02-14 2023-02-14 多传感器融合综合管廊巡检小车定位方法

Country Status (1)

Country Link
CN (1) CN116299383B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117540518A (zh) * 2023-12-06 2024-02-09 北京城建勘测设计研究院有限责任公司 基于三维实景虚实融合的地下管线巡检装备及方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111199578A (zh) * 2019-12-31 2020-05-26 南京航空航天大学 基于视觉辅助激光雷达的无人机三维环境建模方法
US20210374904A1 (en) * 2020-05-26 2021-12-02 Baidu Usa Llc Depth-guided video inpainting for autonomous driving
CN115446834A (zh) * 2022-09-01 2022-12-09 西南交通大学 一种基于占据栅格配准的车底巡检机器人单轴重定位方法
US11543530B1 (en) * 2021-08-06 2023-01-03 Guangdong University Of Technology Indoor SLAM method based on 3D lidar and UWB

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111199578A (zh) * 2019-12-31 2020-05-26 南京航空航天大学 基于视觉辅助激光雷达的无人机三维环境建模方法
US20210374904A1 (en) * 2020-05-26 2021-12-02 Baidu Usa Llc Depth-guided video inpainting for autonomous driving
US11543530B1 (en) * 2021-08-06 2023-01-03 Guangdong University Of Technology Indoor SLAM method based on 3D lidar and UWB
CN115446834A (zh) * 2022-09-01 2022-12-09 西南交通大学 一种基于占据栅格配准的车底巡检机器人单轴重定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
贺利乐;王消为;赵涛;: "未知环境下履带式移动机器人SLAM研究", 传感器与微系统, no. 10 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117540518A (zh) * 2023-12-06 2024-02-09 北京城建勘测设计研究院有限责任公司 基于三维实景虚实融合的地下管线巡检装备及方法

Also Published As

Publication number Publication date
CN116299383B (zh) 2024-02-02

Similar Documents

Publication Publication Date Title
CN110879401B (zh) 基于相机和激光雷达的无人驾驶平台实时目标3d检测方法
CN102573049B (zh) 一种室内定位方法及系统
CN102170697B (zh) 一种室内定位方法及装置
CN116299383B (zh) 多传感器融合综合管廊巡检小车定位方法
CN103024939B (zh) 一种农田作物生长信息网络化采集系统及其构建方法
CN112305559A (zh) 基于地面定点激光雷达扫描的输电线距离测量方法、装置、系统和电子设备
AU2021101693A4 (en) Method for determining water use efficiency of vegetation layer and evapotranspiration water-gross primary production-water use efficiency measuring device
CN105759274A (zh) 一种台风关注区雷达降水估测方法
CN113504543B (zh) 无人机LiDAR系统定位定姿系统及方法
Qi et al. A combined localization algorithm for wireless sensor networks
Yi et al. An Enhanced Hybrid 3D Localization Algorithm Based on APIT and DV-Hop.
Cao et al. LOS compensation and trusted NLOS recognition assisted WiFi RTT indoor positioning algorithm
CN116660953B (zh) 一种城市cors智能监测系统
CN112071027A (zh) 一种超宽带测距山体滑坡预测方法、系统、装置及应用
CN110865367A (zh) 一种雷达视频数据智能融合方法
CN115951369A (zh) 面向复杂港口环境的多传感器融合定位方法
WO2022166510A1 (zh) 风电场前期测风塔规划选址方法、系统、装置及存储介质
Liu et al. Unmanned aerial vehicle positioning algorithm based on the secant slope characteristics of transmission lines
Tang et al. Intelligent height measurement technology for ground encroachments in large‐scale power transmission corridor based on advanced binocular stereovision algorithms
CN114916059A (zh) 基于区间随机对数阴影模型的WiFi指纹稀疏地图扩建方法
Li et al. 3D semantic map construction based on point cloud and image fusion
CN110531397B (zh) 基于gps与微波的室外巡检机器人定位系统及方法
CN113670290A (zh) 一种基于多机器人协作的移动机器人室内地图构建方法
CN110891241A (zh) 一种基于长短时间记忆网络模型与接入点选择策略的指纹定位方法
Guo et al. Vehicle location algorithm based on federated learning and smart phone in GNSS low sampling rate scene

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
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20230623

Assignee: Beijing Duzhong Robot Technology Co.,Ltd.

Assignor: Beijing University of Civil Engineering and Architecture

Contract record no.: X2024980004382

Denomination of invention: Multi sensor fusion comprehensive pipe gallery inspection vehicle positioning method

Granted publication date: 20240202

License type: Common License

Record date: 20240412

EE01 Entry into force of recordation of patent licensing contract