具有非完整约束移动机器人的超宽带-惯导-里程计融合定位与能观测性分析

周柏李，方虹斌，徐鉴; Zhou Boli; Fang Hongbin; Xu Jian

引 en

具有非完整约束移动机器人的超宽带-惯导-里程计融合定位与能观测性分析^*

周柏李^1,2,3
机构：
1. 复旦大学智能机器人研究院,上海 200433
2. 复旦大学智能机器人教育部工程研究中心,上海 200433
3. 复旦大学上海智能机器人工程技术研究中心,上海 200433
×
，方虹斌^1,2,3
机构：
1. 复旦大学智能机器人研究院,上海 200433
2. 复旦大学智能机器人教育部工程研究中心,上海 200433
3. 复旦大学上海智能机器人工程技术研究中心,上海 200433
×
，徐鉴^1,2,3
机构：
1. 复旦大学智能机器人研究院,上海 200433
2. 复旦大学智能机器人教育部工程研究中心,上海 200433
3. 复旦大学上海智能机器人工程技术研究中心,上海 200433
×

1. 复旦大学智能机器人研究院,上海 200433；
2. 复旦大学智能机器人教育部工程研究中心,上海 200433；
3. 复旦大学上海智能机器人工程技术研究中心,上海 200433；

UWB-IMU-ODOMETER FUSION LOCALIZATION AND OBSERVABILITY ANALYSIS FOR MOBILE ROBOTS WITH NONHOLONOMIC CONSTRAINTS^*

Zhou Boli^1,2,3
Affiliation：
1. Institute of AI and Robotics, Fudan University, Shanghai 200433 , China
2. MOE Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China
3. Shanghai Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China
×
，Fang Hongbin^1,2,3
Affiliation：
1. Institute of AI and Robotics, Fudan University, Shanghai 200433 , China
2. MOE Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China
3. Shanghai Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China
×
，Xu Jian^1,2,3
Affiliation：
1. Institute of AI and Robotics, Fudan University, Shanghai 200433 , China
2. MOE Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China
3. Shanghai Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China
×

1. Institute of AI and Robotics, Fudan University, Shanghai 200433 , China；
2. MOE Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China；
3. Shanghai Engineering Research Center of AI & Robotics, Fudan University, Shanghai 200433 , China；

通讯作者:

方虹斌,E-mail:fanghongbin@fudan.edu.cn

中图分类号:O313.7;TP242.6

文献标识码:A

文章编号:1672-6553-2022-20(6)-064-12

DOI:10.6052/1672-6553-2022-042

全文
Comment
参考文献
出版信息

参考文献 1

Liu J,Cai B G,Wang J.Cooperative localization of connected vehicles:Integrating GNSS with DSRC using a robust cubature Kalman filter.IEEE Transactions on Intelligent Transportation Systems,2017,18(8):2111~2125

查找原文

参考文献 2

Huang B,Yang R,Jia B,et al.A theoretical analysis on sampling size in WiFi fingerprint-based localization.IEEE Transactions on Vehicular Technology,2021,70(4):3599~3608

查找原文

参考文献 3

Giannelos E,Andrianakis E,Skyvalakis K,et al.Robust RFID localization in multipath with phase-based particle filtering and a mobile robot.IEEE Journal of Radio Frequency Identification,2021,5(3):302~310

查找原文

参考文献 4

许承宇,徐绍凯.基于视觉延时补偿的无人机室内实时导航系统.动力学与控制学报,2022,20(1):78~84(Xu C Y,Xu S K.UAV indoor real-time navigation system based on visual delay compensation.Journal of Dynamics and Control,2022,20(1):78~84(in Chinese)).

查找原文

参考文献 5

Chen X,Vizzo I,Läbe T,et al.Range image-based LiDAR localization for autonomous vehicles.Proceedings-IEEE International Conference on Robotics and Automation,2021,2021-May(Icra):5802~5808

查找原文

参考文献 6

Shule W,Almansa C M,Queralta J P,et al.UWB-based localization for multi-UAV systems and collaborative heterogeneous multi-robot systems.Procedia Computer Science,2020,175(2019):357~364

查找原文

参考文献 7

Martalo M,Perri S,Verdano G,et al.Improved UWB TDoA-based positioning using a single hotspot for industrial IoT applications.IEEE Transactions on Industrial Informatics,2022,18(6):3915~3925

查找原文

参考文献 8

Li J,Bi Y,Li K,et al.Accurate 3D localization for MAV swarms by UWB and IMU fusion.IEEE International Conference on Control and Automation ICCA 2018,2018-June:100~105

查找原文

参考文献 9

Goudar A,Schoellig A P.Online spatio-temporal calibration of tightly-coupled ultrawideband-aided inertial localization.IEEE International Conference on Intelligent Robots and Systems,2021:1161~1168

查找原文

参考文献 10

You W,Li F,Liao L,et al.Data fusion of UWB and IMU based on unscented Kalman filter for indoor localization of quadrotor UAV.IEEE Access,2020,8:64971~64981

查找原文

参考文献 11

Solà J.Quaternion kinematics for the error-state Kalman filter.2017

查找原文

参考文献 12

Li M G,Zhu H,You S Z,et al.UWB-based localization system aided with inertial sensor for underground coal mine applications.IEEE Sensors Journal,2020,20(12):6652~6669

查找原文

参考文献 13

Cao Y,Yang C,Li R,et al.Accurate position tracking with a single UWB anchor.Proceedings-IEEE International Conference on Robotics and Automation,2020:2344~235.

查找原文

参考文献 14

Brossard M,Barrau A,Bonnabel S.RINS-W:robust inertial navigation system on wheels.IEEE International Conference on Intelligent Robots and Systems,2019:2068~2075

查找原文

参考文献 15

Wang Y,Li X.The IMU/UWB fusion positioning algorithm based on a particle filter.ISPRS International Journal of Geo-Information,2017,6(8):235

查找原文

参考文献 16

Kelly J,Sukhatme G S.Visual-inertial sensor fusion:Localization,mapping and sensor-to-sensor self-calibration.International Journal of Robotics Research,2011,30(1):56~79

查找原文

参考文献 17

Goudar A,Pereida K,Schoellig A P.Ultra-wideband aided inertial navigation:observability analysis and sensor extrinsics calibration.IEEE International Conference.Conf.on Intelligent Robots and Systems,2020

查找原文

参考文献 18

Xia X,Hashemi E,Xiong L,et al.Autonomous vehicles sideslip angle estimation:single antenna GNSS/IMU fusion with observability analysis.IEEE Internet of Things Journal,2021,8(19):14845~14859

查找原文

参考文献 19

Araki B,Gilitschenski I,Ogata T,et al.Range-based cooperative localization with nonlinear observability analysis.IEEE Intelligent Transportation Systems Conference,ITSC 2019,2019:1864~1870

查找原文

参考文献 20

Shi Q,Cui X,Zhao S,et al.Range-only collaborative localization for ground vehicles.Proceedings of the 32nd International Technical Meeting of the Satellite Division of the Institute of Navigation,ION GNSS+ 2019,2019:2063~2077

查找原文

参考文献 21

Fontanelli D,Shamsfakhr F,MacIi D,et al.An uncertainty-driven and observability-based State estimator for nonholonomic robots.IEEE Transactions on Instrumentation and Measurement,2021:70

查找原文

参考文献 22

Hermann R,Krener A J.Nonlinear controllability and observability.IEEE Transactions on Automatic Control,1977,22(5):728~740

查找原文

参考文献 23

MPU-9250 product specification Revision 1.1,InvenSense,June 20,2016.[Online].Available:https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf

查找原文

目录contents

摘要 Abstract
关键词 Keywords
1 系统建模
1.1 IMU测量模型
1.2 里程计测量模型
1.3 UWB测量模型
2 能观测性分析
2.1 非线性能观性的定义
2.2 系统能观性判断
2.2.1 零阶李导数及其梯度:
2.2.2 一阶李导数及其梯度
2.2.3 二阶李导数及其梯度
2.2.4 三阶李导数及其梯度
3 基于ESKF的状态估计
4 实验结果与验证
4.1 定位性能比较
4.2 能观测性条件验证
5 结论
参考文献

摘要

为解决现有超宽带-惯导组合定位系统在轮式移动机器人的定位精度低、依赖高精度IMU等问题,提出了一种采用误差状态卡尔曼滤波融合超宽带-惯导-里程计的定位算法,利用里程计的线速度测量和由非完整约束隐含的伪测量,提高了移动机器人的位置和姿态估计精度. 同时,对于由多传感器测量模型组成的非线性系统,通过基于李导数的能观性秩条件分析方法对该系统的能观测性进行了详细的理论分析与数学证明,得到了系统局部弱可观的条件,从而确定了系统状态可以被无偏估计所需要的测量输出以及控制输入. 仿真结果表明,在满足能观测性条件时,本文提出的方法能够有效地获得移动机器人较准确的六自由度位姿,且相比传统方法显著提升了定位精度.

Abstract

To solve the problems of low positioning accuracy and dependence on high-precision IMU of the existing UWB-IMU positioning system for wheeled mobile robots, a localization algorithm using error state Kalman filter to integrate UWB-IMU-Odometer is proposed to improve the position and attitude estimation accuracy of mobile robots using linear velocity measurement of odometry and pseudo-measurement implied by the nonholonomic constraints. Meanwhile, for the nonlinear system composed of the multi-sensor measurement models, a detailed theoretical analysis and mathematical proof of the observability of the system is carried out by an observability rank condition analysis method based on the Lie derivative, and the conditions under which the system is locally weakly observable are concluded, which determines the required measurement outputs and control inputs for unbiased estimation of the system states. The simulation results show that when the observability conditions are satisfied, the state estimation approach proposed in this paper can effectively obtain the accurate 6-DOF poses of the mobile robot and significantly improve the positioning accuracy compared with the conventional methods.

关键词

多传感器融合；非完整约束；能观测性；误差状态卡尔曼滤波

Keywords

sensor fusion ； nonholonomic constraint ； observability ； error state Kalman filter

引言
定位问题是机器人领域过去几十年的研究热点之一. 精确的位姿估计是机器人实现导航、避障等功能的基础.一方面，室外定位目前主要依赖于全球卫星导航系统（GNSS）^[1]，然而一般的GNSS定位系统在建筑物密集的情况下存在较大的定位误差，并且在室内信号较弱时性能急剧退化甚至失效. 常见的室内定位系统包括基于Wi-Fi的无线网络定位^[2]，基于射频识别（RFID）的定位^[3]等，尽管这些定位系统很容易被建立和部署，但定位精度相对较低. 另一方面，同时定位与建图（SLAM）技术^[4，5].利用机器人自身搭载的激光雷达或摄像头等传感器，获得精确的位姿估计和环境地图，但其对计算资源的需求较高，对复杂环境的鲁棒性较差，且存在长时间累积误差等问题. 相比于这些传感器，超宽带无线通信技术（UWB）可以提供厘米级的测距精度^[6]，具有功耗低、测量精度高和鲁棒性强等优点，使其成为建立室内通信和定位网络的优先考虑方案.
基于UWB的定位系统通常需要在空间中预先部署多个UWB锚点，通过测量移动人员或物体上携带的UWB模块与环境中锚点的距离，并基于到达时间（TOA）法或到达时间差（TDOA）法^[7]，来确定移动体的位置. 还有一些研究采用惯性测量单元（IMU）与UWB组合定位系统来确定机器人的位置和姿态. Li等人^[8]采用拓展卡尔曼滤波（EKF）融合UWB与IMU测量估计无人机的位置和速度，并在线估计IMU的加速度和角速度测量偏差，使得该系统可以在低成本IMU上应用. Goudar等人^[9]针对无人机的位姿估计问题提出考虑UWB与IMU时空参数在线校准的紧耦合定位方法，从而减小了传感器之间的时间延迟与外参对定位精度的影响. You等人^[10]采用无迹卡尔曼滤波（UKF）融合UWB与IMU的测量实现多旋翼无人机的精确位姿估计. 总的来说，这些UWB-IMU组合定位方法依赖于对IMU的加速度和角速度积分进行状态传播和预测^[11]，往往针对的是在运动中加速度和角速度变化较明显的无人机系统，而对于运动相对平缓甚至匀速运动的地面轮式机器人来说，实际的加速度和角速度通常很小甚至接近零，此时低成本IMU通常难以检测出运动，且测量噪声会导致极大的累积漂移误差，因此被迫需要采用高成本、高精度的IMU. Li等人^[12]提出一种应用于地下采矿车的六自由度（三维位置与姿态）定位方法，采用误差状态卡尔曼滤波（ESKF）融合UWB与高精度IMU的测量实现了精确的轮式机器人位姿估计. 除了提升IMU本身的精度外，Cao等人^[13]提出了一种速度估计器来解决低成本IMU和UWB融合时的速度漂移问题，相比于传统对加速度和角速度进行积分，该方法通过连续检测UWB测量的距离变化进行速度估计，实现了基于低成本IMU轮式机器人较精确的二维位姿估计. Brossard等人^[14]利用零速修正（ZUPT）法实现了车辆的纯惯性导航定位，该方法通过深度神经网络检测IMU零速运动情况，在行驶21公里长的轨迹下仅依靠IMU获得了高精度位姿. Wang等人^[15]通过粒子滤波融合UWB和IMU数据实现了室内行人定位，并采用ZUPT法检测IMU的速度状态来约束误差的累积. 这些方法本质上都是对速度进行有效的估计或检测，来约束IMU预测位姿时产生的累积误差. 本质上，误差产生的原因有两方面:第一，轮式机器人不同于无人机系统，其运动相对平缓导致加速度和角速度测量值较小; 第二，低成本IMU的测量偏差和噪声极大. 实际上，对于轮式机器人来说，里程计（即电机编码器的测量输出）能直接提供机器人运动的一维线速度和角速度，是方便且有效的约束速度漂移工具，因而无需设计复杂的速度估计器或采用ZUPT法检测IMU的零速状态. 同时，一般轮式机器人的非完整约束（无法纯侧移或脱离地面向上运动）隐含了速度的伪测量值. 因此，本文提出了结合非完整约束，并融合UWB、IMU和里程计的轮式机器人定位新方法，实现基于低成本IMU轮式机器人的精确位姿估计.
除此之外，能观测性是系统状态估计收敛到真实状态无偏估计的必要条件^[16]. 如果一个系统是能观的，系统的测量输出就提供了必要且充分的信息来确定系统状态，状态可以被有效估计且只存在有界的估计误差. 因此，研究系统的能观测性对于状态估计具有重要意义. 一些研究者已经对部分多传感器系统进行了能观测性分析，例如相机-IMU系统^[16]，UWB-IMU系统^[17]以及GNSS-IMU系统^[18]等. 其中，UWB与其他传感器组合系统的能观测性吸引了极大关注. Goudar等人^[17]对包含外参校准的UWB-IMU紧耦合系统进行了能观测性分析，其应用对象是可沿局部坐标系三轴任意平移和旋转的人或无人机，而非具有非完整约束的轮式机器人. Araki等人^[19]研究了具备UWB-里程计系统双机器人的能观测性，该方法评估的是机器人之间相对位姿的能观测性，而非全局坐标系下的绝对位姿，同时里程计仅能提供二维平面模型. Qin等人^[20]分析了含有UWB-里程计系统的多机器人绝对位姿的能观测性，由于没有加速度和角速度信息，其得到的运动模型也是二维的; 该研究确定了为使机器人二维绝对位姿能观，需要至少两个UWB锚点或静态机器人提供已知位置.Fontanelli等人^[21]研究了UWB-里程计系统在二维平面内状态的全局能观测性，研究得出，对于平面模型，在两个UWB锚点存在时，机器人需要非直线运动以保证系统全局能观. 本文我们研究的对象是具有非完整约束的轮式机器人，分析由UWB-IMU-里程计组成定位系统的能观测性. 由于该系统是一个非线性系统，我们采用一种基于微分几何的能观测性分析方法^[22]，确定六自由度绝对位姿的局部弱可观测性条件.
本文提出一种具有非完整约束移动机器人的UWB-IMU-里程计融合定位方法. 首先，对该多传感器系统进行建模，利用IMU的动力学模型进行系统状态传播，利用里程计和UWB测量模型进行状态修正. 其次，通过基于李导数的能观性秩条件分析方法对该系统的能观测性进行详细的理论分析与数学证明. 随后，采用误差状态卡尔曼滤波进行系统状态估计. 仿真实验验证了所提方法的有效性和优越性.
1 系统建模
本节分别对UWB、IMU和里程计的测量进行建模，建立完整的UWB-IMU-里程计系统模型. 其中，UWB模型利用了机器人与UWB锚点之间的距离测量，IMU模型利用了三轴加速度和角速度测量，里程计模型利用了线速度测量. 假设IMU坐标系与机体坐标系重合，最终目标是估计IMU坐标系在世界坐标系中的位姿，坐标系的建立如图1所示.
对于UWB-IMU-里程计定位系统，估计的系统状态变量如下:

x (t) = \{_{I}^{W} p (t),_{I}^{W} v (t),_{I}^{W} q (t), a_{b} (t), ω_{b} (t)\}

(1)

其中 $_{I}^{W} p （ t ）$ ， $_{I}^{W} v （ t ）$ ， $_{I}^{W} q （ t ）$ 分别表示IMU坐标系{I}相对于世界坐标系{W}的位置、速度和姿态. 这里采用哈密顿形式的单位四元数^[11] $_{I}^{W} q （ t ）$ 描述姿态，即 $q = {[q_{w} ， q_{v}^{T}]}^{T}$ . IMU的加速度和角速度测量偏差在坐标系{I}中被表示为 $a_{b} （ t ）$ ， $ω_{b} （ t ）$ .
图1 轮式机器人运动概念图 {I}为IMU坐标系，{W}为世界坐标系
Fig.1 Conceptual representation of wheeled robot motion {I} represents IMU coordinate system and {W} represents the world coordinate system
1.1 IMU测量模型
常见的惯性测量单元包含加速度计和陀螺仪，其中加速度计可以提供三轴加速度测量，其被建模为

a_{m} (t) = a_{t} (t) + a_{b} (t) + n_{a} (t)

(2)

其中 $a_{m} （ t ）$ 是实际测得的IMU坐标系中的加速度数据， $a_{t} （ t ）$ 是真实的加速度， $n_{a} （ t ）$ 被假设为服从零均值高斯分布的加速度测量噪声.
陀螺仪可以提供三轴角速度测量，其被建模为

ω_{m} (t) = ω_{t} (t) + ω_{b} (t) + n_{ω} (t)

(3)

其中 $ω_{m} （ t ）$ 是实际测得的IMU坐标系中的角速度数据， $ω_{t} （ t ）$ 是真实的角速度， $n_{ω} （ t ）$ 被假设为服从零均值高斯分布的角速度测量噪声.
与文献^[11]类似，这里采用以加速度和角速度为系统输入的三维动力学模型:
$_{I}^{W} \dot{p} (t) =_{I}^{W} v (t)$
$_{I}^{W} \dot{v} (t) =_{I}^{W} R (t) (a_{m} (t) - a_{b} (t) - n_{a} (t)) + g$

\begin{matrix} _{I}^{W} \dot{q} (t) = \frac{1}{2}_{I}^{W} q (t) \otimes (ω_{m} (t) - ω_{b} (t) - n_{ω} (t)) \\ {\dot{a}}_{b} (t) = n_{b a} (t) \\ {\dot{ω}}_{b} (t) = n_{b ω} (t) \end{matrix}

(4)

其中 $_{I}^{W} R （ t ）$ 为单位四元数 $_{I}^{W} q （ t ）$ 所对应的旋转矩阵， $\otimes$ 表示四元数乘法，另外，IMU坐标系下的加速度偏差 $a_{b} （ t ）$ 和角速度偏差 $ω_{b} （ t ）$ 被建模为随机游走过程， $n_{b a} （ t ）$ 和 $n_{b ω} （ t ）$ 表示零均值高斯白噪声. 注意到，微分方程（4）包含了状态量的二次项，因此是一个非线性模型.
1.2 里程计测量模型
移动机器人的里程计，即车轮电机编码器，可以提供IMU坐标系下的线速度测量和绕机体z轴的角速度测量. 一方面，为了约束由IMU加速度积分产生的速度累计误差，本文采用里程计的一维前向线速度测量作为观测量对速度进行修正. 另一方面，考虑一般的具有非完整约束的轮式移动机器人，其侧移速度和垂直于地面向上的速度为0. 将该非完整约束作为伪测量值，与电机编码器测量的前向运动速度结合作为里程计的观测模型，模型表达式为

v (t) =_{W}^{I} R (t)_{I}^{W} v (t) + n_{v} (t) = {[\begin{matrix} v_{m} (t) 0 0 \end{matrix}]}^{T}

(5)

其中 $v_{m} （ t ）$ 为实际测量的沿IMU坐标系x轴的线速度， $n_{v} = {[\begin{matrix} n_{x} （ t ） 0 0 \end{matrix}]}^{T}$ ， $n_{x} （ t ）$ 被假设为零均值高斯白噪声. 另外，注意到，式（5）中包含了状态量的乘积，因此也是一个非线性方程. 本文第4节的实验证实，引入里程计的线速度测量与非完整约束隐含的伪测量能够很好地约束定位过程中的误差漂移.
1.3 UWB测量模型
假设在小型轮式机器人上配备的UWB模块与IMU之间的外参可以忽略. 采用环境中部署的UWB锚点与移动机器人上UWB模块之间的距离测量作为测量模型，即

d_{i} (t) = {∥\underset{A_{i}}{W} p -_{I}^{W} p (t)∥}_{2} + n_{d_{i}} (t), i = 1,2, \dots, s

(6)

其中 $\underset{A_{i}}{W} p$ 表示第i个锚点在世界坐标系下的三维空间位置，为已知值. || · ||₂表示欧几里得距离， $n_{d_{i}}$ 为服从零均值高斯分布的距离测量噪声，s表示UWB锚点的总数量.
2 能观测性分析
如果系统输出和控制输入能够提供必要且充分的信息来确定系统状态，那么该系统是能观测的. 能观测性是保证状态估计值能收敛到系统真实状态无偏估计的必要条件，因此，研究系统的能观测性具有重要意义. 对于非线性系统来说，基于李导数的秩条件分析是常见且有效的能观测性研究方法^[22]. 本文采用该方法分析由式（4）~式（6）组成的非线性系统的局部弱可观性.
2.1 非线性能观性的定义
将状态空间视为一个光滑的m维流形M，考虑如下由状态方程和输出方程组成的非线性系统，并将状态方程写为线性输入（input-linear）或控制仿射（control-affine）的形式:

