CN114623823B - 一种融合里程计的uwb多模态定位系统、方法及装置 - Google Patents

一种融合里程计的uwb多模态定位系统、方法及装置 Download PDF

Info

Publication number
CN114623823B
CN114623823B CN202210525566.0A CN202210525566A CN114623823B CN 114623823 B CN114623823 B CN 114623823B CN 202210525566 A CN202210525566 A CN 202210525566A CN 114623823 B CN114623823 B CN 114623823B
Authority
CN
China
Prior art keywords
vehicle
uwb
base station
uwb base
attitude
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
CN202210525566.0A
Other languages
English (en)
Other versions
CN114623823A (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.)
Qingdao Vehicle Intelligence Pioneers Inc
Original Assignee
Qingdao Vehicle Intelligence Pioneers Inc
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 Qingdao Vehicle Intelligence Pioneers Inc filed Critical Qingdao Vehicle Intelligence Pioneers Inc
Priority to CN202210525566.0A priority Critical patent/CN114623823B/zh
Publication of CN114623823A publication Critical patent/CN114623823A/zh
Application granted granted Critical
Publication of CN114623823B publication Critical patent/CN114623823B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Navigation (AREA)

Abstract

本申请涉及一种定位技术，提供一种融合里程计的UWB多模态定位系统、方法及装置，包括：环境部署模块，用于根据待测量环境部署UWB基站，包括：确定UWB基站的数量和每个UWB基站的坐标；车辆姿态获取模块，用于获取车辆当前时刻、当前时刻的姿态信息，包括不限于里程计实时里程、车体纵向速度的车辆姿态信息；UWB标签，设置于车辆上，用以获取车辆周围UWB基站的数量与测距信息；位置判定模块，用于将预测位置和位置范围于观测距离或位置进行融合计算，进行车辆位姿估计，并输出车辆的最终位置和姿态。通过融合里程计和UWB技术，提高定位精度和速度，并通过可检测UWB基站确定定位模式，使得切换场景更为容易。

Description

