CN115060260A - 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 - Google Patents

一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 Download PDF

Info

Publication number
CN115060260A
CN115060260A CN202210160522.2A CN202210160522A CN115060260A CN 115060260 A CN115060260 A CN 115060260A CN 202210160522 A CN202210160522 A CN 202210160522A CN 115060260 A CN115060260 A CN 115060260A
Authority
CN
China
Prior art keywords
anfis
imu
coordinate system
wio
time
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
CN202210160522.2A
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.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for Science and Technology
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 University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN202210160522.2A priority Critical patent/CN115060260A/zh
Publication of CN115060260A publication Critical patent/CN115060260A/zh
Pending legal-status Critical Current

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/20—Instruments for performing navigational calculations
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于ANFIS‑ESKF的地面移动机器人多传感器融合定位方法。该方法包括：建立轮式‑IMU里程计(WIO)的测量模型，推导***的状态方程；建立视觉里程计(VO)的测量模型，推导***的观测方程；用ESKF对WIO和VO的运动估计信息进行融合；建立自适应神经模糊推理***ANFIS(Adaptive Neural Fuzzy Inference System)，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整误差状态卡尔曼滤波ESKF(Error State Kalman Filter)中预测模型和测量模型的协方差矩阵。本发明与现有技术相比，能够根据***的状态实时调整ESKF参数，定位精度更高，鲁棒性更强。

Description

