CN116939815B - 一种基于激光点云地图的uwb定位基站选择方法 - Google Patents

一种基于激光点云地图的uwb定位基站选择方法 Download PDF

Info

Publication number
CN116939815B
CN116939815B CN202311192112.7A CN202311192112A CN116939815B CN 116939815 B CN116939815 B CN 116939815B CN 202311192112 A CN202311192112 A CN 202311192112A CN 116939815 B CN116939815 B CN 116939815B
Authority
CN
China
Prior art keywords
base station
point cloud
target node
laser
uwb 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
CN202311192112.7A
Other languages
English (en)
Other versions
CN116939815A (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.)
Changshu Institute of Technology
Original Assignee
Changshu Institute of Technology
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 Changshu Institute of Technology filed Critical Changshu Institute of Technology
Priority to CN202311192112.7A priority Critical patent/CN116939815B/zh
Publication of CN116939815A publication Critical patent/CN116939815A/zh
Application granted granted Critical
Publication of CN116939815B publication Critical patent/CN116939815B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/003—Locating users or terminals or network equipment for network management purposes, e.g. mobility management locating network equipment
    • 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
    • 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)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种基于激光点云地图的UWB（Ultra‑Wideband）定位基站选择方法，属于室内定位技术领域。本发明首先在目标室内环境中部署多个UWB定位基站，并进行信号校准；然后利用背包式激光扫描系统获取目标室内环境的激光点云地图，并进行地图处理和特征提取；再测量目标节点与部署的所有UWB定位基站之间的距离和方位角，选取多个候选基站；并对候选基站进行过滤和评估，计算每个基站与目标节点之间的可见路径数量和传播效应；最后选择评估结果最优的候选基站作为目标节点的UWB定位基站，并利用UWB定位技术计算目标节点的准确位置。本发明能够对UWB定位基站进行优选，提高UWB定位的精度和可靠性。

Description

一种基于激光点云地图的UWB定位基站选择方法
技术领域
本发明涉及室内定位技术领域，具体涉及一种基于激光点云地图的UWB（Ultra-Wideband）定位基站选择方法。
背景技术
随着室内定位技术的快速发展，室内定位在各类应用场景中得到广泛应用，例如智能家居、室内导航、工业自动化等领域。而UWB（Ultra-Wideband，超宽带）定位技术由于其高精度、抗干扰能力强等优势，逐渐成为室内定位领域的重要技术手段之一。在UWB定位系统中，定位基站的选择对定位精度和系统性能至关重要。
目前，一些现有的UWB定位基站选择方法通常依赖于接收信号强度（RSSI）或时间差测量（TDoA）等技术来评估基站与目标节点之间的距离，然后选择最优的基站进行定位。然而，这些方法忽略了环境中的复杂结构和障碍物对信号的影响，导致定位精度不稳定，容易受到多径效应和信号遮挡的干扰。
因此，需要一种新颖的UWB定位基站选择方法，能够克服上述问题，提供更高精度和更稳定的室内定位体验。
发明内容
针对上述问题，本发明提出一种基于激光点云地图的UWB定位基站选择方法，该方法结合了激光点云地图和UWB定位技术，旨在解决现有方法存在的问题，提高室内定位的精度和可靠性。
为达到上述目的，本发明的技术方案如下：
一种基于激光点云地图的UWB定位基站选择方法，该方法包括如下步骤：
步骤1、在目标室内环境中部署多个UWB定位基站，并进行信号校准；
步骤2、利用背包式激光扫描系统获取目标室内环境的激光点云地图，并进行地图处理和障碍物特征提取；
步骤3、目标节点启动定位过程，测量目标节点与步骤1部署的所有UWB定位基站之间的距离和方位角，选取多个候选基站；
步骤4、基于步骤2的激光点云地图，对步骤3选取的候选基站进行过滤和评估，计算每个基站与目标节点之间的可见路径数量和传播效应；
步骤5、选择评估结果最优的候选基站作为目标节点的UWB定位基站，并利用UWB定位技术计算目标节点的准确位置。
进一步地，步骤2的具体方法包括如下子步骤：
子步骤2.1、激光扫描：使用背包式激光扫描系统对目标室内环境进行扫描，获取环境的激光点云数据；
子步骤2.2、激光点云地图构建：包括基于激光点云的特征点提取、特征关联、运动状态估计、里程计计算和地图构建；
子步骤2.2.1、特征点提取：特征点提取的参考标准为曲率，这个曲率计算公式如下：
，
其中，是每个激光点的曲率值；/>表示激光在同一帧中返回的连续激光点的集合；/>是激光雷达坐标系下的第k次扫描的激光点云中的第i个激光点，/>是是激光雷达坐标系下的第k次扫描的激光点云中的第j个激光点，/>表示集合/>中激光点的数量，/>表示模长，对于一帧点云中的同一线中的前5个激光点和后5个激光点不做曲率计算；
计算出曲率后，将一次扫描划分为4个相同的子区域，每个子区域按照曲率大小，提供2个边缘特征点和4个平面特征点；
子步骤2.2.2、特征关联：令初时刻激光扫描获取到的点云为/>，在/>时刻内激光扫描获取的点云为/>，利用2.2.1寻找特征点的方法，找到/>中的平面特征点和边缘特征点，假设/>和/>分别为对应边缘特征点组和平面特征点组，那么从/>中获得的边缘特征点组和/>以及平面特征点组和/>便形成了两组对应关系，形成了点云间的对应关系，为了获取相邻两帧间激光点云数据关联情况，应尽可能的让点到线的距离最小，点到面的距离最小，特征关联由距离表征完成，点到线的距离和点到面的距离通过以下公式计算得到：
，
其中，和/>分别为点到线和点到面的距离；/>表示/>时刻内激光扫描获取的点云中的边缘特征点/>的坐标；/>和/>表示/>时刻激光扫描获取到的点云中对应的两个边缘特征点的坐标；/>表示激光雷达/>时刻内激光扫描获取的点云中的平面特征点/>的坐标；/>、/>和/>表示/>时刻激光点云中对应的三个平面特征点的坐标；
子步骤2.2.3、运动状态估计：构建优化目标函数
（4）
（5）
其中表示边缘特征点与边缘特征线间的几何关系，边缘特征线由两个边缘特征点构成；/>表示平面特征点与特征面的几何关系，特征面由三个不共线的平面特征点构成；采用非线性优化的Levenberg-Marquardt方法对式（4）、（5）进行构建求解，获得最优化的参数估计，从而获得状态转移关系/>；
子步骤2.2.4、里程计计算：根据已经获取了时刻激光点云/>、/>时刻内激光扫描获取的点云为/>和/>时刻的状态转移关系/>，从/>提取边缘特征点和平面特征点/>，从/>中提取和/>对应的边缘特征点和平面特征点，并计算点到线的距离和点到面的距离；对每一个特征点分配一个双平方权重，权重与距离的平方成反比，对于距离超过100米的特征点直接放弃，将其权重设为 0，进行位姿求解；如果发现收敛，或达到最大迭代数，非线性优化就会终止，从而得到/>时刻和/>时刻间的位姿；
子步骤2.2.5、点云地图构建：令时刻激光扫描累积的点云地图为/>以及激光雷达在世界坐标系下的位姿/>；通过/>时刻里程计提供的位姿/>,将/>更新至，并将激光雷达坐标系的点云/>投影至世界坐标系，从而更新点云地图/>；
子步骤2.3、障碍物特征提取：针对点云地图，首先对其进行去噪；然后，采用DBSCAN聚类算法将点云进行聚类，并根据聚类结果对点云数据中属于障碍物的点提取出来，形成障碍物的点云集合，将障碍物的点云集合表示为几何图形。
进一步地，子步骤2.2.4中所述最大迭代数取30。
进一步地，步骤3的具体方法包括如下子步骤：
子步骤3.1、目标节点定位：目标节点启动定位过程，使用UWB技术与步骤1中部署的所有UWB定位基站进行通信，测量目标节点与各个UWB定位基站之间的时间差TDoA或接收信号强度RSSI；
子步骤3.2 、候选基站选取：根据子步骤3.1测得的时间差或信号强度，选取多个候选基站，这些候选基站将用于后续的激光点云地图评估。
进一步地，步骤4的具体方法包括如下子步骤：
子步骤4.1、可见路径计算：对于每个候选基站，通过激光点云地图判断其与目标节点之间是否存在可见路径，具体通过计算目标节点到基站连线与激光点云地图中的物体是否相交来实现；
子步骤4.2、可见路径数量：根据子步骤4.1计算的可见路径的情况，评估每个候选基站与目标节点之间的可见路径数量，可见路径数量越多，表示该基站的定位精度可能更高；
子步骤4.3 、传播效应评估：考虑激光点云地图中的障碍物对信号传播的影响，对候选基站进行传播效应评估，传播效应会导致信号的衰减和反射，影响定位精度。
进一步地，步骤5的具体方法包括如下子步骤：
子步骤5.1、目标节点的UWB定位基站：根据步骤4中的评估结果，选择评估最优的基站作为目标节点的UWB定位基站，具体是选择具有较高可见路径数量且传播效应较小的候选基站，选择的目标节点的UWB定位基站数量为6个，且分散在目标节点的四周；
子步骤5.2 、定位计算：利用UWB定位技术和子步骤5.1选择的定位基站，计算目标节点的准确位置，具体采用时间差测量（TDoA）技术实现：
令目标节点所在位置为，目标节点的UWB定位基站f的位置为，/>，则目标节点的UWB定位基站f与目标节点的距离为/>，目标节点的UWB定位基站f与目标节点的UWB定位基站1的距离为/>，目标节点的UWB定位基站f与目标节点的距离/>和目标节点的UWB定位基站1与目标节点的距离/>间的差值/>为：
(6)
目标节点的UWB定位基站f与目标节点的距离的平方和目标节点的UWB定位基站1与目标节点的距离/>的平方间的差值为：
(7)
可得：
（8）
将式（8）改写成矩阵形式：
(9)
其中：，/>，，
将选择的定位基站数据代入式（9），并利用最小二乘法即可确定目标节点的位置信息。
与现有技术相比，本发明具有如下优点：
1）利用激光点云地图提供的环境几何信息，使得基站选择更加准确和可靠，有效克服了多径效应和信号遮挡带来的影响；
2）提高了室内定位的精度和稳定性，适用于对定位精度要求较高的场景；
3）基于激光点云地图的基站选择方法具有一定的通用性，能够适用于不同类型的室内环境。
附图说明
图1为本发明所描述的基于激光点云地图的UWB定位基站选择方法流程图。
图2是本发明的实施例在目标室内环境中部署多个UWB定位基站的分布图，图中1是障碍物，2是目标节点，3是UWB基站。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如图1所示，本实施例的一种基于激光点云地图的UWB定位基站选择方法，该方法包括如下步骤：
步骤1、在目标室内环境中部署多个UWB定位基站，并进行信号校准；如图2所示，部署UWB定位基站根据室内定位需求和覆盖范围，选择合适的位置来部署UWB定位基站。通常，需要考虑基站之间的间隔、高度以及信号覆盖的区域。将UWB定位基站设备固定在确定的位置上，并确保其稳固和安全。在部署好的基站中，进行信号校准工作。这涉及到基站之间的同步和校准，以确保它们在工作时能够准确地测量目标节点与基站之间的时间差。
步骤2、利用背包式激光扫描系统获取目标室内环境的激光点云地图，并进行地图处理和特征提取，包括如下子步骤：
子步骤2.1、激光扫描：使用背包式激光扫描系统对目标室内环境进行扫描，获取环境的激光点云数据；背包式激光扫描系统会发射激光束，然后测量激光束与环境中物体的反射，从而得到点云数据。
子步骤2.2、激光点云地图构建：包括基于激光点云的特征点提取、特征关联、运动状态估计、里程计计算和地图构建；
子步骤2.2.1、特征点提取：特征点提取的参考标准为曲率，由于激光在环境中会返回不均匀分布的各个点，因此定义这样一个局部曲面的平滑度来作为分类的标准。这个曲率计算公式如下：
，
其中，是每个激光点的曲率值；/>表示激光在同一帧中返回的连续激光点的集合；/>是激光雷达坐标系下的第k次扫描的激光点云中的第i个激光点，/>是是激光雷达坐标系下的第k次扫描的激光点云中的第j个激光点，/>表示集合/>中激光点的数量，/>表示模长，对于一帧点云中的同一线中的前5个激光点和后5个激光点不做曲率计算；
计算出曲率后，将一次扫描划分为4个相同的子区域，每个子区域按照曲率大小，提供2个边缘特征点和4个平面特征点；
子步骤2.2.2、特征关联：特征关联将使用到连续两帧的点云数据。令初时刻激光扫描获取到的点云为/>，在/>时刻内激光扫描获取的点云为/>，从时间戳/>到过程中，点云是在不断增量获取的，利用2.2.1寻找特征点的方法，找到/>中的平面特征点和边缘特征点，假设/>和/>分别为对应边缘特征点组和平面特征点组，那么从/>中获得的边缘特征点组和/>以及平面特征点组和/>便形成了两组对应关系，形成了点云间的对应关系，便要求解这个对应关系的数学表达形式，所以构建点云变换时的模型，投影获得的边缘角特征点组通过一个旋转平移到达了对应在新进点云中的相似位置且特征点间的相互关系不变，便可以得到一组关系，平面点也如法炮制，便获得了第二组关系。为了获取相邻两帧间激光点云数据关联情况，应尽可能的让点到线的距离最小，点到面的距离最小，特征关联由距离表征完成，点到线的距离和点到面的距离通过以下公式计算得到：
，
其中，和/>分别为点到线和点到面的距离；/>表示/>时刻内激光扫描获取的点云中的边缘特征点/>的坐标；/>和/>表示/>时刻激光扫描获取到的点云中对应的两个边缘特征点的坐标；/>表示激光雷达/>时刻内激光扫描获取的点云中的平面特征点/>的坐标；/>、/>和/>表示/>时刻激光点云中对应的三个平面特征点的坐标；
子步骤2.2.3、运动状态估计：构建优化目标函数
（4）
（5）
其中表示边缘特征点与边缘特征线间的几何关系，边缘特征线由两个边缘特征点构成；/>表示平面特征点与特征面的几何关系，特征面由三个不共线的平面特征点构成；采用非线性优化的Levenberg-Marquardt方法对式（4）、（5）进行构建求解，获得最优化的参数估计，从而获得状态转移关系/>；
子步骤2.2.4、里程计计算：根据已经获取了时刻激光点云/>、/>时刻内激光扫描获取的点云为/>和/>时刻的状态转移关系/>，从/>提取边缘特征点和平面特征点/>，从/>中提取/>和/>对应的边缘特征点和平面特征点，并计算点到线的距离和点到面的距离；对每一个特征点分配一个双平方权重，权重与距离的平方成反比，对于距离超过100米的特征点直接放弃，将其权重设为 0，进行位姿求解；如果发现收敛，或达到最大迭代数，本实施例中最大迭代数取30，非线性优化就会终止，从而得到/>时刻和/>时刻间的位姿/>；
子步骤2.2.5、点云地图构建：地图构建算法的速度是1Hz，比里程计部分慢许多，在一次扫描时只会调用一次。地图算法不断地将扫描结果映射在世界坐标系中，并匹配地图寄存器。里程计发布了较为稳定的点云消息，经过雷达与世界坐标系的转换来映射到全局地图上，这一过程表示为：令时刻激光扫描累积的点云地图为/>以及激光雷达在世界坐标系下的位姿/>；通过/>时刻里程计提供的位姿/>,将/>更新至/>，并将激光雷达坐标系的点云/>投影至世界坐标系，从而更新点云地图/>；
子步骤2.3、障碍物特征提取：针对点云地图，首先对其进行去噪；然后，采用DBSCAN聚类算法将点云进行聚类，并根据聚类结果对点云数据中属于障碍物的点提取出来，形成障碍物的点云集合，将障碍物的点云集合表示为几何图形，例如多边形或三维模型。这些特征将在后续的基站选择中起到重要作用。
步骤3、目标节点启动定位过程，测量目标节点与步骤1部署的所有UWB定位基站之间的距离和方位角，选取多个候选基站，具体方法包括如下子步骤：
子步骤3.1、目标节点定位：目标节点启动定位过程，使用UWB技术与步骤1中部署的所有UWB定位基站进行通信，测量目标节点与各个UWB定位基站之间的时间差TDoA或接收信号强度RSSI；
子步骤3.2 、候选基站选取：根据子步骤3.1测得的时间差或信号强度，选取多个候选基站，这些候选基站将用于后续的激光点云地图评估。
步骤4、基于步骤2的激光点云地图，对步骤3选取的候选基站进行过滤和评估，计算每个基站与目标节点之间的可见路径数量和传播效应，包括如下子步骤：
子步骤4.1、可见路径计算：对于每个候选基站，通过激光点云地图判断其与目标节点之间是否存在可见路径，具体通过计算目标节点到基站连线与激光点云地图中的物体是否相交来实现；
子步骤4.2、可见路径数量：根据子步骤4.1计算的可见路径的情况，评估每个候选基站与目标节点之间的可见路径数量，可见路径数量越多，表示该基站的定位精度可能更高；
子步骤4.3 、传播效应评估：考虑激光点云地图中的障碍物对信号传播的影响，对候选基站进行传播效应评估，传播效应会导致信号的衰减和反射，影响定位精度。
步骤5、选择评估结果最优的候选基站作为目标节点的UWB定位基站，并利用UWB定位技术计算目标节点的准确位置，包括如下子步骤：
子步骤5.1、目标节点的UWB定位基站：根据步骤4中的评估结果，选择评估最优的基站作为目标节点的UWB定位基站，具体是选择具有较高可见路径数量且传播效应较小的候选基站，选择的目标节点的UWB定位基站数量为6个，且分散在目标节点的四周；
子步骤5.2 、定位计算：利用UWB定位技术和子步骤5.1选择的定位基站，计算目标节点的准确位置，具体采用时间差测量（TDoA）技术实现：
令目标节点所在位置为，目标节点的UWB定位基站f的位置为，/>，则目标节点的UWB定位基站f与目标节点的距离为/>，目标节点的UWB定位基站f与目标节点的UWB定位基站1的距离为/>，目标节点的UWB定位基站f与目标节点的距离/>和目标节点的UWB定位基站1与目标节点的距离/>间的差值/>为：
(6)
目标节点的UWB定位基站f与目标节点的距离的平方和目标节点的UWB定位基站1与目标节点的距离/>的平方间的差值为：
(7)
可得：
（8）
将式（8）改写成矩阵形式：
(9)
其中：，/>，，
将选择的定位基站数据代入式（9），并利用最小二乘法即可确定目标节点的位置信息。
综上所述，这个具体实施例中，我们首先部署UWB定位基站并进行信号校准，然后使用激光扫描仪获取室内环境的激光点云地图并进行地图处理和特征提取。接下来，目标节点通过UWB技术与基站通信，测量与所有基站之间的距离，选取多个候选基站。最后，通过激光点云地图对候选基站进行评估，选择评估结果最优的基站进行定位计算，得到目标节点的准确位置。这样，我们就实现了一种基于激光点云地图的UWB定位基站选择方法，可以提高室内定位的精度和可靠性。
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (5)

1.一种基于激光点云地图的UWB定位基站选择方法，其特征在于，该方法包括如下步骤：
步骤1、在目标室内环境中部署多个UWB定位基站，并进行信号校准；
步骤2、利用背包式激光扫描系统获取目标室内环境的激光点云地图，并进行地图处理和障碍物特征提取；
步骤3、目标节点启动定位过程，测量目标节点与步骤1部署的所有UWB定位基站之间的距离和方位角，选取多个候选基站；
步骤4、基于步骤2的激光点云地图，对步骤3选取的候选基站进行过滤和评估，计算每个基站与目标节点之间的可见路径数量和传播效应；
步骤5、选择评估结果最优的候选基站作为目标节点的UWB定位基站，并利用UWB定位技术计算目标节点的准确位置；
步骤2的具体方法包括如下子步骤：
子步骤2.1、激光扫描：使用背包式激光扫描系统对目标室内环境进行扫描，获取环境的激光点云数据；
子步骤2.2、激光点云地图构建：包括基于激光点云的特征点提取、特征关联、运动状态估计、里程计计算和地图构建；
子步骤2.2.1、特征点提取：特征点提取的参考标准为曲率，这个曲率计算公式如下：
其中，c是每个激光点的曲率值；表示激光在同一帧中返回的连续激光点的集合；/>是激光雷达坐标系下的第k次扫描的激光点云中的第i个激光点，/>是是激光雷达坐标系下的第k次扫描的激光点云中的第j个激光点，/>表示集合/>中激光点的数量，||·||表示模长，对于一帧点云中的同一线中的前5个激光点和后5个激光点不做曲率计算；
计算出曲率后，将一次扫描划分为4个相同的子区域，每个子区域按照曲率大小，提供2个边缘特征点和4个平面特征点；
子步骤2.2.2、特征关联：令tk初时刻激光扫描获取到的点云为Pk，在tk+1时刻内激光扫描获取的点云为Pk+1，利用2.2.1寻找特征点的方法，找到Pk+1中的平面特征点和边缘特征点，假设Ek+1和Hk+1分别为对应边缘特征点组和平面特征点组，那么从Pk中获得的边缘特征点组和Ek+1以及平面特征点组和Hk+1便形成了两组对应关系，形成了点云间的对应关系，为了获取相邻两帧间激光点云数据关联情况，应尽可能的让点到线的距离最小，点到面的距离最小，特征关联由距离表征完成，点到线的距离和点到面的距离通过以下公式计算得到：
其中，dε和分别为点到线和点到面的距离；/>表示tk+1时刻内激光扫描获取的点云中的边缘特征点i1的坐标；/>和/>表示tk时刻激光扫描获取到的点云中对应的两个边缘特征点的坐标；/>表示激光雷达tk+1时刻内激光扫描获取的点云中的平面特征点i2的坐标；/>和/>表示tk时刻激光点云中对应的三个平面特征点的坐标；
子步骤2.2.3、运动状态估计：构建优化目标函数
其中fε表示边缘特征点与边缘特征线间的几何关系，边缘特征线由两个边缘特征点构成；表示平面特征点与特征面的几何关系，特征面由三个不共线的平面特征点构成；采用非线性优化的Levenberg-Marquardt方法对式(4)、(5)进行构建求解，获得最优化的参数估计，从而获得状态转移关系/>
子步骤2.2.4、里程计计算：根据已经获取了tk时刻激光点云Pk、tk+1时刻内激光扫描获取的点云为Pk+1和Tk+1时刻的状态转移关系从Pk+1提取边缘特征点Ek+1和平面特征点Hk+1，从Pk中提取Ek+1和Hk+1对应的边缘特征点和平面特征点，并计算点到线的距离和点到面的距离；对每一个特征点分配一个双平方权重，权重与距离的平方成反比，对于距离超过100米的特征点直接放弃，将其权重设为0，进行位姿求解；如果发现收敛，或达到最大迭代数，非线性优化就会终止，从而得到tk时刻和tk+1时刻间的位姿/>
子步骤2.2.5、点云地图构建：令tk时刻激光扫描累积的点云地图为以及激光雷达在世界坐标系下的位姿/>通过tk+1时刻里程计提供的位姿/>将/>更新至/>并将激光雷达坐标系的点云Pk+1投影至世界坐标系，从而更新点云地图/>
子步骤2.3、障碍物特征提取：针对点云地图，首先对其进行去噪；然后，采用DBSCAN聚类算法将点云进行聚类，并根据聚类结果对点云数据中属于障碍物的点提取出来，形成障碍物的点云集合，将障碍物的点云集合表示为几何图形。
2.根据权利要求1所述的一种基于激光点云地图的UWB定位基站选择方法，其特征在于，步骤2.2.4中所述最大迭代数取30。
3.根据权利要求1所述的一种基于激光点云地图的UWB定位基站选择方法，其特征在于，步骤3的具体方法包括如下子步骤：
子步骤3.1、目标节点定位：目标节点启动定位过程，使用UWB技术与步骤1中部署的所有UWB定位基站进行通信，测量目标节点与各个UWB定位基站之间的时间差TDoA或接收信号强度RSSI；
子步骤3.2、候选基站选取：根据子步骤3.1测得的时间差或信号强度，选取多个候选基站，这些候选基站将用于后续的激光点云地图评估。
4.根据权利要求1所述的一种基于激光点云地图的UWB定位基站选择方法，其特征在于，步骤4的具体方法包括如下子步骤：
子步骤4.1、可见路径计算：对于每个候选基站，通过激光点云地图判断其与目标节点之间是否存在可见路径，具体通过计算目标节点到基站连线与激光点云地图中的物体是否相交来实现；
子步骤4.2、可见路径数量：根据子步骤4.1计算的可见路径的情况，评估每个候选基站与目标节点之间的可见路径数量，可见路径数量越多，表示该基站的定位精度可能更高；
子步骤4.3、传播效应评估：考虑激光点云地图中的障碍物对信号传播的影响，对候选基站进行传播效应评估，传播效应会导致信号的衰减和反射，影响定位精度。
5.根据权利要求1所述的一种基于激光点云地图的UWB定位基站选择方法，其特征在于，步骤5的具体方法包括如下子步骤：
子步骤5.1、目标节点的UWB定位基站：根据步骤4中的评估结果，选择评估最优的基站作为目标节点的UWB定位基站，具体是选择具有较高可见路径数量且传播效应较小的候选基站，选择的目标节点的UWB定位基站数量为6个，且分散在目标节点的四周；
子步骤5.2、定位计算：利用UWB定位技术和子步骤5.1选择的定位基站，计算目标节点的准确位置，具体采用时间差测量(TDoA)技术实现：
令目标节点所在位置为u＝[x y z]T，目标节点的UWB定位基站f的位置为Sf＝[xf yfzf]T，f＝1,…,N(N＝6)，则目标节点的UWB定位基站f与目标节点的距离为Rf＝||sf-u||，目标节点的UWB定位基站f与目标节点的UWB定位基站1的距离为df＝||sf-s1||，目标节点的UWB定位基站f与目标节点的距离Rf和目标节点的UWB定位基站1与目标节点的距离R1间的差值Rf1为：
Rf1＝||sf-u||-||s1-u||＝Rf-R1,f＝2,…,N (6)
目标节点的UWB定位基站f与目标节点的距离Rf的平方和目标节点的UWB定位基站1与目标节点的距离R1的平方间的差值为：
Rf 2-R1 2＝＝dj 2-2(sf-s1)Tu,f＝2,…,N (7)
可得：
将式(8)改写成矩阵形式：
Ax＝b (9)
其中：
将选择的定位基站数据代入式(9)，并利用最小二乘法即可确定目标节点的位置信息。
CN202311192112.7A 2023-09-15 2023-09-15 一种基于激光点云地图的uwb定位基站选择方法 Active CN116939815B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311192112.7A CN116939815B (zh) 2023-09-15 2023-09-15 一种基于激光点云地图的uwb定位基站选择方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311192112.7A CN116939815B (zh) 2023-09-15 2023-09-15 一种基于激光点云地图的uwb定位基站选择方法