一种融合里程计的UWB多模态定位系统、方法及装置
技术领域
本申请涉及一种定位技术，尤其涉及一种融合里程计的UWB多模态定位系统。本申请还提供一种融合里程计的UWB多模态定位方法及装置。
背景技术
随着自动驾驶技术的蓬勃发展，室内定位技术和基于位置的服务也逐渐变得日新月异。为了使得室内定位更加精准，人们不断地在自动驾驶车辆上加装各种各样的传感器，以应对不同的场景，但是仍然没有较大的定位性能提升。UWB（Ultra Wide Band，UWB）技术是一种基于纳秒至微妙级别的无线脉冲通信技术，时间分辨率超高，使得其可以获得高达厘米级别的测距精度，通过测距以及定位算法就能获得厘米级的定位精度，从而广泛地用于室内精准定位。但是，UWB传感器布设方案和UWB信号穿透性对定位精度的影响，就效果而言，车辆定位精度的性能并未大幅度提高。
在传统场景中，自动驾驶车辆一般具有里程传感器，但是里程计在长时间使用后存在累积误差，而且不平坦地面会使得里程计失效。而且，并不是所有的车辆都能直接提供实时里程值，有的只有车体纵向速度，或者后两个轮子的速度，或者四个轮子的速度。另外，在实际应用中，UWB信号受NLOS（Non Line of Sight， MLOS，非视距遮挡）影响，容易受到干扰，造成信号丢失或者畸变，使得UWB传感器的测距结果不稳定。那么，结合UWB的信息对定位估计值进行校准，就能很好地取长补短，提高定位精准度、稳定性和快速性。
综上所述，针对多种情形下的车辆里程计信息，以及UWB传感器信息，本发明提供了一个多种信息多模态处理，融合里程计和UWB传感器的融合定位方法。
当前，通过UWB技术是提高车辆定位精度，得到工程人员的重视，UWB技术是一种基于纳秒至微妙级别的无线脉冲通信技术，时间分辨率超高，使得其可以获得高达厘米级别的测距精度，基于一定的定位算法就能获得厘米级的定位精度，从而广泛地用于室内精准定位。
但是，UWB基站部署费用较高，在基站偏少只有一个或两个基站的情况下无法进行定位，或定位误差较大；而且单纯的UWB技术会受到NLSO影响，定位结果存在波动，定位刷新率低；并且在UWB技术的应用过程中，都是对特定场景进行对应部署的，模式单一，在多场景切换中具有缺陷。
发明内容
本申请提供一种融合里程计的UWB多模态定位系统，以解决以解定位精度低，速度慢以及多场景切换难的问题。同时，本申请还提供一种融合里程计的UWB多模态定位系统、定位方法及装置。
技术解决方案为：一种融合里程计的UWB多模态定位系统，主要应用于车辆定位，该车辆配置有：
车辆姿态获取模块 ，用于获取车辆当前时刻、当前时刻的姿态信息，包括不限于里程计实时里程、车体纵向速度的车辆姿态信息；优选的还包括车辆后两轮速度、车辆前轮速度和角度、车辆是否打滑等车辆姿态信息；优选的，所述车辆姿态获取模块还设有IMU模块和陀螺仪，测量车辆的实时线性加速度和角速度；
车辆运动预测模块，根据车辆运动模型，以及所述车辆姿态获取模块获取到的信息，结合上一时刻车辆的位置和姿态，计算当前时刻车辆的预测位置和可能的位置范围；
环境部署模块，用于根据待测量环境部署UWB基站，包括：确定UWB基站的数量、部署结构和每个UWB基站的物理坐标；根据实际业务需求，需要在周围环境当中部署或确定若干个UWB基站，并且标定好每一个基站的三维坐标
UWB标签，设置于车辆上，用以获取车辆周围UWB基站的数量与测距信息；
UWB计算模块，用于根据预设的合格距离阈值确定有效的UWB基站，根据有效的UWB基站及UWB基站坐标获取当前车辆到有效的UWB基站的观测距离或车辆观测位置；
位置判定模块，用于根据车辆运动预测模块得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块计算得出的车辆与UWB基站的观测距离或位置进行融合计算，进行车辆位姿估计，输出车辆的最终位和姿态。
优选的，车辆运动预测模块使用卡尔曼滤波器进行最优状态估计。
本发明还提供一种融合里程计的UWB多模态定位方法，包括：
S1、确定和配置车辆的运动模型；
S2、根据车辆运动模型，以及所述车辆姿态获取模块获取到的信息，结合上一时刻车辆的位置和姿态，计算当前时刻车辆的预测位置、姿态和可能的位置范围；
S3、利用设置在车辆上的UWB标签获取车辆可检测到的UWB基站，根据预设阈值筛选的有效的UWB基站，并通过UWB计算模块得出车辆与有效的UWB基站之间的距离；
S4、当仅存在一个有效的UWB基站时，将当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块得出的车辆与UWB基站的观测距离直接进行融合计算，进行车辆位姿估计；
当存在多个有效的UWB基站时，首先计算UWB基站所构成的平面的数量，当平面数量满足要求后，进行UWB定位计算并计算该定位计算结果的PDOP值，根据PDOP值判定该定位计算结果是否有效；
如果PDOP值小于设定阈值，则将S2步骤中得出的车辆的预测位置和可能的位置范围，与UWB计算模块计算得出的车辆的观测位置和姿态进行融合，判定车辆的位置和姿态；
如果UWB基站所构成的平面的数量不满足要求，或者PDOP值大于设定阈值，那么就继续将车辆的预测位置和位置范围，与车辆与该UWB基站的观测距离进行融合判定，进行车辆位姿估计，融合得出车辆的估计位置和姿态；
S5、对车辆位置和姿态进行更新，输出车辆的最终位置和姿态。输出车辆的最终位置包括输出车体计算中心位置、前轮中心位置、后轮中心位置等。
优选的，S1步骤中还包括标定UWB标签与车体计算中心的偏移。
优选的，所述的UWB标签为两个或多个。
优选的，S2步骤中使用卡尔曼滤波器进行状态估计。
优选的，S5步骤中还包括输出三维速度。
本发明还提供一种融合里程计的UWB多模态定位装置，包括：
车辆姿态获取模块 ，用于获取车辆的姿态信息，包括不限于里程计实时里程、车体纵向速度的车辆姿态信息；
车辆运动预测模块，根据车辆运动模型，以及所述车辆姿态获取模块获取到的信息，结合前一时刻车辆的位置和姿态，估计当前时刻车辆的预测位置和可能的位置范围；
UWB标签，设置于车辆上，用于获取有效的UWB基站，以及获取车辆到所述UWB基站的距离；
UWB计算模块，用于根据预设的阈值确定有效的UWB基站，根据有效的UWB基站及UWB基站坐标获取当前车辆到有效的UWB基站的观测距离或车辆观测位置；
位置判定模块，用于根据车辆运动预测模块得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块计算得出的车辆与UWB基站的观测距离或车辆观测位置进行融合计算，进行车辆位姿估计，输出车辆的最终位置和姿态。
本申请相对与现有技术具有以下优点：与现有的其他方法相比，本发明能够同时处理不同的里程计模型，尤其是针对在线情况下，UWB有效基站数量不同而采用不同的观测模型，避免了由于UWB基站太少而不参与计算的问题，提高了定位的稳定性，以及观测更新频率，使得能够处理更加复杂情况的传感器情况，从而实现稳定地，可靠地定位。通过融合里程计和UWB技术，提高定位精度和速度，并通过可检测UWB基站确定定位模式，使得切换场景更为容易。
附图说明
图1是本申请融合里程计的UWB多模态定位系统示意图。
图2是本申请融合里程计的UWB多模态定位方法工作流程示意图。
图3是本申请定位模式判定流程图。
图4是本申请三维定位输出示意图。
具体实施方式
在下面的描述中阐述了很多具体细节以便于充分理解本申请，但是本申请能够以很多不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本申请构思的情况下做类似推广，因此本申请不受下面公开的具体实施例的限制。
为了详细的说明本申请的技术方案，下面结合附图对本申请的构思方法进行描述。
请参照图1所示，车辆姿态获取模块，用于获取车辆当前时刻、当前时刻的姿态信息，包括不限于里程计实时里程、车体纵向速度的车辆姿态信息；优选的还包括车辆后两轮速度、车辆前轮速度和角度、车辆是否打滑等车辆姿态信息；优选的，所述车辆姿态获取模块还设有IMU模块，测量车辆的实时线性加速度和角速度；
车辆运动预测模块，根据车辆运动模型，以及所述车辆姿态获取模块获取到的信息，结合上一时刻车辆的位置和姿态，计算当前时刻车辆的预测位置和可能的位置范围；
环境部署模块，用于根据待测量环境部署UWB基站，包括：确定UWB基站的数量、部署结构和每个UWB基站的物理坐标；根据实际业务需求，需要在周围环境当中部署或确定若干个UWB基站，并且标定好每一个基站的三维坐标
UWB标签，设置于车辆上，数量为2个，用以获取车辆周围UWB基站的数量与测距信息；
UWB计算模块，用于根据预设的合格距离阈值确定有效的UWB基站，根据有效的UWB基站及UWB基站坐标获取当前车辆到有效的UWB基站的观测距离或车辆观测位置；
位置判定模块，用于根据车辆运动预测模块得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块计算得出的车辆与UWB基站的观测距离或观测位置进行融合计算，进行车辆位姿估计，输出车辆的最终位和姿态。
如图2、图3、图4所示，本申请还提供的一种融合里程计的UWB多模态定位方法，具体定位实现流程如下：
第一部分，准备工作。
安装部署所需传感器或者设备到自动驾驶车辆上时。假设定位算法的计算中心为车体计算中心，那么，需要标定UWB标签与车体计算中心的偏移，车辆姿态获取模块与车体计算中心的偏移，IMU与车体计算中心的偏移。并将这些偏移值写入定位系统的指定参数文件中。
根据车辆的情况，配置车辆的运动模型，配置相关参数。其中，车辆运动模型在使用之前是确定的，一辆车一种运动模型。
第二部分，系统运行。
车辆运动预测模块根据车辆运动模型以及车辆姿态获取模块获取到的当前时刻车辆姿态信息，调用相应的车辆运动模型进行计算，得到此时的预测位置、预测里程、预测速度。车辆运动预测模块使用卡尔曼滤波器或UKF非线性估计器，该模型对非线性系统有非常好的表现，适合处理多模态模型的情况。首先会根据前一时刻的运动状态进行下一时刻的位姿预测，如根据设定的车辆运动模型，利用IMU测量获得的车辆线性加速度和角速度，结合采样时间，对车辆此时的位置做出估计，并得到此时的姿态以及模型协方差。
针对相关参数，这里做统一说明：其中P表示车辆在三维空间的位置，V表示车辆在三维空间的速度，Q表示车辆在三维空间的姿态；小写表示数值，是标量；大写表示矩阵，是矢量；
Figure 606073DEST_PATH_IMAGE001
表示里程，t表示时刻，
Figure 104050DEST_PATH_IMAGE002
：表示t时刻下的状态值；
Figure 875697DEST_PATH_IMAGE003
：表示t时刻下的观测的测量值；
Figure 295177DEST_PATH_IMAGE004
：表示t时刻下的状态估计值（上标含义下同）；
Figure 443262DEST_PATH_IMAGE005
为t时刻下卡尔曼滤波增益系数。
首先检索车辆里程计观测，结合之前设定的车辆里程计种类，调用相应的计算模型，进行观测信息更新。如车体纵向速度模型模型下(输入值：前一时刻和当前时刻下的车速
Figure 795746DEST_PATH_IMAGE006
及车辆的加速度)。
根据前一时刻的车辆速度v和车辆加速度预测当前时刻的速度
Figure 576444DEST_PATH_IMAGE007
；
观测更新
Figure 483220DEST_PATH_IMAGE008
之后，检索UWB标签观测量，先筛选合格距离数据，剔除畸变测距值，剩下的就是有效测距信息。筛选方法是，计算测量估计值与实际测量值的插值，若大于阈值，则认为是测距畸变值，不参与观测更新：
Figure 169416DEST_PATH_IMAGE009
然后对剩下的观测量进行模式判定，确定使用最佳的观测模型。判定的方式主要是计算基站所构成的平面的数量，在当平面数量满足要求后，就可以进行定位计算，求得当前状态下UWB标签的二维坐标。然后，再计算该定位结果的PDOP值，判定该解算结果是否有效。那么，如果PDOP值小于某一个阈值，说明该解算结果是有效的，则可以使用二维定位模型；若不是，那么就继续使用测量模型。
对两种测量模型，这里做一个解释：
Figure 969882DEST_PATH_IMAGE010
Figure 817752DEST_PATH_IMAGE011
Figure 8562DEST_PATH_IMAGE012
对前后两个标签进行二维定位计算，则能够求得yaw，
Figure 436132DEST_PATH_IMAGE013
Figure 28788DEST_PATH_IMAGE014
Figure 313139DEST_PATH_IMAGE015
（2）使用距离测量模型
Figure 53562DEST_PATH_IMAGE016
Figure 81560DEST_PATH_IMAGE017
Figure 466405DEST_PATH_IMAGE018
最后，结合所得的观测信息，进行一次或者多次的观测更新，然后进行k时刻的位姿更新，得到最终的载体位姿。
实施例2：在某一个模拟隧道场景中，部署了一维UWB基站，并且对每个基站坐标进行精准标定。
然后，使用四轮后驱实验车，在该车体上安装了双标签UWB、IMU传感器，提供了底盘纵向速度接口。那么，基于UKF估计器，一般的计算流程为：
Figure 921657DEST_PATH_IMAGE019
Figure 87060DEST_PATH_IMAGE020
Figure 981066DEST_PATH_IMAGE021
Figure 17155DEST_PATH_IMAGE022
Figure 908888DEST_PATH_IMAGE023
Figure 499269DEST_PATH_IMAGE024
Figure 603492DEST_PATH_IMAGE025
Figure 25246DEST_PATH_IMAGE026
Figure 651661DEST_PATH_IMAGE027
Figure 260497DEST_PATH_IMAGE028
其中，使用IMU运动模型，两个相邻时刻t-1 到t 的位姿是用第t-1时刻的测量值来计算，那么预测模型为：
Figure 699569DEST_PATH_IMAGE029
Figure 913512DEST_PATH_IMAGE030
Figure 147048DEST_PATH_IMAGE031
Figure 243180DEST_PATH_IMAGE032
然后，接收到车体纵向速度，使用车体纵向速度模型模型：
Figure 17101DEST_PATH_IMAGE033
带入真实测量值，进行一次sigma点更新。
实施例3，在隧道口场景中，接收到UWB观测信息，首先计算平面情况。假设平面方程为：
Figure DEST_PATH_IMAGE034
当获得了超过4个基站信息，先选取三个点，进行计算得：
Figure 413447DEST_PATH_IMAGE035
Figure DEST_PATH_IMAGE036
Figure 21146DEST_PATH_IMAGE037
Figure DEST_PATH_IMAGE038
然后，再计算第二个平面方程。当基站所构成的平面的数量超过2个，就可以进行定位计算：
以三个基站为例，假设三个基站的坐标为
Figure 932470DEST_PATH_IMAGE039
得到三个距离信息
Figure DEST_PATH_IMAGE040
，需要求解的标签坐标为
Figure 447765DEST_PATH_IMAGE041
。那么，根据欧式距离关系，可得：
Figure DEST_PATH_IMAGE042
化简可得：
Figure 229776DEST_PATH_IMAGE043
则通过最小二乘法，可以求得：
Figure DEST_PATH_IMAGE044
其中：
Figure 805114DEST_PATH_IMAGE045
然后，再计算该定位结果的PDOP值，判定该解算结果是否有效：
假设定位节点坐标为
Figure DEST_PATH_IMAGE046
待测节点坐标为
Figure 79100DEST_PATH_IMAGE047
，令
Figure DEST_PATH_IMAGE048
则距离观测方程为
Figure 398086DEST_PATH_IMAGE049
求观测伪距对于坐标系X,Y,Z的方向余弦可得
Figure 64298DEST_PATH_IMAGE051
Figure 810537DEST_PATH_IMAGE053
Figure 306240DEST_PATH_IMAGE055
对于第j个由伪距观测的线性观测方程，可以列出矢量表达式
Figure 428917DEST_PATH_IMAGE057
其中，
Figure 123203DEST_PATH_IMAGE059
其中，V为观测值与标准差的残差向量，该向量中的每一个值为单次观测值与其标准值的残差。则解得误差协方差阵为
Figure 102661DEST_PATH_IMAGE061
可得位置精度衰减因子（PDOP）为
Figure 413556DEST_PATH_IMAGE063
当发现PDOP值大于1.0，不满足实际定位要求，则采用测量模型，选取最新的测距消息，带入测量模型，有
Figure 808766DEST_PATH_IMAGE065
若发现PDOP值小于1.0，则采用定位模型，使用定位结果，带入模型，有
观测计算
Figure 560821DEST_PATH_IMAGE067
对前后两个标签进行二维定位计算，则能够求得yaw，即
Figure 914442DEST_PATH_IMAGE069
Figure 181475DEST_PATH_IMAGE071
观测更新
Figure 442692DEST_PATH_IMAGE073
最后，最终完成一次定位状态更新，输出结果如图4所示。
与现有的其他方法相比，本发明能够同时处理不同的里程计模型，尤其是针对在线实时的情况下，UWB有效基站数量不同而采用不同的观测模型，避免了由于UWB基站太少而不参与计算的问题，提高了定位的稳定性，以及观测更新频率，使得能够处理更加复杂情况的传感器情况，从而实现稳定地，可靠地定位。

Claims (10)

1.一种融合里程计的UWB多模态定位方法，其特征在于，包括：
S1、确定和配置车辆的车辆运动模型；
S2、根据所述车辆运动模型，以及车辆姿态获取模块获取到的信息，结合上一时刻车辆的位置和姿态，计算当前时刻的车辆的预测姿态、预测位置和可能的位置范围；
S3、利用设置在车辆上的UWB标签获取车辆可检测到的UWB基站信息，根据预设阈值筛选的有效的UWB基站，并通过UWB计算模块计算得出车辆与有效的UWB基站之间的距离；
S4、当仅存在一个有效的UWB基站时，将S2步骤中得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块得出的车辆与UWB基站的观测距离直接进行融合计算，进行车辆位姿估计；
当存在多个有效的UWB基站时，首先计算UWB基站所构成的平面的数量，当平面数量满足要求后，进行UWB定位计算并计算该定位计算的PDOP值，根据PDOP值判定该定位计算是否有效；
如果PDOP值小于设定阈值，则将S2步骤中得出的当前时刻车辆的预测位置和可能的位置范围，与UWB定位计算得出的车辆的观测位置和观测姿态进行融合计算，进行车辆位姿估计；
如果UWB基站所构成的平面的数量不满足要求，或者PDOP值大于设定阈值，那么就继续将S2步骤中得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块得出的车辆与UWB基站的观测距离直接进行融合计算，进行车辆位姿估计；
S5、对车辆位置和姿态进行更新，输出车辆的最终位置和姿态。
2.根据权利要求1所述融合里程计的UWB多模态定位方法，其特征在于，S1步骤中还包括标定UWB标签与车体计算中心的偏移。
3.根据权利要求1所述融合里程计的UWB多模态定位方法，其特征在于，所述的UWB标签为两个。
4.根据权利要求1所述融合里程计的UWB多模态定位方法，其特征在于，S2步骤中使用卡尔曼滤波器进行状态估计。
5.根据权利要求1所述融合里程计的UWB多模态定位方法，其特征在于，S5步骤中还包括输出三维速度。
6.一种用于实施如权利要求1所述方法的UWB多模态定位系统，其特征在于，包括：
车辆姿态获取模块，用于获取车辆的姿态信息；
车辆运动预测模块，用于根据车辆运动模型，以及所述车辆姿态获取模块获取到的信息，结合上一时刻车辆的位置和姿态，计算当前时刻车辆的预测姿态、预测位置和可能的位置范围；
环境部署模块，用于根据待测量环境部署UWB基站，包括：确定UWB基站的数量、部署结构和每个UWB基站的物理坐标；
UWB标签，设置于车辆上，用以获取车辆周围UWB基站的数量与测距信息；
UWB计算模块，用于根据预设的阈值确定有效的UWB基站，根据有效的UWB基站及UWB基站坐标获取当前车辆到有效的UWB基站的观测距离或观测位置；
位置判定模块，用于将车辆运动预测模块得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块计算得出的车辆与UWB基站的观测距离或观测位置进行融合计算，进行车辆位姿估计，并输出车辆的最终位置和姿态。
7.根据权利要求6所述融合里程计的UWB多模态定位系统，其特征在于，还设有IMU模块，测量车辆的实时线性加速度和角速度。
8.根据权利要求6所述融合里程计的UWB多模态定位系统，其特征在于，车辆运动预测模块使用卡尔曼滤波器进行最优状态估计。
9.根据权利要求6所述融合里程计的UWB多模态定位系统，其特征在于，车辆后两轮速度、车辆前轮速度和角度、车辆是否打滑。
10.一种用于实施如权利要求1所述方法的融合里程计的UWB多模态定位装置，其特征在于，包括：
车辆姿态获取模块，用于获取车辆的姿态信息；
车辆运动预测模块，用于根据车辆运动模型，以及所述车辆姿态获取模块获取到的信息，结合上一时刻车辆的位置和姿态，计算当前时刻车辆的预测姿态、预测位置和可能的位置范围；
UWB标签，设置于车辆上，用以获取车辆周围UWB基站的数量与测距信息；
UWB计算模块，用于根据预设的阈值确定有效的UWB基站，根据有效的UWB基站及UWB基站坐标获取当前车辆到有效的UWB基站的观测距离或车辆观测位置；
位置判定模块，用于将车辆运动预测模块得出的当前时刻车辆的预测位置和可能的位置范围，与UWB计算模块计算得出的车辆与UWB基站的观测距离或观测位置进行融合计算，进行车辆位姿估计，并输出车辆的最终位置和姿态。
CN202210525566.0A 2022-05-16 2022-05-16 一种融合里程计的uwb多模态定位系统、方法及装置 Active CN114623823B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210525566.0A CN114623823B (zh) 2022-05-16 2022-05-16 一种融合里程计的uwb多模态定位系统、方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210525566.0A CN114623823B (zh) 2022-05-16 2022-05-16 一种融合里程计的uwb多模态定位系统、方法及装置