\{\begin{matrix} \dot{x} = f (x, u) \\ y = h (x) \end{matrix} \Rightarrow \{\begin{matrix} \dot{x} = f_{0} (x) + \sum_{j = 1}^{n} f_{j} (x) u_{j} \\ y = h (x) \end{matrix}

(7)

其中x ∈M是状态向量， $u = {[u_{1} ， \dots ， u_{n}]}^{T} \in R^{n}$ 为控制输入向量，y表示观测到的系统输出，h表示输出函数. 函数f₀和f_j为光滑向量场，与控制输入u共同决定系统状态随时间的变化.
根据定义^[22]，如果流形M上不存在具有相同的输入-输出映射的两点x₀和x₁，即x₀和x₁可区分，则系统是全局可观的. 如果对于流形M上的每一点x₀，存在一个邻域 $H \subseteq M$ 使得在每个x₀的邻域 $\subseteq M$ ，不存在其他的状态x₁∈V与x₀不可区分，那么系统是局部弱可观的. 这实际上说明，局部弱可观性要求状态空间中的状态要与其邻域内的状态可区分，不能共享相同的输出.
2.2 系统能观性判断
文献^[16]和^[17]所概述的方法考虑了无噪声非线性系统的情况.在接下来的分析中，忽略噪声参数的影响，并将动力学模型（4）重新写成控制仿射的形式:

\dot{x} = \underset{f_{0}}{\underset{⏟}{[\begin{matrix} _{I}^{W} v \\ -_{I}^{W} R a_{b} + g \\ - \frac{1}{2} Ξ (_{I}^{W} q) ω_{b} \\ 0_{3 \times 1} \\ 0_{3 \times 1} \end{matrix}]}} + \underset{f_{1}}{\underset{⏟}{[\begin{matrix} O_{3 \times 3} \\ _{I}^{W} R \\ 0_{4 \times 3} \\ 0_{3 \times 3} \\ θ_{3 \times 3} \end{matrix}]}} + \underset{f_{2}}{\underset{⏟}{[\begin{matrix} 0_{3 \times 3} \\ 0_{3 \times 3} \\ \frac{1}{2} Ξ (_{1}^{W} q) \\ 0_{3 \times 3} \\ 0_{3 \times 3} \end{matrix}]}} ω_{m}

(8)

其中 $Ξ (_{I}^{W} q) = [\begin{matrix} - q_{v}^{T} \\ q_{0} I - {⌊q_{v}⌋}_{x} \end{matrix}] ， ⌊ \cdot ⌋_{x}$ 为反对称矩阵算子，即

⌊ ω ⌋_{\times} = [\begin{matrix} 0 & - ω_{z} & ω_{y} \\ ω_{z} & 0 & - ω_{x} \\ - ω_{y} & ω_{x} & 0 \end{matrix}], ω = [\begin{matrix} ω_{x} \\ ω_{y} \\ ω_{z} \end{matrix}]

(9)

时间t被省略以简化表达. 为了便于求导运算，UWB测量模型被重新表示为等效形式:

h_{d_{i}} = \frac{1}{2} {∥_{A_{i}}^{W} p -_{I}^{W} p∥}_{2}^{2}

(10)

由于单个UWB锚点测量不足以约束完整的状态，现考虑三个锚点的距离测量. 根据定义^[22]，如果由有限阶李导数的梯度矩阵构成的观测矩阵满秩，则非线性系统是局部弱可观的. 值得注意的是，由于非完整约束的存在，里程计沿IMU坐标系y轴和z轴的线速度测量为0，因此其相应的李导数及其梯度也为0，本质上不会导致观测矩阵的秩增加. 然而，在计算观测矩阵的秩时，由于使用符号运算无法体现出非完整约束的存在，导致该部分的李导数梯度难以被识别为零，进一步影响了观测矩阵秩的大小的判断. 因此，这里用单位向量e₁=[1,0,0]左乘里程计的测量函数，即取一维的前向速度测量，并重新构造系统的测量方程为

h (x) = [\begin{matrix} e_{1}_{W}^{I} R_{I}^{W} v \\ \frac{1}{2} {∥_{A_{1}}^{W} p -_{I}^{W} p∥}_{2}^{2} \\ \frac{1}{2} {∥_{A_{2}}^{W} p -_{I}^{W} p∥}_{2}^{2} \\ \frac{1}{2} {∥_{A_{3}^{}}^{W} p -_{1}^{W} p∥}_{2}^{2} \end{matrix}] \in R^{4}

(11)

然后，给出了相应的李导数及其梯度的计算.
2.2.1 零阶李导数及其梯度:
$h （ x ）$ 的零阶李导数是它本身，即

L^{0} h (x) = h (x)

(12)

其对应的梯度矩阵为

\begin{matrix} \nabla L^{0} h = \frac{\partial L^{0} h (x)}{\partial x} = \\ [\begin{matrix} 0_{1 \times 3} & e_{1}_{I}^{W} R & e_{1} F_{0} & 0_{1 \times 3} & 0_{1 \times 3} \\ p_{r} & 0_{3 \times 3} & 0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}] \end{matrix}

(13)

其中

F_{0} = \frac{\partial (_{W}^{l} R_{I}^{W} v)}{\partial_{I}^{W} q}, p_{r} = [\begin{matrix} {(\underset{I}{W} p -_{A_{1}}^{W} p)}^{T} \\ {(\underset{I}{W} p -_{A_{2}}^{W} p)}^{T} \\ {(\underset{I}{W} p -_{A_{3}}^{W} p)}^{T} \end{matrix}] ≜ [\begin{matrix} p_{r 1} \\ p_{r 2} \\ p_{r 3} \end{matrix}]

(14)

$p_{r} \in R^{3 \times 3}$ 为机器人与锚点的相对位置构成的矩阵.
2.2.2 一阶李导数及其梯度
计算 $L^{0} h （ x ）$ 相对于f₀的一阶李导数，得到

\begin{matrix} L_{f_{0}^{}}^{1} h = \nabla L^{0} h \cdot f_{0} = \\ [\begin{matrix} - e_{1} (a_{b} -_{W}^{I} R g + \frac{1}{2} F_{0} Ξ (_{I}^{W} q) ω_{b}) \\ p_{r} \underset{I}{W} v \end{matrix} \end{matrix}

(15)

对应的李导数梯度为

\begin{matrix} \nabla L_{f_{0}}^{1} h = \frac{\partial L_{f_{0}}^{1} h}{\partial x} = \\ [\begin{matrix} 0_{1 \times 3} & F_{1} & F_{2} & - e_{1} I & F_{3} \\ F_{4} & p_{r} & 0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}] \end{matrix}

(16)

其中矩阵F₁、F₂、F₃、F₄并不影响对最后的观测矩阵的秩的大小判断，这里省略其具体表达式.
2.2.3 二阶李导数及其梯度
计算 $L_{f_{0}}^{1} h （ x ）$ 分别相对于f₀和f₁的二阶李导数，得到

\begin{matrix} L_{f_{0} f_{0}}^{2} h = \nabla L_{f_{0}}^{1} h \cdot f_{0} = \\ [\begin{matrix} F_{1} (-_{I}^{W} R a_{b} + g) + F_{2} (- \frac{1}{2} Ξ (_{I}^{W} q) ω_{b}) \\ F_{4}_{I}^{W} v + p_{r} (-_{I}^{W} R a_{b} + g) \end{matrix}] \end{matrix}

(17)

L_{f_{0} f_{1}}^{2} h = \nabla L_{f_{0}}^{1} h \cdot f_{1} = [\begin{matrix} F_{1} & _{I}^{W} R \\ p_{r} & _{I}^{W} R \end{matrix}]

(18)

其相应的梯度计算为

\begin{matrix} \nabla L_{f_{0} f_{0}}^{2} h = \frac{\partial L_{f_{0} f_{0}}^{2} h}{\partial x} = \\ [\begin{matrix} 0_{1 \times 3} & F_{5} & F_{6} & F_{7} & F_{8} \\ F_{9} & F_{10} & F_{11} & - p_{r}_{I}^{W} R & 0_{3 \times 3} \end{matrix}] \end{matrix}

(19)

\nabla L_{f_{0} f_{1}}^{2} h = \frac{\partial L_{f_{0} f_{1}}^{2} h}{\partial x} = [\begin{matrix} F_{12} & 0_{9 \times 3} & F_{13} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & F_{14} & F_{15} & F_{16} \end{matrix}]

(20)

其中

\begin{matrix} F_{13} = [\begin{matrix} F_{13}^{1} \\ F_{13}^{2} \\ F_{13}^{3} \end{matrix}], F_{13}^{i} = \frac{\partial (_{W}^{I} R p_{r i})}{\partial_{I}^{W} q}, \\ F_{16} = [\begin{matrix} 0 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & - 1 \end{matrix}] \end{matrix}

(21)

矩阵F₅~F₁₂，以及F₁₄，F₁₅不影响观测矩阵的秩的大小，因此省略其具体表达式.
2.2.4 三阶李导数及其梯度
计算 $L_{f_{0} f_{1}}^{2} h （ x ）$ 相对于f₀的三阶李导数，得到

\begin{matrix} L_{f_{0} f_{1} f_{0}}^{3} h = \nabla L_{f_{0} f_{1}}^{2} h \cdot f_{0} = \\ [\begin{matrix} F_{12}_{I}^{W} v + F_{13} (- \frac{1}{2} Ξ (_{I}^{W} q) ω_{b}) \\ F_{14} (- \frac{1}{2} Ξ (_{I}^{W} q) ω_{b}) \end{matrix}] \end{matrix}

(22)

其对应的梯度计算为

\begin{matrix} \nabla L_{f_{0} f_{1} f_{0}}^{3} h = \frac{\partial L_{f_{0} f_{1} f_{0}}^{3} h}{\partial x} = \\ [\begin{matrix} F_{17} & F_{18} & F_{19} & 0_{9 \times 3} & F_{20} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}] \end{matrix}

(23)

其中

\begin{matrix} F_{20} = [\begin{matrix} 0_{3 \times 1} & p_{r}_{I}^{W} R e_{3}^{T} & - p_{r}_{I}^{W} R e_{2}^{T} \\ - p_{r}_{I}^{W} R e_{3}^{T} & 0_{3 \times 1} & p_{r}_{I}^{W} R e_{1}^{T} \\ p_{r}_{I}^{W} R e_{2}^{T} & - p_{r}_{I}^{W} R e_{1}^{T} & 0_{3 \times 1} \end{matrix}], \\ e_{2} = [0,1, 0] \\ e_{3} = [0,0, 1] \end{matrix}

(24)

矩阵F₁₇，F_18，F₁₉不影响观测矩阵的秩的大小，因此省略其具体表达式.
将梯度矩阵按行堆叠，可以得到完整的观测矩阵O

O = [\begin{matrix} \nabla L^{0} h \\ \nabla L_{f_{0}}^{1} h \\ \nabla L_{f_{0} f_{0}}^{2} h \\ \nabla L_{f_{0} f_{1}}^{2} h \\ \nabla L_{f_{0} f_{1} f_{0}}^{3} h \end{matrix}] = [\begin{matrix} 0_{1 \times 3} & e_{1}_{I}^{W} R & e_{1} F_{0} & 0_{1 \times 3} & 0_{1 \times 3} \\ p_{r} & 0_{3 \times 3} & 0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{1 \times 3} & F_{1} & F_{2} & - e_{1} I & F_{3} \\ F_{4} & p_{r} & 0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{1 \times 3} & F_{5} & F_{6} & F_{7} & F_{8} \\ F_{9} & F_{10} & F_{11} & - p_{r}_{I}^{W} R & 0_{3 \times 3} \\ F_{12} & 0_{9 \times 3} & F_{13} & 0_{9 \times 3} & 0_{9 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & F_{14} & F_{15} & F_{16} \\ F_{17} & F_{18} & F_{19} & 0_{9 \times 3} & F_{20} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 4} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}]

