CN117213476A - 一种隧道施工人员定位方法及装置 - Google Patents
一种隧道施工人员定位方法及装置 Download PDFInfo
- Publication number
- CN117213476A CN117213476A CN202311097272.3A CN202311097272A CN117213476A CN 117213476 A CN117213476 A CN 117213476A CN 202311097272 A CN202311097272 A CN 202311097272A CN 117213476 A CN117213476 A CN 117213476A
- Authority
- CN
- China
- Prior art keywords
- positioning result
- positioning
- ultra
- sensor
- wideband
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 89
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 57
- 238000001514 detection method Methods 0.000 claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000005259 measurement Methods 0.000 claims description 21
- 230000004927 fusion Effects 0.000 claims description 20
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 11
- 238000009499 grossing Methods 0.000 claims description 10
- 238000004891 communication Methods 0.000 claims description 9
- 230000010354 integration Effects 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 5
- 238000003860 storage Methods 0.000 claims description 5
- 230000000452 restraining effect Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 abstract description 18
- 230000008569 process Effects 0.000 description 15
- 230000000875 corresponding effect Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 8
- 230000009471 action Effects 0.000 description 4
- 230000005021 gait Effects 0.000 description 4
- 230000001360 synchronised effect Effects 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 239000004566 building material Substances 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 239000004035 construction material Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002146 bilateral effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 238000006880 cross-coupling reaction Methods 0.000 description 1
- 230000001627 detrimental effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 239000007769 metal material Substances 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- 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
- Navigation (AREA)
Abstract
本发明公开了一种隧道施工人员定位方法及装置，所述方法包括：获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。本发明通过隧道施工人员定位方法实现了室内以及隧道动态变化的施工环境中的高精度定位。
Description
技术领域
本发明涉及计算机和位置服务领域，尤其涉及一种隧道施工人员定位方法及装置。
背景技术
在智能化施工过程中，准确的位置信息可以降低工人的劳动强度，提高生产效率，确保工人安全；然而，施工现场存在人员密集、设备数量多、噪音污染严重、施工材料杂乱等复杂的环境问题，给施工人员的定位带来了挑战。
在室内定位中，原本适用于室外定位的全球导航卫星系统(Global Navi gationSatellite System,GNSS)在室内环境以及隧道中的信号会变弱或无法接收，而WiFi、超宽带(Ultra-Wide Band,UWB)、基于惯性单元的定位、室内同步定位和测绘(SimultaneousLocalization and Mapping,SLAM)、射频识别(Radio Frequency Identification,RFID)标签和声学定位多种定位方法主要是建立在信号在传输过程中不受干扰的情况下；而建筑材料、过往车辆或墙壁可能会影响复杂建筑环境中的信号传播，因此目前的定位系统用于室内以及隧道动态变化的施工环境时会由于复杂环境导致测量不准确。
因此，现有技术还有待于改进和发展。
发明内容
本申请要解决的技术问题在于，针对目前的定位系统用于室内动态变化的施工环境时会由于复杂环境导致测量不准确的问题，提供隧道施工人员定位方法及装置，本发明能够通过隧道施工人员定位方法实现了室内以及隧道动态变化的施工环境中的高精度定位。
为了解决上述现有技术问题的不足，本申请实施例第一方面提供了一种隧道施工人员定位方法，所述方法包括：
获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；
获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；
基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；
将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
所述获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果，具体包括：
采用扩展卡尔曼滤波器对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果。
所述获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果，具体包括：
所述获取惯性传感器采集到的惯性定位数据包括获取惯性传感器中加速度计和陀螺仪采集到的数据；
所述机械编排算法对所述加速度计和陀螺仪采集到的数据进行校正，并求解姿态矩阵，输出惯性传感器定位结果。
所述机械编排算法对所述加速度计和陀螺仪采集到的数据进行校正，并求解姿态矩阵，输出惯性传感器定位结果，具体包括：
加速度计和陀螺仪收集数据并执行误差补偿，对陀螺仪数据进行地球自转率修正，然后求解姿态矩阵，对加速度数据通过姿态矩阵消除有害积分。
所述采用线性化检测和杠杆臂算法修正惯性传感器定位结果，具体包括：
采用线性化检测判断所述超宽带传感器定位结果中定位信息是否缺失；
当判断所述超宽带传感器定位结果中定位信息未缺失，基于所述超宽带传感器定位结果，采用杠杆臂算法修正惯性传感器定位结果；
当判断所述超宽带传感器定位结果中定位信息缺失，则对陀螺仪数据积分处理，通过积分处理结果约束惯性传感器定位结果误差。
所述杠杆臂算法，具体包括：
基于所述超宽带传感器定位结果和惯性传感器定位结果，构建位置测量模型，通过所述位置测量模型得到误差反馈；
将所述误差反馈输入到误差校正卡尔曼滤波器中，从而对惯性传感器定位结果进行校正。
所述将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出，具体包括：
采用滤波器将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果；
通过RTS平滑方法对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
本申请实施例第二方面提供了一种隧道施工人员定位装置，所述隧道施工人员定位装置包括：
超宽带传感器定位获取模块，获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；
惯性传感器定位获取模块，获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；
修正模块，基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；
融合优化模块，将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
本申请实施例第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上任意所述的隧道施工人员定位方法中的步骤。
本申请实施例第四方面提供了一种终端设备，包括：处理器、存储器及通信总线；所述存储器上存储有可被所述处理器执行的计算机可读程序；
所述通信总线实现处理器和存储器之间的连接通信；
所述处理器执行所述计算机可读程序时实现如上任意所述的隧道施工人员定位方法中的步骤。
有益效果：与现有技术相比，本申请提供了一种隧道施工人员定位方法及装置，所述方法包括获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。通过上述方法，本发明通过获取对应的超宽带传感器和惯性传感器的定位数据，对应的采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果以及对应融合优化后，实现融合超宽带传感器和惯性定位传感器以实现高精度和鲁棒性的定位；且在该过程中使用杠杆臂算法利用超宽带传感器信息修正惯性传感器的累积误差，从而使得惯性传感器得到的数据更加准确；在所述方法中在超宽带传感器数据出现异常时，采用线性检测算法对应校正惯性传感器的定位结果；并且本发明在最终实现对超宽带传感器和惯性传感器的数据融合，并采用RTS平滑方法(Rauch-tung-sriebel、RTS)对定位结果进行优化，最终实现室内以及隧道动态变化的施工环境中的高精度定位。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种隧道施工人员定位方法的流程图；
图2为本发明实施例提供的定位框架示意图；
图3为本发明实施例提供的三边定位模型示意图；
图4为本发明实施例提供的捷联式惯导原理框图；
图5为本发明实施例提供的步态周期中头部、脚跟和腰部之间的相对姿势关系示意图；
图6为本发明实施例提供的隧道施工人员传感器定位装置安装示意图；
图7为本发明实施例提供的隧道施工人员定位装置的原理框图。
具体实施方式
本申请提供一种隧道施工人员定位方法及装置，为使本申请的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本申请进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样被特定定义，否则不会用理想化或过于正式的含义来解释。
另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
随着智能施工方法的应用，确保施工人员的安全尤为重要，而在智能化施工过程中，准确的位置信息可以降低工人的劳动强度，提高生产效率，确保工人安全。然而，施工现场存在人员密集、设备数量多、噪音污染严重、施工材料杂乱等复杂的环境问题，给施工人员的定位带来了挑战。
目前使用全球导航卫星系统(Global Navigation Satellite System,GNS S)进行定位，在室外施工现场人员定位方面取得了良好的效果；然而，GN SS在室内环境中的信号会变弱或无法接收。而多种传感器在室内使用时都会被环境所干扰导致，例如WiFi、超宽带(Ultra-Wide Band,UWB)、基于惯性单元的定位、室内同步定位和测绘(SimultaneousLocalization and Mappi ng,SLAM)、射频识别(Radio Frequency Identification,RFID)标签和声学定位；其中UWB在弱信号干扰环境中具有穿透力强、精度高的特点；然而，这些定位方法主要是建立在信号在传输过程中不受干扰的情况下，在室内以及隧道内使用时，建筑材料、过往车辆或墙壁可能会影响复杂建筑环境中的信号传播，其中，单个UWB定位系统也不能用于动态变化的施工环境。
目前，多传感器融合定位在室内建筑定位中具有明显的优势，而本发明所述的隧道施工人员定位方法对UWB和惯性传感器(Inertial Measureme nt Unit、IMU)进行融合定位，其中UWB的优点是它有效地限制了惯性定位的误差，同时，惯性定位在短期内提供了高精度定位，以克服UWB误差，因此融合超宽带和惯性定位系统以实现高精度和鲁棒性是可行的。基于此，本发明提出一种隧道施工人员定位方法及装置，不依赖于外部GNSS信号，就可以实现室内以及隧道动态变化的施工环境中的高精度定位。
实例性方法
如图1中所示，为本发明实施例提供的一种隧道施工人员定位方法的流程图，所述隧道施工人员定位方法可以应用于终端设备。本发明实施例中，结合图一对所述方法进行描述，所述方法包括如下步骤：
步骤S10、获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；
本发明中，通过超宽带传感器测量初始定位结果，获取对应的超宽带传感器测量得到的初始定位结果，并对所述初始定位结果进行线性化处理，将复杂施工环境中的非线性的电磁波信号转化为线性化的超宽带传感器定位结果。
其中，所述超宽带传感器(Ultra-Wide Band,UWB)采用超宽带技术，定位功能的实现可以分为“测距-定位”两个步骤，即从布置固定坐标位置的参考节点(通常称为基站、地标等)中获得节点坐标位置，然后通过定位技术计算待测节点坐标；其中测距可以采用双边双向测距法、RSSI测距定位算法等多种算法，从而测量出基站和待测节点的距离；其中在本发明中所述定位方法采用三边测量法，如图3所示，利用已知点的位置可以有效估计出待测目标节点的坐标。本发明中三边测量法首先基于ToA对三个基站的位置进行距离测量，然后以基站与标签的测量距离为半径，已知位置的基站坐标为圆心，做出三个圆周，三个圆周线上的交点或者重叠部分即为待测标签的所有可能位置；其中TOA是通过定位标签发出的信号到达基站的传播时间，来计算出定位标签和基站之间的距离，通过空间交会实现定位；在所述三边测定法中，传统的基于线性最小二乘的三角剖分方法计算三组测距信息，无需任何冗余信息，而在复杂的室内环境中，信号通常通过多径反射到达接收器，因此，采用多基站测量和非线性几何方法来解决定位稳定性和精度问题，其中计算过程中的数据冗余可以确保至少有三个可靠的测量值参与坐标计算，非线性方程减弱了信号反射的影响。
进一步的，所述获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果，具体包括：
采用扩展卡尔曼滤波器对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果。
在得到UWB测得的初始定位结果后，由于复杂施工环境中的电磁波信号一般是非线性的，因此采用扩展卡尔曼滤波器(ExtendedKalmanFilter、EKF算法)通过在非线性系统的参考点处对其进行泰勒级数展开，可以将非线性系统线性化，然后进行卡尔曼滤波。
更进一步的，扩展卡尔曼滤波通过以下公式来表示系统模型与测量和状态之间的关系如公式(1)、(2)所示：
xk+1＝f(xk-1，uk，ωk) (1)
zk＝h(Xk，vk) (2)
其中，f是控制矩阵，h是测量矩阵，x是估计状态值，z是测量值，u是控制参数，ω是噪声，k是时刻；通过公式(1)求解k+1时刻的估计状态值，而根据公式(2)来根据(1)在上一次求出的估计状态值求得更新的测量值，更新后的测量值zk相当于下一次的xk，用于公式(1)进行状态值估计求解；基于上述迭代求解的估计状态值从而最终实现传感器定位数据的最佳估计。
步骤S20、获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；
本发明，通过惯性传感器来获取惯性定位数据，通过获取到的惯性定位数据来获取对应的惯性传感器定位结果。其中，基于惯性传感器的定位采用捷联式惯导定位(Strap-down Inertial Navigation System，SINS)方法实现惯性导航系统，机械编排方程是实现惯性导航算法的关键，惯性导航算法是通过推导牛顿力学定律建立的。通过分析IMU数据与导航系统中载体的加速度、角速度和速度的关系，可以得到捷联惯导系统在全球坐标系下的机械编排方程；其中所述惯性传感器中加速度计与陀螺仪可以采集到不同方向的加速度与角速度，两者的传感器数据具有互补特性，将两者进行融合，获得准确的传感器姿态数据。
进一步的，所述获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果，具体包括：
所述获取惯性传感器采集到的惯性定位数据包括获取惯性传感器中加速度计和陀螺仪采集到的数据；
所述机械编排算法对所述加速度计和陀螺仪采集到的数据进行校正，并求解姿态矩阵，输出惯性传感器定位结果。
本发明中，基于惯性传感器的定位采用捷联式惯导定位(Strap-down In ertialNavigation System，SINS)方法实现惯性导航系统，机械编排方程是实现惯性导航算法的关键，惯性导航算法是通过推导牛顿力学定律建立的；通过分析IMU(InertialMeasurement Unit，惯性传感器)数据与导航系统中载体的加速度、角速度和速度的关系，可以得到捷联惯导系统在全球坐标系下的机械编排方程。
所述机械编排算法对所述加速度计和陀螺仪采集到的数据进行校正，并求解姿态矩阵，输出惯性传感器定位结果，具体包括：
加速度计和陀螺仪收集数据并执行误差补偿，对陀螺仪数据进行地球自转率修正，然后求解姿态矩阵，对加速度数据通过姿态矩阵消除有害积分。
如图4所示，是本发明实施例提供的捷联导航原理的框图，其中加速度计和陀螺仪部件收集数据并执行误差补偿；加速度数据通过姿态矩阵消除有害积分。对陀螺仪数据进行地球自转率等修正，然后求解姿态矩阵。利用捷联惯性定位方法得到位置、速度和姿态矩阵。
惯性导航机械化方程如下：
行人相对于世界坐标系的位置、速度和加速度分别为其中wb表示所述参数是表示行人相对于世界坐标系的参数；相对于惯性系的加速度为αib，其中ib表示参数是行人相对于惯性坐标系的参数，i表示惯性坐标系，b表示载体即IMU坐标系，也是行人所佩戴的IMU对应的坐标系，w表示世界坐标系，/>的关系如公式(3)、(4)所示：
其中t表示时间，公式(3)、(4)的积分下限w表示速度；其中αib和rwb的关系为如公式(5)所示，其中：
其中αib和具有以下转换关系：
其中，积分下限w表示速度，而ω表示速率，iw表示是世界坐标系相对于惯性坐标系参数表示；
由上式(6)可得公式(7)：
加速度计直接测量的数据称为比力(f)，f的物理含义是载体相对于惯性系统的加速度与重力加速度之间的差值：
f＝αib-G (8)
式中，G是载体相对于惯性系位于物体表面某一点处的加速度，包括地球重力和旋转的影响。
G＝g+ωiw×(ωiw×rwb)(9)
将方程(7)和(9)代入方程(8)，得到的比力方程为公式(10)所示：
本发明采用四元数微分方程进行姿态更新，设为表示世界坐标系对载体坐标系姿态的单位四元数，/>表示更新后的单位四位数，则对应的姿态四元数微分方程为：
其中Ω*为姿态矩阵，表示载体坐标系相对于世界坐标系的速率在载体坐标系下的投影，
其中，
是由陀螺仪直接测量的，表示载体坐标系相对于惯性坐标系的速率在载体坐标系下的投影，/>表示世界坐标系相对于惯性坐标系的速率在世界坐标系下的投影，/>表示载体坐标系相对于世界坐标系的速率在载体坐标系下的投影，/>表示世界坐标系到载体坐标系的方向余弦矩阵，/>表示载体坐标系到世界坐标系的方向余弦矩阵，和/>分别表示载体坐标系相对于世界坐标系的速率在载体坐标系下的投影在x、y、z轴上的分量。
步骤S30、基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；
在得到惯性传感器数据后，惯性传感器获取的数据由于随机噪声、交叉耦合误差和比例因子会导致存在误差，因此本发明中借助超宽带传感器定位结果采用杠杆臂算法修正惯性传感器定位结果，同时当超宽带传感器定位结果存在数据包丢失情况时，也可以采用线性化检测对所述惯性传感器定位结果进行修正。
进一步的，所述采用线性化检测和杠杆臂算法修正惯性传感器定位结果，具体包括：
采用线性化检测判断所述超宽带传感器定位结果中定位信息是否缺失；
当判断所述超宽带传感器定位结果中定位信息未缺失，基于所述超宽带传感器定位结果，采用杠杆臂算法修正惯性传感器定位结果；
当判断所述超宽带传感器定位结果中定位信息缺失，则对陀螺仪数据积分处理，通过积分处理结果约束惯性传感器定位结果误差。
具体的，在本发明中，无线信号可能会受到各种条件的影响，例如金属材料，这可能会导致超宽带传感器信号中的数据包丢失，因此在借助超宽带传感器进行信号修正时，对应的会先通过线性化检测判断超宽带传感器信号在设定的固定阈值间隔内定位信息缺失或定位信息数量不满足要求，则判定判断所述超宽带传感器定位结果中定位信息缺失，即UWB信号不足以实现对惯性传感器定位结果的修正，此时则对应的则对陀螺仪数据积分处理，通过积分处理结果约束惯性传感器定位结果误差；而当判断所述超宽带传感器定位结果中定位信息未缺失，基于所述超宽带传感器定位结果，采用杠杆臂算法修正惯性传感器定位结果。其中，固定阈值间隔为用户根据具体环境设定，定位信息缺失通过预设的阈值来判断，和定位信息数量也通过预设的阈值进行判断；所述对应的则对陀螺仪数据积分处理，通过积分处理结果约束惯性传感器定位结果误差，具体包括，将连续几秒钟的陀螺仪方向的变化进行积分，如果角度变化小于10°,它将被视为直线，以约束惯性导航系统的累积误差，其中所述连线几秒为用户提前设定的时间间隔。
更进一步的，所述杠杆臂算法，具体包括：
基于所述超宽带传感器定位结果和惯性传感器定位结果，构建位置测量模型，通过所述位置测量模型得到误差反馈；
将所述误差反馈输入到误差校正卡尔曼滤波器中，从而对惯性传感器定位结果进行校正。
具体的，机械编排算法无法防止惯性传感器在定位过程中在短时间内积累大量误差，无论该设备是安装在脚上、腰上还是头上，因此，需要大量相对准确的测量来确保惯性导航系统定位的可靠性；而行人行为语义(如转弯检测)和运动模型(如行人航位推算方法)可以为机械编排INS提供修正帮助。如图5所示，为本发明实施例提供的步态周期中头部、脚跟和腰部之间的相对姿势关系示意图，每个人在行走时不断重复四种步态，行人行走重复这四种动作(图5中(e)和(a)是重复动作)；在图5中，很难准确确定其中(a)、(c)和(d)中对应动作的头部装置和脚部之间的精确姿势关系，然而，如果发生动作(b)，则可以准确检测动作，并可以准确确定头部和脚的相对姿势；如图5所示，动作(b)是运动过程中出现的瞬时站立姿势，这种姿势在人群中很常见，也就是说，此时，头部和脚跟(即杠杆臂)之间的相对姿势在每个步态周期中都非常一致。因此可以采用头盔上的UWB信号得到的超宽带传感器定位结果来修正脚后跟惯性传感器的累积误差，给出的位置测量模型如下公式(12)所示：
其中，和/>分别表示UWB和脚后跟的位置，/>是状态转移矩阵，εheel表示超宽带定位的位置误差，L是相对姿态，即超宽带和脚后跟惯性导航设备的杠杆臂传递，通过上述公式修正惯性传感器定位结果。
杠杆臂模型的建立可以实时准确地将UWB定位坐标转换为脚坐标，再通过误差反馈将其应用到误差校正卡尔曼滤波器中，对后跟MEMS惯性导航的位置、速度和姿态进行校正。
此外，如图6所示，为本发明实施例提供的隧道施工人员传感器定位装置安装示意图，其中所述UWB安装在安全帽上，即相应的设置在用户头顶，而所述惯性传感器可以设置在用户腰上或者是脚上，其中一种较佳的实施例中，将惯性传感器设置在用户脚上实现杠杆臂算法。
步骤S40、将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
在得到对应的超宽带传感器定位结果和修正惯性传感器定位结果后，可以对两种定位结果进行融合，并对融合后的结构相应的进行优化从而得到最终可以输出的准确的优化后的最终定位结果。
进一步的，所述将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出，具体包括：
采用滤波器将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果；
通过RTS平滑方法对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
具体的，在得到所述超宽带传感器定位结果和所述修正惯性传感器定位后采用滤波器进行融合，其中所述滤波器融合可以使用原始卡尔曼滤波器、无迹卡尔曼滤波器、自适应卡尔曼滤波器等方法进行滤波融合；而对融合后的融合定位结果采用RTS平滑方法进行优化，该算法从初始时间0到时间T完成T次正向递归，然后从时间T开始向后重复T次，完成整个RTS平滑过程；从本质上讲，RTS平滑过程是一个卡尔曼滤波过程，即卡尔曼平滑算法；其中，前向递归和后向递归的输入和输出参数相反；通过RTS平滑方法显著提高卡尔曼滤波器的估计精度和稳定性，使输出的数据更加准确。
进一步的，本发明通过图2进一步对所述视觉与激光传感器高精度外参校正方法进行描述，具体的在UWB测量过程中，对得到的初始定位结果进行线性化之前可以采用测距误差补偿模型来补偿超宽带测距误差，以尽可能确保室内环境中的测距精度；即该方案整体可以描述为获取超宽带传感器测量到的初始定位结果，采用测距误差补偿模型来补偿超宽带测距误差，再对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。本发明中进一步的对所述传感器安装位置可以进一步的调整，将所述UWB或IMU安装到腰部等；而在传感器定位中，也可以使用不同的传感器定位数据组合，比如加入相机的图像信息，声音信息，其他射频数据等。
如图7所示，本发明实施例第二方面提供了一种隧道施工人员定位装置，所述隧道施工人员定位装置包括：
超宽带传感器定位获取模块71，获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；
惯性传感器定位获取模块72，获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；
修正模块73，基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；
融合优化模块74，将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
本申请实施例第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上任意所述的隧道施工人员定位方法中的步骤。
本申请实施例第四方面提供了一种终端设备，包括：处理器、存储器及通信总线；所述存储器上存储有可被所述处理器执行的计算机可读程序；
所述通信总线实现处理器和存储器之间的连接通信；
所述处理器执行所述计算机可读程序时实现如上任意所述的隧道施工人员定位方法中的步骤。
综上所述，本发明提供了一种隧道施工人员定位方法及装置，所述方法包括获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。通过上述方法，本发明通过获取对应的超宽带传感器和惯性传感器的定位数据，对应的采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果以及对应融合优化后，实现融合超宽带传感器和惯性定位传感器以实现高精度和鲁棒性的定位；且在该过程中使用杠杆臂算法利用超宽带传感器信息修正惯性传感器的累积误差，从而使得惯性传感器得到的数据更加准确；在所述方法中在超宽带传感器数据出现异常时，采用线性检测算法对应校正惯性传感器的定位结果；并且本发明在最终实现对超宽带传感器和惯性传感器的数据融合，并采用RTS平滑方法(Rauch-tung-sriebel、RTS)对定位结果进行优化，最终实现室内以及隧道动态变化的施工环境中的高精度定位。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本发明所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPRO M)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRA M)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRS DRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRA M)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
应当理解的是，本发明的应用不限于上述的举例，对本领域的技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变化都属于本发明所附权利要求的保护范围。
Claims (10)
1.一种隧道施工人员定位方法，其特征在于，所述方法包括：
获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；
获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；
基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；
将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
2.根据权利要求1所述的一种隧道施工人员定位方法，其特征在于，所述获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果，具体包括：
采用扩展卡尔曼滤波器对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果。
3.根据权利要求1所述的一种隧道施工人员定位方法，其特征在于，所述获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果，具体包括：
所述获取惯性传感器采集到的惯性定位数据包括获取惯性传感器中加速度计和陀螺仪采集到的数据；
所述机械编排算法对所述加速度计和陀螺仪采集到的数据进行校正，并求解姿态矩阵，输出惯性传感器定位结果。
4.根据权利要求3所述的一种隧道施工人员定位方法，其特征在于，所述机械编排算法对所述加速度计和陀螺仪采集到的数据进行校正，并求解姿态矩阵，输出惯性传感器定位结果，具体包括：
加速度计和陀螺仪收集数据并执行误差补偿，对陀螺仪数据进行地球自转率修正，然后求解姿态矩阵，对加速度数据通过姿态矩阵消除有害积分。
5.根据权利要求1所述的一种隧道施工人员定位方法，其特征在于，所述采用线性化检测和杠杆臂算法修正惯性传感器定位结果，具体包括：
采用线性化检测判断所述超宽带传感器定位结果中定位信息是否缺失；
当判断所述超宽带传感器定位结果中定位信息未缺失，基于所述超宽带传感器定位结果，采用杠杆臂算法修正惯性传感器定位结果；
当判断所述超宽带传感器定位结果中定位信息缺失，则对陀螺仪数据积分处理，通过积分处理结果约束惯性传感器定位结果误差。
6.根据权利要求1所述的一种隧道施工人员定位方法，其特征在于，所述杠杆臂算法，具体包括：
基于所述超宽带传感器定位结果和惯性传感器定位结果，构建位置测量模型，通过所述位置测量模型得到误差反馈；
将所述误差反馈输入到误差校正卡尔曼滤波器中，从而对惯性传感器定位结果进行校正。
7.根据权利要求1所述的一种隧道施工人员定位方法，其特征在于，所述将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出，具体包括：
采用滤波器将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果；
通过RTS平滑方法对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
8.一种隧道施工人员定位装置，其特征在于，所述装置包括：
超宽带传感器定位获取模块，获取超宽带传感器测量到的初始定位结果，对所述超宽带传感器测量到的初始定位结果进行线性化，得到超宽带传感器定位结果；
惯性传感器定位获取模块，获取惯性传感器采集到的惯性定位数据，通过机械编排算法得到惯性传感器定位结果；
修正模块，基于超宽带传感器定位结果，采用线性化检测和杠杆臂算法修正惯性传感器定位结果，得到修正惯性传感器定位结果；
融合优化模块，将所述超宽带传感器定位结果和所述修正惯性传感器定位结果进行融合，得到融合定位结果，并对所述融合定位结果进行优化，得到优化后的最终定位结果并输出。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如权利要求1-7任意一项所述的隧道施工人员定位方法中的步骤。
10.一种终端设备，其特征在于，包括：处理器、存储器及通信总线；所述存储器上存储有可被所述处理器执行的计算机可读程序；
所述通信总线实现处理器和存储器之间的连接通信；
所述处理器执行所述计算机可读程序时实现如权利要求1-7任意一项所述的隧道施工人员定位方法中的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311097272.3A CN117213476A (zh) | 2023-08-28 | 2023-08-28 | 一种隧道施工人员定位方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311097272.3A CN117213476A (zh) | 2023-08-28 | 2023-08-28 | 一种隧道施工人员定位方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117213476A true CN117213476A (zh) | 2023-12-12 |
Family
ID=89034455
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311097272.3A Pending CN117213476A (zh) | 2023-08-28 | 2023-08-28 | 一种隧道施工人员定位方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117213476A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118067148A (zh) * | 2024-04-17 | 2024-05-24 | 深圳市科乐达电子科技有限公司 | 一种基于大数据分析的车载导航系统 |
-
2023
- 2023-08-28 CN CN202311097272.3A patent/CN117213476A/zh active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118067148A (zh) * | 2024-04-17 | 2024-05-24 | 深圳市科乐达电子科技有限公司 | 一种基于大数据分析的车载导航系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112097768B (zh) | 机器人位姿的确定方法、装置、机器人及存储介质 | |
| CN111207774B (zh) | 一种用于激光-imu外参标定的方法及系统 | |
| US10352959B2 (en) | Method and system for estimating a path of a mobile element or body | |
| CN108519615A (zh) | 基于组合导航和特征点匹配的移动机器人自主导航方法 | |
| CN113108791B (zh) | 一种导航定位方法及导航定位设备 | |
| KR101308555B1 (ko) | 실내보행 위치산출방법 | |
| CN107490378B (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
| CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
| Ibrahim et al. | Inertial measurement unit based indoor localization for construction applications | |
| CN114166221B (zh) | 动态复杂矿井环境中辅助运输机器人定位方法及系统 | |
| CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
| CN117213476A (zh) | 一种隧道施工人员定位方法及装置 | |
| KR101642286B1 (ko) | 보행특성을 이용한 실내에서의 보행자 이동방향 추정 방법 | |
| KR101576424B1 (ko) | 실내 측위를 위한 지자기 센서 자동 보정 방법 | |
| CN111683337A (zh) | 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质 | |
| CN112362044A (zh) | 室内定位方法、装置、设备和系统 | |
| CN115950418A (zh) | 多传感器融合定位方法 | |
| WO2022160811A1 (zh) | 足式机器人轨迹跟踪方法、设备及可读存储介质 | |
| KR101301462B1 (ko) | 저가형 관성 센서를 이용한 보행자 관성 항법 장치 및 그 항법 | |
| US11992961B2 (en) | Pose determination method, robot using the same, and computer readable storage medium | |
| CN115451946A (zh) | 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法 | |
| CN113483762A (zh) | 一种位姿优化方法及设备 | |
| CN112414407A (zh) | 定位方法、装置、计算机设备和存储介质 | |
| CN105091881A (zh) | 一种带有静止状态检测的无线传感网室内定位方法 | |
| CN110579212B (zh) | 室内定位方法及装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication |