CN115711619A - 一种双基站uwb/ins组合的密林环境人员定位方法 - Google Patents

一种双基站uwb/ins组合的密林环境人员定位方法 Download PDF

Info

Publication number
CN115711619A
CN115711619A CN202211449905.8A CN202211449905A CN115711619A CN 115711619 A CN115711619 A CN 115711619A CN 202211449905 A CN202211449905 A CN 202211449905A CN 115711619 A CN115711619 A CN 115711619A
Authority
CN
China
Prior art keywords
forest
uwb
personnel
vehicle platform
base station
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
Application number
CN202211449905.8A
Other languages
English (en)
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.)
CETC 54 Research Institute
Original Assignee
CETC 54 Research Institute
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 CETC 54 Research Institute filed Critical CETC 54 Research Institute
Priority to CN202211449905.8A priority Critical patent/CN115711619A/zh
Publication of CN115711619A publication Critical patent/CN115711619A/zh
Pending legal-status Critical Current

Links

Images

Abstract

本发明公开了一种双基站UWB/INS组合的密林环境人员定位方法，属于导航定位及信息融合应用技术领域。本发明利用无人机、车辆和进入密林人员组成的空天地协同定位系统，无人机和车辆通过GNSS接收机获取卫星观测量，取得绝对定位结果，通过UWB基站和进入密林的人员UWB标签实现测距，密林人员同时配备惯性导航IMU实现人员的航迹推算，实现密林人员定位解析，并对IMU/UWB状态更新模型进行全局优化。本发明方法简单易行，能够在山地密林环境中为人员提供位置信息保障。

Description