Publications (2)

Publication Number Publication Date
CN116939815A CN116939815A (zh) 2023-10-24
CN116939815B true CN116939815B (zh) 2023-12-05

Family

ID=88386430

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311192112.7A Active CN116939815B (zh) 2023-09-15 2023-09-15 一种基于激光点云地图的uwb定位基站选择方法

Country Status (1)

Country Link
CN (1) CN116939815B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111641919A (zh) * 2020-05-29 2020-09-08 浙江水利水电学院 一种uwb基站迭代自定位与校准方法
WO2021253430A1 (zh) * 2020-06-19 2021-12-23 深圳市大疆创新科技有限公司 绝对位姿确定方法、电子设备及可移动平台
CN114501300A (zh) * 2021-12-16 2022-05-13 江苏集萃未来城市应用技术研究所有限公司 一种基于空间环境误差模型的分布式定位算法
CN116112870A (zh) * 2022-12-27 2023-05-12 广州市中海达测绘仪器有限公司 一种基于uwb链式定位的基站选择方法及选择系统
CN116736220A (zh) * 2023-05-17 2023-09-12 武汉大学 一种uwb定位系统基站坐标测量方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111641919A (zh) * 2020-05-29 2020-09-08 浙江水利水电学院 一种uwb基站迭代自定位与校准方法
WO2021253430A1 (zh) * 2020-06-19 2021-12-23 深圳市大疆创新科技有限公司 绝对位姿确定方法、电子设备及可移动平台
CN114501300A (zh) * 2021-12-16 2022-05-13 江苏集萃未来城市应用技术研究所有限公司 一种基于空间环境误差模型的分布式定位算法
CN116112870A (zh) * 2022-12-27 2023-05-12 广州市中海达测绘仪器有限公司 一种基于uwb链式定位的基站选择方法及选择系统
CN116736220A (zh) * 2023-05-17 2023-09-12 武汉大学 一种uwb定位系统基站坐标测量方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
融合超宽带与激光雷达的室内SLAM 方法实现 唐;唐浩，张卫强，查云飞，沈磊，易迎春;《数据通信》;全文 *