(25)

定理1 观测矩阵O满秩的充分条件是:（a）至少三个非共线的锚点存在;（b）机器人不与三个非共线锚点共面;（c）加速度 $a_{x} ， a_{y} ， a_{z}$ 均被激励.
证明假设三个UWB锚点的位置为 $\underset{A_{i}}{W} p = {[_{A_{i}}^{W} x ， \underset{A_{i}}{W} y ，_{A_{i}}^{W} z]}^{T} ， i = 1，2 ， 3$ . 因为世界坐标系的选择不会影响系统自身的能观测性. 为了简化证明，选择锚点1的位置为世界坐标系的原点，即 $_{A_{1}}^{W} x = \underset{A_{1}}{W} y =_{A_{1}}^{W} z = 0$ . x轴被设置为锚点1指向锚点2的方向. 因此， $_{A_{2}}^{W} x \neq 0 ， \underset{A_{2}}{W} y =_{A_{2}}^{W} z = 0$ .以三个锚点所在的平面为世界坐标系的x-o-y平面，因此， $_{A_{3}}^{W} z$ =0.
采用高斯消元法来证明观测矩阵满秩，通过依次计算式（25）中以下块矩阵的行列式来验证它们为满列秩:第一列第二行的矩阵 $p_{r}$ ，第二列第四行的矩阵 $p_{r}$ ，第三列第七行的矩阵F₁₃，第四列第六行的矩阵 $- p_{r}_{I}^{W} R$ ，以及第五列由第八行和第九行元素F₁₆和F₂₀组成的矩阵. 同时，每次将这些矩阵所在行通过线性运算对其余行进行高斯消元，最终化简观测矩阵.
首先，计算矩阵 $p_{r}$ 的行列式为

|p_{r}| =_{A_{2}}^{W} x_{A_{3}}^{W} y_{I}^{W} z

(26)

由于 $_{A_{2}}^{W} x \neq 0$ ，矩阵 $p_{r}$ 行列式不为0（即矩阵 $p_{r}$ 满秩）的条件是 $_{A_{3}}^{W} y \neq 0$ 和 $_{I_{}}^{W} z \neq 0$ . $_{A_{3}}^{W} y \neq 0$ 意味着锚点3不能和锚点1与锚点2共线，因此，产生了条件（a）. $_{I_{}}^{W} z \neq 0$ 意味着机器人不能与三个非共线锚点共面，这产生了条件（b）. 此时，根据高斯消元法，由于式（25）的第二行除 $p_{r}$ 以外均为零矩阵，可以将第二行经过线性运算消除其余每一行的第一列元素. 类似地，利用消除后的矩阵的第四行可以将其余每一行的第二列元素消除.
在将式（25）的前两列元素经过高斯消元后，计算第三列元素中矩阵F₁₃的行列式. F₁₃是一个9×4的矩阵，F₁₃前三行对应a_x的激励，中间三行对应a_y的激励，最后三行对应a_z的激励. 从中任意取出四行计算它们的行列式，只要任何一个行列式不为0，则F₁₃列满秩. 取出其中的第1，2，4，7行，第1，3，4，7行，第2，3，6，9行，并分别计算它们的行列式，可以得到

\begin{matrix} d e t (1,2, 4,7) = 16_{A 2}^{W} x_{A 3}^{W} y_{I}^{W} z^{} {(_{I}^{W} p^{T}_{I}^{W} R)}_{[1,3]}, \\ d e t (1,3, 4,7) = - 16_{A 2}^{W} x_{A 3}^{W} y_{I}^{W} z^{} {(_{I}^{W} p^{T}_{I}^{W} R)}_{[1,2]}, \\ d e t (2,3, 6,9) = 16_{A 2}^{W} x_{A 3}^{W} y_{I}^{W} z^{} {(_{I}^{W} p_{}^{T}_{I}^{W} R)}_{[1,1]} \end{matrix}

(27)

其中下标[i，j]表示矩阵的第i行第j列元素. 根据条件（a）和（b）可知 $_{A 2}^{W} x_{A 3}^{W} y_{I}^{W} z^{}$ ≠0. 同时，由于旋转矩阵 $_{I}^{W} R$ 是可逆矩阵，如果 $_{I}^{W} p^{T}_{I}^{W} R$ 为零向量，则 $_{I}^{W} p^{T}$ 一定是零向量，这与 $_{I}^{W} z^{}$ ≠0相矛盾. 因此， $_{I}^{W} p^{T}_{I}^{W} p$ 为非零向量，这意味着式（27）中一定存在不为零的行列式，进一步说明矩阵F₁₃列满秩，即矩阵F₁₃的秩为4. 同样地，利用F₁₃并采用高斯消元法可以消去式（25）中第三列的其他元素. 从式（27）中可以发现，任意一个行列式都对应了 $a_{x} ， a_{y} ， a_{z}$ 均被激励. 实际上，从 $L_{f_{0}}^{1} h （ x ）$ 沿f₁的二阶李导数（即 $L_{f_{0} f_{1}}^{2} h$ ）的每一列元素都非零也可以看出系统（8）中f₁对应的三轴加速度输入均需要被激励. 因此，产生了条件（c）.
进一步地，观察式（25）中第四列第六行元素，即矩阵 $- p_{r}_{I}^{W} R$ 是否满秩. 由于满足条件（a）和（b），矩阵 $p_{r}$ 满秩，旋转矩阵本身是满秩矩阵，因此，两个满秩矩阵相乘之后也满秩. 利用高斯消元法，式（25）除第六行以外其余行的第四列也可以被消除.
最后，观察F₁₆（即式（21））以及F₂₀（即式（24））可知，只要F₂₀中的第一列不为零，由F₁₆和F₂₀组成的高阵就列满秩. 实际上，F₂₀第一列中的元素 $- p_{r}_{I}^{W} R e_{2}^{T}$ 是矩阵 $p_{r}_{I}^{W} R$ 的第二列，由于 $p_{r}_{I}^{W} R$ 满秩，因此，第二列不可能全为零. 此时，F₁₆与F₂₀按行纵向堆叠的矩阵为列秩为3的矩阵，采用高斯消元法可消去式（25）中同列的其余元素.
最后，等式（25）可转化为