一种双基站UWB/INS组合的密林环境人员定位方法
技术领域
本发明涉及一种双基站UWB/INS组合的密林环境人员定位方法，属于导航定位及信息融合应用技术领域。
背景技术
随着北斗导航系统的建立，高精度定位服务逐步走向更加泛在的环境和场景，然而，受到树林遮蔽对卫星导航信号的影响，传统卫星导航设备难以取得有效的定位能力，实时连续、精准无缝的高精度导航定位是保障森林消防、森林公安、护林员等安全行进、作业的重要基础，因此，密林环境下的高精度定位是亟待解决的重大难题。鉴于卫星导航信号干扰，性能脆弱不足，视觉特征跟踪受限于大尺度、光照等林区环境，激光雷达场景辨识能力差，难以闭环全局修正，低成本惯性导航累积误差时间漂移等，现阶段，具备强穿透性的UWB定位方法是解决密林环境下高精度导航定位难题的有效手段。
当前，已知的UWB定位技术尽管已取得了一定的效果，但实际技术应用仅限于室内，工业园区，地下走廊等结构化环境，定位结果的优化依赖UWB基站的不设密度和标定，技术难以在密林环境进行应用推广。亟需发展面向密林环境的UWB有效定位技术研究，但是，现有技术中尚没有相关应用。
发明内容
有鉴于此，本发明提出了一种双基站UWB/INS组合的密林环境人员定位方法，该方法突破了传统卫星导航遮蔽环境限制，实现了密林环境下绝对定位。
为了实现上述目的，本发明采用的技术方案为：
一种双基站UWB/INS组合的密林环境人员定位方法，包括如下步骤：
(1)建立密林环境空天地协同定位基准与系统，包括无人机平台、车辆平台和进入密林人员，其中，无人机平台和车辆平台均搭建卫星导航和UWB基站，无人机平台飞行在密林上空，车辆平台在林区边缘，进入密林人员搭载惯性导航IMU和UWB标签；进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立解析方程，得到不同时刻密林人员位置；
(2)进入密林人员通过惯性导航IMU进行航迹推算，获取密林人员在不同时刻的位置变化；
(3)将通过TWR双向测距得到的位置变化作为观测值，将通过惯性导航IMU进行航迹推算得到的位置变化作为状态值，建立误差方程，将观测值为边，状态值为点，利用图优化进行全局约束，获得密林人员准确位置。
进一步的，步骤(1)中进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立解析方程，具体过程如下：
车辆平台通过卫星导航获取当前绝对位置L0(b0,l0,h0)，并定义为空天地协同定位系统基准局部坐标原点L0(0,0,0)；无人机平台通过卫星导航获取当前绝对位置L1(b1,l1,h1)，并计算无人机在空天地协同定位系统基准局部坐标L1(x1,y1,z1)：
x1＝(b1-b0)λb
y1＝(l1-l0)λl
z1＝(h1-h0)
式中，λb，λl分别表示大地坐标系纬度和经度测量值转换为空间直角坐标系系数；
进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立如下解析方程：
Figure BDA0003951231600000021
Figure BDA0003951231600000022
Figure BDA0003951231600000023
Figure BDA0003951231600000024
式中，W0(x0,y0,z0)为t0时刻密林人员位置，
Figure BDA0003951231600000025
为t0时刻密林人员与车辆平台UWB基站的测距距离，
Figure BDA0003951231600000026
为t0时刻密林人员与无人机平台UWB基站测距距离；W1(x1,y1,z1)为t1时刻密林人员位置，
Figure BDA0003951231600000027
为t1时刻密林人员与车辆平台UWB基站的测距距离，
Figure BDA0003951231600000028
为t1时刻密林人员与无人机平台UWB基站测距距离。
进一步的，步骤(2)具体过程如下：
密林人员通过惯性导航IMU进行航迹推算，估计运动状态速度vt+Δt、位置rt+Δt以及旋转qt+Δt；
Figure BDA0003951231600000031
Figure BDA0003951231600000032
Figure BDA0003951231600000033
式中，at,ωt分别为t时刻的IMU原始加速度和角速度量测；
Figure BDA0003951231600000034
分别为加速度量测偏差和随机白噪声，
Figure BDA0003951231600000035
分别为角速度量测偏差和随机白噪声；g为世界坐标系W的重力加速度，
Figure BDA0003951231600000036
为惯性本体坐标系到世界坐标系的旋转；
通过航迹推算结果获取密林人员在t0时刻到t1时刻的位置变化为Δr(Δx,Δy,Δz)。
本发明与现有技术相比所取得的有益效果为：
1、本发明利用无人机、车辆和进入密林人员组成的空天地协同定位系统，借助UWB强穿透性实现密林环境下人员绝对位置的传递，并在密林人员IMU/UWB融合状态更新模型的基础上，进行全局优化。
2、本发明可实现密林环境下可靠性定位。
3、本发明方法简单易行，能够在山地密林等遮蔽环境中为工作人员提供定位技术保障。
附图说明
图1本发明实施例方法的原理示意图。
具体实施方式
为了更好的说明本发明的目的和优点，下面对本发明的技术方案作进一步说明。
一种双基站UWB/INS组合的密林环境人员定位方法，该方法所需的设备包括无人机、车辆、GNSS接收机、惯性导航IMU、远距离UWB。如图1所示，无人机和车辆通过GNSS接收机获取卫星观测量，取得绝对定位结果，通过UWB基站和进入密林的人员UWB标签实现测距，密林人员同时配备惯性导航IMU实现人员的航迹推算，实现密林人员定位解析。
该方法具体包括如下步骤：
(1)建立密林环境空天地双基站UWB/INS组合协同定位基准与系统；具体过程如下：
建立密林环境空天地协同定位基准与系统，定义无人机平台U，定义车辆平台V，定义进入密林人员为W，其中，无人机搭建卫星导航和UWB基站，飞行在密林上空，车辆平台搭载卫星导航和UWB基站，在林区边缘，进入密林人员搭载惯性导航IMU和UWB标签；
车辆平台通过卫星导航获取当前绝对位置L0(b0,l0,h0)，并定义为空天地协同定位系统基准局部坐标原点L0(0,0,0)；无人机平台通过卫星导航获取当前绝对位置L1(b1,l1,h1)，定义无人机在空天地协同定位系统基准局部坐标L1(x1,y1,z1)，其中：
x1＝(b1-b0)λb
y1＝(l1-l0)λl
z1＝(h1-h0)
式中，λb，λl分别表示大地坐标系纬度和经度测量值转换为空间直角坐标系系数；
进入密林人员通过UWB标签与无人机平台UWB基站和车辆平台UWB基站进行TWR(TwoWayRanging)双向测距，假定t0时刻密林人员位置为W0(x0,y0,z0)，与车辆上UWB基站测距距离为
Figure BDA0003951231600000041
与无人机UWB基站测距距离为
Figure BDA0003951231600000042
可建立如下解析方程：
Figure BDA0003951231600000043
Figure BDA0003951231600000044
同理，假设t1时刻密林人员位置为W1(x1,y1,z1)，与车辆上UWB基站测距距离为
Figure BDA0003951231600000045
与无人机UWB基站测距距离为
Figure BDA0003951231600000046
建立解析方程：
Figure BDA0003951231600000047
Figure BDA0003951231600000048
得到不同时刻密林人员位置。
(2)进入密林人员通过惯性导航IMU进行航迹推算，获取密林人员在不同时刻的位置变化；具体实现方式为：
密林人员通过惯性导航IMU进行航迹推算，运动状态速度vt+Δt、位置rt+Δt以及旋转qt+Δt的估计模型为：
Figure BDA0003951231600000049
Figure BDA00039512316000000410
Figure BDA00039512316000000411
式中，at,ωt分别为t时刻的IMU原始加速度和角速度量测；
Figure BDA00039512316000000412
分别为加速度量测偏差和随机白噪声，
Figure BDA00039512316000000413
分别为角速度量测偏差和随机白噪声；g为世界坐标系W的重力加速度，
Figure BDA00039512316000000414
为惯性本体坐标系到世界坐标系的旋转；
通过上述航迹推算结果获取密林人员在t0时刻到t1时刻的位置变化为Δr(Δx,Δy,Δz)。
(3)将通过TWR双向测距得到的位置变化作为观测值，将通过惯性导航IMU进行航迹推算得到的位置变化作为状态值，建立误差方程，将观测值为边，状态值为点，利用图优化进行全局约束，获得密林人员准确位置。
在理想情况下，应当满足密林人员在t0时刻到t1时刻，UWB位置变化和IMU位置变化结果一致，定义系统测量误差为δ(δx,δy,δz)，UWB位置变化作为观测值，IMU位置变化作为状态值，观测方程：Z＝h(Δr)，误差方程：δ＝Z-h(Δr)，即：
Figure BDA0003951231600000051
密林人员连续定位观测方程满足：Z＝h(Δr1,Δr2,Δr3,…)，定义观测值为边，状态值为点，利用图优化进行全局约束。

