Pose Tracking System and Method Based on Heterogeneous Sensor Information Fusion

Abstract

The present invention provides a pose tracking method and system based on heterogeneous sensor information fusion, including: Step S1: Extract motion data based on internal sensors to establish a six-degree-of-freedom motion model, and perform pose state prediction based on the six-degree-of-freedom motion model; Step S2: Extract environmental feature information based on external sensors to establish an observation model, and perform observation update of the pose state prediction based on the observation model; Step S3: Perform nonlinear state space modeling on the pose states of heterogeneous sensors to obtain a nonlinear state space model; Step S4: Use the PaRIS algorithm to perform online tangent filtering estimation on the nonlinear state space model, and use the stochastic gradient descent algorithm to update the external parameters of multi-sensor calibration in real time, while obtaining high-precision pose estimation. The fusion pose tracking algorithm adopted by the present invention can effectively improve the accuracy and robustness of positioning.

Description

Technical Field

[0001] The present invention relates to the technical fields of data set acquisition of a pose tracking system and multi-sensor fusion, etc. Specifically, it relates to a pose tracking system and method based on heterogeneous sensor information fusion. More specifically, it relates to an online joint calibration and pose estimation scheme for multi-sensors in a pose tracking system during long-term operation or data acquisition process. Background Art

[0002] The multi-sensor fusion technology is a research hotspot in the fields of mobile robots, intelligent driving, etc. at the present stage. Compared with a single sensor which is restricted by its own working environment: bad weather will cause the measurement accuracy of lidar to decline or even fail; the oblique sunlight in the morning and evening may form light spots in the camera's field of view, and insufficient exposure at night brings great difficulties to machine vision. Multi-sensor fusion positioning has higher accuracy and better robustness [1].

[0003] During the long-term operation of a pose tracking system or the long-term operation of a data set, in addition to being affected by external factors, the degradation of sensors, the asynchronous internal clocks of sensors, vibrations and deviations caused by unstable structures will also bring new challenges to the fusion positioning algorithm. How to determine the relative time delay and relative pose between sensors, so as to overcome factors such as vibrations and structural deformations during the movement process, is the primary condition for providing a reliable true value benchmark for long-term data sets.

[0004] The mainstream domestic and foreign solutions include three types: based on deep learning, based on optimization, and based on filtering. [2] proposed a self-supervised deep learning network DeepVIO for monocular visual inertial odometry, which provides absolute trajectory estimation by directly combining 2D optical flow features and IMU data. However, the method based on deep learning is highly dependent on the quality of training data and is not interpretable, making it difficult to be applied in practice; [3] proposed VINS-Mono: a robust and general monocular visual inertial state estimator. This method starts with a robust procedure for estimator initialization and fault recovery. It adopts a method based on tightly coupled and nonlinear optimization, and obtains high-precision visual inertial odometry by fusing pre-integrated IMU measurements and feature observations. The method based on optimization has a relatively high computational complexity and requires high computational performance for pose tracking; [4] proposed the MSCKF2.0 algorithm, which is based on the extended Kalman filter and can achieve consistent estimation, ensuring the correct observability of the established linear system model and being able to perform online estimation and correction of the external parameters of the camera-IMU. The method based on filtering has the characteristic of simple calculation and can usually run in real time, but the accuracy is low.

[0005] The present invention uses a method based on particle filtering to achieve pose estimation. Different from existing filtering schemes, the present invention adopts a scheme that combines high-efficiency forward filtering and backward smoothing in pose estimation, which further improves the accuracy of the algorithm without reducing the computational efficiency too much.

[0006] Based on the work of predecessors, the present invention innovatively proposes a multi-sensor fusion localization algorithm that can be used in the long-term data acquisition process. This method can dynamically adjust the relative poses between various sensors and can be used for the long-term operation of a pose tracking system and as a ground truth solution for long-term data acquisition.

[0007] [1] Nagla K S, Uddin M, Singh D. Multisensor data fusion and integration for mobile robots: A review[J]. IAES International Journal of Robotics and Automation, 2014, 3(2): 131.

[0008] [2] Han L, Lin Y, Du G, et al. Deepvio: Self-supervised deep learning of monocular visual inertial odometry using 3d geometric constraints[C] / / 2019 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019: 6906-6913.

[0009] [3] Qin T, Li P, Shen S. Vins-mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[0010] [4]Li M,Mourikis A I.High-precision,consistent EKF-based visual-inertial odometry[J].The International Journal of Robotics Research,2013,32(6):690-711. Summary of the Invention

[0011] Aiming at the defects in the prior art,the object of the present invention is to provide a pose tracking method and system based on heterogeneous sensor information fusion.

[0012] A pose tracking method based on heterogeneous sensor information fusion provided by the present invention includes:[[]]

[0013] Step S1: Extract motion data based on internal sensors to establish a six-degree-of-freedom motion model, and perform pose state prediction based on the six-degree-of-freedom motion model;

[0014] Step S2: Extract environmental feature information based on external sensors to establish an observation model, and perform observation update and pose state prediction based on the observation model;

[0015] Step S3: Perform nonlinear state space modeling on the pose states of heterogeneous sensors to obtain a nonlinear state space model;

[0016] Step S4: Use the PaRIS algorithm to perform online tangent filtering estimation on the nonlinear state space model, and use the stochastic gradient descent algorithm to update the external parameters of multi-sensor calibration in real time, and at the same time obtain high-precision pose estimation.

[0017] Preferably, in step S1, the following is adopted: construct a six-degree-of-freedom motion model based on internal motion data, and determine the state transition prior;