一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位 方法
技术领域
本发明属于导航技术领域，特别是一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法。
背景技术
里程计***使用本体传感器来估计车辆相对于起始点的瞬时位置和方向，这是自动导航***中的一个关键模块，可以在外部传感器不可用时提供位置信息。常用的机载传感器包括车轮编码器、惯性测量单元(IMU)、摄像机和激光雷达。使用单个传感器的里程计的性能受传感器特性、环境干扰和车辆状态的影响，通常不能提供稳定和满意的定位结果。
多传感器融合技术可以融合多个传感器的信息得到稳定的定位结果。应用最广泛的传感器融合方法是卡尔曼滤波器(KF)的变种，如扩展卡尔曼滤波器(EKF)和无迹卡尔曼滤波器(UKF)。如国内专利CN201310455813.5《GPS盲区下融合多源信息的车辆高精度定位方法及装置》，该方法使用陀螺仪输出的角速率信息和加速度信息、路面图像信息输入到卡尔曼滤波器得到最终融合信息、再由道路旁边设置的锚节点重新矫正融合算法产生的累计误差，可以提供稳定、可靠、高精度的车辆位置信息。如专利CN201310369354.9《一种面向室内移动机器人的光流场视觉/INS组合导航方法》中所述，利用光流场视觉和IMU等信息输入到扩展卡尔曼滤波器进行融合计算，能够提高室内移动机器人的导航精度。
尽管卡尔曼滤波器及其变种在某些应用中被证明是有效的，但它们有一个共同的假设，对于实际的工作场景可能是有问题的——不变协方差。在现实中，测量噪声不确定，模型误差存在，车辆经历暂时不可预测的运动变化，环境局部改变。这种变化的条件需要适当处理，否则定位性能将严重降低。以往的误差状态卡尔曼滤波(ESKF)方法在预测模型和测量模型中都不能解决具有不同不确定性的传感器融合问题。基于ANFIS-ESKF(AdaptiveNetwork-Based Fuzzy Inference System–Error State KalmanFilter，自适应网络模糊推理***-误差状态卡尔曼滤波)方法可以对***的瞬时变化做出响应，并且在融合算法中可以独立确定两个协方差矩阵，从而提高定位***的鲁棒性。
发明内容
为了解决现有技术问题，本发明的目的在于克服已有技术存在的卡尔曼滤波及其变种模型在多传感器融合技术中不足，提供一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，能够在线检测异常工况并且调整融合增益的基于ANFIS-ESKF的地面移动机器人多传感器融合定位，能够解决现有卡尔曼滤波技术的弊端，从而实现对多传感器融合的瞬时变化做出响应，并确定独立确定两个协方差矩阵，提高定位***的鲁棒性。
为达到上述发明创造目的，本发明采用如下技术方案：
一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，包括模块1、模块2、模块3和模块4共四个计算模块，其特征在于：
所述模块1，建立轮式-IMU里程计(WIO)的测量模型，推导***的状态方程；
所述模块2，建立视觉里程计(VO)的测量模型，推导***的观测方程；
所述模块3，用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合；
所述模块4，建立自适应模糊神经推理***(ANFIS)，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵；该模块在使用前需要根据GPS数据对ANFIS进行训练；
所述模块1的输入是轮式里程计数据和IMU数据，输出是相邻时刻IMU坐标系的位姿估计；模块2的输入是双目相机拍摄的照片，输出是相邻时刻相机坐标系的位姿估计；模块3的输入是模块1和模块2的输出，输出是相邻时刻IMU坐标系的位姿估计；模块4的输入模块1中WIO和模块2中VO的中间数据，输出是模块3中需要使用的WIO和VO的协方差矩阵；
整个定位程序的输入是IMU数据、轮式里程计数据、相机数据，输出是IMU坐标系的位姿变化。
优选地，所述模块1中建立WIO测量模型，具体如下：
IMU包括三轴陀螺仪和三轴加速度计，考虑到测试***加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-IMU里程计WIO，提供完整的六自由度运动估计；WIO为ESKF***提供移动机器人运动状态的预测；
下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化：
Figure BDA0003514431310000021
其中
Figure BDA0003514431310000022
是i+1时刻测得的IMU坐标系的姿态变化，
Figure BDA0003514431310000023
是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系，
Figure BDA0003514431310000024
是轮式里程计坐标系相对于IMU坐标系的姿态标定关系，
Figure BDA0003514431310000025
是i+1时刻测得的轮式里程计坐标系的位置变化，I是单位矩阵。
优选地，所述模块1中建立***状态方程，具体如下：
定义***的状态为：
Figure BDA0003514431310000031
其中pi为i时刻IMU坐标系相对于初始时刻的位置，qi为i时刻IMU坐标系相对于初始时刻的姿态，
Figure BDA0003514431310000032
为i时刻IMU角速度偏移；
那么根据WIO测量模型得到***的名义状态方程为：
Figure BDA0003514431310000033
其中
Figure BDA0003514431310000034
是i时刻轮式里程计的测量值，
Figure BDA00035144313100000311
是i时刻IMU坐标系相对于0时刻的姿态，
Figure BDA0003514431310000035
是i时刻IMU测得的角速度，
Figure BDA0003514431310000036
是i时刻IMU的角速度偏移，
Figure BDA0003514431310000037
Δt是IMU的采样周期，[γi]×指的是γi对应的反对称矩阵；pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置，
Figure BDA0003514431310000038
表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数，
Figure BDA0003514431310000039
表示i+1时刻IMU角速度偏移对时间的导数；
误差状态方程为：
Figure BDA00035144313100000310
其中θi是qi对应的李代数，
Figure BDA0003514431310000041
是IMU的测量噪声，
Figure BDA0003514431310000042
是IMU角速度偏移的导数，
Figure BDA0003514431310000043
表示i时刻轮式里程计的测量噪声；ωn i表示i时刻IMU的角速度测量噪声；Δt表示IMU采样周期。
优选地，所述模块2中建立视觉里程计(VO)的测量模型，推导***的观测方程，具体如下：
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化
Figure BDA0003514431310000044
和位置变化
Figure BDA0003514431310000045
观测模型为：
Figure BDA0003514431310000046
Figure BDA0003514431310000047
Figure BDA0003514431310000048
是相机观测到的从第j-1帧到第j帧的位移，它和
Figure BDA0003514431310000049
相等；
Figure BDA00035144313100000410
是的从时间j-1到时间j的旋转向量，和
Figure BDA00035144313100000411
等价；ΔpC，n是随机位移误差，服从高斯分布ΔpC，n～N(0，V1)；ΔθC，n是随机的姿态误差，服从高斯分布ΔθC，n～N(0，V2)；
Figure BDA00035144313100000412
表示第j-1帧到第j帧的相机位移变化的测量值；
Figure BDA00035144313100000413
表示从时间j-1到时间j的相机姿态变化的测量值；
根据IMU坐标系{I}和相机坐标系{C}的位姿关系
Figure BDA00035144313100000414
和
Figure BDA00035144313100000415
VO可提供相机观测到的相对于IMU坐标系{Ij-1}的实时位姿变化，如下式(7)所示：
Figure BDA00035144313100000416
进一步地，所述模块2中建立***的观测方程，具体如下：
Figure BDA00035144313100000417
上式中，
Figure BDA00035144313100000418
表示相机观测到的相机坐标系的位移变化；
Figure BDA00035144313100000419
表示相机观测到的IMU坐标系的位移变化；
Figure BDA00035144313100000420
表示相机观测到的IMU坐标系的姿态变化。
优选地，在所述模块4中，建立自适应模糊神经推理***ANFIS，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵，具体步骤如下：
采用两个ANFIS，第一个ANFIS记作WIO-ANFIS，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是WIO的协方差矩阵Q1；第二个ANFIS记作VO-ANFIS，其输入的是视觉里程计的特征点个数Nu和重投影误差Er，输出是VO的协方差矩阵V；
定义WIO-ANFIS的输入为Xw＝(ω，ρ)T，定义它的模糊集为A；为A中的每一个成员Aa设计一个隶属度函数，并且记作Amf-a(xw)∈[0,1]；Amf-a(xw)越接近1，Xw属于Aa的可能性越大；VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作xv＝(Nu，Er)T，定义它的模糊集为B；构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似；
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
在A中为ρ定义了四个成员，它们分别趋向于+∞(A1)，趋向于1(A2)，趋向于-1(A3)，趋向于-∞(A4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；A中的ω有两个成员：小(A5)和大(A6)；它们分别对应着慢转弯和快转弯的运动情况；同样，B中Er的模糊集分别为小(B1)和大(B2)，Nu的模糊集分别为小的(B3)和大的(B4)；
隶属度函数定义了输入与其对应的模糊成员之间的关系，在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的***情况；表示A和B中的自适应变量分别为AW和Bv；AW1指定了ρ完全属于A1时初始值；AW2指定了ρ属于A2的终点；AW3指定了ρ开始属于A3的点，AW4指定了ρ完全属于A4时的最大值；函数的另一端固定在临界点±1，简化了复杂度，降低了ANFIS训练中需要优化的变量的维数；AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点；Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点；Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点；曲线形状的控制方式同AW1,AW2,AW3,AW4；
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对WIO和VO的测量方差进行了推理；
对于WIO-ANFIS，当输入组合为(A3,A6)和(A5,A2)时，方差为低，用WL表示；物理的解释是，当车辆正在急转弯(A3)，并且IMU测量表明同样的情况(A6)，或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5)，移动机器人运动时可能没有打滑，从而WIO的方差应该比较小；否则，IMU的方差应该比较大，用WH表示；
对于每一条规则，例如πw7(A3,A6)，它的输出定义如式子(9)所示；根据同样的规则，得到πwa的输出，用GaW表示；
G7W＝min(Amf-6(xw),Amf-3(xw)) (9)
类似地，也为VO-ANFIS建立了规则库；为视觉的运动估计定义了高中低三个方差，分别用VH,VM,VL,表示，相应的模糊规则的输出用GbC表示；
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果
Figure BDA0003514431310000061
类似地，得到VO-ANFIS的
Figure BDA0003514431310000062
a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号，na表示WIO-ANFIS中的模糊规则总数；
Figure BDA0003514431310000063
根据各种工况的具体测量方差VH,VM,VL,WH WL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到：
Figure BDA0003514431310000064
上式中Wπwa表示规则πwa对应的权重，Vπvb表示规则πvb对应的权重，a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号。
进一步优选地，在所述模块4中，模糊集与相应***情况的关系如下表所示：
Figure BDA0003514431310000071
在本发明中，模糊推理中隶属函数的参数也可根据经验和直觉来选择；而为了实现ANFIS***的适应性，需要对里程计的测量有一个真值；它是为车辆提供无漂移定位的GPS单元；利用GPS测量值作为训练集的真值的难点在于GPS与IMU的位置不一致；这种偏差很大，特别是在车辆急转弯时，会引入训练数据的误差，导致参数优化错误；因此，将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在IMU坐标系{I}中的坐标可从***设计参数中导出，但对于训练目的来说不够精确，因此在使用它进一步训练ANFIS参数之前有必要对其进行优化。
优选地，在所述模块4中，建立自适应模糊神经推理***(ANFIS)的训练方法，具体步骤如下：
将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在坐标系{I0}中的位置可表示为：
Figure BDA0003514431310000072
其中，IXG是GPS接收机在IMU坐标系{I}中的坐标，
Figure BDA0003514431310000073
是在时间i时，坐标系{I}相对于坐标系{I0}的旋转矩阵；
Figure BDA0003514431310000074
是时间i时GPS路径点在GPS测量坐标系{GM}中的表示，
Figure BDA0003514431310000075
是坐标系{GM}和坐标系{I0}之间的旋转矩阵；
相应地，定义相对于GPS测量的估计轨迹误差为:
Figure BDA0003514431310000076
其中
Figure BDA0003514431310000077
为里程计的估计轨迹；
IXG从***设计参数中导出，在使用它进一步训练ANFIS参数之前，有必要对其进行优化；对于优化后的GPS相对于坐标系{I}的位置估计，可根据求式子(14)的解得到；
Figure BDA0003514431310000081
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
需要优化的参数包括Aw，Bv和方差WH，WL，VH，VM，VL，用式(15)表示：
xANFIS＝[AW，Bv，WH，WL，VH，VM，VL] (15)
已知IXG和
Figure BDA0003514431310000082
的情况下，式子(15)中的误差用来构造如下代价优化；XANFIS的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
Figure BDA0003514431310000083
其中
Figure BDA0003514431310000084
是根据参数XANFIS利用ANFIS-ESKF估计的GPS接收机在时间i时相对于坐标系{I0}的位置。
本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著优点：
1.本发明经过训练的ANFIS***能够根据地面移动机器人***的状态，比如机器人是否打滑或者环境中的纹理信息是否足够多等，来实时调整ESKF的参数，与传统的固定增益滤波器相比，大大减少了内部故障和外部干扰造成的误差，使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度，鲁棒性更强；
2.采用本发明方法，在移动机器人上对算法进行验证，结果表明，与固定增益ESKF相比，基于ANFIS的ESKF的平均轨迹误差降低了80.6％，即使有意引入外部干扰，其平均位置漂移也在1％以下。
附图说明
图1为本发明提供的基于ANFIS-ESKF的移动机器人多传感器融合定位方法的原理图。
图2为本发明优选实施例提供的ANFIS框架，其中(a)为WIO的协方差推算的ANFIS框架，(b)为VO的协方差推算的ANFIS框架。
图3为本发明优选实施例提供的车轮里程计在一个采样周期内的航位推算方法。
图4为本发明优选实施例的ANFIS的隶属度函数。
具体实施方式
以下结合具体的实施例子对上述方案做进一步说明，本发明的优选实施例详述如下：
实施例一：
在本实施例中，参见图1，一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，包括模块1、模块2、模块3和模块4共四个计算模块；
所述模块1，建立轮式-IMU里程计(WIO)的测量模型，推导***的状态方程；
所述模块2，建立视觉里程计(VO)的测量模型，推导***的观测方程；
所述模块3，用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合；
所述模块4，建立自适应模糊神经推理***(ANFIS)，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵；该模块在使用前需要根据GPS数据对ANFIS进行训练；
所述模块1的输入是轮式里程计数据和IMU数据，输出是相邻时刻IMU坐标系的位姿估计；模块2的输入是双目相机拍摄的照片，输出是相邻时刻相机坐标系的位姿估计；模块3的输入是模块1和模块2的输出，输出是相邻时刻IMU坐标系的位姿估计；模块4的输入是模块1中WIO和模块2中VO的中间数据，输出是模块3中需要使用的WIO和VO的协方差矩阵；
整个定位程序的输入是IMU数据、轮式里程计数据、相机数据，输出是IMU坐标系的位姿变化。
本实施例能够在线检测异常工况并且调整融合增益实现基于ANFIS-ESKF的地面移动机器人多传感器融合定位，能够实现对多传感器融合的瞬时变化做出响应。
实施例二：
本实施例与实施例一基本相同，特别之处在于：
在本实施例中，一种基于ANFIS-ESKF的移动机器人多传感器融合定位方法，包括以下模块：
模块1，建立轮式-IMU里程计(WIO)的测量模型，推导***的状态方程；
模块2，建立视觉里程计(VO)的测量模型，推导***的观测方程；
模块3，用ESKF对WIO和VO的运动估计信息进行融合；
模块4，建立ANFIS***，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵；
所述模块1中建立WIO测量模型，具体如下：
IMU包括三轴陀螺仪和三轴加速度计，考虑到测试***加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-IMU里程计WIO，提供完整的六自由度运动估计；WIO为ESKF***提供移动机器人运动状态的预测；
下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化：
Figure BDA0003514431310000101
其中
Figure BDA0003514431310000102
是i+1时刻测得的IMU坐标系的姿态变化，
Figure BDA0003514431310000103
是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系，
Figure BDA0003514431310000104
是轮式里程计坐标系相对于IMU坐标系的姿态标定关系，
Figure BDA0003514431310000105
是i+1时刻测得的轮式里程计坐标系的位置变化，I是单位矩阵。
优选地，所述模块1中建立***状态方程，具体如下：
定义***的状态为：
Figure BDA0003514431310000106
其中pi为i时刻IMU坐标系相对于初始时刻的位置，qi为i时刻IMU坐标系相对于初始时刻的姿态，
Figure BDA0003514431310000107
为i时刻IMU角速度偏移；
那么根据WIO测量模型得到***的名义状态方程为：
Figure BDA0003514431310000108
其中
Figure BDA0003514431310000109
是i时刻轮式里程计的测量值，
Figure BDA00035144313100001013
是i时刻IMU坐标系相对于0时刻的姿态，
Figure BDA00035144313100001010
是i时刻IMU测得的角速度，
Figure BDA00035144313100001011
是i时刻IMU的角速度偏移，
Figure BDA00035144313100001012
Δt是IMU的采样周期，[γi]×指的是γi对应的反对称矩阵；pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置，
Figure BDA0003514431310000111
表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数，
Figure BDA0003514431310000112
表示i+1时刻IMU角速度偏移对时间的导数；
误差状态方程为：
Figure BDA0003514431310000113
其中θi是qi对应的李代数，ωn i是IMU的测量噪声，
Figure BDA0003514431310000114
是IMU角速度偏移的导数，
Figure BDA0003514431310000115
表示i时刻轮式里程计的测量噪声；ωn i表示i时刻IMU的角速度测量噪声；Δt表示IMU采样周期。
在本实施例中，所述模块2中建立视觉里程计(VO)的测量模型，推导***的观测方程，具体如下：
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化
Figure BDA0003514431310000116
和位置变化
Figure BDA0003514431310000117
观测模型为：
Figure BDA0003514431310000118
Figure BDA0003514431310000119
Figure BDA00035144313100001110
是相机观测到的从第j-1帧到第j帧的位移，它和
Figure BDA00035144313100001111
相等；
Figure BDA00035144313100001112
是的从时间j-1到时间j的旋转向量，和
Figure BDA00035144313100001113
等价；ΔPC，n是随机位移误差，服从高斯分布ΔpC，n～N(0，V1)；ΔθC，n是随机的姿态误差，服从高斯分布ΔθC，n～N(0，V2)；
Figure BDA00035144313100001114
表示第j-1帧到第j帧的相机位移变化的测量值；
Figure BDA00035144313100001115
表示从时间j-1到时间j的相机姿态变化的测量值；
根据IMU坐标系{I}和相机坐标系{C}的位姿关系
Figure BDA0003514431310000121
和
Figure BDA0003514431310000122
VO可提供相机观测到的相对于IMU坐标系{Ij-1}的实时位姿变化，如下式(7)所示：
Figure BDA0003514431310000123
在本实施例中，所述模块2中建立***的观测方程，具体如下：
Figure BDA0003514431310000124
上式中，
Figure BDA0003514431310000125
表示相机观测到的相机坐标系的位移变化；
Figure BDA0003514431310000126
表示相机观测到的IMU坐标系的位移变化；
Figure BDA0003514431310000127
表示相机观测到的IMU坐标系的姿态变化。
优选地，在所述模块4中，建立自适应模糊神经推理***ANFIS，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵，具体步骤如下：
采用两个ANFIS，第一个ANFIS记作WIO-ANFIS，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是WIO的协方差矩阵Q1；第二个ANFIS记作VO-ANFIS，其输入的是视觉里程计的特征点个数Nu和重投影误差Er，输出是VO的协方差矩阵V；
定义WIO-ANFIS的输入为xw＝(ω，ρ)T，定义它的模糊集为A；为A中的每一个成员Aa设计一个隶属度函数，并且记作Amf-a(xw)∈[0,1]；Amf-a(xw)越接近1，Xw属于Aa的可能性越大；VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作xv＝(Nu，Er)T，定义它的模糊集为B；构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似；
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
在A中为ρ定义了四个成员，它们分别趋向于+∞(A1)，趋向于1(A2)，趋向于-1(A3)，趋向于-∞(A4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；A中的ω有两个成员：小(A5)和大(A6)；它们分别对应着慢转弯和快转弯的运动情况；同样，B中Er的模糊集分别为小(B1)和大(B2)，Nu的模糊集分别为小的(B3)和大的(B4)；
隶属度函数定义了输入与其对应的模糊成员之间的关系，在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的***情况；表示A和B中的自适应变量分别为AW和Bv；AW1指定了ρ完全属于A1时初始值；AW2指定了ρ属于A2的终点；AW3指定了ρ开始属于A3的点，AW4指定了ρ完全属于A4时的最大值；函数的另一端固定在临界点±1，简化了复杂度，降低了ANFIS训练中需要优化的变量的维数；AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点；Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点；Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点；曲线形状的控制方式同AW1,AW2,AW3,AW4；
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对WIO和VO的测量方差进行了推理；
对于WIO-ANFIS，当输入组合为(A3,A6)和(A5,A2)时，方差为低，用WL表示；物理的解释是，当车辆正在急转弯(A3)，并且IMU测量表明同样的情况(A6)，或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5)，移动机器人运动时可能没有打滑，从而WIO的方差应该比较小；否则，IMU的方差应该比较大，用WH表示；
对于每一条规则，例如πw7(A3,A6)，它的输出定义如式子(9)所示；根据同样的规则，得到πwa的输出，用GaW表示；
G7W＝min(Amf-6(xw),Amf-3(xw)) (9)
类似地，也为VO-ANFIS建立了规则库；为视觉的运动估计定义了高中低三个方差，分别用VH,VM,VL,表示，相应的模糊规则的输出用GbC表示；
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果
Figure BDA0003514431310000131
类似地，得到VO-ANFIS的
Figure BDA0003514431310000132
a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号，na表示WIO-ANFIS中的模糊规则总数；
Figure BDA0003514431310000133
根据各种工况的具体测量方差VH,VM,VL,WH WL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到：
Figure BDA0003514431310000141
上式中，Wπwa表示规则πwa对应的权重，Vπvb表示规则πvb对应的权重，a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号。
在本实施例中，在所述模块4中，模糊集与相应***情况的关系如下表所示：
Figure BDA0003514431310000142
在本实施例中，模糊推理中隶属函数的参数也可根据经验和直觉来选择；而为了实现ANFIS***的适应性，需要对里程计的测量有一个真值；它是为车辆提供无漂移定位的GPS单元；利用GPS测量值作为训练集的真值的难点在于GPS与IMU的位置不一致；这种偏差很大，特别是在车辆急转弯时，会引入训练数据的误差，导致参数优化错误；因此，将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在IMU坐标系{I}中的坐标可从***设计参数中导出，但对于训练目的来说不够精确，因此在使用它进一步训练ANFIS参数之前有必要对其进行优化。
在本实施例中，在所述模块4中，建立自适应模糊神经推理***(ANFIS)的训练方法，具体步骤如下：
将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在坐标系{I0}中的位置可表示为：
Figure BDA0003514431310000151
其中，IXG是GPS接收机在IMU坐标系{I}中的坐标，
Figure BDA0003514431310000152
是在时间i时，坐标系{I}相对于坐标系{I0}的旋转矩阵；
Figure BDA0003514431310000153
是时间i时GPS路径点在GPS测量坐标系{GM}中的表示，
Figure BDA0003514431310000154
是坐标系{GM}和坐标系{I0}之间的旋转矩阵；
相应地，定义相对于GPS测量的估计轨迹误差为:
Figure BDA0003514431310000155
其中
Figure BDA0003514431310000156
为里程计的估计轨迹；
IXG从***设计参数中导出，在使用它进一步训练ANFIS参数之前，有必要对其进行优化；对于优化后的GPS相对于坐标系{I}的位置估计，可根据求式子(14)的解得到；
Figure BDA0003514431310000157
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
需要优化的参数包括AW，Bv和方差WH，WL，VH，VM，VL，用式(15)表示：
XANFIS＝[AW，Bv，WH，WL，VH，VM，VL] (15)
已知IXG和
Figure BDA0003514431310000158
的情况下，式子(15)中的误差用来构造如下代价优化；XANFIS的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
Figure BDA0003514431310000159
其中
Figure BDA00035144313100001510
是根据参数XANFIS利用ANFIS-ESKF估计的GPS接收机在时间i时相对于坐标系{I0}的位置。
本实施例基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法建立轮式-IMU里程计(WIO)的测量模型，推导***的状态方程；建立视觉里程计(VO)的测量模型，推导***的观测方程；用ESKF对WIO和VO的运动估计信息进行融合；建立ANFIS(AdaptiveNeural Fuzzy Inference System)，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF(Error State Kalman Filter)中预测模型和测量模型的协方差矩阵。本实施例方法经过训练的ANFIS***能够根据地面移动机器人***的状态，比如机器人是否打滑或者环境中的纹理信息是否足够多等，来实时调整ESKF的参数，与传统的固定增益滤波器相比，能够根据***的状态实时调整ESKF参数，大大减少了内部故障和外部干扰造成的误差，使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度，定位精度更高，鲁棒性更强。
实施例三：
本实施例与实施例一基本相同，特别之处在于：
参见图1-图4，在本实施例中，所述模块1中建立WIO测量模型，具体如下：
IMU包括三轴陀螺仪和三轴加速度计，考虑到测试***加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-IMU里程计WIO，提供完整的六自由度运动估计；WIO为ESKF***提供移动机器人运动状态的预测；
图3展示了四轮移动机器人运动学模型的推导，假设左侧车轮速度相同，右侧车轮的速度也相同，αL和αR表示在一个采样周期内，左右车轮的旋转角度；假设路面平滑，车轮不打滑，机器人在采样周期内以圆心O1为中心从点O移动到点M；左右车轮对应的行驶距离分别为ΔSL和ΔSR；式子(17)中ΔSL和ΔSR的原始测量值中的噪声ΔSn服从高斯分布
Figure BDA0003514431310000161
轮式里程计坐标系{W}的原点位于车轮的几何中心，因此机器人在此采样时间内的行走距离ΔS可以由式子(17)得到；
Figure BDA0003514431310000162
其中Rw是车轮的半径；
ΔS＝(ΔSR+ΔSL)/2 (18)
IMU的三轴陀螺仪测量移动机器人的角速度；为了实现ESKF，采用了姿态积分误差测量模型；
Figure BDA0003514431310000163
其中ωt是由三个轴合成的真实角速度，ωm是测量的角速度，ωn是服从高斯分布
Figure BDA0003514431310000164
的随机游走，ωbt是角速度偏移的真实值，ωb是角速度偏移的名义值，δωb是角速度偏移的误差，ωbt的导数服从高斯分布
Figure BDA0003514431310000165
ωw表示角速度偏移的噪声，
Figure BDA0003514431310000166
表示角速度偏移的误差的导数；
在IMU坐标系{I}中测得的角速度矢量可以表示为：
Figure BDA0003514431310000171
定义IMU在第i个采样时间内相对IMU初始坐标系{I0}测得的四元数表示的姿态变化为
Figure BDA0003514431310000172
相对坐标系{I0}的瞬时旋转速度可以由下式计算：
Figure BDA0003514431310000173
其中
Figure BDA0003514431310000174
是四元数的乘法运算，
Figure BDA0003514431310000175
是
Figure BDA0003514431310000176
的四元数表示；
IMU在第i+1个采样周期内的姿态可以由角速度的迭代积分得到；
Figure BDA0003514431310000177
WIO相对于{I0}的姿态由式子(22)计算得到，位置的输出需要更多的推断；定义
Figure BDA0003514431310000178
和
Figure BDA0003514431310000179
的偏差为
Figure BDA00035144313100001710
另外，也可用旋转矩阵
Figure BDA00035144313100001711
来表示；车轮坐标系的相应的姿态变化可通过下式来获得：
Figure BDA00035144313100001712
WIO相对于轮式里程计坐标系{W}的瞬时位移为：
Figure BDA00035144313100001713
它可通过式子(1)转换到坐标系{I}中表示，下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化：
Figure BDA00035144313100001714
在第i+1个采样时间处移动机器人相对于初始坐标系{I0}的位置可以通过式子(25)计算：
Figure BDA00035144313100001715
其中
Figure BDA00035144313100001716
是对应于
Figure BDA00035144313100001717
的旋转矩阵，式子(22)和式子(22)是六自由度WIO的位姿估计输出的迭代形式；其中
Figure BDA00035144313100001718
是i+1时刻测得的IMU坐标系的姿态变化，
Figure BDA00035144313100001719
是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系，
Figure BDA0003514431310000181
是轮式里程计坐标系相对于IMU坐标系的姿态标定关系，
Figure BDA0003514431310000182
是i+1时刻测得的轮式里程计坐标系的位置变化，I是单位矩阵。
在本实施例中，所述模块1中建立***状态方程，具体如下：
定义***的状态为：
Figure BDA0003514431310000183
其中pi为i时刻IMU坐标系相对于初始时刻的位置，qi为i时刻IMU坐标系相对于初始时刻的姿态，
Figure BDA0003514431310000184
为i时刻IMU角速度偏移；
那么根据WIO测量模型得到***的名义状态方程为：
Figure BDA0003514431310000185
其中
Figure BDA0003514431310000186
是i时刻轮式里程计的测量值，
Figure BDA00035144313100001812
是i时刻IMU坐标系相对于0时刻的姿态，
Figure BDA0003514431310000187
是i时刻IMU测得的角速度，
Figure BDA0003514431310000188
是i时刻IMU的角速度偏移，
Figure BDA0003514431310000189
Δt是IMU的采样周期，[γi]×指的是γi对应的反对称矩阵；pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置，
Figure BDA00035144313100001810
表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数，
Figure BDA00035144313100001811
表示i+1时刻IMU角速度偏移对时间的导数；
误差状态方程为：
Figure BDA0003514431310000191
其中θi是qi对应的李代数，ωn i是IMU的测量噪声，
Figure BDA0003514431310000192
是IMU角速度偏移的倒数，
Figure BDA0003514431310000193
表示i时刻轮式里程计的测量噪声；ωn i表示i时刻IMU的角速度测量噪声；Δt表示IMU采样周期。
在本实施例中，所述模块2中建立视觉里程计(VO)的测量模型，推导***的观测方程，具体如下：
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化
Figure BDA0003514431310000194
和位置变化
Figure BDA0003514431310000195
观测模型为：
Figure BDA0003514431310000196
Figure BDA0003514431310000197
Figure BDA0003514431310000198
是相机观测到的从第j-1帧到第j帧的位移，它和
Figure BDA0003514431310000199
相等；
Figure BDA00035144313100001910
是的从时间j-1到时间j的旋转向量，和
Figure BDA00035144313100001911
等价；ΔpC，n是随机位移误差，服从高斯分布ΔpC，n～N(0，V1)；ΔθC，n是随机的姿态误差，服从高斯分布ΔθC，n～N(0，V2)；
Figure BDA00035144313100001912
表示第j-1帧到第j帧的相机位移变化的测量值；
Figure BDA00035144313100001913
表示从时间j-1到时间j的相机姿态变化的测量值；
根据IMU坐标系{I}和相机坐标系{C}的位姿关系
Figure BDA00035144313100001914
和
Figure BDA00035144313100001915
VO可提供相机观测到的相对于IMU坐标系{Ij-1}的实时位姿变化，如下式(7)所示：
Figure BDA0003514431310000201
在本实施例中，所述模块2中建立***的观测方程，具体如下：
Figure BDA0003514431310000202
上式中，
Figure BDA0003514431310000203
表示相机观测到的相机坐标系的位移变化；
Figure BDA0003514431310000204
表示相机观测到的IMU坐标系的位移变化；
Figure BDA0003514431310000205
表示相机观测到的IMU坐标系的姿态变化。
进一步地，模块3所述用ESKF对WIO和VO的运动估计信息进行融合，具体如下：
ESKF分为预测和校正两个阶段；
ESKF的预测过程具体如下：
根据卡尔曼滤波模型，误差状态δxi+1和误差协方差矩阵P由下列式子更新：
Figure BDA0003514431310000206
上式中，
Figure BDA0003514431310000207
表示的扰动；Fi表示误差状态方程相对于扰动的雅可比矩阵；Fx表示误差状态方程相对于误差状态的雅可比矩阵；Fi T表示Fi的转置；Qi表示扰动的协方差矩阵；Fx T表示Fx的转置；Pi表示i时刻误差状态的协方差矩阵，Pi+1表示i+1时刻误差状态的协方差矩阵；
式子(4)的结果给出了误差状态模型的线性表示，式子(26)中的矩阵Fx和矩阵Fi为：
Figure BDA0003514431310000208
上式中，I表示单位矩阵；Δt表示IMU采样周期；
另外，随机误差的协方差矩阵Q被定义为：
Figure BDA0003514431310000211
Q1,Q2,Q3是相应的三个误差状态的测量噪声的方差，它们被定义为：
Figure BDA0003514431310000212
其中
Figure BDA0003514431310000213
分别是Sn、ωn、ωw的方差；
ESKF的校正过程具体如下：
Figure BDA0003514431310000214
其中，
Figure BDA0003514431310000215
是输出矩阵；
Figure BDA0003514431310000216
是视觉测量的误差协方差矩阵；Δxj表示***误差状态的预测值；
Figure BDA0003514431310000217
表示***状态的观测值与预测值之间的误差；HT表示H的转置；Sj是卡尔曼增益的分母；Pj表示***误差状态的协方差的预测值；
Figure BDA0003514431310000218
表示卡尔曼增益；
Figure BDA0003514431310000219
表示***误差状态的修正值；
定义名义模型在第j帧输出的机器人位姿为：
Figure BDA00035144313100002110
其中，qj表示j时刻IMU坐标系相对于0时刻IMU坐标系的姿态的名义值；Xj表示j时刻IMU坐标系相对于0时刻IMU坐标系的位置的名义值；
用修正的误差状态
Figure BDA00035144313100002111
补偿Xj可得到ESKF估计的位姿：
Figure BDA0003514431310000221
其中，
Figure BDA0003514431310000222
和
Figure BDA0003514431310000223
分别是
Figure BDA0003514431310000224
中的位移和四元数形式的旋转。
在本实施例中，在所述模块4中，建立自适应模糊神经推理***ANFIS，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵，具体步骤如下：
采用两个ANFIS，第一个ANFIS记作WIO-ANFIS，结构如图2(a)所示，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是WIO的协方差矩阵Q1；第二个ANFIS记作VO-ANFIS，结构如图2(b)所示，其输入的是视觉里程计的特征点个数Nu和重投影误差Er，输出是VO的协方差矩阵V；
定义WIO-ANFIS的输入为Xw＝(ω，ρ)T，定义它的模糊集为A；为A中的每一个成员Aa设计一个隶属度函数，并且记作Amf-a(xw)∈[0,1]；Amf-a(xw)越接近1，Xw属于Aa的可能性越大；VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作Xv＝(Nu，Er)T，定义它的模糊集为B；构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似；
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡，其数学表达式和曲线形状见图4；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
在A中为ρ定义了四个成员，它们分别趋向于+∞(A1)，趋向于1(A2)，趋向于-1(A3)，趋向于-∞(A4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；A中的ω有两个成员：小(A5)和大(A6)；它们分别对应着慢转弯和快转弯的运动情况；同样，B中Er的模糊集分别为小(B1)和大(B2)，Nu的模糊集分别为小的(B3)和大的(B4)；
在本实施例中，在所述模块4中，模糊集与相应***情况的关系如表1所示：
表1.模糊集与工作情况的关系表
Figure BDA0003514431310000231
隶属度函数定义了输入与其对应的模糊成员之间的关系，如图4所示；在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的***情况；表示A和B中的自适应变量分别为AW和Bv；AW1指定了ρ完全属于A1时初始值；AW2指定了ρ属于A2的终点；AW3指定了ρ开始属于A3的点，AW4指定了ρ完全属于A4时的最大值；函数的另一端固定在临界点±1，简化了复杂度，降低了ANFIS训练中需要优化的变量的维数；AW5，AW6，AW7，AW8，指定了ω的隶属度函数的临界点；Bv1，Bv2，Bv3，Bv4指定了Er的隶属度函数的临界点；Bv5，Bv6，Bv7，Bv8指定了Nu的隶属度函数的临界点；曲线形状的控制方式同AW1，AW2，AW3，AW4；具体情况见图4；
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对WIO和VO的测量方差进行了推理；建立的规则库以及每条规则对应的测量方差列于表2和表3中；
表2.WIO-ANFIS的规则库
ω ρ 规则库 方差
A<sub>6</sub> A<sub>1</sub> π<sub>w5</sub> W<sub>H</sub>
A<sub>6</sub> A<sub>2</sub> π<sub>w6</sub> W<sub>H</sub>
A<sub>6</sub> A<sub>4</sub> π<sub>w8</sub> W<sub>H</sub>
A<sub>5</sub> A<sub>1</sub> π<sub>w1</sub> W<sub>H</sub>
A<sub>5</sub> A<sub>4</sub> π<sub>w4</sub> W<sub>H</sub>
A<sub>6</sub> A<sub>3</sub> π<sub>w7</sub> W<sub>L</sub>
A<sub>5</sub> A<sub>2</sub> π<sub>w2</sub> W<sub>L</sub>
A<sub>5</sub> A<sub>3</sub> π<sub>w8</sub> W<sub>H</sub>
表3.VO-ANFIS的规则库
N<sub>u</sub> E<sub>r</sub> 规则库 方差
B<sub>2</sub> B<sub>3</sub> π<sub>v2</sub> V<sub>L</sub>
B<sub>2</sub> B<sub>4</sub> π<sub>v4</sub> V<sub>H</sub>
B<sub>1</sub> B<sub>3</sub> π<sub>v1</sub> V<sub>M</sub>
B<sub>1</sub> B<sub>4</sub> π<sub>v3</sub> V<sub>H</sub>
对于WIO-ANFIS，当输入组合为(A3，A6)和(A5，A2)时，方差为低，用WL表示；物理的解释是，当车辆正在急转弯(A3)，并且IMU测量表明同样的情况(A6)，或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5)，移动机器人运动时可能没有打滑，从而WIO的方差应该比较小；否则，IMU的方差应该比较大，用WH表示；
对于每一条规则，例如πw7(A3，A6)，它的输出定义如式子(9)所示；根据同样的规则，得到πwa的输出，用Gaw表示；
G7W＝min(Amf-6(xw)，Amf-3(xw)) (9)
类似地，也为VO-ANFIS建立了规则库；为视觉的运动估计定义了高中低三个方差，分别用VH，VM，VL，表示，相应的模糊规则的输出用GbC表示；
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果
Figure BDA0003514431310000241
类似地，得到VO-ANFIS的
Figure BDA0003514431310000242
a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号，na表示W1O-ANFIS中的模糊规则总数；
Figure BDA0003514431310000243
根据各种工况的具体测量方差VH，VM，VL，WH WL，则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到：
Figure BDA0003514431310000251
上式中，Wπwa表示规则πwa对应的权重，Vπvb表示规则πvb对应的权重，a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号。
在本实施例中，模糊推理中隶属函数的参数也可根据经验和直觉来选择；而为了实现ANFIS***的适应性，需要对里程计的测量有一个真值；它是为车辆提供无漂移定位的GPS单元；利用GPS测量值作为训练集的真值的难点在于GPS与IMU的位置不一致；这种偏差很大，特别是在车辆急转弯时，会引入训练数据的误差，导致参数优化错误；因此，将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在IMU坐标系{I}中的坐标可从***设计参数中导出，但对于训练目的来说不够精确，因此在使用它进一步训练ANFIS参数之前有必要对其进行优化。
在本实施例中，在所述模块4中，建立自适应模糊神经推理***(ANFIS)的训练方法，具体步骤如下：
将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在坐标系{I0}中的位置可表示为：
Figure BDA0003514431310000252
其中，IXG是GPS接收机在IMU坐标系{I}中的坐标，
Figure BDA0003514431310000253
是在时间i时，坐标系{I}相对于坐标系{I0}的旋转矩阵；
Figure BDA0003514431310000254
是时间i时GPS路径点在GPS测量坐标系{GM}中的表示，
Figure BDA0003514431310000255
是坐标系{GM}和坐标系{I0}之间的旋转矩阵；
相应地，定义相对于GPS测量的估计轨迹误差为:
Figure BDA0003514431310000256
其中
Figure BDA0003514431310000257
为里程计的估计轨迹；
IXG从***设计参数中导出，在使用它进一步训练ANFIS参数之前，有必要对其进行优化；对于优化后的GPS相对于坐标系{I}的位置估计，可根据求式子(14)的解得到；
Figure BDA0003514431310000261
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
需要优化的参数包括AW，Bv和方差WH，WL，VH，VM，VL，用式(15)表示：
XANFIS＝[AW，Bv，WH，WL，VH，VM，VL] (15)
已知IXG和
Figure BDA0003514431310000262
的情况下，式子(15)中的误差用来构造如下代价优化；XANFIS的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
Figure BDA0003514431310000263
其中
Figure BDA0003514431310000264
是根据参数XANFIS利用ANFIS-ESKF估计的GPS接收机在时间i时相对于坐标系{I0}的位置。
本实施例方法经过训练的ANFIS***能够根据地面移动机器人***的状态，比如机器人是否打滑或者环境中的纹理信息是否足够多等，来实时调整ESKF的参数，与传统的固定增益滤波器相比，大大减少了内部故障和外部干扰造成的误差，使得地面移动机器人在各种复杂工况下都能够保持较高的定位精度，鲁棒性更强。采用本实施例方法，在移动机器人上对算法进行验证，结果表明，与固定增益ESKF相比，基于ANFIS的ESKF的平均轨迹误差降低了80.6％，即使有意引入外部干扰，其平均位置漂移也在1％以下。
本发明上述实施例基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法。ANFIS分别推断WIO和VO的方差，并将结果用于在线自适应调整ESKF的融合增益。由于模糊网络能够根据***当前状态识别***异常，因此估计算法能够即时调整融合参数，与传统的固定增益滤波器相比，上述实施例方法大大减少了内部故障和外部干扰造成的误差。与固定增益ESKF相比，基于ANFIS的ESKF平均轨迹误差要小很多，即使有意引入外部干扰，其平均位置漂移也能维持在很小的范围内。
上面对本发明实施例结合附图进行了说明，但本发明不限于上述实施例，还可以根据本发明的发明创造的目的做出多种变化，凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化，均应为等效的置换方式，只要符合本发明的发明目的，只要不背离本发明的技术原理和发明构思，都属于本发明的保护范围。