Claims (3)

1.一种双基站UWB/INS组合的密林环境人员定位方法，其特征在于，包括如下步骤：
(1)建立密林环境空天地协同定位基准与系统，包括无人机平台、车辆平台和进入密林人员，其中，无人机平台和车辆平台均搭建卫星导航和UWB基站，无人机平台飞行在密林上空，车辆平台在林区边缘，进入密林人员搭载惯性导航IMU和UWB标签；进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立解析方程，得到不同时刻密林人员位置；
(2)进入密林人员通过惯性导航IMU进行航迹推算，获取密林人员在不同时刻的位置变化；
(3)将通过TWR双向测距得到的位置变化作为观测值，将通过惯性导航IMU进行航迹推算得到的位置变化作为状态值，建立误差方程，将观测值为边，状态值为点，利用图优化进行全局约束，获得密林人员准确位置。
2.根据权利要求1所述的一种双基站UWB/INS组合的密林环境人员定位方法，其特征在于，步骤(1)中进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立解析方程，具体过程如下：
车辆平台通过卫星导航获取当前绝对位置L0(b0,l0,h0)，并定义为空天地协同定位系统基准局部坐标原点L0(0,0,0)；无人机平台通过卫星导航获取当前绝对位置L1(b1,l1,h1)，并计算无人机在空天地协同定位系统基准局部坐标L1(x1,y1,z1)：
x1＝(b1-b0)λb
y1＝(l1-l0)λl
z1＝(h1-h0)
式中，λb，λl分别表示大地坐标系纬度和经度测量值转换为空间直角坐标系系数；
进入密林人员通过UWB标签分别与无人机平台UWB基站和车辆平台UWB基站进行TWR双向测距，建立如下解析方程：
Figure FDA0003951231590000011
Figure FDA0003951231590000012
Figure FDA0003951231590000013
Figure FDA0003951231590000014
式中，W0(x0,y0,z0)为t0时刻密林人员位置，
Figure FDA0003951231590000015
为t0时刻密林人员与车辆平台UWB基站的测距距离，
Figure FDA0003951231590000021
为t0时刻密林人员与无人机平台UWB基站测距距离；W1(x1,y1,z1)为t1时刻密林人员位置，
Figure FDA0003951231590000022
为t1时刻密林人员与车辆平台UWB基站的测距距离，
Figure FDA0003951231590000023
为t1时刻密林人员与无人机平台UWB基站测距距离。
3.根据权利要求1所述的一种双基站UWB/INS组合的密林环境人员定位方法，其特征在于，步骤(2)具体过程如下：
密林人员通过惯性导航IMU进行航迹推算，估计运动状态速度vt+Δt、位置rt+Δt以及旋转qt+Δt；
Figure FDA0003951231590000024
Figure FDA0003951231590000025
Figure FDA0003951231590000026
式中，at,ωt分别为t时刻的IMU原始加速度和角速度量测；
Figure FDA0003951231590000027
分别为加速度量测偏差和随机白噪声，
Figure FDA0003951231590000028
分别为角速度量测偏差和随机白噪声；g为世界坐标系W的重力加速度，
Figure FDA0003951231590000029
为惯性本体坐标系到世界坐标系的旋转；
通过航迹推算结果获取密林人员在t0时刻到t1时刻的位置变化为Δr(Δx,Δy,Δz)。
CN202211449905.8A 2022-11-19 2022-11-19 一种双基站uwb/ins组合的密林环境人员定位方法 Pending CN115711619A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211449905.8A CN115711619A (zh) 2022-11-19 2022-11-19 一种双基站uwb/ins组合的密林环境人员定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211449905.8A CN115711619A (zh) 2022-11-19 2022-11-19 一种双基站uwb/ins组合的密林环境人员定位方法