[0018] The input u of the pose tracking system at time t t =[v t , ω t , including the linear velocity v t and the angular velocity ω t , and is obtained by internal sensors; subject to Gaussian noise n S represents white noise subject to a Gaussian distribution; represents a Gaussian distribution; ∑ S represents the covariance matrix of the Gaussian distribution; the state vector x t =[p t q t , the state vector consists of the position p t and the orientation q t ;

[0019] The internal motion data is the motion data obtained by a sensor that directly acquires pose information. The sensor includes an IMU and an odometer;

[0020] The six-degree-of-freedom motion model adopts:

[0021] p t+1 = p t + v t Δt (1)

[0022]

[0023]

[0024] where p t propagates based on a uniform motion model; v t represents the linear velocity; ω t represents the angular velocity; Δ t represents the time difference between two internal sensor measurements; I 4×4 represents the 4×4 identity matrix; q t represents propagation based on zero-order quaternion integration; ∈ represents a very small number set to prevent numerical instability; the angular velocity ω = [ω x ω y ω z .

[0025] Preferably, the observation model adopts: constructing an observation model based on externally collected data;

[0026] The externally collected data is environmental information obtained by a sensor that directly collects data from the outside. The sensor includes a camera and a lidar;

[0027] The observation model adopts:

[0028]

[0029] where the subscript B represents the body coordinate system corresponding to the motion model; O represents the sensor coordinate system; W represents the world coordinate system; P W represents the 3D feature point spatial position in the world coordinate system; C(·) represents a function that converts a quaternion into a rotation matrix; C(q XY ) and p XY respectively represent the rotation matrix and displacement vector from coordinate system Y to coordinate system X; θ = [p BO q BO represents the external parameters of the external environmental sensor; the superscript T represents the transpose matrix;

[0030] The observation model has Gaussian noise The feature points at time t have the following probability density, including:

[0031]

[0032] where x t represents the six - degree - of - freedom pose, y t represents the actual observation information of the external sensor, ∑ O represents the observation covariance matrix of the sensor, represents the predicted external sensor observation information;

[0033] The Jacobian of the log - likelihood function with respect to θ is:

[0034]

[0035] where ^ is the skew - symmetric matrix operator.

[0036] Preferably, in step S3, the following is adopted: using heterogeneous sensor data and adopting the corresponding positioning algorithm to separately estimate the pose, obtaining the corresponding estimated poses [X O X I X c X L , where X O 、X I 、X c 、X L respectively represent the estimated poses of the odometer, IMU, camera, and lidar;

[0037] The non - linear state - space model adopts:

[0038] Define and as the measurable space, and and are Markov transition kernels determined by the parameter d∈N * ; Define as the initial distribution of the state X0, where is the set of probability measures; For Q θ and G θ , there correspond a transition density function q θ and g θ , satisfying:

[0039] Q θ f(x)=∫f(x′)q θ (x,x′)μ(dx′) (7)

[0040] G θ h(x)=∫h(y)g θ (x,y)μ(dy) (8)

[0041] Among them, Q θ is the state transition kernel, G θ is the observation update kernel, x is the current state, x' is the state at the next moment, y is the observation, f is a function defined on X, and h is a function defined on Y.

[0042] The density function g θ (x, y t ) is abbreviated as g t；θ , which represents the observation probability density at time t when the state is x and the parameter is θ. For any three times (s, s', t) ∈ N such that s ≤ s', 3 , denote φ s:s′|t；θ as the probability distribution of X 0:t under the condition of given Y s:s′ ; for any f is a function defined on (x s , x s+1 ,..., x s′ ), and this distribution is expressed as:

[0043]

[0044] Among them, L θ (y 0:t ) represents the likelihood function when the observations from 0 to t are y 0:t ;

[0045]

[0046] For the filtering distribution φ t:t|t；θ and the prediction distribution φ t+1:t+1|t；θ , they are abbreviated as φ t；θ and π t+1；θ respectively; based on the filtering recursion, for t ∈ N and the following formula holds:

[0047]

[0048] π t+1；θ f = φ t；θ Q θ f (12)

[0049] Among them, φ t；θ is the filtering distribution at time t with parameter θ, and π t；θ is the prediction distribution at time t with parameter θ;

[0050] When s ≤ t, the state X s+1 at s + 1 moment and the observations Y 0:t from 0 to t, the state Xs The probability distribution is denoted as the backward kernel It is expressed as:

[0051]

[0052] φ s；θ is the filtering distribution with parameter θ at time s;

[0053] Using the backward kernel, the joint smoothing distribution φ 0:t|t；θ , that is, given the observations Y from time 0 to t 0:t and the parameter θ, the joint probability distribution of the states x from time 0 to t 0:t can be expressed as:

[0054] φ 0:t|t；θ = φ t；θ T t；θ (14)

[0055] where T t；θ is the joint probability distribution of the states x from time 0 to t - 1 given the observations Y from time 0 to t 0:t and the state x at time t t , and can be calculated by the following formula 0:t-1 :

[0056]

[0057] where denotes the product of probability distributions. When the additive objective function satisfies:

[0058]

[0059] that is, for any function h t (x 0:t ), there exists a function set satisfying the above formula. Then {T t；θ h t} t∈N can be recursively calculated by the following formula:

[0060]

[0061] where is the backward kernel at time t + 1;

[0062] In the external parameter calibration process, the odometer state X O is set as the state variable X, and the states of the IMU, camera, and lidar [X I X c X L are set as the observation variable Y. Then the transition kernel Q θDetermined by the system kinematic model, the transition kernel G θ From the external parameters to be estimated And the sensor measurement model.

[0063] Preferably, the step S4 adopts:

[0064] Particle update step: Update the particle set based on the importance resampling method where N is the number of particles, is the i-th particle in the particle set at time t;

[0065] Auxiliary statistic update step: The backward kernel The estimate of is determined by the following formula:

[0066]

[0067] where, is a weighted particle set of size N; is the i-th particle in the particle set at time t, is the weight corresponding to this particle;

[0068] Auxiliary statistic {T t；θ h t} t∈N The particle estimate of Obtained by the PaRIS algorithm:

[0069]

[0070] where, is the number of reverse index samplings, which is very small compared to N, is the j-th reverse index of the i-th particle at time t+1, independently sampled from the distribution Then φ 0:t|t-1；θ h t Estimated by the following formula:

[0071]

[0072] where is the N-particle filter estimate of the distribution φ 0:t|t-1；θ ;

[0073] Tangent filter calculation step: The tangent filter η t；θ Is defined as follows:

[0074]

[0075] where is the Jacobian of the prediction distribution π t；θ with respect to the parameter θ, then in the particle filter framework, the estimate of η t；θ Is:

[0076]

[0077] External parameter estimation update step: Use the Robbins-Monro algorithm to iteratively calculate the external parameter θ at time t t :

[0078] θ t+1 = θ t + γ t+1 ζ t+1 (23)

[0079] where

[0080]

[0081] where

[0082]

[0083]

[0084]

[0085] where π t+1 is the predicted distribution at time t+1, is the value of the gradient of the observation probability density function at time t+1 at θ t , η t+1 is the tangent filter at time t+1;

[0086] and {γ t} t∈N is the learning rate, satisfying

[0087] According to a pose tracking system based on heterogeneous sensor information fusion provided by the present invention, it includes:

[0088] Module M1: Establish a six-degree-of-freedom motion model based on the motion data extracted by the internal sensor, and perform pose state prediction based on the six-degree-of-freedom motion model;

[0089] Module M2: Establish an observation model based on the environmental feature information extracted by the external sensor, and perform observation update pose state prediction based on the observation model;

[0090] Module M3: Perform non-linear state space modeling on the pose states of heterogeneous sensors to obtain a non-linear state space model;

[0091] Module M4: Use the PaRIS algorithm to perform online tangent filter estimation on the non-linear state space model, and use the stochastic gradient descent algorithm to update the external parameters of multi-sensor calibration in real time, and at the same time obtain high-precision pose estimation.

[0092] Preferably, the module M1 adopts: constructing a six-degree-of-freedom motion model based on internal motion data to determine the prior of state transition;

[0093] The input u of the pose tracking system at time t t =[v t , ω t , including the linear velocity v t and the angular velocity ω t , and is obtained by internal sensors; subject to Gaussian noise n S represents white noise subject to Gaussian distribution; represents Gaussian distribution; ∑ S represents the covariance matrix of Gaussian distribution; the state vector x t =[p t q t , the state vector consists of the position p t and the orientation q t ;

[0094] The internal motion data is the motion data obtained by sensors that directly acquire pose information, and the sensors include IMU and odometer;

[0095] The six-degree-of-freedom motion model adopts:

[0096] p t+1 =p t +v t Δt (1)

[0097]

[0098]

[0099] where p t propagates based on the uniform motion model; v t represents the linear velocity; ω t represents the angular velocity; Δt represents the time difference between two internal sensor measurements; I 4×4 represents the identity matrix of size 4×4; q t represents propagation based on zero-order quaternion integration; ∈ represents a very small number set to prevent numerical instability; the angular velocity ω = [ω x ω y ω z .

[0100] Preferably, the observation model adopts: constructing an observation model based on externally collected data;

[0101] The external acquired data is environmental information obtained through sensors that directly acquire data from the outside. The sensors include cameras and lidars;

[0102] The observation model adopts:

[0103]

[0104] where the subscript B represents the body coordinate system corresponding to the motion model; O represents the sensor coordinate system; W represents the world coordinate system; P W represents the 3D feature point spatial position in the world coordinate system; C(·) represents a function that converts a quaternion into a rotation matrix; C(q XY ) and p XY respectively represent the rotation matrix and displacement vector from coordinate system Y to coordinate system X; θ = [p BO q BO represents the external parameters of the external environmental sensor; the superscript T represents the transpose matrix;

[0105] The observation model has Gaussian noise At time t, the feature point has the following probability density, including:

[0106]

[0107] where x t represents the six-degree-of-freedom pose, y t represents the actual observation information of the external sensor, ∑ O represents the observation covariance matrix of the sensor, represents the predicted external sensor observation information;

[0108] The Jacobian of the log-likelihood function with respect to θ is:

[0109]

[0110] where ^ is the skew-symmetric matrix operator.

[0111] Preferably, the module M3 adopts: using heterogeneous sensor data and corresponding positioning algorithms to separately estimate the pose, obtaining the corresponding estimated poses [X O X I X c X L , where X O , X I , X c , X L respectively represent the estimated poses of the odometer, IMU, camera, and lidar;

[0112] The non-linear state space model adopts:

[0113] Define and as measurable spaces, and and be Markov transition kernels determined by the parameter d ∈ N * ; define as the initial distribution of the state X0, where is the set of probability measures; for Q θ and G θ , there corresponds a transition density function q θ and g θ , respectively, satisfying:

[0114] Q θ f(x) = ∫f(x′)q θ (x, x′)μ(dx′) (7)

[0115] G θ h(x) = ∫h(y)g θ (x, y)μ(dy) (8)

[0116] where Q θ is the state transition kernel, G θ is the observation update kernel, x is the current state, x′ is the next state, y is the observation, f is a function defined on X, h is a function defined on Y,

[0117] the density function g θ (x, y t ) is abbreviated as g t；θ , which represents the observation probability density at time t with state x and parameter θ. For any three times (s, s′, t) ∈ N such that s ≤ s′, 3 denote φ s:s′|t；θ as the probability distribution of X 0:t given Y s:s′ ; for any f is a function defined on (x s , x s+1 ,..., x s′ ), this distribution is expressed as:

[0118]

[0119] where L θ (y 0:t ) represents the likelihood function when the observations from time 0 to t are y 0:t ;

[0120]

[0121] For the filtering distribution φ t:t|t；θ and the prediction distribution φ t+1:t+1|t；θ , abbreviated as φ t；θ and π t+1；θ respectively; based on the filtering recursion, for t ∈ N and the following equation holds:

[0122]

[0123] π t+1；θ f = φ t；θ Q θ f (12)

[0124] where φ t；θ is the filtering distribution with parameter θ at time t, and π t；θ is the prediction distribution with parameter θ at time t;

[0125] When s ≤ t, the probability distribution of the state X s+1 at time s + 1 and the observations Y 0:t from 0 to t, and the state X s at time s is denoted as the backward kernel expressed as:

[0126]

[0127] φ s；θ is the filtering distribution with parameter θ at time s;

[0128] Using the backward kernel, the joint smoothing distribution φ 0:t|t；θ , that is, the joint probability distribution of the states x 0:t from 0 to t given the observations Y 0:t from 0 to t and the parameter θ can be expressed as:

[0129] φ 0:t|t；θ = φ t；θ T t；θ (14)

[0130] where T t；θ is the joint probability distribution of the states x 0:t from 0 to t - 1 given the observations Y t from 0 to t and the state x 0:t-1 at time t, and can be calculated by the following formula

[0131]

[0132] where represents the product of probability distributions, and when the additive objective function satisfies:

[0133]

[0134] That is, for any function h t (x 0:t ), there exists a function set that satisfies the above equation. Then {T t；θ h t} t∈N can be recursively calculated by the following formula:

[0135]

[0136] where, is the backward kernel at time t + 1;

[0137] In the external parameter calibration process, the odometer state X O is set as the state variable X, and the states [X I X c X L of the IMU, camera, and lidar are set as the observation variable Y. Then the transition kernel Q θ is determined by the system kinematic model, and the transition kernel G θ is determined by the external parameter to be estimated and the sensor measurement model.

[0138] Preferably, the module M4 adopts:

[0139] Particle update step: Update the particle set based on the importance resampling method where N is the number of particles, and is the i-th particle in the particle set at time t;

[0140] Auxiliary statistic update step: The estimation of the backward kernel is determined by the following formula:

[0141]

[0142] where, is a weighted particle set of size N; is the i-th particle in the particle set at time t, is the weight corresponding to this particle;

[0143] The particle estimation of the auxiliary statistic {T t；θ h t} t∈N is obtained through the PaRIS algorithm: by:

[0144]

[0145] where, is the number of reverse index samplings, which is very small compared to N. is the j-th reverse index of the i-th particle at time t+1, independently sampled from the distribution Then φ 0:t|t-1；θ h t is estimated by the following formula:

[0146]

[0147] where is the N-particle filter estimate of the distribution φ 0:t|t-1；θ ;

[0148] Steps of tangent filtering calculation: The tangent filter η t；θ is defined as follows:

[0149]

[0150] where is the Jacobian of the prediction distribution π t；θ with respect to the parameter θ. Then, in the particle filter framework, the estimate of η t；θ is:

[0151]

[0152] Steps for updating the external parameter estimate: Use the Robbins-Monro algorithm to iteratively calculate the external parameter θ at time t t :

[0153] θ t+1 = θ t + γ t+1 ζ t+1 (23)

[0154] where

[0155]

[0156] where

[0157]

[0158]

[0159]

[0160] where π t+1 is the prediction distribution at time t+1, is the value of the gradient of the observation probability density function at time t+1 at θ t , η t+1 is the tangent filter at time t+1;

[0161] And {γ t}t∈N is the learning rate, satisfying

[0162] Compared with the prior art, the present invention has the following beneficial effects:

[0163] 1. The present invention enables the pose tracking system to online calibrate the external parameters of the sensor based on particle filtering while positioning, achieving the technical effects of calibrating the external parameters of the sensor with errors and improving the positioning accuracy, without the need for cumbersome offline calibration;

[0164] 2. The fusion pose tracking algorithm adopted by the present invention can effectively improve the accuracy and robustness of positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0165] By reading the following detailed description of the non-limiting embodiments with reference to the accompanying drawings, other features, objects, and advantages of the present invention will become more apparent:

[0166] Figure 1 is a flowchart of a pose tracking method based on heterogeneous sensor information fusion. DETAILED DESCRIPTION OF THE INVENTION

[0167] The present invention will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those of ordinary skill in the art can make several changes and improvements without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0168] The present invention provides a pose tracking system based on heterogeneous sensor information fusion, which can provide high-precision positioning data in an indoor environment for mobile robots, unmanned vehicles, pedestrians, etc. The system integrates internal sensors such as an inertial measurement unit (IMU) and an odometer, and external sensors such as a camera and a lidar. By establishing a motion model for internal sensors such as the IMU and the odometer, the pose state is predicted; based on the characteristics of the external sensor model, an observation model is established to directly extract environmental feature information and perform observation updates. For the pose states of heterogeneous sensors, a non-linear state space model is established for the multi-external sensor fusion process; finally, the PaRIS (Particle-based Rapid Incremental Smoother) algorithm is used to perform online tangent filtering estimation on the fusion model, and the random gradient descent algorithm is used to update the multi-sensor calibration external parameters in real time, while obtaining a high-precision pose estimation of the system.

[0169] Example 1

[0170] A pose tracking method based on heterogeneous sensor information fusion provided by the present invention, as Figure 1 shown, includes:

[0171] Step S1: Establish a six-degree-of-freedom motion model based on the motion data extracted by the internal sensor, and perform pose state prediction based on the six-degree-of-freedom motion model;

[0172] Step S2: Establish an observation model based on the environmental feature information extracted by the external sensor, and perform observation update on the posterior probability distribution of the six-degree-of-freedom pose based on the observation model;

[0173] Step S3: Perform nonlinear state space modeling on the pose states of heterogeneous sensors to obtain a nonlinear state space model. At the same time, use the measurement information of both internal sensors and external sensors to estimate the pose of the system, rather than just using the information of a single sensor, which reflects multi-sensor fusion positioning;

[0174] Step S4: Use the PaRIS algorithm to perform online tangent filtering estimation on the nonlinear state space model to obtain the gradient of the external parameter update, and use the stochastic gradient descent algorithm to update the multi-sensor calibration external parameters in real time. Use the updated external parameters to correct the external parameters with errors, and use the more accurate external parameters to improve the multi-sensor fusion pose estimation accuracy and obtain high-precision pose estimation.

[0175] Specifically, in step S1, it is adopted: construct a six-degree-of-freedom motion model based on the internal motion data, determine the state transition prior (i.e., the six-degree-of-freedom pose prior), and the input u of the pose tracking system at time t t =[v t , ω t includes the linear velocity v t and the angular velocity ω t , which are obtained by the internal sensor (IMU) and follow Gaussian noise n S represents white noise following a Gaussian distribution; represents a Gaussian distribution; ∑ S represents the covariance matrix of the Gaussian distribution; the state vector x t =[p t q t , and the state vector is composed of the position p t and the orientation q t ;

[0176] The internal motion data is the motion data obtained by the sensors that directly acquire pose information. The sensors include IMU and odometer. Such sensors are not easily affected by external interference, but the cumulative error is prone to inflation;

[0177] The six-degree-of-freedom motion model adopts:

[0178] p t+1 = p t + v t Δt (1)

[0179]

[0180]

[0181] Wherein, the position p t propagates based on a uniform motion model; v t represents the linear velocity; ω t represents the angular velocity; Δt represents the time difference between two internal sensor measurements; I 4×4 represents the identity matrix of size 4×4; the orientation q t represents propagation based on zero-order quaternion integration; ∈ represents a very small number set to prevent numerical instability; the angular velocity ω = [ω x ω y ω z .

[0182] Specifically, the observation model adopts: constructing an observation model based on externally collected data;

[0183] The externally collected data is environmental information obtained through sensors that directly collect data from the outside. The sensors include cameras and lidar; since the environmental information collected by such sensors needs to be converted into the system's own pose through coordinate transformation;

[0184] The observation model adopts: establishing an observation model, and the observation model is based on the point feature model and adopts:

[0185]

[0186] Wherein, the subscript B represents the body coordinate system corresponding to the motion model; O represents the sensor coordinate system; W represents the world coordinate system; P W represents the 3D feature point spatial position in the world coordinate system; C(·) represents a function that converts a quaternion into a rotation matrix; C(q XY ) and p XY respectively represent the rotation matrix and displacement vector from coordinate system Y to coordinate system X; θ = [p BO q BO represents the external parameters of the external environmental sensor; the superscript T represents the transpose matrix;

[0187] The observation model has Gaussian noise At time t, the feature point has the following probability density, including:

[0188]

[0189] Among them, x t represents a six-degree-of-freedom pose, y t represents the actual observation information of the external sensor, and ∑ O represents the observation covariance matrix of the sensor, represents the predicted external sensor observation information;

[0190] The Jacobian of the log-likelihood function with respect to θ is:

[0191]

[0192] Among them, ^ is the skew-symmetric matrix operator.

[0193] Specifically, step S3 adopts: using heterogeneous sensor data and corresponding positioning algorithms to separately estimate the pose, obtaining the corresponding estimated poses [X O X I X c X L , where X O , X I , X c , X L respectively represent the estimated poses of the odometer, IMU, camera, and lidar.

[0194] Specifically, the non-linear state space model adopts:

[0195] Define and as the measurable space, and and are Markov transition kernels determined by the parameter d ∈ N * ; define as the initial distribution of the state X0, where is the set of probability measures; for Q θ and G θ , they respectively correspond to a transition density function q θ and g θ , satisfying:

[0196] Q θ f(x) = ∫f(x′)q θ (x, x′)μ(dx′) (7)

[0197] G θ h(x) = ∫h(y)g θ (x, y)μ(dy) (8)

[0198] Among them, Q θ is the state transition kernel, and G θLet \(K\) be the observation update kernel, \(x\) be the current state, \(x'\) be the state at the next time step, \(y\) be the observation, \(f\) be a function defined on \(X\), and \(h\) be a function defined on \(Y\).

[0199] For convenience of expression, the density function \(g\) θ (x, y t ) is abbreviated as \(g\) t；θ , which represents the observation probability density at time \(t\) with state \(x\) and parameter \(\theta\). For any three time steps \((s, s', t)\in\mathbb{N}\) such that \(s\leq s'\), 3 , let \(\varphi\) s:s′|t；θ be the probability distribution of \(X\) 0:t given \(Y\) s:s′ ; for any If \(f\) is a function defined on \((x\) s , \(x\) s+1 , \(\cdots\), \(x\) s′ ), this distribution is expressed as:

[0200]

[0201] where \(L\) θ (y 0:t ) represents the likelihood function when the observations from time \(0\) to \(t\) are \(y\) 0:t ;

[0202]

[0203] For the filtering distribution \(\varphi\) t:t|t；θ and the prediction distribution \(\varphi\) t+1:t+1|t；θ , they are abbreviated as \(\varphi\) t；θ and \(\pi\) t+1；θ respectively. Based on the filtering recursion, for \(t\in\mathbb{N}\) and the following equation holds:

[0204]

[0205] \(\pi\) t+1；θ f=\varphi\) t；θ Q\) θ f(12)

[0206] where \(\varphi\) t；θ is the filtering distribution at time \(t\) with parameter \(\theta\), and \(\pi\) t；θ is the prediction distribution at time \(t\) with parameter \(\theta\);

[0207] When \(s\leq t\), the probability distribution of the state \(X\) s+1 at time \(s + 1\) and the observations \(y\) 0:t from time \(0\) to \(t\), and the state \(X\) s at time \(s\) is denoted as the backward kernel and is expressed as:

[0208]

[0209] φ s；θ is the filtered distribution with parameter θ at time s;

[0210] Using the backward kernel, the joint smoothing distribution φ 0:t|t；θ , that is, given the observations Y from time 0 to t 0:t and the parameter θ, the joint probability distribution of the states x from time 0 to t 0:t can be expressed as:

[0211] φ 0:t|t；θ = φ t；θ T t；θ (14)

[0212] where T t；θ is the joint probability distribution of the states x from time 0 to t - 1 given the observations Y from time 0 to t 0:t and the state x at time t t , and can be calculated by the following formula 0:t-1 :

[0213]

[0214] where denotes the product of probability distributions, and when the additive objective function satisfies:

[0215]

[0216] that is, for any function h t (x 0:t ), there exists a set of functions satisfying the above formula, then {T s；θ h t} t∈N can be recursively calculated by the following formula:

[0217]

[0218] where is the backward kernel at time t + 1.

[0219] In the external parameter calibration process, the odometer state X O is set as the state variable X, and the states of the IMU, camera, and lidar [X I X c X L are set as the observation variable Y. Then the transition kernel Q θ is determined by the system kinematic model, and the transition kernel G θ is determined by the external parameter to be estimated and the sensor measurement model.

[0220] Specifically, the step S4 adopts:

[0221] Particle update step: Update the particle set based on the importance resampling method where N is the number of particles, is the i-th particle in the particle set at time t.

[0222] Auxiliary statistic update step: The backward kernel is estimated by the following formula:

[0223]

[0224] where, is a weighted particle set of size N; is the i-th particle in the particle set at time t, is the weight corresponding to this particle;

[0225] The particle estimate of the auxiliary statistic {T t；θ h t} t∈N is obtained through the PaRIS algorithm:

[0226]

[0227] where, is the number of reverse index samplings, which is very small compared to N, is the j-th reverse index of the i-th particle at time t + 1, independently sampled from the distribution Then φ 0:t|t-1；θ h t is estimated by the following formula:

[0228]

[0229] where is the N-particle filter estimate of the distribution φ 0:t|t-1；θ .

[0230] Tangent filter calculation step: The tangent filter η t；θ is defined as follows:

[0231]

[0232] where is the Jacobian of the prediction distribution π t；θ with respect to the parameter θ, then the estimate of η t；θ in the particle filter framework is:

[0233] ​

[0234] External parameter estimation update step: Use the Robbins-Monro algorithm to iteratively calculate the external parameter θ at time t t :

[0235] θ t+1 = θ t + γ t+1 ζ t+1 (23)

[0236] where

[0237]

[0238] where

[0239]

[0240]

[0241]

[0242] where π t+1 is the predicted distribution at time t+1, is the value of the gradient of the observation probability density function at time t+1 at θ t , η t+1 is the tangent filter at time t+1.

[0243] And {γ t} t∈N is the learning rate, satisfying

[0244] According to a pose tracking system based on heterogeneous sensor information fusion provided by the present invention, it includes: a variety of heterogeneous sensors composed of internal sensors such as IMU and odometer, and external sensors such as cameras and lidar, and a pose tracking algorithm based on multi-heterogeneous sensor information fusion;

[0245] Module M1: Extract motion data based on internal sensors to establish a six-degree-of-freedom motion model, and perform pose state prediction based on the six-degree-of-freedom motion model;

[0246] Module M2: Extract environmental feature information based on external sensors to establish an observation model, and perform observation update of the posterior probability distribution of the six-degree-of-freedom pose based on the observation model;

[0247] Module M3: Perform non-linear state space modeling on the pose states of heterogeneous sensors to obtain a non-linear state space model, and at the same time use the measurement information of internal sensors and external sensors together to estimate the pose of the system, rather than just using the information of a single sensor, which reflects multi-sensor fusion positioning;

[0248] Module M4: Use the PaRIS algorithm to perform online tangent filtering estimation on the non-linear state space model to obtain the gradient for updating the external parameters, and use the stochastic gradient descent algorithm to update the multi-sensor calibration external parameters in real time. Use the updated external parameters to correct the external parameters with errors, and use the more accurate external parameters to improve the multi-sensor fusion pose estimation accuracy to obtain high-precision pose estimation.

[0249] Specifically, the module M1 adopts: constructing a six-degree-of-freedom motion model based on internal motion data, determining the state transition prior (i.e., the six-degree-of-freedom pose prior), and the input u of the pose tracking system at time t t =[v t , ω t includes the linear velocity v t and the angular velocity ω t , which are obtained by the internal sensor (IMU) and follow Gaussian noise n S represents white noise following a Gaussian distribution; represents a Gaussian distribution; ∑ S represents the covariance matrix of the Gaussian distribution; the state vector x t =[p t q t , and the state vector consists of the position p t and the orientation q t ;

[0250] The internal motion data is the motion data obtained by sensors that directly acquire pose information. The sensors include IMU and odometer. Such sensors are not easily affected by external interference, but the cumulative error is prone to expansion;

[0251] The six-degree-of-freedom motion model adopts:

[0252] p t+1 =p t +v t Δt (1)

[0253]

[0254]

[0255] Among them, the position p t propagates based on the uniform motion model; v t represents the linear velocity; ω t represents the angular velocity; Δt represents the time difference between two internal sensor measurements; I 4×4 represents the identity matrix of size 4×4; the orientation q t represents propagation based on zero-order quaternion integration; ∈ represents a very small number set to prevent numerical instability; the angular velocity ω = [ω x ωy ω z ].

[0256] Specifically, the observation model is adopted as follows: constructing an observation model based on externally collected data;

[0257] The externally collected data is environmental information obtained through sensors directly collecting data from the outside. The sensors include cameras and lidar. Since the environmental information collected by such sensors needs to be converted into the system's own pose through coordinate transformation;

[0258] The observation model is adopted as follows: establishing an observation model, and the observation model is based on the point feature model and adopted as follows:

[0259]

[0260] Among them, the subscript B represents the body coordinate system corresponding to the motion model; O represents the sensor coordinate system; W represents the world coordinate system; P W represents the 3D feature point spatial position in the world coordinate system; C(·) represents a function that converts a quaternion into a rotation matrix; C(q XY ) and p XY respectively represent the rotation matrix and displacement vector from coordinate system Y to coordinate system X; θ = [p BO q BO represents the external parameters of the external environmental sensor; the superscript T represents the transpose matrix;

[0261] The observation model has Gaussian noise At time t, the feature points have the following probability density, including:

[0262]

[0263] Among them, x t represents the six-degree-of-freedom pose, y t represents the actual observation information of the external sensor, ∑ O represents the observation covariance matrix of the sensor, represents the predicted external sensor observation information;

[0264] The Jacobian of the log-likelihood function with respect to θ is:

[0265]

[0266] Among them, ^ is the skew-symmetric matrix operator.

[0267] Specifically, the module M3 is adopted as follows: using heterogeneous sensor data and adopting a corresponding positioning algorithm to separately estimate the pose, and obtaining the corresponding estimated pose [X O X I X cX L , where X O , X I , X c , X L respectively represent the estimated poses of the odometer, IMU, camera, and lidar.

[0268] Specifically, the non-linear state space model adopts:

[0269] Define and as the measurable space, and and are the Markov transition kernels determined by the parameter d ∈ N * ; define as the initial distribution of the state X0, where is the set of probability measures; for Q θ and G θ , there corresponds a transition density function q θ and g θ , satisfying:

[0270] Q θ f(x) = ∫f(x′)q θ (x, x′)μ(dx′) (7)

[0271] G θ h(x) = ∫h(y)g θ (x, y)μ(dy) (8)

[0272] where Q θ is the state transition kernel, G θ is the observation update kernel, x is the current state, x′ is the next state, y is the observation, f is a function defined on X, h is a function defined on Y,

[0273] For the sake of convenient expression, the abbreviation of the density function g θ (x, y t ) is g t；θ , representing the observation probability density at time t with state x and parameter θ. For any three times (s, s′, t) ∈ N such that s ≤ s′ 3 , denote φ s:s′|t；θ as the probability distribution of X 0:t given Y s:s′ ; for any f is a function defined on (x s , x s+1 ,..., x s′ ), this distribution is expressed as:

[0274]

[0275] wherein, L θ (y 0:t ) represents the likelihood function when the observation from time 0 to t is y 0:t ;

[0276]

[0277] For the filtering distribution φ t:t|t；θ and the prediction distribution φ t+1:t+1|t；θ , they are abbreviated as φ t；θ and π t+1；θ respectively. Based on the filtering recursion, for t ∈ N and the following equation holds:

[0278]

[0279] π t+1；θ f = φ t；θ Q θ f (12)

[0280] wherein, φ t；θ is the filtering distribution at time t with parameter θ, and π t；θ is the prediction distribution at time t with parameter θ;

[0281] When s ≤ t, the probability distribution of the state X s+1 at time s + 1 and the observation Y 0:t from time 0 to t, and the state X s at time s is denoted as the backward kernel and expressed as:

[0282]

[0283] φ s；θ is the filtering distribution at time s with parameter θ;

[0284] Using the backward kernel, the joint smoothing distribution φ 0:t|t；θ , that is, the joint probability distribution of the state x 0:t from time 0 to t given the observation Y 0:t from time 0 to t and the parameter θ, can be expressed as:

[0285] φ 0:t|t；θ = φ t；θ T t；θ (14)

[0286] wherein, T t；θ is given the observation Y 0:t from time 0 to t and the state x tUnder the condition of, the state x from 0 to t-1 0:t-1 The joint probability distribution of can be calculated by the following formula

[0287]

[0288] where represents the product of probability distributions. When the additive objective function satisfies:

[0289]

[0290] That is, for any function h t (x 0:t ), there exists a function set satisfying the above formula. Then {T t；θ h t} t∈N can be recursively calculated by the following formula:

[0291]

[0292] where is the backward kernel at time t+1.

[0293] In the external parameter calibration process, the odometer state X O is set as the state variable X, and the states [X I X c X L of the IMU, camera, and lidar are set as the observation variable Y. Then the transition kernel Q θ is determined by the system kinematic model, and the transition kernel G θ is determined by the external parameter to be estimated and the sensor measurement model.

[0294] Specifically, the module M4 adopts:

[0295] Particle update step: Update the particle set based on the importance resampling method where N is the number of particles,, is the i-th particle in the particle set at time t.

[0296] Auxiliary statistic update step: The estimation of the backward kernel is determined by the following formula:

[0297]

[0298] where is a weighted particle set of size N; is the i-th particle in the particle set at time t, is the weight corresponding to this particle;

[0299] Auxiliary statistic {T t；θ h t} t∈N is obtained by the particle estimation through the PaRIS algorithm:

[0300]

[0301] where is the number of reverse index samplings, which is very small compared to N, is the j-th reverse index of the i-th particle at time t + 1, independently sampled from the distribution Then φ 0:t|t-1；θ h t is estimated by the following formula:

[0302]

[0303] where is the N-particle filter estimation of the distribution φ 0:t|t-1；θ .

[0304] Tangent filter calculation steps: The tangent filter η t；θ is defined as follows:

[0305]

[0306] where is the Jacobian of the prediction distribution π t；θ with respect to the parameter θ. Then, the estimation of η t；θ in the particle filter framework is:

[0307]

[0308] External parameter estimation update steps: Use the Robbins-Monro algorithm to iteratively calculate the external parameter θ t at time t:

[0309] θ t+1 = θ t + γ t+1 ζ t+1 (23)

[0310] where

[0311]

[0312] where

[0313]

[0314]

[0315]

[0316] where π t+1 is the predicted distribution at time t + 1, is the value of the gradient of the observation probability density function at time t + 1 at θ t , η t+1 is the tangent filter at time t + 1.

[0317] And {γ t} t∈N is the learning rate, satisfying

[0318] Those skilled in the art know that, in addition to implementing the systems, devices, and their respective modules provided by the present invention in the form of pure computer-readable program code, the method steps can be logically programmed to enable the systems, devices, and their respective modules provided by the present invention to be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers, etc., to implement the same program. Therefore, the systems, devices, and their respective modules provided by the present invention can be considered as a kind of hardware component, and the modules included therein for implementing various programs can also be regarded as the structures within the hardware component; the modules for implementing various functions can also be regarded as either software programs for implementing the methods or the structures within the hardware component.

[0319] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Without conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A pose tracking method based on heterogeneous sensor information fusion, characterized in that Including: Step S1: Establish a six-degree-of-freedom motion model based on the motion data extracted by the internal sensor, and perform pose state prediction based on the six-degree-of-freedom motion model; Step S2: Establish an observation model based on the environmental feature information extracted by the external sensor, and perform observation update of the pose state prediction based on the observation model; Step S3: Perform nonlinear state space modeling on the pose states of heterogeneous sensors to obtain a nonlinear state space model; Step S4: Use the PaRIS algorithm to perform online tangent filtering estimation on the nonlinear state space model, and use the stochastic gradient descent algorithm to update the external parameters of multi-sensor calibration in real time, and at the same time obtain high-precision pose estimation; The step S4 adopts: Particle update step: Update the particle set based on the importance resampling method where N is the number of particles, is the i-th particle in the particle set at time t; Auxiliary statistic update step: backward kernel is estimated by the following formula: Among them, is a weighted particle set of size N; is the i-th particle in the particle set at time t, is the weight corresponding to this particle; Auxiliary statistic {T t；θ h t} t∈N Particle estimate of obtained by the PaRIS algorithm: where, is the number of reverse index samplings, which is very small compared to N, is the j-th reverse index of the i-th particle at time t + 1, independently sampled from the distribution Then φ 0:t|t-1；θ h t is estimated by the following formula: wherein is the N particle filter estimations of the distribution φ 0:t|t-1；θ ; Tangent filtering calculation steps: tangent filter η t；θ is defined as follows: where is the predictive distribution π t；θ is the Jacobian with respect to the parameter θ, then the estimate of η t；θ under the particle filtering framework is given by: External parameter estimation update step: Use the Robbins-Monro algorithm to iteratively calculate the external parameter θ at time t t : θ t+1 = θ t + γ t+1 ζ t+1 (23) Where Where where π t+1 is the predicted distribution at time t + 1, is the value of the gradient of the observation probability density function at time t + 1 at θ t , η t+1 is the tangent filter at time t + 1; where {γ t} t∈N is the learning rate, satisfying 2. The pose tracking method based on heterogeneous sensor information fusion according to claim 1, characterized in that The step S1 adopts: Construct a six-degree-of-freedom motion model based on the internal motion data to determine the prior of state transition; The input \(u\) of the pose tracking system at time \(t\) t =\([v t ,\omega t \), including the linear velocity \(v t and the angular velocity \(\omega t \), and is obtained by an internal sensor; subject to Gaussian noise n S represents white noise subject to a Gaussian distribution; represents a Gaussian distribution; \(\sum S represents the covariance matrix of the Gaussian distribution; the state vector \(x t =\([p t ,q t \), the state vector consists of the position \(p t and the orientation \(q t \); The internal motion data is the motion data obtained by the sensor that directly obtains pose information, and the sensors include IMU and odometer; The six-degree-of-freedom motion model adopts: p t+1 = p t + v t Δt (1) Among them, position p t Propagated based on a uniform motion model; v t Represents the linear velocity; ω t Represents the angular velocity; Δt represents the time difference between two internal sensor measurements; I 4×4 Represents the identity matrix of size 4×4; Towards q t Indicates propagation based on zero-order quaternion integration; ∈ represents a very small number set to prevent numerical instability; angular velocity ω = [ω x ω y ω z .

3. The pose tracking method based on heterogeneous sensor information fusion according to claim 2, characterized in that The observation model adopts: Construct an observation model based on the externally collected data; The externally collected data is the environmental information obtained by the sensor that directly collects data from the outside, and the sensors include cameras and lidars; The observation model adopts: Among them, the subscript B represents the body coordinate system corresponding to the motion model; O represents the sensor coordinate system; W represents the world coordinate system; P W represents the spatial position of the 3D feature point in the world coordinate system; C(·) represents the function that converts the quaternion into a rotation matrix; C(q XY ) and p XY respectively represent the rotation matrix and the displacement vector from coordinate system Y to coordinate system X; θ = [p BO q BO represents the external parameters of the external environmental sensor; the superscript T represents the transpose matrix; The observation model has Gaussian noise The feature points at time t have the following probability density, including: Among them, x t represents a six-degree-of-freedom pose, y t represents the actual observation information of the external sensor, ∑ O represents the observation covariance matrix of the sensor, represents the predicted external sensor observation information; The Jacobian of the log-likelihood function with respect to θ is: Where, ^ is the skew-symmetric matrix operator.

4. The pose tracking method based on heterogeneous sensor information fusion according to claim 3, wherein The step S3 adopts: using heterogeneous sensor data and corresponding positioning algorithms to separately estimate the pose, and obtaining the corresponding estimated pose [X O X O X c X L , where X O 、X I 、X c 、X L respectively represent the estimated poses of the odometer, IMU, camera, and lidar; The nonlinear state space model adopts: Definition and be measurable spaces, and let Q θ : and G θ : be Markov transition kernels determined by the parameter d ∈ N * ; define as the initial distribution of the state X0, where is the set of probability measures; for Q θ and G θ , there corresponds a transition density function q θ and g θ , respectively, satisfying: Q θ f(x) = ∫f(x′)q θ (x, x′)μ(dx′) (7) G θ h(x) = ∫h(y)g θ (x, y)μ(dy) (8) where Q θ is the state transition kernel, G θ is the observation update kernel, x is the current state, x′ is the state at the next time step, y is the observation, f is a function defined on X, and h is a function defined on Y The density function g θ (x, y t ) is abbreviated as g t；θ , representing the observation probability density at time t with state x and parameter θ. For any three times (s, s′, t) ∈ N such that s ≤ s′ 3 , let φ s:s′|t；θ be the probability distribution of X 0:t given Y s:s′ ; for any f is a function defined on (x s , x s+1 ,..., x s′ ), this distribution is expressed as: where, L θ (y 0:t ) represents the likelihood function when the observations from time 0 to t are y 0:t ; For the filtering distribution φ t:t|t；θ and the prediction distribution φ t+1:t+1|t；θ , abbreviated as φ t；θ and π t+1；θ respectively; based on the filtering recursion, for t ∈ N and f ∈ F(χ), the following equation holds: π t+1；θ f = φ t；θ Q θ f(12) where, φ t；θ is the filtering distribution with parameter θ at time t, and π t；θ is the prediction distribution with parameter θ at time t; When s ≤ t, the state X at time s + 1 s+1 and the observations Y from time 0 to time t 0:t , the state X at time s s The probability distribution of is denoted as the backward kernel Expressed as: φ s；θ is the filtered distribution at time s with parameter θ; Using the backward kernel, jointly smooth the distribution φ 0:t|t；θ , that is, given the observations Y from time 0 to t 0:t and the parameter θ, the joint probability distribution of the state x from time 0 to t 0:t is expressed as: φ 0:t|t；θ = φ t；θ T t；θ (14) where, T t；θ is the conditional joint probability distribution of the state x from 0 to t - 1 given the observations Y from 0 to t 0:t and the state x at time t t and can be calculated by the following formula 0:t-1 ​ Among them, represents the product of probability distributions, when the additive objective function satisfies: That is, for any function h t (x 0:t ), there exists a function set that satisfies the above equation. Then {T t；θ h t} t∈N can be recursively calculated by the following formula: Among them, is the reverse kernel at time t + 1; In the external parameter calibration process, the odometer state X O is set as the state variable X, and the states of the IMU, camera, and lidar [X I X c X L are set as the observation variable Y. Then the transition kernel Q θ is determined by the system kinematic model, and the transition kernel G θ is determined by the external parameter to be estimated and the sensor measurement model.

5. A pose tracking system based on heterogeneous sensor information fusion, characterized in that, Including: Module M1: Establish a six-degree-of-freedom motion model based on the motion data extracted by the internal sensor, and perform pose state prediction based on the six-degree-of-freedom motion model; Module M2: Establish an observation model based on the environmental feature information extracted by the external sensor, and perform observation update of the pose state prediction based on the observation model; Module M3: Perform nonlinear state space modeling on the pose states of heterogeneous sensors to obtain a nonlinear state space model; Module M4: Use the PaRIS algorithm to perform online tangent filtering estimation on the nonlinear state space model, and use the stochastic gradient descent algorithm to update the external parameters of multi-sensor calibration in real time, and at the same time obtain high-precision pose estimation; The module M4 adopts: Particle update step: Update the particle set based on the importance resampling method where N is the number of particles, is the i-th particle in the particle set at time t; Auxiliary statistic update step: backward kernel is estimated as determined by the following formula: Among them, is a weighted particle set of size N; is the i-th particle in the particle set at time t, is the weight corresponding to this particle; Auxiliary statistic {T t；θ h t} t∈N Particle estimate of obtained by the PaRIS algorithm: where, is the number of reverse index samplings, which is very small compared to N, is the j-th reverse index of the i-th particle at time t + 1, sampled independently from the distribution Then φ 0:t|t-1；θ h t is estimated by the following formula: where is the N particle filter estimations of the distribution φ 0:t|t-1；θ ; Tangent filtering calculation steps: tangent filter η t；θ is defined as follows: where is the predictive distribution π t；θ is the Jacobian with respect to the parameter θ, then the estimate of η t；θ under the particle filter framework is: External parameter estimation update step: Use the Robbins-Monro algorithm to iteratively calculate the external parameter θ at time t t : θ t+1 = θ t + γ t+1 ζ t+1 (23) Where Where where π t+1 is the predicted distribution at time t + 1, is the value of the gradient of the observation probability density function at time t + 1 at θ t , η t+1 is the tangent filter at time t + 1; where {γ t} t∈N is the learning rate, satisfying 6. The pose tracking system based on heterogeneous sensor information fusion according to claim 5, characterized in that, The module M1 adopts: Construct a six-degree-of-freedom motion model based on the internal motion data to determine the prior of state transition; The input u of the pose tracking system at time t t = [v t , ω t , including the linear velocity v t and the angular velocity ω t , and is obtained by an internal sensor; subject to Gaussian noise n S denotes white noise that follows a Gaussian distribution; denotes a Gaussian distribution; ∑ S denotes the covariance matrix of a Gaussian distribution; the state vector x t = [p t q t , the state vector consists of the position p t and the orientation q t ; The internal motion data is the motion data obtained by the sensor that directly obtains pose information, and the sensors include IMU and odometer; The six-degree-of-freedom motion model adopts: p t+1 = p t + v t Δt (1) where p t propagates based on a uniform motion model; v t represents the linear velocity; ω t represents the angular velocity; Δt represents the time difference between two internal sensor measurements; I 4×4 represents the 4×4 identity matrix; q t represents propagation based on zero-order quaternion integration; ∈ represents a very small number set to prevent numerical instability; the angular velocity ω = [ω x ω y ω z .

7. The pose tracking system based on heterogeneous sensor information fusion according to claim 6, wherein The observation model adopts: Construct an observation model based on the externally collected data; The externally collected data is the environmental information obtained by the sensor that directly collects data from the outside, and the sensors include cameras and lidars; The observation model adopts: Among them, the subscript B represents the body coordinate system corresponding to the motion model; O represents the sensor coordinate system; W represents the world coordinate system; P W represents the spatial position of the 3D feature point in the world coordinate system; C(·) represents the function that converts the quaternion into a rotation matrix; C(q XY ) and p XY respectively represent the rotation matrix and the displacement vector from coordinate system Y to coordinate system X; θ = [p BO q BO represents the external parameters of the external environment sensor; the superscript T represents the transpose matrix; The observation model has Gaussian noise The feature points at time t have the following probability density, including: Among them, x t represents a six-degree-of-freedom pose, y t represents the actual observation information of the external sensor, ∑ O represents the observation covariance matrix of the sensor, represents the predicted external sensor observation information; The Jacobian of the log-likelihood function with respect to θ is: Where, ^ is the skew-symmetric matrix operator.

8. The pose tracking system based on heterogeneous sensor information fusion according to claim 7, wherein The module M3 adopts the following method: using heterogeneous sensor data, corresponding positioning algorithms are used to separately estimate the pose, and the corresponding estimated poses [X O X I X c X L are obtained, where X O 、X I 、X c 、X L respectively represent the estimated poses of the odometer, IMU, camera, and lidar; The nonlinear state space model adopts: Define and as measurable spaces, and let Q θ : and G θ : be Markov transition kernels determined by the parameter d ∈ N * ; Define as the initial distribution of the state X0, where is the set of probability measures; For Q θ and G θ , there correspond a transition density function q θ and g θ , respectively, satisfying: Q θ f(x) = ∫f(x′)q θ (x, x′)μ(dx′) (7) G θ h(x) = ∫h(y)g θ (x, y)μ(dy) (8) where Q θ is the state transition kernel, G θ is the observation update kernel, x is the current state, x′ is the state at the next moment, y is the observation, f is a function defined on X, and h is a function defined on Y The density function g θ (x, y t ) is abbreviated as g t；θ , representing the observation probability density at time t with state x and parameter θ. For any three times (s, s′, t) ∈ N such that s ≤ s′ 3 , let φ s:s′|t；θ be the probability distribution of X 0:t given Y s:s′ . For any f is a function defined on (x s , x s+1 ,..., x s′ ), this distribution is expressed as: Among them, L θ (y 0:t ) represents the likelihood function when the observations from time 0 to t are y 0:t ; For the filtering distribution φ t:t|t；θ and the prediction distribution φ t+1:t+1|t；θ , abbreviated as φ t；θ and π t+1；θ respectively; based on the filtering recursion, for t ∈ N and the following equation holds: π t+1；θ f = φ t；θ Q θ f(12) where, φ t；θ is the filtering distribution with parameter θ at time t, and π t；θ is the prediction distribution with parameter θ at time t; When s ≤ t, the state X at time s + 1 s+1 and the observations Y from time 0 to time t 0:t , the state X at time s s The probability distribution of is denoted as the backward kernel Expressed as: φ s；θ is the filtered distribution at time s with parameter θ; Using the backward kernel, jointly smooth the distribution φ 0:t|t；θ , that is, given the observations Y from time 0 to t 0:t and the parameter θ, the joint probability distribution of the state x from time 0 to t 0:t is expressed as: φ 0:t|t；θ = φ t；θ T t；θ (14) where, T t；θ is the observation Y from time 0 to time t 0:t and the state x at time t t under the condition of which, the state x from time 0 to time t - 1 0:t-1 The joint probability distribution can be calculated by the following formula Among them, represents the product of probability distributions when the additive objective function satisfies: That is, for any function h t (x 0:t ), there exists a function set satisfying the above formula. Then {T t；θ h t} t∈N can be recursively calculated by the following formula: Among them, is the reverse kernel at time t + 1; In the external parameter calibration process, the odometer state X O is set as the state variable X, and the states of the IMU, camera, and lidar [X I X c X L are set as the observation variable Y. Then the transition kernel W θ is determined by the system kinematic model, and the transition kernel G θ is determined by the external parameter to be estimated and the sensor measurement model.

Images