Publications (2)

Publication Number Publication Date
CN114623823A CN114623823A (zh) 2022-06-14
CN114623823B true CN114623823B (zh) 2022-09-13

Family

ID=81906919

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210525566.0A Active CN114623823B (zh) 2022-05-16 2022-05-16 一种融合里程计的uwb多模态定位系统、方法及装置

Country Status (1)

Country Link
CN (1) CN114623823B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115235452A (zh) * 2022-07-22 2022-10-25 上海师范大学 基于uwb/imu和视觉信息融合的智能泊车定位系统及方法
CN116659487A (zh) * 2022-11-02 2023-08-29 丰疆智能(深圳)有限公司 位姿调整方法、位姿调整装置、电子设备及可读存储介质
CN116437288B (zh) * 2023-05-04 2024-02-09 青岛柯锐思德电子科技有限公司 一种基于信号强度选择los基站算法设计方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110456305A (zh) * 2019-08-27 2019-11-15 全图通位置网络有限公司 一种超宽带定位方法
WO2021048500A1 (fr) * 2019-09-12 2021-03-18 Dronisos Procédé et système de positionnement automatique de drones en essaim
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN113438615A (zh) * 2020-03-04 2021-09-24 北京京东乾石科技有限公司 定位方法和装置
CN113494910A (zh) * 2020-04-02 2021-10-12 广州汽车集团股份有限公司 一种基于uwb定位的车辆定位方法、装置及存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA3151391C (en) * 2019-10-17 2024-05-14 Walter KINIO Portable positioning and odometry system
CN111811503B (zh) * 2020-07-15 2022-02-18 桂林电子科技大学 基于超宽带和二维码的无迹卡尔曼滤波融合定位方法
CN111833717B (zh) * 2020-07-20 2022-04-15 阿波罗智联(北京)科技有限公司 用于定位交通工具的方法、装置、设备和存储介质
CN114088091B (zh) * 2022-01-21 2022-05-17 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110456305A (zh) * 2019-08-27 2019-11-15 全图通位置网络有限公司 一种超宽带定位方法
WO2021048500A1 (fr) * 2019-09-12 2021-03-18 Dronisos Procédé et système de positionnement automatique de drones en essaim
CN113438615A (zh) * 2020-03-04 2021-09-24 北京京东乾石科技有限公司 定位方法和装置
CN113494910A (zh) * 2020-04-02 2021-10-12 广州汽车集团股份有限公司 一种基于uwb定位的车辆定位方法、装置及存储介质
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于3D LiDAR的矿山无人驾驶车行驶边界检测;陈龙等;《煤炭学报》;20201231(第06期);第238-244页 *