Publications (1)

Publication Number Publication Date
CN115711619A true CN115711619A (zh) 2023-02-24

Family

ID=85233809

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211449905.8A Pending CN115711619A (zh) 2022-11-19 2022-11-19 一种双基站uwb/ins组合的密林环境人员定位方法

Country Status (1)

Country Link
CN (1) CN115711619A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法

Similar Documents

Publication Publication Date Title
CN109946730B (zh) 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN108225302B (zh) 一种石化工厂巡检机器人定位系统和方法
Zhang et al. UWB/INS integrated pedestrian positioning for robust indoor environments
CN107289941B (zh) 一种基于惯导的室内定位方法与装置
Woo et al. Application of WiFi-based indoor positioning system for labor tracking at construction sites: A case study in Guangzhou MTR
Puente et al. Review of mobile mapping and surveying technologies
CN103197279B (zh) 一种移动目标协同定位系统的定位方法
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
Stromback et al. Foot-mounted inertial navigation and cooperative sensor fusion for indoor positioning
Chi et al. Automatic guidance of underground mining vehicles using laser sensors
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN109974694B (zh) 一种基于uwb/imu/气压计的室内行人3d定位方法
EP3617749B1 (en) Method and arrangement for sourcing of location information, generating and updating maps representing the location
JP6380936B2 (ja) 移動体及びシステム
CN107247275B (zh) 基于公交车的城市gnss脆弱性监测系统及其方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN111751852A (zh) 基于点云配准的无人车gnss定位可靠性评价方法
US11874366B2 (en) High-precision vehicle positioning method based on ultra-wideband in intelligent vehicle infrastructure cooperative systems
CN102998689B (zh) 基于虚拟传感器的区域判断方法
CN115711619A (zh) 一种双基站uwb/ins组合的密林环境人员定位方法
CN114111802B (zh) 一种行人航迹推算辅助uwb的定位方法
Patruno et al. An embedded vision system for real-time autonomous localization using laser profilometry
CN104251699A (zh) 一种室内空间定位设备及定位方法
Cheng et al. Autonomous dynamic docking of UAV based on UWB-vision in GPS-denied environment
Grejner-Brzezinska et al. From Mobile Mapping to Telegeoinformatics

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