Also Published As

Publication number Publication date
CN116939815A (zh) 2023-10-24

Similar Documents

Publication Publication Date Title
CN109275093B (zh) 基于uwb定位与激光地图匹配的定位方法和移动终端
CN108490473B (zh) 一种融合gnss和uwb的无人机增强定位方法与系统
CN105547305B (zh) 一种基于无线定位和激光地图匹配的位姿解算方法
Zhuang et al. Autonomous smartphone-based WiFi positioning system by using access points localization and crowdsourcing
CN103402258B (zh) 一种基于Wi‑Fi的室内定位系统和方法
Diaz et al. Bluepass: An indoor bluetooth-based localization system for mobile applications
CN112533163B (zh) 基于NB-IoT改进的融合超宽带和蓝牙的室内定位方法
Ma et al. Wi-Fi RTT ranging performance characterization and positioning system design
KR102116824B1 (ko) 딥러닝 기반 측위 시스템 및 이의 구축 방법
WO2009099773A2 (en) Systems and methods for providing location based services (lbs) utilizing wlan and/or gps signals for seamless indoor and outdoor tracking
KR101163335B1 (ko) 수신신호강도의 거리 추정 방식에 의거한 실내 무선 측위 방법 및 이 방법을 실시하기 위한 프로그램이 기록된 기록매체
CN112505671B (zh) Gnss信号缺失环境下毫米波雷达目标定位方法及装置
CN111751785B (zh) 一种隧道环境下车辆可见光定位方法
CN102291817A (zh) 移动通信网络中的基于位置测量样本的群定位方法
WO2022100272A1 (zh) 室内定位方法及相关装置
CN110636436A (zh) 基于改进chan算法的三维uwb室内定位方法
CN108413966A (zh) 基于多种传感测距技术室内定位系统的定位方法
CN106455051B (zh) 通过距离校准设备提升WiFi定位精度的方法
Kwon et al. RF signal source search and localization using an autonomous UAV with predefined waypoints
CN116939815B (zh) 一种基于激光点云地图的uwb定位基站选择方法
CN110267342B (zh) 一种复杂室内场景下基于wifi的定位方法
CN109640253B (zh) 一种移动机器人定位方法
Caceres et al. WLAN-based real time vehicle locating system
CN114758364B (zh) 基于深度学习的工业物联网场景融合定位方法及系统
CN111314848B (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