CN112703367B - 使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工具导航辅助方法及装置 - Google Patents

使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工具导航辅助方法及装置 Download PDF

Info

Publication number
CN112703367B
CN112703367B CN201980053365.1A CN201980053365A CN112703367B CN 112703367 B CN112703367 B CN 112703367B CN 201980053365 A CN201980053365 A CN 201980053365A CN 112703367 B CN112703367 B CN 112703367B
Authority
CN
China
Prior art keywords
carrier
coordinate system
vector
kalman filter
reference coordinate
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
CN201980053365.1A
Other languages
English (en)
Other versions
CN112703367A (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.)
Safran Electronics and Defense SAS
Original Assignee
Safran Electronics and Defense SAS
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 Safran Electronics and Defense SAS filed Critical Safran Electronics and Defense SAS
Publication of CN112703367A publication Critical patent/CN112703367A/zh
Application granted granted Critical
Publication of CN112703367B publication Critical patent/CN112703367B/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/51—Relative positioning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/40—Control within particular dimensions
    • G05D1/46—Control of position or course in three dimensions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/211—Selection of the most significant subset of features
    • G06F18/2113—Selection of the most significant subset of features by ranking or filtering the set of features, e.g. using a measure of variance or of feature cross-correlation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F7/00—Methods or arrangements for processing data by operating upon the order or content of the data handled
    • G06F7/38—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation
    • G06F7/40—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation using contact-making devices, e.g. electromagnetic relay
    • G06F7/405—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation using contact-making devices, e.g. electromagnetic relay binary
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F7/00—Methods or arrangements for processing data by operating upon the order or content of the data handled
    • G06F7/38—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation
    • G06F7/40—Methods or arrangements for performing computations using exclusively denominational number representation, e.g. using binary, ternary, decimal representation using contact-making devices, e.g. electromagnetic relay
    • G06F7/42—Adding; Subtracting
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/0047—Navigation or guidance aids for a single aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Computing Systems (AREA)
  • Evolutionary Computation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种用于辅助第一交通工具(1)导航的方法，第一交通工具相对于在参考坐标系中可移动的第二交通工具(2)静止，所述方法包括：通过至少一个本体感受传感器(6)获取(112)第一交通工具(1)的移动数据；以及通过不变卡尔曼滤波器使用第二交通工具(2)的导航状态(X2)作为观测值，估计第一交通工具(1)的导航状态(X1)，第一交通工具(1)的导航状态包括：表示将与第一交通工具(1)相关联的第一位置标记关联到参考坐标系的刚性变换(T1)的变量；以及表示将与第二交通工具(2)相关联的位置标记关联到第一位置标记的刚性变换(T21)的变量，不变卡尔曼滤波器使用包括这两个刚性变换的逐项组合的法则作为内部组合法则。

Description