Claims (6)

1.一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，包括模块1、模块2、模块3和模块4共四个计算模块，其特征在于：
所述模块1，建立轮式-IMU里程计(WIO)的测量模型，推导***的状态方程；
所述模块2，建立视觉里程计(VO)的测量模型，推导***的观测方程；
所述模块3，用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合；
所述模块4，建立自适应模糊神经推理***(ANFIS)，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵；该模块在使用前需要根据GPS数据对ANFIS进行训练；
所述模块1的输入是轮式里程计数据和IMU数据，输出是相邻时刻IMU坐标系的位姿估计；模块2的输入是双目相机拍摄的照片，输出是相邻时刻相机坐标系的位姿估计；模块3的输入是模块1和模块2的输出，输出是相邻时刻IMU坐标系的位姿估计；模块4的输入是模块1中WIO和模块2中VO的中间数据，输出是模块3中需要使用的WIO和VO的协方差矩阵；
整个定位程序的输入是IMU数据、轮式里程计数据、相机数据，输出是IMU坐标系的位姿变化。
2.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，所述模块1中建立WIO测量模型，具体如下：
IMU包括三轴陀螺仪和三轴加速度计，考虑到测试***加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-IMU里程计WIO，提供完整的六自由度运动估计；WIO为ESKF***提供移动机器人运动状态的预测；
下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化：
Figure FDA0003514431300000011
其中
Figure FDA0003514431300000012
是i+1时刻测得的IMU坐标系的姿态变化，
Figure FDA0003514431300000013
是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系，
Figure FDA0003514431300000014
是轮式里程计坐标系相对于IMU坐标系的姿态标定关系，
Figure FDA0003514431300000015
是i+1时刻测得的轮式里程计坐标系的位置变化，I是单位矩阵。
3.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，其特征在于：所述模块1中建立***状态方程，具体如下：
定义***的状态为：
Figure FDA0003514431300000021
其中pi为i时刻IMU坐标系相对于初始时刻的位置，qi为i时刻IMU坐标系相对于初始时刻的姿态，
Figure FDA0003514431300000022
为i时刻IMU角速度偏移；
那么根据WIO测量模型得到***的名义状态方程为：
Figure FDA0003514431300000023
其中
Figure FDA0003514431300000024
是i时刻轮式里程计的测量值，
Figure FDA00035144313000000211
是i时刻IMU坐标系相对于0时刻的姿态，
Figure FDA0003514431300000025
是i时刻IMU测得的角速度，
Figure FDA0003514431300000026
是i时刻IMU的角速度偏移，
Figure FDA0003514431300000027
Δt是IMU的采样周期，[γi]×指的是γi对应的反对称矩阵；pi+1表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置，
Figure FDA0003514431300000028
表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数，
Figure FDA0003514431300000029
表示i+1时刻IMU角速度偏移对时间的导数；
误差状态方程为：
Figure FDA00035144313000000210
其中θi是qi对应的李代数，ωn i是IMU的测量噪声，
Figure FDA0003514431300000031
是IMU角速度偏移的导数；
Figure FDA0003514431300000032
表示i时刻轮式里程计的测量噪声；ωn i表示i时刻IMU的角速度测量噪声；Δt表示IMU采样周期。
4.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，其特征在于：所述模块2中建立视觉里程计(VO)的测量模型，推导***的观测方程，具体如下：
视觉里程计可估计移动机器人在两个序列帧之间的姿态变化
Figure FDA0003514431300000033
和位置变化
Figure FDA0003514431300000034
观测模型为：
Figure FDA0003514431300000035
Figure FDA0003514431300000036
Figure FDA0003514431300000037
是相机观测到的从第j-1帧到第j帧的位移，它和
Figure FDA0003514431300000038
相等；
Figure FDA0003514431300000039
是的从时间j-1到时间j的旋转向量，和
Figure FDA00035144313000000310
等价；ΔpC，n是随机位移误差，服从高斯分布ΔpC，n～N(0，V1)；ΔθC，n是随机的姿态误差，服从高斯分布ΔθC，n～N(0，V2)；
Figure FDA00035144313000000319
表示第j-1帧到第j帧的相机位移变化的测量值；
Figure FDA00035144313000000318
表示从时间j-1到时间j的相机姿态变化的测量值；
根据IMU坐标系{I}和相机坐标系{C}的位姿关系
Figure FDA00035144313000000311
和
Figure FDA00035144313000000312
VO可提供相机观测到的相对于IMU坐标系{Ij-1}的实时位姿变化，如下式(7)所示：
Figure FDA00035144313000000313
进一步地，所述模块2中建立***的观测方程，具体如下：
Figure FDA00035144313000000314
上式中，
Figure FDA00035144313000000315
表示相机观测到的相机坐标系的位移变化；
Figure FDA00035144313000000316
表示相机观测到的IMU坐标系的位移变化；
Figure FDA00035144313000000317
表示相机观测到的IMU坐标系的姿态变化。
5.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，其特征在于：在所述模块4中，建立自适应模糊神经推理***ANFIS，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵，具体步骤如下：
采用两个ANFIS，第一个ANFIS记作WIO-ANFIS，其输入的是移动机器人的角速度ω和左右车轮的转速差ρ，输出是WIO的协方差矩阵Q1；第二个ANFIS记作VO-ANFIS，其输入的是视觉里程计的特征点个数Nu和重投影误差Er，输出是VO的协方差矩阵V；
定义WIO-ANFIS的输入为Xw＝(ω，ρ)T，定义它的模糊集为A；为A中的每一个成员Aa设计一个隶属度函数，并且记作Amf-a(xw)∈[0,1]；Amf-a(xw)越接近1，Xw属于Aa的可能性越大；VO-ANFIS的输入是特征点数量Nu和重投影误差Er记作xv＝(Nu，Er)T，定义它的模糊集为B；构建隶属度函数Bmf-b(xν)的方法与Amf-a(xw)类似；
在设计隶属函数时考虑了模糊集合中成员之间的平滑过渡；曲线形状通过改变函数中的自适应参数进行调整；选择合适的自适应参数的原则是保证函数的灵活性和保持有限的变量数量；
在A中为ρ定义了四个成员，它们分别趋向于+∞(A1)，趋向于1(A2)，趋向于-1(A3)，趋向于-∞(A4)；它们分别对应于移动机器人滑转、直走、旋转和滑转；A中的ω有两个成员：小(A5)和大(A6)；它们分别对应着慢转弯和快转弯的运动情况；同样，B中Er的模糊集分别为小(B1)和大(B2)，Nu的模糊集分别为小的(B3)和大的(B4)；
隶属度函数定义了输入与其对应的模糊成员之间的关系，在每个隶属函数中，有一两个变量使其可调整，以更好地适应真实的***情况；表示A和B中的自适应变量分别为AW和Bv；AW1指定了ρ完全属于A1时初始值；AW2指定了ρ属于A2的终点；AW3指定了ρ开始属于A3的点，AW4指定了ρ完全属于A4时的最大值；函数的另一端固定在临界点±1，简化了复杂度，降低了ANFIS训练中需要优化的变量的维数；AW5,AW6,AW7,AW8,指定了ω的隶属度函数的临界点；Bv1,Bv2,Bv3,Bv4指定了Er的隶属度函数的临界点；Bv5,Bv6,Bv7,Bv8指定了Nu的隶属度函数的临界点；曲线形状的控制方式同AW1,AW2,AW3,AW4；
模糊规则库定义了模糊集与输出之间的逻辑关系；根据不同的输入条件，对WIO和VO的测量方差进行了推理；
对于WIO-ANFIS，当输入组合为(A3,A6)和(A5,A2)时，方差为低，用WL表示；物理的解释是，当车辆正在急转弯(A3)，并且IMU测量表明同样的情况(A6)，或移动机器人运动中没有大旋转(A2)并且IMU测量表明同样的情况(A5)，移动机器人运动时可能没有打滑，从而WIO的方差应该比较小；否则，IMU的方差应该比较大，用WH表示；
对于每一条规则，例如πw7(A3,A6)，它的输出定义如式子(9)所示；根据同样的规则，得到πwa的输出，用GaW表示；
G7W＝min(Amf-6(xw),Amf-3(xw)) (9)
类似地，也为VO-ANFIS建立了规则库；为视觉的运动估计定义了高中低三个方差，分别用VH,VM,VL,表示，相应的模糊规则的输出用GbC表示；
根据式子(10)用GaW来计算WIO-ANFIS的归一化推理结果
Figure FDA0003514431300000051
类似地，得到VO-ANFIS的
Figure FDA0003514431300000052
a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号，na表示WIO-ANFIS中的模糊规则总数；
Figure FDA0003514431300000053
根据各种工况的具体测量方差VH,VM,VL,WHWL,则WIO的方差Q1和VO的方差V的最终结果由式子(11)得到：
Figure FDA0003514431300000054
上式中，Wπwa表示规则πwa对应的权重，Vπvb表示规则πvb对应的权重，a表示WIO-ANFIS中模糊规则的编号，b表示VO-ANFIS中模糊规则的编号。
6.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，其特征在于：在所述模块4中，建立自适应模糊神经推理***(ANFIS)的训练方法，具体步骤如下：
将GPS的直接测量转化到IMU坐标系{I}中，以提供准确的训练数据；GPS接收机在坐标系{I0}中的位置可表示为：
Figure FDA0003514431300000061
其中，IXG是GPS接收机在IMU坐标系{I}中的坐标，
Figure FDA0003514431300000062
是在时间i时，坐标系{I}相对于坐标系{I0}的旋转矩阵；
Figure FDA0003514431300000063
是时间i时GPS路径点在GPS测量坐标系{GM}中的表示，
Figure FDA0003514431300000064
是坐标系{GM}和坐标系{I0}之间的旋转矩阵；
相应地，定义相对于GPS测量的估计轨迹误差为:
Figure FDA0003514431300000065
其中
Figure FDA0003514431300000066
为里程计的估计轨迹；
IXG从***设计参数中导出，在使用它进一步训练ANFIS参数之前，有必要对其进行优化；对于优化后的GPS相对于坐标系{I}的位置估计，可根据求式子(14)的解得到；
Figure FDA0003514431300000067
其中i表示时间戳的索引，ni表示时间戳的总数，s表示训练轨迹的索引，ns表示训练轨迹的总数；
需要优化的参数包括Aw，Bv和方差WH，WL，VH，VM，VL，用式(15)表示：
XANFIS＝[AW，Bv，WH，WL，VH，VM，VL] (15)
已知IXG和
Figure FDA0003514431300000068
的情况下，式子(15)中的误差用来构造如下代价优化；XANFIS的解是模糊网络的优化后的参数集合，用来拟合训练数据集；
Figure FDA0003514431300000069
其中
Figure FDA00035144313000000610
是根据参数XANFIS利用ANFIS-ESKF估计的GPS接收机在时间i时相对于坐标系{I0}的位置。
CN202210160522.2A 2022-02-22 2022-02-22 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 Pending CN115060260A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210160522.2A CN115060260A (zh) 2022-02-22 2022-02-22 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210160522.2A CN115060260A (zh) 2022-02-22 2022-02-22 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法

