CN114022553B 基于异构传感器信息融合的位姿跟踪系统及方法 （上海交通大学）

MAOTianyang等.Lidar-Visual-InerOdometrywithOnlineExCalibration.J.ShanghaiJiaoTongUniv.基于异构传感器信息融合的位姿跟踪系统本发明提供了一种基于异构传感器信息融模型，基于观测模型进行观测更新位姿状态预骤S4：利用PaRIS算法对非线性状态空间模型进2步骤S1：基于内部传感器提取运动数据建立六自由度运动步骤S2：基于外部传感器提取环境特征信息建立观测步骤S3：针对异构传感器位姿状态进行非线性状态空间建3其中πt+1为t+1时刻的预测分布，Dogrxrslo-o:为t+1时刻的观测概率密度函数梯度在θt获得；服从高斯噪声ns~N(0,ES),nS表示服从高斯分布的白噪声；表所述内部运动数据是通过直接获取位姿信息的传感器获取的运动所述外部采集数据是通过直接从外部采集数据的传感器获取的环境4对数似然函数关于θ的雅克比为：Ic定义X,X)和(Y,Y)为可测空间，且Qθ:XXX→[0,1]和Gθ:YXY→[0,1]为由参数θθ3θ0:t)表示0到t时刻的观测为y0:t时的似然函数；5在外参标定流程中，将里程计状态XO设为状态变量X，将IMU，相机，激光雷达的状态[XIXcXL]设为观测变量Y,则转移核Qθ由系统运动学模型确定，转移核Gθ由待估计外参模块M1：基于内部传感器提取运动数据建立六自由度运动模型，基于六模块M2：基于外部传感器提取环境特征信息建立观测模型，基于观模块M3：针对异构传感器位姿状态进行非线性状态空间建模，得到非线6所述内部运动数据是通过直接获取位姿信息的传感器获取的运动7所述外部采集数据是通过直接从外部采集数据的传感器获取的环境j=c(qso)"c(qr)"(BV-p)-c(qso对数似然函数关于θ的雅克比为：Ic定义X,X)和(Y,Y)为可测空间，且Qθ:XXX→[0,1]和Gθ:YXY→[0,1]为由参数θ8θ3θ0:t)表示0到t时刻的观测为y0:t时的似然函数；对于t∈N和fEF(X),下面的式子成立：在外参标定流程中，将里程计状态XO设为状态变量X，将IMU，相机，激光雷达的状态[XIXcXL]设为观测变量Y,则转移核Wθ由系统运动学模型确定，转移核Gθ由待估计外参及传感器测量模型确定。9在长期工作或者数据采集过程中的多传感器的在线联合标定及位一传感器受限于自身工作环境的约束：恶劣天气会造成激光雷达测量精度下降甚至失效；[0007][1]NaglaKS,UddinM,SinghD.Multisensordatafusionandintegrationformobilerobots:Areview[J].IAESInternationalJournalofRoboticsandAutomation,2014,3(2):131.[0008][2]HanL,LinY,DuG,etal.Deepvio:Self-superviseddeeplearningofmonocularvisualinertialodometryusing3dgeometricconstraints[C]//2019IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems[0009][3]QinT,LiP,ShenS.Vins-mono:Arobustandversatilemonocularvisual-inertialstateestimator[J].IEEETransactionsonRobotics,2018,34(4):1004-1020.[0010][4]LiM,MourikisAI.High-precision,consistentEKF-basedvisual-inertialodometry[J].TheInternationalJournalofRoboticsResearch,2013,32感器获得；服从高斯噪声ns~N(0,s),nS表示服从高斯分布的白噪声；表示高斯分布；[0028]j=c(qcd∈N*确定的马尔科夫转移核；定义xeM1(x)为状态X0的初始分布，其中M1(x)为概率测3表示0到t时刻的观测为y0:t时的似然函数；[0050]当s≤t,s+1时刻的状态Xs+1和0到t时刻的观测Y0:t，s时刻的状态Xs的概率分布记[0059]即对于任意的函数ht(x0:t)，存在函数集合满足上式，那么{Tt；θ及传感器测量模型确定。[0067]其中，为大小为N的带权重的粒子集；为tc[0113]定义X,X)和(Y,Y)为可测空间，且和为由参数3表示0到t时刻的观测为y0:t时的似然函数；[0125]当s≤t,s+1时刻的状态Xs+1和0到t时刻的观测Y0:t，s时刻的状态Xs的概率分布记[0134]即对于任意的函数ht(x0:t)，存在函数集合满足上式，那么{Tt；θ及传感器测量模型确定。[0142]其中，为大小为N的带[0150]其中为预测分布πt；θ关于参[0165]通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、[0168]本发明提供了一种基于异构传感器信息融合的位姿跟踪系统，可作为移动机器人、无人车或是行人等提供室内环境下的高精度定位数据。该系统集成惯性测量单元对多外部传感器融合过程进行非线性状态空间建模；最后，利用PaRIS(Particle-basedRapidIncrementalSmoother)算法对融合模型进行在线正切滤波估计，并利用随机梯度[0174]步骤S4：利用PaRIS算法对非线性状态空间模型进行在线正切滤波估计得到外参c[0195]定义x,x)和yrer(re-),f为定义在(xs,xs+1,...,xs′)上的函数，该表示0到t时刻的观测为y0:t时的似然函数；[0207]当s≤t,s+1时刻的状态Xs+1和0到t时刻的观测y0:t，s时刻的状态Xs的概率分布记[0216]即对于任意的函数ht(x0:t)，存在函数集合满足上式，那么及传感器测量模型确定。[0248]模块M4：利用PaRIS算法对非线性状态空间模型进行在线正切滤波估计得到外参c表示0到t时刻的观测为y0:t时的似然函数；[0281]当s≤t,s+1时刻的状态Xs+1和0到t时刻的观测Y0:t，s时刻的状态Xs的概率分布记[0290]即对于任意的函数ht(x0:t)，存在函数集合[0298]其中，为大小为N的带权重的