O^{'} = [\begin{matrix} 0_{1 \times 3} 0_{1 \times 3} 0_{1 \times 4} 0_{1 \times 3} 0_{1 \times 3} \\ I_{3 \times 3} 0_{3 \times 3} 0_{3 \times 4} 0_{3 \times 3} 0_{3 \times 3} \\ 0_{1 \times 3} 0_{1 \times 3} 0_{1 \times 4} 0_{1 \times 3} 0_{1 \times 3} \\ 0_{3 \times 3} I_{3 \times 3} 0_{3 \times 4} 0_{3 \times 3} 0_{3 \times 3} \\ 0_{1 \times 3} 0_{1 \times 3} 0_{1 \times 4} 0_{1 \times 3} 0_{1 \times 3} \\ 0_{3 \times 3} 0_{3 \times 3} 0_{3 \times 4} I_{3 \times 3} 0_{3 \times 3} \\ 0_{4 \times 3} 0_{4 \times 3} I_{4 \times 4} 0_{4 \times 3} 0_{4 \times 3} \\ 0_{3 \times 3} 0_{3 \times 3} 0_{3 \times 4} 0_{3 \times 3} I_{3 \times 3} \\ 0_{3 \times 3} 0_{3 \times 3} 0_{3 \times 4} 0_{3 \times 3} 0_{3 \times 3} \end{matrix}]

(28)

此时，容易判断观测矩阵为列满秩. 与文献^[9]中的能观性条件对比可以发现，机器人三轴角速度无需被激励. 因此，一定程度上，该多传感器定位系统放宽了系统状态能观测的要求.
另外，也可以从几何的角度形象地解释该系统的位置变量的能观性. 当仅有一个UWB锚点以及相应的距离测量时，机器人的位置在以该锚点为球心的球面上; 当两个锚点存在时，机器人位于一个圆上; 当有三个非共线的锚点存在时，机器人的位置是两个不连续的点中可能的一个（图2（a）中点A或点A′），或者唯一的一个（图2（a）中点B）. 实际上，在一个或两个锚点存在的情况下，系统位置既不全局可观，也不局部弱可观，因为处于球面或圆上的点无法与流形内的其他点相区分. 对于三个非共线锚点的例子，机器人的位置是局部弱可观的，因为局部弱可观仅考虑与领域内的点的可区分性. 当三个锚点共线时，类似于两个锚点的情况，机器人位于一个圆上（图2（b）），因此其位置也是不可观的.
图2 位置能观性的几何解释其中三角形表示锚点，圆点表示机器人
Fig.2 Geometric interpretation of the observability for positions. The triangles represent the anchors, and the red dot represents the robot
3 基于ESKF的状态估计
基于以上能观测性分析，在特定的条件下，系统状态（1）能被可靠地估计且误差有界. 考虑到系统的非线性，采用误差状态卡尔曼滤波方法融合UWB，IMU和里程计的测量，对状态向量（1）进行估计. 使用误差状态使得姿态的表示维度是最小的，避免了四元数表示中的冗余自由度和欧拉角表示的奇异性问题. 在ESKF框架中，系统的真实状态x由名义状态 $\hat{x}$ 和误差状态 $\tilde{x}$ 组成. 名义状态 $\hat{x}$ 由IMU的测量输入进行传播，其动力学方程为

\begin{matrix} _{I}^{W} \dot{\hat{p}} =_{I}^{W} \hat{v} \\ _{I}^{W} \dot{\hat{v}} =_{I}^{W} \hat{R} (a_{m} - a_{b}) + g \\ _{I}^{W} \dot{\hat{q}} = \frac{1}{2}_{1}^{W} \hat{q} \otimes (ω_{m} - ω_{b}) \\ {\dot{\hat{a}}}_{b} = 0 \\ {\dot{\hat{ω}}}_{b} = 0 \end{matrix}

(29)

采用在全局坐标系下定义的角度误差 $\tilde{θ}$ ，即

_{I}^{W} R \approx (I + ⌊ \tilde{θ} ⌋_{\times})_{I}^{W} \hat{R}

(30)

结合式（4），式（29）和式（30），误差状态动力学方程表示为

\begin{matrix} _{I}^{W} \dot{\tilde{p}} =_{I}^{W} \tilde{v} \\ _{I}^{W} \dot{\tilde{v}} = - {⌊_{I}^{W} R (a_{m} - a_{b})⌋}_{\times} \tilde{θ} -_{I}^{W} R {\tilde{a}}_{b} -_{I}^{W} R n_{a} \\ \dot{\tilde{θ}} = -_{I}^{W} R {\tilde{ω}}_{b} -_{I}^{W} R n_{ω} \\ {\dot{\tilde{a}}}_{b} = n_{b a} \\ {\dot{\tilde{ω}}}_{b} = n_{b ω} \end{matrix}

(31)

在实际应用中，连续形式的微分方程需要被离散化. 通过采用欧拉数值积分传播（29）中的名义状态和（31）中的误差状态，离散形式的误差状态和相应的协方差可以表示为

{\tilde{x}}_{k + 1} = F_{x} {\tilde{x}}_{k} + G_{x} n

(32)

P_{k + 1} = F_{x} P_{k} F_{x}^{T} + G_{x} Q G_{x}^{T}

(33)

其中， ${\tilde{x}}_{k}$ 和P_k分别表示第k时刻的状态和协方差， ${\tilde{x}}_{k + 1}$ 和 $P_{k + 1}$ 分别表示第k +1时刻的状态和协方差. 式（32）和式（33）中，F_x是系统的状态转移矩阵，表示为

F_{x} = [\begin{matrix} I & I Δ t & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I & - {⌊_{I}^{W} R (a_{m} - a_{b})⌋}_{\times} Δ t & -_{I}^{W} R Δ t & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & I & 0_{3 \times 3} & -_{I}^{W} R Δ t \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I \end{matrix}]

(34)

其中Δt表示采样间隔. G_x为扰动雅克比矩阵，表达式为

G_{x} = [\begin{matrix} 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ I & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & I & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I \end{matrix}]

(35)

式（32）中的n是施加在速度、姿态和偏差上的随机脉冲，由白高斯噪声建模，Q是噪声协方差矩阵，这两项可在IMU的数据手册中获得.
利用IMU对机器人的状态进行预测后，通过里程计和UWB测量对状态进行更新. 根据能观测性分析，采用三个UWB锚点. 结合式（5）和式（6），系统的观测方程为
$y = z (x) + v = [\begin{matrix} {∥\underset{A_{1}}{W} p -_{I}^{W} p∥}_{2} \\ {∥\underset{A_{2}}{W} p -_{I}^{W} p∥}_{2} \\ {∥\underset{A_{3}}{W} p -_{I}^{W} p∥}_{2} \end{matrix}] + v$

v = {[n_{v}^{T}, n_{d_{1}}, n_{d_{2}}, n_{d_{3}}]}^{T}

(36)

其中v是均值为零、协方差为V的高斯白噪声，即 $v \sim N {0 ， V}$ . 计算式（36）的测量雅克比矩阵，可以得到

H = \frac{\partial y}{\partial x} \frac{\partial x}{\partial \tilde{x}} = [\begin{matrix} H_{1} \\ H_{2} \end{matrix}]

(37)

这里，对应里程计测量的雅克比矩阵H₁为

\begin{matrix} H_{1} = \frac{\partial v}{\partial x} \frac{\partial x}{\partial \tilde{x}} = \\ [\begin{matrix} 0_{3 \times 3} & _{W}^{I} \hat{R} & _{W}^{I} \hat{R} {⌊_{I}^{W} \hat{v}]}_{\times} & 0_{3 \times 3} & 0_{3 \times 3} \end{matrix}] \end{matrix}

(38)

其中，线速度测量对角度误差的导数计算如下:

\begin{matrix} \frac{\partial (_{W}^{I} R_{I}^{W} v)}{\partial \tilde{θ}} = \frac{\partial ({((I + ⌊ \tilde{θ} ⌋_{\times})_{I}^{W} \hat{R})}^{- 1}_{I}^{W} \hat{v})}{\partial \tilde{θ}} = \\ \frac{\partial (_{W}^{I} \hat{R} (I - ⌊ \tilde{θ} ⌋_{\times})_{I}^{W} \hat{v})}{\partial \tilde{θ}} = \\ \frac{\partial (-_{W}^{I} \hat{R} ⌊ \tilde{θ} ⌋_{\times}_{I}^{W} \hat{v})}{\partial \tilde{θ}} = \frac{\partial (_{W}^{I} \hat{R} {⌊_{I}^{W} \hat{v}⌋}_{\times} \tilde{θ})}{\partial \tilde{θ}} = \\ _{W}^{I} \hat{R} {⌊_{I}^{W} \hat{v}⌋}_{\times} \end{matrix}

(39)

对应UWB测量的雅克比矩阵H₂为

\begin{matrix} H_{2} = [\begin{matrix} H_{2}^{1} \\ H_{2}^{2} \\ H_{2}^{3} \end{matrix}], H_{2}^{i} = \frac{\partial d_{i}}{\partial x} \frac{\partial x}{\partial \tilde{x}} = \\ [\frac{{(_{I}^{W} \hat{p} -_{A_{i}}^{W} p)}^{T}}{{∥{(_{I}^{W} \hat{p} -_{A_{i}}^{W} p)}^{T}∥}_{2}} O_{1 \times 12}] \end{matrix}

(40)

此时，卡尔曼增益为

K = P H^{T} {(H P H^{T} + V)}^{- 1}

(41)

误差状态以及协方差矩阵的更新为

\tilde{x} = K (y - z (\hat{x}))

(42)

P = (I - K H) P

(43)

最后，将误差状态与名义状态合并，得到状态的估计值

x = \hat{x} \oplus \tilde{x}

(44)