Publications (1)

Publication Number Publication Date
CN115060260A true CN115060260A (zh) 2022-09-16

Family

ID=83196894

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210160522.2A Pending CN115060260A (zh) 2022-02-22 2022-02-22 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法

Country Status (1)

Country Link
CN (1) CN115060260A (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN112556719A (zh) * 2020-11-27 2021-03-26 广东电网有限责任公司肇庆供电局 一种基于cnn-ekf的视觉惯性里程计实现方法
CN113252033A (zh) * 2021-06-29 2021-08-13 长沙海格北斗信息技术有限公司 基于多传感器融合的定位方法、定位***及机器人

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法
CN112556719A (zh) * 2020-11-27 2021-03-26 广东电网有限责任公司肇庆供电局 一种基于cnn-ekf的视觉惯性里程计实现方法
CN113252033A (zh) * 2021-06-29 2021-08-13 长沙海格北斗信息技术有限公司 基于多传感器融合的定位方法、定位***及机器人

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
ZHU JIE等: "Multisensor Fusion Using Fuzzy Inference System for a Visual-IMU-Wheel Odometry", IEEE, 26 January 2021 (2021-01-26), pages 1 - 11 *
徐田来;崔平远;崔祜涛;: "车载多传感器组合导航***设计与实现", ***工程与电子技术, no. 04, 15 April 2008 (2008-04-15) *
荣健;乔文钊;: "基于模糊神经***的多传感器数据融合算法", 电子科技大学学报, no. 03, 30 May 2010 (2010-05-30) *
路丹晖;周文晖;龚小谨;刘济林;: "视觉和IMU融合的移动机器人运动解耦估计", 浙江大学学报(工学版), no. 06, 15 June 2012 (2012-06-15) *

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及***
CN102538781B (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
Liu et al. Visual-inertial odometry tightly coupled with wheel encoder adopting robust initialization and online extrinsic calibration
CN112505737B (zh) 一种gnss/ins组合导航方法
CN107014376A (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
Wang et al. Vehicle localization at an intersection using a traffic light map
Marin et al. Event-based localization in ackermann steering limited resource mobile robots
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN107607113A (zh) 一种两轴姿态倾角测量方法
CN112254741A (zh) 里程传感器的异常检测方法、自移动机器人及存储介质
CN106679672A (zh) 一种基于dbn和卡尔曼滤波算法的agv定位算法
CN110763224A (zh) 一种自动导引运输车导航方法及导航***
Tang et al. OdoNet: Untethered speed aiding for vehicle navigation without hardware wheeled odometer
CN115060260A (zh) 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法
CN112697153A (zh) 自主移动设备的定位方法、电子设备及存储介质
Su et al. GR-SLAM: Vision-based sensor fusion SLAM for ground robots on complex terrain
Guo et al. Model-based deep learning for low-cost IMU dead reckoning of wheeled mobile robot
Beauvisage et al. Multimodal visual-inertial odometry for navigation in cold and low contrast environment
CN116300991A (zh) 一种基于强化学习和事件触发的无人飞行器轨迹跟踪方法
Song An integrated GPS/vision UAV navigation system based on Kalman filter
Javed et al. Attitude in motion: Constraints aided accurate vehicle orientation tracking in harsh environment
Zhou et al. IMU dead-reckoning localization with RNN-IEKF algorithm
Leng et al. An improved method for odometry estimation based on EKF and Temporal Convolutional Network
Zang et al. Event-triggered Extended Kalman Filter for UAV Monitoring System
Hong et al. Angle measurements for mobile robots with filtering of short-term noise in MEMS gyroscopes

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