Also Published As

Publication number Publication date
CN114623823A (zh) 2022-06-14

Similar Documents

Publication Publication Date Title
CN114623823B (zh) 一种融合里程计的uwb多模态定位系统、方法及装置
CN110412635B (zh) 一种环境信标支持下的gnss/sins/视觉紧组合方法
US20190033867A1 (en) Systems and methods for determining a vehicle position
CN110554376A (zh) 用于运载工具的雷达测程法
CN113899375B (zh) 车辆定位方法和装置、存储介质及电子设备
CN108051839B (zh) 一种车载三维定位装置及三维定位的方法
CN107274721B (zh) 一种智能交通系统中多车辆协作定位方法
CN110160545B (zh) 一种激光雷达与gps的增强定位系统及方法
CN110702091A (zh) 一种沿地铁轨道移动机器人的高精度定位方法
KR102331312B1 (ko) 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템
WO2024041156A1 (zh) 车辆定位校准方法、装置、计算机设备及存储介质
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
CN110458885B (zh) 基于行程感知与视觉融合的定位系统和移动终端
CN113295174B (zh) 一种车道级定位的方法、相关装置、设备以及存储介质
CN112923919B (zh) 基于图优化的行人定位方法及系统
CN105043392A (zh) 一种飞行器位姿确定方法及装置
CN111025366A (zh) 基于ins及gnss的网格slam的导航系统及方法
CN114509069B (zh) 基于蓝牙aoa和imu融合的室内导航定位系统
CN113671454B (zh) 一种车载雷达的位置参数标定方法、装置及存储介质
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN113324544B (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
Chu et al. Performance comparison of tight and loose INS-Camera integration
JP2021085880A (ja) 移動体の定位誤差の分析
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统

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