其中 $\oplus$ 表示一种合并运算（即四元数相乘，其余状态量相加）. 在将误差状态与名义状态合并之后，需要将误差量重设为0，并更新协方差.
4 实验结果与验证
在这部分，本文通过MATLAB仿真验证了提出的超宽带-惯导-里程计融合位姿估计方法的表现和精度.将该方法与传统UWB-IMU融合方法进行对比，展示融合里程计测量和非完整约束隐含的伪测量对于轮式机器人定位精度的提升. 同时，进一步验证能观测性理论中得出的局部弱可观条件. 采用位置和姿态的均方根误差（RMSE）作为评估精度的指标. 仿真中设置的传感器噪声等参数符合市场上实际传感器的参数特性.
4.1 定位性能比较
在环境中设置三个不共线的UWB锚点，其位置分别为（10，0，5），（-15，-5，5），（0，12，2.5）. UWB距离测量受到零均值、标准差为5cm的高斯白噪声腐蚀. UWB测量频率为50Hz. IMU的角速度噪声密度为1.7×10^-4 rad/s/ $\sqrt{H z}$ ，角速度随机游走为5×10^-5 rad/s²/ $\sqrt{H z}$ ，加速度噪声密度为2.94×10^-3 m/s²/ $\sqrt{H z}$ ，加速度随机游走为5×10^-4 rad/s³/ $\sqrt{H z}$ . 测量频率为100Hz. 这些参数参考了市场上低成本商用级的IMU的数据手册^[23]. 编码器的速度测量受到零均值、标准差为0.05m/s的白噪声干扰. 图3定性地展示了针对轨迹TR1基于UIW-ESKF方法和基于UI-ESKF方法的位置估计结果. 与真实值相对比，可以看出基于UIW-ESKF方法的位置估计精度明显优于UI-ESKF方法. 此外，还对其他的多个运动轨迹进行了定位测试，如图4所示，UIW-ESKF方法的结果相比于UI-ESKF方法均展现出更优的位置估计表现. 为了进行定量比较，本文计算出两种方法通过50次蒙特卡洛仿真后位置和旋转角度的RMSE. 表1详细列举了应用两种方法在不同运动轨迹下机器人位置和旋转角度的RMSE，可以看出基于UIW-ESKF方法的位姿估计精度均优于传统的UI-ESKF方法，且在姿态估计精度上有了明显的提高，这得益于里程计和非完整约束对速度的有效观测.
图3 不同方法下的轨迹TR1的位置估计结果作为参考，真实轨迹（Ground truth，GT）也被提供
Fig.3 Position estimation results based on different approaches of TR1. For reference, the Ground truth (GT) is also provided
图4 不同运动轨迹的状态估计表现. 黑线表示真实轨迹，绿线表示基于UIW-ESKF方法的估计轨迹，蓝线表示基于UI-ESKF方法的估计轨迹
Fig.4 State estimation performance of different motion trajectories. The black curves represent the true trajectories, the green curves represent the estimated trajectories based on the UIW-ESKF method, and the blue curves represent the estimated trajectories based on the UI-ESKF method
4.2 能观测性条件验证
为了验证系统的能观测性条件，笔者进行了多组实验，每组实验仅违背其中一个局部弱可观条件.
首先对轨迹TR1进行位姿估计测试. 由于轨迹TR1本身满足定理1的条件（c），图5（a）~（c）展示了不满足定理1中条件（a）和（b）的位姿估计表现. 图5（a）的实验中三个UWB锚点共线，图5（b）的实验中机器人与三个非共线锚点共面，图5（c）的实验中使用了2个UWB锚点. 为便于比较，图5（d）给出了满足所有能观性条件下采用UIW-ESKF估计器的定位表现.可以看出，实验（a）~（c）的位置估计均不能精确地收敛到真实轨迹. 表2列出了每组实验计算出的位置和旋转RMSE. 另外，尽管从几何的角度，当机器人与三个非共线锚点共面时，其状态量中的位置是局部可观的，但此时，锚点与机器人组成的几何形状会导致较高的几何精度衰减因子（GDOP）. 这与使用GPS的卫星导航定位系统类似，在相同的测量误差精度下，GPS接收机与卫星之间组成的几何形状越健壮（strong），例如由四个卫星和接收机组成的金字塔形状体积越大，GDOP的值越小，对误差的放大作用越小，定位精度越高.由于机器人与锚点共面导致了较大GDOP，使得机器人在z轴上反映出较大的定位误差. 表2列出了每组实验经过50次蒙特卡洛仿真后计算出的估计位姿与真实位姿相比的RMSE.
表1 UIW-ESKF与UI-ESKF方法的估计状态与真实状态相比的位置和姿态均方根误差
Table1 Position and orientation RMSE between estimated states and true states based on the UIW-ESKF method and the UI-ESKF method
图5 在不同实验条件下的位置估计结果.（a）三个锚点共线，（b）机器人与三个非共线锚点共面，（c）使用两个锚点，（d）所有条件均满足.黑线代表真实值，蓝线代表估计值
Fig.5 Position estimation results under different experimental conditions. (a) The three anchors are collinear. (b) The robot is coplanar with the three non-collinear anchors. (c) Two anchors are used. (d) All observability conditions are satisfied. The black curves represent the true values and the blue curves represent the estimated values
为了验证定理1中的条件（c），本文对匀速直线运动的移动机器人进行位姿估计，该运动轨迹不存在沿运动切线方向（即IMU坐标系x轴）的加速度a_x和沿运动法线方向（即IMU坐标系y轴）的向心加速度a_y，仅在z轴存在重力加速度的激励. 机器人在加速度欠激励下的状态估计表现如图6所示. 此时估计的轨迹与真实轨迹相比，位置RMSE为0.068 m，姿态RMSE为0.207 rad. 尽管该轨迹与图3中的轨迹不同，位姿估计的误差无法与TR1轨迹的误差进行严格比较，但可以大致看出，匀速直线运动的位置估计结果具有较高的精度，而由于缺乏三轴加速度的激励，姿态估计误差较大.
表2 不同实验条件下基于UIW-ESKF方法的位姿估计值与真实值的均方根误差
Table2 Position and orientation RMSE between estimated states and true states based on the UIW-ESKF method under different experimental conditions
图6 加速度欠激励时的状态估计表现
Fig.6 State estimation performance when some accelerations are unexcited
5 结论
首先，本文针对具有非完整约束的轮式机器人提出了一种融合超宽带、惯导和里程计的定位方法. 该方法在传统超宽带和惯导组合定位系统的基础上，结合了里程计的线速度测量和由非完整约束引入的伪测量，并利用ESKF进行多传感器融合，提高了机器人位姿估计的精度，尤其是极大地降低了姿态估计的误差. 其次，本文对由这三个传感器组成的系统进行了能观测性分析，通过基于李导数的能观性秩条件分析方法进行了详细的理论分析与数学证明，得出了该系统局部弱可观的条件. 最后，仿真实验验证了当满足能观性条件时，本文提出的状态估计方法能有效且精确地估计出机器人的位姿，且相比传统方法具有更高的位姿估计精度.
参考文献
- [1] Liu J,Cai B G,Wang J.Cooperative localization of connected vehicles:Integrating GNSS with DSRC using a robust cubature Kalman filter.IEEE Transactions on Intelligent Transportation Systems,2017,18(8):2111~2125
- [2] Huang B,Yang R,Jia B,et al.A theoretical analysis on sampling size in WiFi fingerprint-based localization.IEEE Transactions on Vehicular Technology,2021,70(4):3599~3608
- [3] Giannelos E,Andrianakis E,Skyvalakis K,et al.Robust RFID localization in multipath with phase-based particle filtering and a mobile robot.IEEE Journal of Radio Frequency Identification,2021,5(3):302~310
- [4] 许承宇,徐绍凯.基于视觉延时补偿的无人机室内实时导航系统.动力学与控制学报,2022,20(1):78~84(Xu C Y,Xu S K.UAV indoor real-time navigation system based on visual delay compensation.Journal of Dynamics and Control,2022,20(1):78~84(in Chinese)).
- [5] Chen X,Vizzo I,Läbe T,et al.Range image-based LiDAR localization for autonomous vehicles.Proceedings-IEEE International Conference on Robotics and Automation,2021,2021-May(Icra):5802~5808
- [6] Shule W,Almansa C M,Queralta J P,et al.UWB-based localization for multi-UAV systems and collaborative heterogeneous multi-robot systems.Procedia Computer Science,2020,175(2019):357~364
- [7] Martalo M,Perri S,Verdano G,et al.Improved UWB TDoA-based positioning using a single hotspot for industrial IoT applications.IEEE Transactions on Industrial Informatics,2022,18(6):3915~3925
- [8] Li J,Bi Y,Li K,et al.Accurate 3D localization for MAV swarms by UWB and IMU fusion.IEEE International Conference on Control and Automation ICCA 2018,2018-June:100~105
- [9] Goudar A,Schoellig A P.Online spatio-temporal calibration of tightly-coupled ultrawideband-aided inertial localization.IEEE International Conference on Intelligent Robots and Systems,2021:1161~1168
- [10] You W,Li F,Liao L,et al.Data fusion of UWB and IMU based on unscented Kalman filter for indoor localization of quadrotor UAV.IEEE Access,2020,8:64971~64981
- [11] Solà J.Quaternion kinematics for the error-state Kalman filter.2017
- [12] Li M G,Zhu H,You S Z,et al.UWB-based localization system aided with inertial sensor for underground coal mine applications.IEEE Sensors Journal,2020,20(12):6652~6669
- [13] Cao Y,Yang C,Li R,et al.Accurate position tracking with a single UWB anchor.Proceedings-IEEE International Conference on Robotics and Automation,2020:2344~235.
- [14] Brossard M,Barrau A,Bonnabel S.RINS-W:robust inertial navigation system on wheels.IEEE International Conference on Intelligent Robots and Systems,2019:2068~2075
- [15] Wang Y,Li X.The IMU/UWB fusion positioning algorithm based on a particle filter.ISPRS International Journal of Geo-Information,2017,6(8):235
- [16] Kelly J,Sukhatme G S.Visual-inertial sensor fusion:Localization,mapping and sensor-to-sensor self-calibration.International Journal of Robotics Research,2011,30(1):56~79
- [17] Goudar A,Pereida K,Schoellig A P.Ultra-wideband aided inertial navigation:observability analysis and sensor extrinsics calibration.IEEE International Conference.Conf.on Intelligent Robots and Systems,2020
- [18] Xia X,Hashemi E,Xiong L,et al.Autonomous vehicles sideslip angle estimation:single antenna GNSS/IMU fusion with observability analysis.IEEE Internet of Things Journal,2021,8(19):14845~14859
- [19] Araki B,Gilitschenski I,Ogata T,et al.Range-based cooperative localization with nonlinear observability analysis.IEEE Intelligent Transportation Systems Conference,ITSC 2019,2019:1864~1870
- [20] Shi Q,Cui X,Zhao S,et al.Range-only collaborative localization for ground vehicles.Proceedings of the 32nd International Technical Meeting of the Satellite Division of the Institute of Navigation,ION GNSS+ 2019,2019:2063~2077
- [21] Fontanelli D,Shamsfakhr F,MacIi D,et al.An uncertainty-driven and observability-based State estimator for nonholonomic robots.IEEE Transactions on Instrumentation and Measurement,2021:70
- [22] Hermann R,Krener A J.Nonlinear controllability and observability.IEEE Transactions on Automatic Control,1977,22(5):728~740
- [23] MPU-9250 product specification Revision 1.1,InvenSense,June 20,2016.[Online].Available:https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf

基本信息

中图分类号: O313.7;TP242.6
文献标识码: A
DOI: 10.6052/1672-6553-2022-042
文章编号: 1672-6553-2022-20(6)-064-12

基金信息

国家重点研发计划“智能机器人”重点专项(2020YFB1312900)；
装备预研教育部联合基金 (8091B032150)；
国家自然科学基金资助项目(11932015)；
This research was supported by the National Key Research and Development Program of China (2020YFB1312900)；
the Joint Fund of Ministry of Education for Equipment Pre-research (8091B032150)；
the National Natural Science Foundation of China (11932015)；

引用信息

稿件历史

收稿日期: 2022-08-18

参考文献

[1] Liu J,Cai B G,Wang J.Cooperative localization of connected vehicles:Integrating GNSS with DSRC using a robust cubature Kalman filter.IEEE Transactions on Intelligent Transportation Systems,2017,18(8):2111~2125
[2] Huang B,Yang R,Jia B,et al.A theoretical analysis on sampling size in WiFi fingerprint-based localization.IEEE Transactions on Vehicular Technology,2021,70(4):3599~3608
[3] Giannelos E,Andrianakis E,Skyvalakis K,et al.Robust RFID localization in multipath with phase-based particle filtering and a mobile robot.IEEE Journal of Radio Frequency Identification,2021,5(3):302~310
[4] 许承宇,徐绍凯.基于视觉延时补偿的无人机室内实时导航系统.动力学与控制学报,2022,20(1):78~84(Xu C Y,Xu S K.UAV indoor real-time navigation system based on visual delay compensation.Journal of Dynamics and Control,2022,20(1):78~84(in Chinese)).
[5] Chen X,Vizzo I,Läbe T,et al.Range image-based LiDAR localization for autonomous vehicles.Proceedings-IEEE International Conference on Robotics and Automation,2021,2021-May(Icra):5802~5808
[6] Shule W,Almansa C M,Queralta J P,et al.UWB-based localization for multi-UAV systems and collaborative heterogeneous multi-robot systems.Procedia Computer Science,2020,175(2019):357~364
[7] Martalo M,Perri S,Verdano G,et al.Improved UWB TDoA-based positioning using a single hotspot for industrial IoT applications.IEEE Transactions on Industrial Informatics,2022,18(6):3915~3925
[8] Li J,Bi Y,Li K,et al.Accurate 3D localization for MAV swarms by UWB and IMU fusion.IEEE International Conference on Control and Automation ICCA 2018,2018-June:100~105
[9] Goudar A,Schoellig A P.Online spatio-temporal calibration of tightly-coupled ultrawideband-aided inertial localization.IEEE International Conference on Intelligent Robots and Systems,2021:1161~1168
[10] You W,Li F,Liao L,et al.Data fusion of UWB and IMU based on unscented Kalman filter for indoor localization of quadrotor UAV.IEEE Access,2020,8:64971~64981
[11] Solà J.Quaternion kinematics for the error-state Kalman filter.2017
[12] Li M G,Zhu H,You S Z,et al.UWB-based localization system aided with inertial sensor for underground coal mine applications.IEEE Sensors Journal,2020,20(12):6652~6669
[13] Cao Y,Yang C,Li R,et al.Accurate position tracking with a single UWB anchor.Proceedings-IEEE International Conference on Robotics and Automation,2020:2344~235.
[14] Brossard M,Barrau A,Bonnabel S.RINS-W:robust inertial navigation system on wheels.IEEE International Conference on Intelligent Robots and Systems,2019:2068~2075
[15] Wang Y,Li X.The IMU/UWB fusion positioning algorithm based on a particle filter.ISPRS International Journal of Geo-Information,2017,6(8):235
[16] Kelly J,Sukhatme G S.Visual-inertial sensor fusion:Localization,mapping and sensor-to-sensor self-calibration.International Journal of Robotics Research,2011,30(1):56~79
[17] Goudar A,Pereida K,Schoellig A P.Ultra-wideband aided inertial navigation:observability analysis and sensor extrinsics calibration.IEEE International Conference.Conf.on Intelligent Robots and Systems,2020
[18] Xia X,Hashemi E,Xiong L,et al.Autonomous vehicles sideslip angle estimation:single antenna GNSS/IMU fusion with observability analysis.IEEE Internet of Things Journal,2021,8(19):14845~14859
[19] Araki B,Gilitschenski I,Ogata T,et al.Range-based cooperative localization with nonlinear observability analysis.IEEE Intelligent Transportation Systems Conference,ITSC 2019,2019:1864~1870
[20] Shi Q,Cui X,Zhao S,et al.Range-only collaborative localization for ground vehicles.Proceedings of the 32nd International Technical Meeting of the Satellite Division of the Institute of Navigation,ION GNSS+ 2019,2019:2063~2077
[21] Fontanelli D,Shamsfakhr F,MacIi D,et al.An uncertainty-driven and observability-based State estimator for nonholonomic robots.IEEE Transactions on Instrumentation and Measurement,2021:70
[22] Hermann R,Krener A J.Nonlinear controllability and observability.IEEE Transactions on Automatic Control,1977,22(5):728~740
[23] MPU-9250 product specification Revision 1.1,InvenSense,June 20,2016.[Online].Available:https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf

分享给微信好友或者朋友圈

使用微信“扫一扫”功能。

具有非完整约束移动机器人的超宽带-惯导-里程计融合定位与能观测性分析*

UWB-IMU-ODOMETER FUSION LOCALIZATION AND OBSERVABILITY ANALYSIS FOR MOBILE ROBOTS WITH NONHOLONOMIC CONSTRAINTS*

摘要

Abstract

关键词

Keywords

1 系统建模

(1)

1.1 IMU测量模型

(2)

(3)

(4)

1.2 里程计测量模型

(5)

1.3 UWB测量模型

(6)

2 能观测性分析

2.1 非线性能观性的定义

(7)

2.2 系统能观性判断

(8)

(9)

(10)

(11)

2.2.1 零阶李导数及其梯度:

(12)

(13)

(14)

2.2.2 一阶李导数及其梯度

(15)

(16)

2.2.3 二阶李导数及其梯度

(17)

(18)

(19)

(20)

(21)

2.2.4 三阶李导数及其梯度

(22)

(23)

(24)

(25)

(26)

(27)

(28)

3 基于ESKF的状态估计

(29)

(30)

(31)

(32)

(33)

(34)

(35)

(36)

(37)

(38)

(39)

(40)

(41)

(42)

(43)

(44)

4 实验结果与验证

4.1 定位性能比较

4.2 能观测性条件验证

5 结论

参考文献

基本信息

基金信息

引用信息

稿件历史

参考文献

WeChat

Mobile website

具有非完整约束移动机器人的超宽带-惯导-里程计融合定位与能观测性分析^*

UWB-IMU-ODOMETER FUSION LOCALIZATION AND OBSERVABILITY ANALYSIS FOR MOBILE ROBOTS WITH NONHOLONOMIC CONSTRAINTS^*