使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工 具导航辅助方法及装置
技术领域
本发明涉及一种使用不变卡尔曼滤波器辅助载体导航的方法。
背景技术
估计物理系统的状态的问题通常如下所示。系统在时刻n的状态由向量Xn表示，以及在时刻n可用的观测值由另一个向量Yn表示。系统的演化写成这样：
Xn+1＝f(Xn)，
其中，f是已知函数(通常称为传播函数)，可取决于传感器的测量值。观测值Yn通过已知的观测函数h与系统的状态相关：
Yn＝h(Xn)。
从序列(Yn)n≥0建立状态Xn的良好估计
Figure GDA0003351088450000011
通常是个难题，但是在某些情况下可得以简化。
“线性系统”是指具有以下形式的系统的特殊情况：
Xn+1＝FXn+wn，
Yn＝HXn+Vn，
其中，F是传播矩阵，H是观测矩阵，wn和Vn是干扰预测和测量的噪声。
在这种线性情况下，已知的方法包括建立称为“卡尔曼滤波器”的估计器。该卡尔曼滤波器实现以下计算：
Figure GDA0003351088450000012
Figure GDA0003351088450000013
其中，指数n+1|n和n+1|n+1分别表示在不考虑观测值Yn+1的情况下，在n+1时刻计算的估计值，以及在考虑观测值Yn+1的情况下，在n+1时刻计算的估计值。矩阵Kn称为“增益矩阵”，其可以使用Riccati(黎卡提)方程来计算。于是估计误差定义为：
Figure GDA0003351088450000021
(在考虑观测值Yn之后)，
Figure GDA0003351088450000022
(在考虑观测值Yn+1之前)。
可以容易地验证该误差遵循以下演化：
en+1|n＝Fen|n(在考虑观测值Yn+1之前)，
en+1|n+1＝(I-Kn+1H)en+1|n(在考虑观测值Yn+1之后)，其中，I表示单位矩阵。
上述方程不依赖于Xn，因此可以建立对系统的任何实际轨迹都有效的估计器，而对于任何非线性系统则不是这种情况。
在非线性系统的情况下，普通的卡尔曼滤波器无法实现。因此，提出了一种卡尔曼滤波器变型，称为“扩展”卡尔曼滤波器，适用于非线性系统。然而，当使用扩展卡尔曼滤波器时，在线性情况下观测的简化不再发生，因此得到涉及Xn和
Figure GDA0003351088450000023
的误差方程。这个问题是使用扩展卡尔曼滤波器时遇到的大多数发散情况的根源。
然而，使估计问题更简单的第二种特殊情况是“仿射群”观测系统的情况，也就是说，针对该系统，二元运算(即，在下文中将用星号*表示的运算)定义在所考虑的状态空间上，使得以下两个特性得到验证。
a.传播函数f验证状态空间的任何一对元素a，b的关系：
f(a*b)＝f(a)*f(Id)-1*f(b)，
其中，Id是运算*引起的群的单位元素。
b.观察函数h的形式为h(X)＝l(x，y0)，其中y0是(Yn所属的)观测空间中的元素，以及l(.，.)是群动作，即函数，其用于验证：
l(a*b，y)＝l(a，l(b，y))。
在这两个条件下，可以定义称为“不变”的扩展卡尔曼滤波器(通常更简单地称为“不变卡尔曼滤波器”)，不变卡尔曼滤波器由以下方程控制：
Figure GDA0003351088450000024
Figure GDA0003351088450000031
其中，exp(·)是指数映射(如果定义了李群(Lie group)，则一旦二元运算已知，该函数就已知)，Kn是如线性情况下的“增益矩阵”。这样就可以表明估计误差也会像在线性情况下那样具有自主的演化。因此，即使所考虑的系统不是线性的，估计状态的问题也得以简化。
当条件b未得到验证时，可以使用滤波器，其形式为：
Figure GDA0003351088450000032
Figure GDA0003351088450000033
在实践中，通常在不满足条件a的系统上实现卡尔曼滤波器，但是该系统近似于满足该条件a的系统。
因此，不变卡尔曼滤波器用于载体的导航辅助。在这种导航辅助的环境中使用的不变卡尔曼滤波器估计表示所考虑的载体的运动的导航状态。
使用不变卡尔曼滤波器需要找到一种二元运算*，其条件a和b得到验证或几乎被验证，以使估计问题更简单。找到这种操作不存在通用方法，各种出版物都旨在为特定系统提供正确的运算。例如：
a)下面的文献描述了用于姿态和速度估计问题的运算：Bonnabel，S.，Martin，P.和Salaün，E.(2009)，不变扩展卡尔曼滤波器：用于速度辅助的姿态估计问题的理论与应用，发表于2009年举办的决策与控制会议，其与2009第28届中国控制会议联合召开，CDC/CCC 2009，第48届IEEE会议的记录，在(第1297-1304页)，IEEE。
b)下面的文献描述了用于惯性导航、估计姿态、速度和位置的运算：Barrau，A.和Bonnabel，S.(2017)，作为稳定观测器的不变扩展卡尔曼滤波器，IEEE自动控制学报，62(4)，1797-1812。
c)Barrau，A.和Bonnabel，S.在2015年发表的文献“一种具有一致属性的EKF-SLAM算法”中描述了用于SLAM(即时定位和地图构建)的操作，也就是说，使用载体环境中的固定参考点进行导航，arXiv preprintarXiv：1510.06263。
d)Barrau，A.和Bonnabel，S.在2017年发表的文献“分组线性观测系统(I)”中进一步描述了其操作已知的实际系统的列表。
在这些应用中的每个中，载体的本体感受传感器和不变卡尔曼滤波器用于估计该载体的导航状态。
这些应用有两种类型。其中一些应用使用在参考坐标系中表示的运动数据，载体在该参考坐标系中可移动。然而，这些运动数据并非始终可用。其他应用不使用这样的运动数据，但是另一方面，不能估计载体在参考坐标系中的导航状态；这些应用只能产生与初始情况相关的导航状态。
发明内容
本发明的目的是克服上述缺点。
因此，根据本发明的第一方面，提出了一种用于辅助第一载体导航的方法，第一载体相对于本身能够在参考坐标系中移动的第二载体静止，方法包括由第一载体实现的以下步骤：
·接收由第二载体提供的、在第二载体的参考坐标系中的导航状态；
·通过第一载体的至少一个本体感受传感器获取第一载体的移动数据；
·通过不变卡尔曼滤波器使用第二载体的导航状态作为观测数据，估计第一载体的导航状态，其中，第一载体的导航状态包括：
о第一变量，表示将附接于第一载体的坐标系关联到参考坐标系的第一刚性变换；以及
о第二变量，表示将附接于第二载体的坐标系关联到附接于第一载体的坐标系的第二刚性变换，
其中，不变卡尔曼滤波器使用包括第一刚性变换和第二刚性变换的逐项组合的运算作为二元运算。
所提出的方法有利地利用了以下事实：第一载体相对于第二载体静止，而该第二载体的估计的导航状态是已知的，构成绝对性质的信息，也就是说，相对于参考坐标系，第二载体在该参考坐标系中可移动。
即使第一载体不知道其相对于第二载体的位置(换句话说，当第一载体和第二载体未协调时)，该方法也具有起作用的优点。
所提出的方法还证明，该方法中使用的二元运算使得能够近似在背景技术中列出的条件a和b。因此，由不变卡尔曼滤波器实现的计算特别容易实现。
根据本发明的第一方面的方法可以进一步包括以下可选的特征，当技术上可行时，可以单独或组合地采用以下可选的特征。
优选地，第一变量包括表示第一载体的姿态的旋转矩阵，以及第二变量包括表示第二载体相对于第一载体的姿态的旋转矩阵。
优选地，第一变量包括第一载体的位置向量，以及第二变量包括第二载体相对于第一载体的位置向量。
优选地，所估计的第一载体的导航状态进一步包括第一载体在参考坐标系中的速度向量。
优选地，二元运算将相同的变换应用于位置向量之一以及速度向量。
优选地，所接收的第二载体的导航状态包括：第二载体在参考坐标系中的位置向量和表示第二载体在参考坐标系中的姿态的旋转矩阵。
优选地，不变卡尔曼滤波器使用创新，该创新包括具有以下形式的向量Zx：
Figure GDA0003351088450000051
其中：
·
Figure GDA0003351088450000052
是第一变量的估计值；
·
Figure GDA0003351088450000053
是第二变量的估计值，其中，第二刚性变换使得能够从第一坐标系切换到第二坐标系；
·Y表示不变卡尔曼滤波器使用的观测数据；
·
Figure GDA0003351088450000054
表示组合；
·log(·)表示李群的对数映射。
优选地，所接收的第二载体的导航状态包括：第二载体在参考坐标系中的速度向量；和表示第二载体在参考坐标系中的姿态的旋转矩阵；由本体感受传感器获取的第一载体的移动数据包括第一载体的角速度。
优选地，第一变量包括表示第一载体的姿态的旋转矩阵，以及第二变量包括表示第二载体相对于第一载体的姿态的旋转矩阵；不变卡尔曼滤波器使用创新，该创新包括具有以下形式的向量Zv：
Figure GDA0003351088450000061
其中：
·
Figure GDA0003351088450000062
是表示第一载体的姿态的旋转矩阵的估计值；
·
Figure GDA0003351088450000063
是表示第二载体相对于第一载体的姿态的旋转矩阵的估计值；
·
Figure GDA0003351088450000064
是限定第二刚性变换的平移向量的估计值；
·R2是表示第二载体在参考坐标系中的姿态的旋转矩阵；
·v2是第二载体在参考坐标系中的速度向量；
·ω是第一载体的角速度；
·×表示向量积；
·log(·)表示李群SE(3)的对数映射。
优选地，向量Zv包括表示旋转的大小为3的第一向量
Figure GDA0003351088450000065
以及大小同样为3的第二向量
Figure GDA0003351088450000066
其中，不变卡尔曼滤波器使用具有以下形式的创新Z：
Figure GDA0003351088450000067
优选地，所估计的第一载体的导航状态进一步包括：本体感受传感器所特有的至少一个误差状态，对于本体感受传感器所特有的该误差变量，二元运算是相加。
优选地，第一载体是飞行器，以及第二载体是飞行器载体；或者第一载体是炮弹，以及第二载体是运载炮弹的飞行器；或者第一载体是交通工具的惯性单元，以及第二载体是交通工具的外感受性传感器。
优选地，估计的步骤由多个不变卡尔曼滤波器并行实现，以获得第一载体的导航状态的多个估计值，以及方法进一步包括以下步骤：
·对于每个估计值，确定估计值的似然度量；
·基于估计值及其关联的似然度量，生成第一载体的导航状态的合并估计值。
根据本发明的第二方面，还提出了一种用于辅助第一载体导航的装置，第一载体相对于本身能够在参考坐标系中移动的第二载体静止，装置包括：
·至少一个本体感受传感器，配置为获取第一载体的移动数据；
·通信接口，用于接收由第二载体提供的第二载体的导航状态；
·不变卡尔曼滤波器，配置为通过使用第二载体的导航状态作为观测数据，
估计第一载体的导航状态，其中，导航状态包括：
о第一变量，表示将附接于第一载体的坐标系关联到参考坐标系的第一刚性变换；以及
о第二变量，表示将附接于第二载体的坐标系关联到附接于第一载体的坐标系的第二刚性变换，
其中，不变卡尔曼滤波器使用包括第一刚性变换和第二刚性变换的逐项组合的运算作为二元运算。
附图说明
本发明的其他特征、目的和优点将从下面的描述中显现出来，该描述仅是说明性的而非限制性的并应结合附图进行阅读，其中：
·图1示意性地示出了两个载体以及参考系。
·图2示意性地示出了根据本发明的一个实施例的图1的载体的内部组件。
·图3是根据本发明的一个实施例的导航辅助方法的步骤的流程图。
在所有附图中，相似的元件具有相同的附图标记。
具体实施方式
1、对系统的描述
参考图1，系统包括第一载体1和相对于第一载体1静止的至少一个第二载体2。
在下文中，考虑不同的参考系：附接于第一载体1的第一坐标系、附接于第二载体2的第二坐标系以及参考坐标系，两个载体1、2在参考坐标系中可移动。参考坐标系是例如附接于恒星或地球的天体坐标系。
在图1中，第二载体示意性地由平行六面体表示，以及第一载体示意性地由放置在第一载体上的较小尺寸的对象表示。然而，该表示仅是示意性的，且两个载体可以是各种类型。
在一个实施例中，第一载体是飞行器，例如飞机或直升机，以及第二载体是飞行器载体，例如放置飞机的飞机载体类型的船。
在另一实施例中，第一载体是炮弹，例如导弹，以及第二载体是运载炮弹的飞行器，例如飞机或直升机。
在另一实施例中，第一载体是交通工具(例如，地面车辆)的惯性单元，以及第二载体是相同交通工具的外感受性传感器。
参考图2，第一载体1包括与第二载体2进行数据通信的通信接口。通信接口4是无线电类型(例如，WI-FI、蓝牙)，并例如包括天线。作为变型，该接口是有线的。
第一载体1包括至少一个本体感受传感器6。所使用的每个本体感受传感器被配置为获取第一载体在第一坐标系中的移动数据。这些数据通常包括角速度和加速度。
本体感受传感器包括例如惯性单元，该惯性单元包括多个惯性传感器，例如陀螺仪和加速度计。
作为变型或此外，本体感受传感器包括至少一个里程表。
第一载体1进一步包括数据处理单元10。处理单元10被布置成处理由通信接口4接收的数据。
数据处理单元10通常包括至少一个处理器，该至少一个处理器被配置为通过不变卡尔曼滤波器来实现下文将描述的导航辅助方法。不变卡尔曼滤波器通常处于可由数据处理单元的处理器执行的计算机程序的形式。不变卡尔曼滤波器的通常操作本身是已知的。然而，下面将看到，由处理单元10实现的、用于配置不变卡尔曼滤波器的二元运算是以特定的方式来选择。
优选地，处理单元10适于并行实现多个卡尔曼滤波器。
此外，第二载体2包括用于估计该第二载体在参考坐标系中的导航状态的装置。这些装置本身是已知的。例如，可以使用Jay Farell于2008年出版的题为“辅助导航：具有高速率传感器的GPS”的文献中描述的装置。
第二载体2进一步包括与第一载体1进行通信的通信接口，该通信接口与通信接口X具有相同的类型。
2、不变卡尔曼滤波器的配置
由处理单元10实现的不变卡尔曼滤波器被配置为估计第一载体1在参考坐标系中的导航状态。
第一载体的导航状态包括：第一变量，表示将第一坐标系(附接于第一载体1)关联到参考坐标系的第一刚性变换；以及第二变量，表示将第二坐标系(附接于第二载体2)关联到第一坐标系的第二刚性变换。
以众所周知的方式，刚性变换(也称为仿射等距)是保留实体的每一对点之间的距离的变换。因此，第一刚性变换和第二刚性变换中的每一个可以通过旋转和平移的组合来表征。
在下文中，将详细描述实施例，其中，第一载体的表示为X1的导航状态包括以下元素：
X1＝(R1,v1,x1,R21,x21)，
其中：
·R1是旋转矩阵，表示第一载体在参考坐标系中的姿态(转换的旋转分量，其使得一个点能够从在第一坐标系中的坐标切换到相同载体点在参考坐标系中的坐标)；
·v1是速度向量，表示第一载体在参考坐标系中的速度；
·x1是位置向量，表示第一载体在参考坐标系中的位置(x1是转换的平移分量，其使得一个点能够从在第一坐标系中的坐标切换到相同点在参考坐标系中的坐标)；
·R21是旋转矩阵，表示第一坐标系和第二坐标系之间的相对姿态。例如，这里是使得能够从第一坐标系(附接于第一载体1)切换到第二坐标系(附接于第二载体2)的矩阵，反之也完全可以。
·x21是第一载体1和第二载体2之间的杠杆臂。杠杆臂是使得一个点能够从在第一坐标系中的坐标切换到相同点在第二坐标系中的坐标的平移向量。
在该特定实施例中，第一变量是R1、x1和v1，以及第二变量是R21、x21。
在下文中，认为第一刚性变换为T1＝(R1,x1)，第二刚性变换为T21＝(R21,x21)。
不变卡尔曼滤波器进一步被配置为将在参考坐标系中表示的第二载体2的导航状态X2用作观测数据。
第二载体2的导航状态通常包括：
·旋转矩阵R2，表示第二载体在参考坐标系中的姿态(刚性变换的旋转分量，使得一个点能够从在第二坐标系中的坐标切换到相同载体点在参考坐标系中的坐标)；
·位置向量x2，表示第二载体在参考坐标系中的位置(x2是刚性变换的平移分量，使得一个点能够从在第二坐标系中的坐标切换到相同载体点在参考坐标系中的坐标)。
可以设想，在这种状态下包括速度向量v2，速度向量v2表示第二载体在参考坐标系中的速度。然而，下面将考虑这种状态不包括这种速度。
那么，不变卡尔曼滤波器的观测值写成：
Y＝T2＝(R2,x2)。
不变卡尔曼滤波器被配置为将第一刚性变换和第二刚性变换的逐项组合用作二元运算(用*表示)。
当第一载体的速度向量也包括在第一载体的导航状态中时，该组合运算可扩展到第一载体的速度向量。在这种情况下，二元运算*将相同的变换应用于位置向量之一以及速度向量。
二元运算以以下方式应用于状态(R1,v1,x1,R21,x21)和(R′1,v,x′1,R′21,x′21)：
(R1,v1,x1,R21,x21)*(R′1,v′1,x′1,R′21,x′21)＝(R1R′1,v1+R1v′1,x1+R1x′1,R21R′21,x21+R21x′21)。
由于事实是通过该运算以相同的方式处理位置和速度，因此这里考虑到二元运算*的乘积的第二项和第三项具有相同的形式。
3、用于辅助第一载体导航的方法
参考图3，根据一个实施例的用于辅助第一载体1导航、并实现如第2节中指示的那样配置的不变卡尔曼滤波器的方法包括以下步骤。
假设已通过不变卡尔曼滤波器来估计第一载体1的导航状态的估计值
Figure GDA0003351088450000116
此外，假设第二载体已使用其内部装置12估计该第二载体在参考坐标系中的导航状态X2。该导航状态X2包括由对(R2,x2)形成的刚性变换T2。
在步骤102中，第一载体1的通信接口4从第二载体2的通信接口14接收表示刚性变换T2的数据。然后这些数据被发送到处理单元10。
在步骤104中，处理单元10以以下方式计算不变卡尔曼滤波器的创新Z：
Figure GDA0003351088450000111
其中：
·Y表示不变卡尔曼滤波器使用的观测数据(如前所述，对应于T2)；
·
Figure GDA0003351088450000112
表示刚性变换的组合运算；
·log()表示在本领域技术人员已知的李群理论的含义中的对数映射。
因为该创新计算使得能够满足在背景技术中列出的条件a和b，因此该创新计算特别有利。
在校正步骤106中，数据处理单元10将创新Z与被称为“增益”矩阵的矩阵K相乘，在线性校正dX1＝KZ中表示Z，以应用于系统状态X1。
增益的选择是大多数估计方法共有的经典问题(参见下文)。
在退缩步骤108中，处理单元10将线性校正dX1转换为与
Figure GDA0003351088450000113
具有相同性质的非线性校正C1(因为状态
Figure GDA0003351088450000114
包含旋转，因此状态
Figure GDA0003351088450000115
不是向量)。所使用的变换是将状态X1的维度(在本实例中为15)的向量作为参数并返回与X1具有相同性质的对象的任何函数，但是特别有效的选择是刚性变换对的李群的逐项指数化。
然后，由处理单元10实现非线性更新步骤110。在该步骤110中，处理单元10将系统状态的估计值X1与非线性校正C1相结合，以建立校正的估计值：
Figure GDA0003351088450000121
选择增益矩阵K，以稳定由以下等式定义的非线性估计误差e：
Figure GDA0003351088450000122
其中，符号.-1是与二元运算*相关联的常见反演。在该实施例中，获得估计误差e的左侧不变。当然可设想修改前面的等式以获得右侧不变(然而，左侧不变是优选的实施方式)。
在获取步骤112中，本体感受传感器3进一步获取第一载体1在第一坐标系中的移动数据。这些移动数据通常包括加速度和/或速度，例如角速度。这些获取的移动数据被发送到处理单元10。
步骤112可以在步骤102、104、106、108、110中的任一步骤之前、期间或之后实现。
在对于本领域技术人员而言本身是已知的传播步骤114中，处理单元10从状态
Figure GDA0003351088450000123
生成传播的导航状态。为此，处理单元10以本身已知的方式应用从对本体感受传感器6获取的数据进行积分而得出的演化模型。
上述步骤形成了不变卡尔曼滤波器的迭代。
由于不变卡尔曼滤波器，获得了在线性情况下也能获得的特性：估计误差的演化是自主的(既不依赖于X1也不依赖于
Figure GDA0003351088450000124
)。
第二载体的导航状态发射随时间而重复，如此使得这些状态被第一载体1接收。
针对所接收的第二载体的每个新状态，处理单元10在不变卡尔曼滤波器的新迭代中重复这些相同的步骤104、106、108、110、112、114。在给定的迭代的传播步骤112期间估计的状态用作针对下一个迭代的创新计算步骤104和非线性更新步骤110的输入数据。
最终，由于该方法，第一载体1可使用在第二载体2中已经可用的、关于第二载体2自身的数据获得关于第一载体1自身的导航的辅助。
有利地，处理单元10并行地实现多个卡尔曼滤波器，以获得第一载体1的导航状态的多个估计值。
应注意的是，对于所有有关的卡尔曼滤波器，一些处理操作只能执行一次。特别地，这些处理操作之一是本领域技术人员已知的Ricatti(黎卡提)方程的解析。
在这种情况下，针对每个估计值，处理单元确定估计值的数据偏差度量。
例如，该度量L写成：
L＝ZTS-1Z。
其中，S是创新Z的协方差，如通常在更新传统的卡尔曼滤波器的步骤中计算的。
对于给定的测量值，可计算度量L。作为变型，度量L是在一组过去的测量值中获得的值的总和。
在合并步骤116中，基于估计值及其关联的似然度量L，处理单元10生成第一载体的导航状态的合并估计值。
在一个实施例中，选择由具有反映最小数据偏差的度量的滤波器之一所获得的估计值作为合并估计值。
在另一实施例中，合并估计值是由不同滤波器确定的估计值的平均值，该平均值由度量加权。
所估计的状态X1本质上不是向量，本领域技术人员可使用适合于变化的情况的平均值。
在其他变型中，可以用其他感兴趣的变量来补充所估计的状态X1，这样的变量例如本体感受传感器所特有的误差状态(偏差、比例因子、偏移等)。
在这种情况下，对于本体感受传感器的误差状态，二元运算*是相加。
假设B为所考虑的本体感受传感器的误差状态。
二元运算将变成：
(R1,v1,x1,R21,x21,B)*(R′1,v′1,x′1,R′21,x′21,B′)＝(R1R′1,v1+R1v′1,x1+R1x′1,R21R′21,x21+R21x′21,B+B′)。
此外，如上所指示的，矩阵R21可以由矩阵R12代替，矩阵R12使得能够从第二坐标系(附接于第二载体2)切换到第一坐标系(附接于第一载体1)。同样，向量x21可以由向量x12代替，向量x12是使得一个点能够从在第二坐标系中的坐标切换到相同点在第一坐标系中的坐标的平移向量。
进一步写出
Figure GDA0003351088450000141
在这种情况下，用新变量表示的二元运算变成：
(R1,v1,x1,R12,x12)*(R′1,v′1,x′1,R′12,x′12)＝(R1R′1,v1+R1v′1,x1+R1x′1,R′12R12,x′12+R′12x12)。
在这种情况下，创新Z变成：
Figure GDA0003351088450000142
在另一实施例中，载体1的本体感受测量值包括角速度ω。此外，所接收的第二载体2的导航状态X2包括：第二载体2在参考坐标系中的速度向量；和表示第二载体在参考坐标系中的姿态的旋转矩阵。那么，不变卡尔曼滤波器使用具有以下形式的创新Z：
Figure GDA0003351088450000143
其中：
·
Figure GDA0003351088450000144
是包括在刚性变换T1中的旋转矩阵R1的估计值。更具体地，R1使得一个点在附接于载体1的坐标系中的坐标u∈R3，在参考坐标系中变成R1u+x1，其中，x1是第一载体在参考坐标系中的位置。
·
Figure GDA0003351088450000145
和
Figure GDA0003351088450000146
是旋转矩阵
Figure GDA0003351088450000147
的估计值和限定刚性变换T21＝(R21,x21)的平移向量x21的估计值。更具体地，
Figure GDA0003351088450000148
和
Figure GDA0003351088450000149
使得一个点在附接于载体1的坐标系中的坐标u∈R3，在附接于载体2的坐标系中变成R21u+x21。
·R2是表示载体2的姿态的旋转矩阵(如上所指示的，可作为观测值)。更具体地，R2使得一个点在附接于载体2的坐标系中的坐标u∈R3，在参考坐标系中变成R2u+x2，其中，x2是载体2在参考坐标系中的位置(对于本实施例，x2不需要被观测)。
·v2是载体2在参考坐标系中的速度(可作为观测值)。
·
Figure GDA0003351088450000151
是载体1在参考坐标系中的估计的速度。
·×表示传统的向量积。
·log(·)表示李群SE(3)的对数函数(指数函数的倒数)。应注意的是，作为对数函数的输入的对象是属于李群SE(3)并被写成一对的对象，该一对包括旋转矩阵和大小为3的向量。
该另一实施例使得能够及其近似背景技术中列出的条件a和b，并仅在第二载体是“平坦”的前提下，即在角速度测量值ω始终在同一轴上且该轴也是旋转R21的轴的前提下，才满足条件a和b。特别地，如果关注的所有旋转(R1和R2)都具有垂直轴-在地面车辆上通常是这样的情况，则这一假设得到验证。这就是为什么Z＝Zx的实施例更有利的原因。
向量Zv包括表示旋转的大小为3的第一向量
Figure GDA0003351088450000154
和大小同样为3的第二向量
Figure GDA0003351088450000152
在另一实施例中，还可以设想将上述数据结合起来，形成更复杂的创新Z，更复杂的创新Z具有以下形式：
Figure GDA0003351088450000153

Claims (14)

1.一种用于辅助第一载体(1)导航的方法，所述第一载体相对于本身能够在参考坐标系中移动的第二载体(2)静止，所述方法包括由所述第一载体(1)实现的以下步骤：
接收(102)由所述第二载体(2)提供的、在所述第二载体(2)的参考坐标系中的导航状态X2；
通过所述第一载体(1)的至少一个本体感受传感器(6)获取(112)所述第一载体(1)的移动数据；
通过不变卡尔曼滤波器使用所述第二载体(2)的导航状态X2作为观测数据，估计所述第一载体(1)的导航状态X1，其中，所述第一载体(1)的导航状态包括：
第一变量，表示将附接于所述第一载体(1)的第一坐标系关联到所述参考坐标系的第一刚性变换T1；以及
第二变量，表示将附接于所述第二载体(2)的第二坐标系关联到附接于所述第一载体(1)的第一坐标系的第二刚性变换T21，
其中，所述不变卡尔曼滤波器使用包括所述第一刚性变换和所述第二刚性变换的逐项组合的运算作为二元运算。
2.根据权利要求1所述的方法，其中，所述第一变量包括表示所述第一载体(1)的姿态的旋转矩阵，以及所述第二变量包括表示所述第二载体(2)相对于所述第一载体(1)的姿态的旋转矩阵。
3.根据权利要求1或2所述的方法，其中，所述第一变量包括所述第一载体(1)的位置向量，以及所述第二变量包括所述第二载体(2)相对于所述第一载体(1)的位置向量。
4.根据权利要求3所述的方法，其中，所估计的第一载体(1)的导航状态进一步包括所述第一载体(1)在所述参考坐标系中的速度向量。
5.根据权利要求4所述的方法，其中，所述二元运算将相同的变换应用于位置向量之一以及速度向量。
6.根据权利要求1或2所述的方法，其中，所接收的第二载体(2)的导航状态X2包括：所述第二载体(2)在所述参考坐标系中的位置向量和表示所述第二载体在所述参考坐标系中的姿态的旋转矩阵。
7.根据权利要求1或2所述的方法，其中，所述不变卡尔曼滤波器使用创新，所述创新包括具有以下形式的向量Zx：
Figure FDA0003351088440000021
其中：
Figure FDA0003351088440000022
是所述第一变量的估计值；
Figure FDA0003351088440000023
是所述第二变量的估计值，其中，所述第二刚性变换使得能够从所述第一坐标系切换到所述第二坐标系；
Y表示所述不变卡尔曼滤波器使用的观测数据；
Figure FDA0003351088440000024
表示组合；
log(·)表示李群的对数映射。
8.根据权利要求1或2所述的方法，其中，所接收的第二载体(2)的导航状态X2包括：所述第二载体(2)在所述参考坐标系中的速度向量；和表示所述第二载体在所述参考坐标系中的姿态的旋转矩阵；并且其中，由所述本体感受传感器(6)获取的所述第一载体(1)的移动数据包括所述第一载体(1)的角速度。
9.根据权利要求7所述的方法，其中，所述第一变量包括表示所述第一载体(1)的姿态的旋转矩阵，以及所述第二变量包括表示所述第二载体(2)相对于所述第一载体(1)的姿态的旋转矩阵；并且其中，所述不变卡尔曼滤波器使用创新，所述创新包括具有以下形式的向量Zv：
Figure FDA0003351088440000025
其中：
Figure FDA0003351088440000026
是表示所述第一载体(1)的姿态的旋转矩阵的估计值；
Figure FDA0003351088440000031
是表示所述第二载体(2)相对于所述第一载体(1)的姿态的旋转矩阵的估计值；
Figure FDA0003351088440000032
是限定所述第二刚性变换的平移向量的估计值；
R2是表示所述第二载体在所述参考坐标系中的姿态的旋转矩阵；
Figure FDA0003351088440000033
是所述第一载体(1)在所述参考坐标系中的估计的速度；
v2是所述第二载体(2)在所述参考坐标系中的速度向量；
ω是所述第一载体(1)的角速度；
×表示向量积；
log(·)表示李群SE(3)的对数映射。
10.根据权利要求9所述的方法，其中，所述向量Zv包括表示旋转的大小为3的第一向量
Figure FDA0003351088440000034
以及大小同样为3的第二向量
Figure FDA0003351088440000035
并且其中，所述不变卡尔曼滤波器使用具有以下形式的创新Z：
Figure FDA0003351088440000036
11.根据权利要求1或2所述的方法，其中，所估计的第一载体(1)的导航状态进一步包括：所述本体感受传感器所特有的至少一个误差状态，并且其中，对于所述本体感受传感器所特有的至少一个误差变量，所述二元运算是相加。
12.根据权利要求1或2所述的方法，其中：
所述第一载体(1)是飞行器，以及所述第二载体(2)是飞行器载体；或者
所述第一载体(1)是炮弹，以及所述第二载体(2)是运载所述炮弹的飞行器；或者
所述第一载体(1)是交通工具的惯性单元，以及所述第二载体(2)是所述交通工具的外感受性传感器。
13.根据权利要求1或2所述的方法，其中，所述估计的步骤由多个不变卡尔曼滤波器并行实现，以获得所述第一载体(1)的导航状态的多个估计值，所述方法进一步包括以下步骤：
对于每个估计值，确定所述估计值的似然度量；
基于所述估计值及其关联的似然度量，生成所述第一载体(1)的导航状态的合并估计值。
14.一种用于辅助第一载体(1)导航的装置，所述第一载体相对于本身能够在参考坐标系中移动的第二载体(2)静止，所述装置包括：
至少一个本体感受传感器(6)，配置为获取所述第一载体(1)的移动数据；
通信接口(4)，用于接收由所述第二载体(2)提供的所述第二载体(2)的导航状态X2；
不变卡尔曼滤波器(10)，配置为通过使用所述第二载体(2)的导航状态作为观测数据，估计所述第一载体(1)的导航状态X1，其中，所述导航状态包括：
第一变量，表示将附接于所述第一载体(1)的第一坐标系关联到所述参考坐标系的第一刚性变换T1；以及
第二变量，表示将附接于所述第二载体(2)的第二坐标系关联到附接于所述第一载体(1)的第一坐标系的第二刚性变换T21，
其中，所述不变卡尔曼滤波器使用包括所述第一刚性变换和所述第二刚性变换的逐项组合的运算作为二元运算。
CN201980053365.1A 2018-07-23 2019-07-23 使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工具导航辅助方法及装置 Active CN112703367B (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
FR1856802 2018-07-23
FR1856802A FR3084176B1 (fr) 2018-07-23 2018-07-23 Procede et dispositif d'aide a la navigation d'un porteur a l'aide d'un filtre de kalman invariant et d'un etat de navigation d'un deuxieme porteur
PCT/FR2019/051826 WO2020021194A1 (fr) 2018-07-23 2019-07-23 Procédé et dispositif d'aide à la navigation d'un porteur à l'aide d'un filtre de kalman invariant et d'un état de navigation d'un deuxième porteur

Publications (2)

Publication Number Publication Date
CN112703367A CN112703367A (zh) 2021-04-23
CN112703367B true CN112703367B (zh) 2022-03-15

Family

ID=65031489

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201980053365.1A Active CN112703367B (zh) 2018-07-23 2019-07-23 使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工具导航辅助方法及装置

Country Status (6)

Country Link
US (1) US11941079B2 (zh)
EP (1) EP3827221B1 (zh)
CN (1) CN112703367B (zh)
FR (1) FR3084176B1 (zh)
IL (1) IL280344B2 (zh)
WO (1) WO2020021194A1 (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR3086052B1 (fr) * 2018-09-13 2020-10-02 Ixblue Système de localisation, et procédé de localisation associé
US11668794B2 (en) * 2020-01-07 2023-06-06 Ford Global Technologies, Llc Sensor calibration

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106068441A (zh) * 2013-11-22 2016-11-02 萨基姆防卫安全 惯性单元的校准方法
CN107110650A (zh) * 2014-10-29 2017-08-29 赛峰电子与防务公司 在能观测性方面受约束的导航状态的估计方法
CN107690567A (zh) * 2015-04-01 2018-02-13 赛峰电子与防务公司 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR3023623B1 (fr) * 2014-07-09 2023-05-05 Sagem Defense Securite Procede d'estimation de l'etat d'un systeme mobile et centrale inertielle correspondante.

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106068441A (zh) * 2013-11-22 2016-11-02 萨基姆防卫安全 惯性单元的校准方法
CN107110650A (zh) * 2014-10-29 2017-08-29 赛峰电子与防务公司 在能观测性方面受约束的导航状态的估计方法
CN107690567A (zh) * 2015-04-01 2018-02-13 赛峰电子与防务公司 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Invariant EKF Design for Scan Matching-Aided Localization;Martin Barczyk，等;《IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY》;20151130;第23卷(第6期);2440-2448 *
The Invariant Extended Kalman Filter;AXEL BARRAU，等;《IEEE TRANSACTIONS ON AUTOMATIC CONTROL》;20170401;第62卷(第4期);1797-1812 *

Also Published As

Publication number Publication date
FR3084176B1 (fr) 2020-06-19
EP3827221A1 (fr) 2021-06-02
EP3827221B1 (fr) 2023-03-08
US20210295718A1 (en) 2021-09-23
IL280344B2 (en) 2024-01-01
WO2020021194A1 (fr) 2020-01-30
IL280344A (en) 2021-03-01
IL280344B1 (en) 2023-09-01
CN112703367A (zh) 2021-04-23
FR3084176A1 (fr) 2020-01-24
US11941079B2 (en) 2024-03-26

Similar Documents

Publication Publication Date Title
Kottath et al. Multiple model adaptive complementary filter for attitude estimation
CN110517324B (zh) 基于变分贝叶斯自适应算法的双目vio实现方法
WO2008051240A2 (en) System, apparatus and methods for augmenting filter with adaptive element
CN112703367B (zh) 使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工具导航辅助方法及装置
JP2014089113A (ja) 姿勢推定装置及びプログラム
WO2011012882A1 (en) Estimating positions of a device and at least one target in an environment
CN107339987B (zh) 一种基于函数迭代积分的刚体姿态解算方法
Lotfi et al. An approach for velocity and position estimation through acceleration measurements
Hashim Gps-denied navigation: Attitude, position, linear velocity, and gravity estimation with nonlinear stochastic observer
CN109141411B (zh) 定位方法、定位装置、移动机器人及存储介质
CN110186465B (zh) 一种基于单目视觉的空间非合作目标相对状态估计方法
Fagogenis et al. Improving underwater vehicle navigation state estimation using locally weighted projection regression
CN114022556A (zh) 定位初始化方法、装置和计算机可读存储介质
CN112567203B (zh) 用于使用不变卡尔曼滤波器辅助一队交通工具导航的方法和装置
Kang et al. Finite-memory-structured online training algorithm for system identification of unmanned aerial vehicles with neural networks
CN114147717B (zh) 机器人运动轨迹估计方法、装置、控制器及存储介质
EP3410074B1 (en) Method and device for improving performance of relative-position sensor, and computer storage medium
Maley et al. Unit quaternion-based parameterization for point features in visual navigation
CN114323007A (zh) 一种载体运动状态估计方法及装置
JP5315585B2 (ja) 航跡生成システム、その誤差共分散行列初期値設定装置、航跡生成方法およびその誤差共分散行列初期値設定方法
Aghli et al. Online system identification and calibration of dynamic models for autonomous ground vehicles
De Silva et al. Relative localization with symmetry preserving observers
KR20110056185A (ko) 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치
CN110543919A (zh) 一种机器人定位控制方法、终端设备及存储介质
Khoa et al. An adaptive filter for imu/encoder data fusion for acceleration estimation in robot arms

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