Pose Estimation Method, Device, Equipment and Medium for Combining Thermal Infrared Camera and IMU

Abstract

The present disclosure provides a pose estimation method, apparatus, device, and medium for combining a thermal infrared camera and an IMU, including: obtaining image representation data of a current frame image and image representation data of a previous frame image collected by a thermal infrared camera in a mobile device; determining whether the current frame image has lost target feature points based on the image representation data of the current frame image and the image representation data of the previous frame image; if it is determined that the current frame image has lost target feature points, obtaining the world coordinate position of the target feature points when they are in the previous frame image; obtaining the current moment data measured by the IMU in the mobile device; performing multi-state constrained Kalman filter fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature points when they are in the previous frame image to obtain target pose error data, so as to determine the current estimated pose of the mobile device according to the target pose error data and the current moment data measured by the IMU in the mobile device. Thus, the accuracy of pose estimation is effectively improved.

Description

Technical Field

[0001] Embodiments of the present disclosure relate to the technical field of pose estimation. Specifically, they relate to a pose estimation method, device, equipment, and medium applicable to the combination of a thermal infrared camera and an IMU. Background Art

[0002] Visual Inertial Odometry (VIO) technology is widely used in various fields and can replace humans in performing complex and dangerous tasks, such as industrial inspections, remote sensing, search and rescue. Due to its flexibility in minimizing task risks, the demand for the ability to safely and reliably achieve perception and positioning in more challenging environments, such as fire scenes, underground environments, and GPS-denied environments, is also increasing.

[0003] In the related art, when positioning a mobile device (such as a robot), pose estimation is usually achieved by combining a standard visible spectrum camera and an IMU to position the mobile device. However, for applications that are not resource-constrained and allow the use of active sensors for night operations, they still cannot adapt to other visually degraded environmental scenarios, such as scenes with obscuring objects in the air like thick smoke, thick fog, sand and dust, and scenes with poor lighting conditions like snowy days and darkness. In these scenarios, the data information collected by the visible spectrum camera may be significantly reduced.

[0004] However, with the existing methods, the accuracy of pose estimation is not high. Summary of the Invention

[0005] The embodiments described herein provide a pose estimation method, device, equipment, and medium for the combination of a thermal infrared camera and an IMU, which overcome the above problems.

[0006] In a first aspect, according to the content of the present disclosure, a pose estimation method for the combination of a thermal infrared camera and an IMU is provided, including:

[0007] Obtain the image representation data of the current frame image and the image representation data of the previous frame image collected by the thermal infrared camera in the mobile device. The image representation data of the current frame image is the environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is the environmental data collected by the thermal infrared camera at the previous moment;

[0008] Based on the image representation data of the current frame image and the image representation data of the previous frame image, determine whether the current frame image has lost target feature points. The target feature points are the feature points included in the previous frame image but not included in the current frame image;

[0009] If it is determined that the current frame image loses the target feature point based on the image representation data of the current frame image and the image representation data of the previous frame image, obtain the world coordinate position of the target feature point when it is in the previous frame image;

[0010] Obtain the current moment data measured by the IMU in the mobile device;

[0011] Perform multi-state constrained Kalman filter fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame image to obtain target attitude error data, so as to determine the current estimated pose of the mobile device according to the target attitude error data and the current moment data measured by the IMU in the mobile device;

[0012] Among them, the multi-state constrained Kalman filter fusion is used to perform error state propagation according to the measurement data of the IMU to fuse the acquisition data of the thermal infrared camera to obtain target attitude error data.

[0013] In a second aspect, according to the content of the present disclosure, a pose estimation device combining a thermal infrared camera and an IMU is provided, including:

[0014] A first acquisition module, configured to acquire the image representation data of the current frame image collected by the thermal infrared camera in the mobile device and the image representation data of the previous frame image, where the image representation data of the current frame image is the environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is the environmental data collected by the thermal infrared camera at the previous moment;

[0015] A determination module, configured to determine whether the current frame image loses a target feature point based on the image representation data of the current frame image and the image representation data of the previous frame image, where the target feature point is a feature point included in the previous frame image and not included in the current frame image;

[0016] A second acquisition module, configured to obtain the world coordinate position of the target feature point when it is in the previous frame image if it is determined that the current frame image loses the target feature point based on the image representation data of the current frame image and the image representation data of the previous frame image;

[0017] A third acquisition module, configured to acquire the current moment data measured by the IMU in the mobile device;

[0018] A fusion module, configured to perform multi-state constrained Kalman filter fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame of image, so as to obtain target attitude error data, and determine the current estimated pose of the mobile device according to the target attitude error data and the current moment data measured by the IMU in the mobile device;

[0019] Among them, the multi-state constrained Kalman filter fusion is used to perform error state propagation according to the measurement data of the IMU, so as to fuse the acquisition data of the thermal infrared camera to obtain target attitude error data.

[0020] In a third aspect, a computer device is provided, including a memory and a processor. A computer program is stored in the memory, and when the processor executes the computer program, the steps of the pose estimation method of combining the thermal infrared camera and the IMU in any of the above embodiments are implemented.

[0021] In a fourth aspect, a computer-readable storage medium is provided. A computer program is stored on the computer-readable storage medium, and when the computer program is executed by the processor, the steps of the pose estimation method of combining the thermal infrared camera and the IMU in any of the above embodiments are implemented.

[0022] The pose estimation method of combining the thermal infrared camera and the IMU provided in the embodiments of the present application obtains the image representation data of the current frame of image collected by the thermal infrared camera in the mobile device and the image representation data of the previous frame of image. The image representation data of the current frame of image is the environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame of image is the environmental data collected by the thermal infrared camera at the previous moment; based on the image representation data of the current frame of image and the image representation data of the previous frame of image, it is determined whether the current frame of image has lost the target feature point, and the target feature point is the feature point included in the previous frame of image and not included in the current frame of image; if it is determined that the current frame of image has lost the target feature point based on the image representation data of the current frame of image and the image representation data of the previous frame of image, then obtain the world coordinate position of the target feature point when it is in the previous frame of image; obtain the current moment data measured by the IMU in the mobile device; perform multi-state constrained Kalman filter fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame of image, so as to obtain target attitude error data, and determine the current estimated pose of the mobile device according to the target attitude error data and the current moment data measured by the IMU in the mobile device; among them, the multi-state constrained Kalman filter fusion is used to perform error state propagation according to the measurement data of the IMU, so as to fuse the acquisition data of the thermal infrared camera to obtain target attitude error data. In this way, by combining the fusion data of the thermal infrared camera and the IMU, accurate autonomous navigation and environmental perception under various environmental conditions are realized, and the pose estimation accuracy of the mobile device is effectively improved.

[0023] The above description is only an overview of the technical solution of the embodiment of the present application. In order to better understand the technical means of the embodiment of the present application, it can be implemented according to the content of the specification. And in order to make the above and other purposes, features and advantages of the embodiment of the present application more obvious and understandable, the specific implementation manners of the present application are given below. Description of the Drawings

[0024] In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the drawings of the embodiments will be briefly described below. It should be understood that the following described drawings only relate to some embodiments of the present disclosure and do not limit the present disclosure, where:

[0025] Figure 1 is a schematic flow chart of a pose estimation method combining a thermal infrared camera and an IMU provided by the present disclosure.

[0026] Figure 2 is a schematic structural diagram of a pose estimation device combining a thermal infrared camera and an IMU provided by the present disclosure.

[0027] Figure 3 is a schematic structural diagram of a computer device provided by the present disclosure.

[0028] It should be noted that the elements in the drawings are schematic and not drawn to scale. Detailed Embodiments

[0029] In order to make the purposes, technical solutions and advantages of the embodiments of the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be clearly and completely described below with reference to the drawings. Obviously, the described embodiments are some, but not all, of the embodiments of the present disclosure. Based on the described embodiments of the present disclosure, all other embodiments obtained by those skilled in the art without creative efforts shall also fall within the scope of protection of the present disclosure.

[0030] Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by those of ordinary skill in the art to which the subject matter of the present disclosure belongs. Further, it will be understood that terms such as those defined in commonly used dictionaries shall be interpreted as having a meaning consistent with their meaning in the context of the specification and the relevant art, and will not be interpreted in an idealized or overly formal form unless expressly defined herein. As used herein, the statement of joining or coupling two or more parts together shall mean that these parts are directly joined together or joined through one or more intermediate components.

[0031] References to "embodiments" in this specification mean that specific features, structures, or characteristics described in connection with the embodiments can be included in at least one embodiment of the present application. The phrase "embodiment" appearing in various places in the specification does not necessarily refer to the same embodiment, nor are they independent or alternative embodiments mutually exclusive of other embodiments. Those skilled in the art will explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0032] As used herein, the term "and / or" is merely a description of the relationship between associated objects, indicating that three relationships can exist. For example, A and / or B can mean: the existence of A, the simultaneous existence of A and B, and the existence of B. Additionally, the character " / " in this text generally indicates that the associated objects before and after are in an "or" relationship. Terms such as "first" and "second" are only used to distinguish one component (or a part of a component) from another component (or another part of a component).

[0033] In the description of the present application, unless otherwise specified, "a plurality" means two or more (including two). Similarly, "a plurality of groups" means two or more groups (including two groups).

[0034] To enable those skilled in the art to better understand the solution of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings.

[0035] Figure 1 is a schematic flow chart of a pose estimation method combining a thermal infrared camera and an IMU provided by an embodiment of the present disclosure, as Figure 1 shown, the specific process of the pose estimation method combining a thermal infrared camera and an IMU includes:

[0036] S110. Obtain the image representation data of the current frame image collected by the thermal infrared camera in the mobile device and the image representation data of the previous frame image.

[0037] Among them, the image representation data of the current frame image is the environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is the environmental data collected by the thermal infrared camera at the previous moment.

[0038] The image representation data of the current frame image can be used to describe the image coordinates of the feature points of each object at the current moment captured by the thermal infrared camera during the movement of the mobile device. The image representation data of the previous frame image can be used to describe the image coordinates of the feature points of each object at the previous moment captured by the thermal infrared camera during the movement of the mobile device.

[0039] S120. Based on the image representation data of the current frame image and the image representation data of the previous frame image, determine whether the current frame image has lost target feature points.

[0040] Among them, the target feature points are the feature points included in the previous frame image but not included in the current frame image.

[0041] In some embodiments, based on the image representation data of the current frame image and the image representation data of the previous frame image, determining whether the current frame image loses target feature points includes:

[0042] Matching the image representation data of the current frame image and the image representation data of the previous frame image; if it is matched that there are target feature points in the previous frame image and there are no target feature points in the current frame image, then it is determined that the current frame image loses target feature points; if it is matched that there are target feature points in the previous frame image and there are still target feature points in the current frame image, then it is determined that the current frame image does not lose target feature points.

[0043] Among them, the way of image matching can be carried out through the image representation data of the current frame image and the image representation data of the previous frame image to identify whether the feature points included in the current frame image are the same as the feature points included in the previous frame image. Thus, it is effectively determined whether the current frame image loses target feature points.

[0044] S130. If it is determined that the current frame image loses target feature points based on the image representation data of the current frame image and the image representation data of the previous frame image, then obtain the world coordinate position of the target feature points when they are in the previous frame image.

[0045] Among them, the image coordinates of the target feature points in the image representation data of the previous image can be combined with the camera pose for feature triangulation processing to obtain the position coordinates corresponding to the world coordinate system when the target feature points are in the previous frame image, that is, the world coordinate position.

[0046] S140. Obtain the current moment data measured by the IMU in the mobile device.

[0047] Among them, the current moment data measured by the IMU (Inertial Measurement Unit) is the acceleration, angular velocity, position coordinates, orientation and other information of the mobile device measured by the IMU at the current moment during the movement of the mobile device.

[0048] S150. Perform multi-state constraint Kalman filter fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature points when they are in the previous frame image to obtain the target attitude error data.

[0049] Among them, multi-state constrained Kalman filter fusion is performed on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame of image, so as to obtain the target attitude error data, which is convenient for determining the current estimated pose of the mobile device according to the target attitude error data and the current moment data measured by the IMU in the mobile device.

[0050] The multi-state constrained Kalman filter fusion is used to perform error state propagation according to the measurement data of the IMU, so as to fuse the acquisition data of the thermal infrared camera to obtain the target attitude error data. That is, according to the measurement data of the IMU, the error state of the IMU at the current moment and the system covariance are updated, the Kalman gain is calculated according to the covariance, and the error state of the IMU at the next moment is calculated according to the measurement data of the IMU at the next moment, so as to perform error state propagation.

[0051] In some embodiments, multi-state constrained Kalman filter fusion is performed on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame of image, so as to obtain the target attitude error data, including:

[0052] Based on the current moment data measured by the IMU in the mobile device, predict the initial attitude error data of the IMU at the current moment; determine the Kalman gain based on the covariance matrix data corresponding to the initial attitude error data; perform state update on the initial attitude error data based on the Kalman gain and the world coordinate position of the target feature point when it is in the previous frame of image, so as to obtain the target attitude error data.

[0053] Among them, the initial attitude error data is used to describe the current error estimation state of the IMU, that is, the error data of the pose information of the mobile device estimated by the IMU, and specifically may include but is not limited to: rotation error, coordinate error, etc.

[0054] In some embodiments, based on the current moment data measured by the IMU in the mobile device, predicting the initial attitude error data of the IMU at the current moment includes:

[0055] Use the first motion state model to perform error state prediction on the current moment data measured by the IMU in the mobile device to obtain the actual error state data of the IMU at the current moment; use the first motion state model to perform error state prediction on the current moment data measured by the IMU in the mobile device to obtain the estimated error state data of the IMU at the current moment; determine the data difference between the actual error state data and the estimated error state data of the IMU at the current moment as the initial attitude error data of the IMU at the current moment.

[0056] Among them, the first motion state model can be used to describe the real state motion model of the IMU, and the second motion state model can be used to describe the estimated state motion model of the IMU, that is, the ideal state motion model.

[0057] For example, the state vector X of the true state motion model of the IMU I is expressed as shown in Equation (1).

[0058]

[0059] In Equation (1), is the quaternion describing the transformation from the inertial coordinate system {G} to the IMU coordinate system {I} (i.e., a different parameterization of the rotation matrix C) G v I (t) and G p I (t) represent the velocity and position of the IMU coordinate system in the inertial coordinate system, respectively; b g (t) and b a (t) represent the gyroscope and accelerometer biases, respectively.

[0060] Considering that the feature is static (with negligible dynamics) and using the IMU motion dynamics, the continuous-time dynamics of Equation (1) can be expressed as shown in Equation (2).

[0061]

[0062] In Equation (2), w(t) = [w x w y w z T represents the angular velocity in the IMU frame, denoted by {I}, G a I (t) represents the IMU acceleration, denoted by {G}, n wg (t) and n wa (t) are Gaussian white noise processes driving the IMU biases, and is a skew-symmetric matrix.

[0063] A typical IMU provides gyroscope and accelerometer data w m and a m , both expressed in the IMU coordinate system {I}, as shown in Equations (3) and (4).

[0064] w m (t) = I w(t) + b g (t) + n g (t) (3)

[0065]

[0066] In Equation (4),​G g is the acceleration due to gravity with {G}, and n g and n a is Gaussian noise (zero mean). The state vector of the IMU estimated state motion model is expressed as shown in Equation (5).

[0067]

[0068] Under the current state estimation, the linearization of Equation (2) yields a continuous-time state estimation propagation model, which can be expressed as shown in Equation (6).

[0069]

[0070] In Equation (6),

[0071] Therefore, by taking the "difference" between the above true state and the estimated state, the IMU error state motion model is defined as shown in Equation (7).

[0072]

[0073] Among them, the difference of quaternions is different from ordinary subtraction. Therefore, an error quaternion is introduced to represent the rotation error where the true and estimated postures are consistent, as shown in Equation (8).

[0074]

[0075] So the rotation error can be represented by the three-dimensional vector δθ, and thus the error state of the IMU can be expressed as shown in Equation (9).

[0076]

[0077] Then, the continuous-time error state propagation equation is as shown in Equation (10).

[0078]

[0079] In Equation (10), is the system noise; the vector n g and n a represent the measurement noise (Gaussian) of the gyroscope and accelerometer, n wg and n wa are the random walk rates of the biases of the gyroscope and accelerometer; F is the continuous-time error state transition matrix, as shown in Equation (11); G is the input noise matrix, as shown in Equation (12).

[0080]

[0081]

[0082] For the processing of each frame of IMU data, the IMU state at the next moment is estimated directly according to the motion equation of the IMU, and other state values (camera pose) remain unchanged. Then, the EKF (Extended Kalman Filter) is performed according to the error state motion equation to facilitate the update of the obtained estimated value.

[0083] To process discrete-time IMU data, the high-order Runge-Kutta numerical integration method is used to discretize the continuous-time dynamics state equation formula (2), so as to predict the state estimation quantity of the IMU; other state quantities remain unchanged (the camera pose remains unchanged).

[0084] The continuous-time propagation model has been described above using IMU measurements. However, in any practical EKF implementation, a discrete-time state transition matrix Φ k := Φ(t k+1 , t k ) is required to propagate the error covariance from time t k to t k+1 . According to linear system theory, the discretized form of the error state propagation equation is shown in formula (13).

[0085]

[0086] In formula (13), the calculation formula of the state transition matrix Φ k from t k+1 to t k is shown in formula (14).

[0087]

[0088] When the time interval is very short, F(t) can be regarded as a constant matrix within this time period: F(t) ≈ F(t k ) = F k , t k ≤ t < t k+1 , and at this time, it is as shown in formula (15).

[0089]

[0090] Let: Then the discretized equation form is as shown in formula (16).

[0091]

[0092] The calculation formula of the covariance matrix Q k of the discrete-time system noise term is shown in formula (17).

[0093]

[0094] In formula (17),

[0095] Considering the influence of the change of the IMU error state on the covariance matrix of the system error state. The covariance matrix of the error state is shown in formula (18).

[0096]

[0097] Then, the covariance matrix of the predicted IMU error state at time k+1 is shown in formula (19).

[0098]

[0099] The covariance matrix of the predicted error state at time k+1 is shown in formula (20).

[0100]

[0101] In some embodiments, before determining the Kalman gain based on the covariance matrix data corresponding to the initial attitude error data, it further includes:

[0102] Obtain the sensor attitude corresponding to the current moment of the IMU; based on the sensor attitude corresponding to the current moment of the IMU and the extrinsic parameters of the thermal infrared camera, determine the camera attitude corresponding to the current moment of the thermal infrared camera; based on the camera attitude corresponding to the current moment of the thermal infrared camera, perform matrix data amplification on the covariance matrix data corresponding to the initial attitude error data.

[0103] Among them, the error state vector of the MSCKF (Multi-State Constraint Kalman Filter based on improvement and fusion) system is shown in formula (21).

[0104]

[0105] The error state vector of the system: includes the IMU error state and N camera states. Specifically, when there is no image coming in, predict the IMU state and calculate the covariance matrix of the error state; when there is an image coming in, calculate the pose of the current camera according to the relative extrinsic parameters of the camera and the IMU, and add the latest camera state to the system state vector to amplify the covariance matrix of the error state.

[0106] For example, when recording a new frame of image, calculate the pose estimation of the camera according to the IMU pose estimation and the extrinsic parameters of the camera, and the calculation formula is shown in formula (22).

[0107]

[0108] The current camera pose is added to the state vector, and the error state covariance matrix of the EKF is correspondingly augmented as shown in Equation (23).

[0109]

[0110] In Equation (23), the Jacobian matrix J can be derived from Equation (22) as shown in Equation (24).

[0111]

[0112] In some embodiments, based on the Kalman gain and the world coordinate position of the target feature point in the previous frame image, the state of the initial pose error data is updated to obtain the target pose error data, including:

[0113] Determining the world coordinate position of the camera feature point based on the world coordinate position of the target feature point in the previous frame image and the image representation data of the current frame image; determining the camera residual term data based on the world coordinate position of the camera feature point; determining the state update data based on the camera residual term data and the Kalman gain; and determining the target pose error data based on the state update data and the initial pose error data.

[0114] For example, considering the case of observing a single feature f j from a set of M camera poses a camera measurement model is proposed. Each of the M observations of this feature can be described by a model as shown in Equation (25). j j

[0115]

[0116] In Equation (25), is a 2×1 image noise vector with covariance matrix

[0117] The camera feature points may include: the target feature point and all feature points in the current frame image.

[0118] The world coordinate position of the camera feature point represented in the camera coordinate system is as shown in Equation (26).

[0119]

[0120] In Equation (26), represents the 3D feature point position in the world coordinate system. Since this is unknown, the least squares method can be used with the measurement value and the filter estimate of the camera pose at the corresponding time to obtain an estimate of the feature point position ​​

[0121] The measurement residual (i.e., the initial residual term data) is shown in formula (27).

[0122]

[0123] In formula (27),

[0124] By linearizing the estimation of the camera pose and features, the residual of formula (27) can be approximated as shown in formula (28).

[0125]

[0126] In formula (27), and are the Jacobian matrices of the measured value with respect to the state vector and the feature position respectively; correspondingly, is the error of the position estimation f j By superimposing the residuals of all M j measurements of this feature, formula (29) can be obtained.

[0127]

[0128] In formula (29), r (j) , n (j) is the block vector or block matrix of the element .

[0129] In some embodiments, based on the world coordinate position of the camera feature points, the camera residual term data is determined, including:

[0130] Based on the world coordinate position of the camera feature points, the initial residual term data is determined; the relevant term data corresponding to the world coordinate position of the camera feature points is removed from the initial residual term data to obtain the camera residual term data.

[0131] Among them, since the residual formula cannot be directly applied to the EKF update that is independent of the feature point position coordinates, to overcome this problem, a new residual term (i.e., the camera residual term data) is defined, and projecting r (j) onto the left null space of the matrix can obtain what is shown in formula (30).

[0132]

[0133] Because the 2M j ×3 matrix is column full rank, its left null space should be 2M j-3D, thus, is a 2M j -3 vector. This residual term is independent of the error in the feature point position coordinates, so the EKF update can be performed based on this. The above residual formula defines the linear constraints between all camera poses where the feature points are observed, expressing the measured values for camera pose M j provides all available information. Except for the inaccuracies caused by linearization, the resulting EKF update is optimal.

[0134] The EKF update is triggered by one of the following two events: ① When the feature that is tracked in multiple images can no longer be detected, triangulation of the feature points is performed to process all measurements of this feature. This situation also occurs most frequently because the feature has moved outside the camera's field of view. ② When a new image frame is recorded, a copy of the current camera pose estimate is included in the state vector. If the maximum number N max of camera poses is exceeded, the previous old camera poses must be deleted. All feature observations at the corresponding time will be used before deleting the state, using the positioning data they provide.

[0135] Since the feature measurements are statistically independent, the noise vectors are uncorrelated. Therefore, the covariance matrix of the noise vectors is equal to where the residual term r o has a dimension of A problem that occurs in practice is that d may be a rather large number. To reduce the computational complexity in the EKF update process, the multi-state constrained Kalman filter fusion algorithm performs a QR decomposition on H X as shown in formula (31).

[0136]

[0137] In formula (31), Q1 and Q2 are unitary matrices, whose columns form the bases of the range and null space of H X , and the corresponding T H is an upper triangular matrix. According to the above definitions, formula (32) can be obtained.

[0138]

[0139] It can be clearly seen that by projecting the residual term r o onto the basis vectors of the range of H X , all useful information in the measurement is retained which is just the noise term and can be completely discarded. For this reason, the residual term in the original formula (32) can be replaced, so that the following residual term can be used for the EKF update, as shown in formula (33).

[0140]

[0141] In formula (33), is a noise vector, and its covariance matrix is equal to where r is the number of columns of Q1. Therefore, the Kalman gain calculated by EKF update is shown in formula (34).

[0142]

[0143] The update of the state vector is given by the vector ΔX = Kr n , and the update of the state estimate value is shown in formula (35).

[0144]

[0145] The update of the covariance matrix is shown in formula (36).

[0146] P k+1|k+1 = (I ξ - KT H ) P k+1|k (I ξ - KT H ) T + KR n K T (36)

[0147] In formula (36), ξ = 6N + 15 is the dimension of the covariance matrix. In terms of computational complexity, the residual term r n is the same as the matrix T H , and its computational complexity is O(r 2 d). On the one hand, the product of square matrices of dimension ξ is involved in the calculation of the covariance matrix update, and the computational complexity is O(ξ 3 ). Therefore, the computational complexity of the EKF update cost is max(O(r 2 d), O(ξ 3 )). On the other hand, if the residual term r o is used without projecting it onto the range of T X , then the computational cost of calculating the Kalman gain is O(d 3 ). Since typically d >> ξ, r, using the residual r n will greatly save resources in terms of computational cost.

[0148] In this embodiment, the image representation data of the current frame image collected by the thermal infrared camera in the mobile device and the image representation data of the previous frame image are obtained. The image representation data of the current frame image is the environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is the environmental data collected by the thermal infrared camera at the previous moment. Based on the image representation data of the current frame image and the image representation data of the previous frame image, it is determined whether the current frame image has lost target feature points. The target feature points are the feature points included in the previous frame image but not included in the current frame image. If it is determined that the current frame image has lost target feature points based on the image representation data of the current frame image and the image representation data of the previous frame image, the world coordinate position of the target feature points when they are in the previous frame image is obtained. The current moment data measured by the IMU in the mobile device is obtained. The current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature points when they are in the previous frame image are subjected to multi-state constrained Kalman filter fusion to obtain target pose error data, so as to determine the current estimated pose of the mobile device according to the target pose error data and the current moment data measured by the IMU in the mobile device. Among them, the multi-state constrained Kalman filter fusion is used to perform error state propagation according to the measurement data of the IMU to fuse the acquisition data of the thermal infrared camera to obtain target pose error data. In this way, by combining the fusion data of the thermal infrared camera and the IMU, precise autonomous navigation and environmental perception under various environmental conditions can be realized, effectively improving the pose estimation accuracy of the mobile device.

[0149] In this embodiment, the performance of the visible light camera of the visual inertial odometer (VIO) decreases in special environmental scenarios, resulting in unreliable motion estimation of the robot. By replacing the visible light camera in the system with a thermal infrared camera, compared with the visible light camera, the thermal infrared camera has better robustness to lighting changes and occlusions such as dust and smoke. In an environment scene with visual degradation, the thermal infrared camera odometer is an ideal choice for achieving better robust pose estimation. The multi-sensor fusion of thermal infrared images and inertial measurement units is used to implement pose state estimation for mobile robots. However, different from visible light images, the contrast of thermal infrared images is related to the radiation energy in the environment, more specifically, determined by the difference in infrared emissivity.

[0150] In this embodiment, inertial information is used to predict the state and covariance of the robot and track the features of two frames of images. The feature points are not added to the state vector, and only the pose information of this frame is added to the state vector when new image information arrives. The state vector is a sliding window that contains inertial information and several image pose information that forms geometric constraints. When certain conditions are met, marginalization is performed to eliminate the old state and retain the new state, keeping the size of the sliding window within a certain limit; when the feature points that have been continuously tracked cannot be observed or the size of the sliding window reaches a certain threshold, the position of the feature points is calculated using triangulation and iterative optimization algorithms to obtain an accurate three-dimensional spatial point position, and the geometric constraints on the poses within the sliding window using these feature point information are used to estimate the state vector of the system through a Kalman filter.

[0151] Figure 2 FIG. 4 is a schematic structural diagram of a pose estimation device combining a thermal infrared camera and an IMU provided in this embodiment. The pose estimation device combining a thermal infrared camera and an IMU may include: a first acquisition module 210, a determination module 220, a second acquisition module 230, a third acquisition module 240, and a fusion module 250.

[0152] The first acquisition module 210 is configured to acquire the image representation data of the current frame image collected by the thermal infrared camera in the mobile device and the image representation data of the previous frame image. The image representation data of the current frame image is the environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is the environmental data collected by the thermal infrared camera at the previous moment.

[0153] The determination module 220 is configured to determine whether the current frame image has lost target feature points based on the image representation data of the current frame image and the image representation data of the previous frame image. The target feature points are the feature points included in the previous frame image but not included in the current frame image.

[0154] The second acquisition module 230 is configured to, if it is determined that the current frame image has lost target feature points based on the image representation data of the current frame image and the image representation data of the previous frame image, acquire the world coordinate position of the target feature points when they are in the previous frame image.

[0155] The third acquisition module 240 is configured to acquire the current moment data measured by the IMU in the mobile device.

[0156] The fusion module 250 is configured to perform multi-state constraint Kalman filter fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature points when they are in the previous frame image to obtain target pose error data, so as to determine the current estimated pose of the mobile device based on the target pose error data and the current moment data measured by the IMU in the mobile device.

[0157] Among them, the multi-state constrained Kalman filter fusion is used to perform error state propagation based on the measurement data of the IMU, so as to fuse the acquisition data of the thermal infrared camera to obtain the target attitude error data.

[0158] In this embodiment, optionally, the fusion module 250 includes: a prediction unit, a determination unit, and an update unit.

[0159] The prediction unit is used to predict the initial attitude error data of the IMU at the current moment based on the data measured by the IMU in the mobile device. The initial attitude error data is used to describe the current error estimation state of the IMU.

[0160] The first determination unit is used to determine the Kalman gain based on the covariance matrix data corresponding to the initial attitude error data.

[0161] The update unit is used to perform state update on the initial attitude error data based on the Kalman gain and the world coordinate position of the target feature point in the previous frame image, so as to obtain the target attitude error data.

[0162] In this embodiment, optionally, it further includes: an acquisition unit, a second determination unit, and an amplification unit.

[0163] The acquisition unit is used to acquire the sensor attitude corresponding to the current moment of the IMU.

[0164] The second determination unit is used to determine the camera attitude corresponding to the current moment of the thermal infrared camera based on the sensor attitude corresponding to the current moment of the IMU and the external camera parameters of the thermal infrared camera.

[0165] The amplification unit is used to perform matrix data amplification on the covariance matrix data corresponding to the initial attitude error data based on the camera attitude corresponding to the current moment of the thermal infrared camera.

[0166] In this embodiment, optionally, the update unit is specifically used for:

[0167] Based on the world coordinate position of the target feature point in the previous frame image and the image representation data of the current frame image, determine the world coordinate position of the camera feature point; based on the world coordinate position of the camera feature point, determine the camera residual term data; based on the camera residual term data and the Kalman gain, determine the state update data; based on the state update data and the initial attitude error data, determine the target attitude error data.

[0168] In this embodiment, optionally, the update unit is specifically used for:

[0169] Based on the world coordinate position of the camera feature points, determine the initial residual term data; remove the relevant term data corresponding to the world coordinate position of the camera feature points from the initial residual term data to obtain the camera residual term data.

[0170] In this embodiment, optionally, the prediction unit is specifically configured to:

[0171] Use the first motion state model to predict the error state of the current moment data measured by the IMU in the mobile device, and obtain the actual error state data of the IMU at the current moment; use the first motion state model to predict the error state of the current moment data measured by the IMU in the mobile device, and obtain the estimated error state data of the IMU at the current moment; determine the data difference between the actual error state data and the estimated error state data of the IMU at the current moment as the initial attitude error data of the IMU at the current moment.

[0172] In this embodiment, optionally, the determination module 220 is specifically configured to:

[0173] Match the image representation data of the current frame image and the image representation data of the previous frame image; if it is matched that there are target feature points in the previous frame image and there are no target feature points in the current frame image, it is determined that the current frame image has lost the target feature points; if it is matched that there are target feature points in the previous frame image and there are still target feature points in the current frame image, it is determined that the current frame image has not lost the target feature points.

[0174] The pose estimation device combining the thermal infrared camera and the IMU provided by the present disclosure can execute the above method embodiments, and for its specific implementation principle and technical effects, reference can be made to the above method embodiments, which will not be elaborated herein.

[0175] An embodiment of the present application also provides a computer device. Specifically, please refer to Figure 3 , Figure 3 which is the basic structural block diagram of the computer device in this embodiment.

[0176] The computer device includes a memory 310 and a processor 320 that are communicatively connected to each other via a system bus. It should be noted that only the computer device with the memory 310 and the processor 320 is shown in the figure. However, it should be understood that it is not required to implement all the shown components, and more or fewer components can be implemented alternatively. Among them, those skilled in the art of the present technology can understand that the computer device here is a device that can automatically perform numerical calculations and / or information processing according to pre-set or stored instructions, and its hardware includes but is not limited to microprocessors, application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), digital signal processors (DSPs), embedded devices, etc.

[0177] The computer device can be a computing device such as a desktop computer, a notebook, a palm computer, and a cloud server. The computer device can interact with the user through a keyboard, a mouse, a remote control, a touchpad, or a voice control device, etc.

[0178] The memory 310 includes at least one type of readable storage medium, which includes non-volatile memory or volatile memory, such as flash memory, hard disk, multimedia card, card-type memory (e.g., SD or DX memory, etc.), random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), programmable read-only memory (PROM), magnetic memory, magnetic disk, optical disc, etc. RAM can include static RAM or dynamic RAM. In some embodiments, the memory 310 can be an internal storage unit of the computer device, such as the hard disk or memory of the computer device. In other embodiments, the memory 310 can also be an external storage device of the computer device, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, or Flash Card equipped on the computer device. Of course, the memory 310 can also include both the internal storage unit and the external storage device of the computer device. In this embodiment, the memory 310 is generally used to store the operating system installed on the computer device and various application software, such as the program code of the above method. In addition, the memory 310 can also be used to temporarily store various data that have been output or will be output.

[0179] The processor 320 is generally used to execute the overall operations of the computer device. In this embodiment, the memory 310 is used to store program code or instructions, and the program code includes computer operation instructions. The processor 320 is used to execute the program code or instructions stored in the memory 310 or process data, such as running the program code of the above method.

[0180] In this text, the bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. This bus system can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience of representation, only a thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0181] Another embodiment of the present application further provides a computer-readable medium, which can be a computer-readable signal medium or a computer-readable medium. A processor in the computer reads the computer-readable program code stored in the computer-readable medium, so that the processor can execute the functional actions specified in each step or the combination of steps in the above method; and generate a device for implementing the functional actions specified in each block or the combination of blocks in the block diagram.

[0182] The computer-readable medium includes but is not limited to electronic, magnetic, optical, electromagnetic, infrared memories or semiconductor systems, devices or apparatuses, or any suitable combination of the foregoing. The memory is used to store program code or instructions, and the program code includes computer operation instructions. The processor is used to execute the program code or instructions of the above method stored in the memory.

[0183] For the definitions of the memory and the processor, reference can be made to the description of the foregoing computer device embodiments, and details are not described herein again.

[0184] In several embodiments provided by the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only illustrative. For example, the division of modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection to each other can be through some interfaces, and the indirect coupling or communication connection of the devices or units can be in an electrical, mechanical or other form.

[0185] In each embodiment of the present application, each functional unit or module can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated unit can be implemented in the form of hardware or in the form of a software functional unit.

[0186] When an integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor to execute all or part of the steps of the methods of various embodiments of this application. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs that can store program codes.

[0187] In the claims, any reference signs placed between parentheses shall not be construed as limiting the claim. The "including" described in this application does not exclude the existence of elements or steps not listed in the claim. The word "a" or "an" before an element does not exclude the existence of a plurality of such elements. This application can be implemented by means of hardware including several different elements and by means of a properly programmed computer. In the claim enumerating several units of a device, several of these units of the device can be embodied by the same hardware item. The use of the first, second, and third, etc. does not indicate any order and these words can be interpreted as names. The steps in the above embodiments, unless otherwise specified, should not be construed as limiting the execution order.

[0188] The above embodiments are only used to illustrate the technical solutions of this application, rather than to limit them; although this application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of various embodiments of this application.

Claims

1. A method for posture estimation combining a thermal infrared camera and an IMU, characterized in that: include: Acquire image representation data of a current frame image and image representation data of a previous frame image captured by a thermal infrared camera in a mobile device, wherein the image representation data of the current frame image is environmental data captured by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is environmental data captured by the thermal infrared camera at the previous moment; Based on the image representation data of the current frame image and the image representation data of the previous frame image, determining whether the current frame image loses a target feature point, wherein the target feature point is a feature point included in the previous frame image but not included in the current frame image; If it is determined based on the image representation data of the current frame image and the image representation data of the previous frame image that the current frame image loses the target feature point, obtaining the world coordinate position of the target feature point when it is in the previous frame image; Obtaining current time data measured by the IMU in the mobile device; Performing multi-state constrained Kalman filtering fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame image to obtain target attitude error data, so as to determine the current estimated posture of the mobile device according to the target attitude error data and the current moment data measured by the IMU in the mobile device; Among them, the multi-state constrained Kalman filter fusion is used to propagate the error state according to the measurement data of the IMU, so as to fuse the collected data of the thermal infrared camera to obtain the target attitude error data.

2. The method according to claim 1, characterized in that The step of performing multi-state constrained Kalman filtering fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame image to obtain target attitude error data includes: Based on the current moment data measured by the IMU in the mobile device, predict the initial posture error data of the IMU at the current moment, wherein the initial posture error data is used to describe the current error estimation state of the IMU; Determining a Kalman gain based on covariance matrix data corresponding to the initial attitude error data; Based on the Kalman gain and the world coordinate position of the target feature point when it is in the previous frame image, the initial posture error data is updated to obtain the target posture error data.

3. The method according to claim 2, characterized in that Before determining the Kalman gain based on the covariance matrix data corresponding to the initial attitude error data, the method further includes: Get the sensor posture corresponding to the IMU at the current moment; Determine the camera posture corresponding to the current moment of the thermal infrared camera based on the sensor posture corresponding to the IMU at the current moment and the camera extrinsic parameters of the thermal infrared camera; Based on the camera posture corresponding to the thermal infrared camera at the current moment, matrix data amplification is performed on the covariance matrix data corresponding to the initial posture error data.

4. The method according to claim 2, characterized in that: The step of updating the initial attitude error data based on the Kalman gain and the world coordinate position of the target feature point when the target feature point is in the previous frame image to obtain the target attitude error data includes: Determine the world coordinate position of the camera feature point based on the world coordinate position of the target feature point when it is in the previous frame image and the image representation data of the current frame image; Determining camera residual item data based on the world coordinate position of the camera feature point; Determining state update data based on the camera residual term data and the Kalman gain; The target attitude error data is determined based on the state update data and the initial attitude error data.

5. The method according to claim 4, characterized in that The determining of camera residual item data based on the world coordinate position of the camera feature point includes: Determining initial residual item data based on the world coordinate position of the camera feature point; The camera residual item data is obtained by removing the related item data corresponding to the world coordinate position of the camera feature point from the initial residual item data.

6. The method according to claim 2, characterized in that The method of predicting the initial posture error data of the IMU at the current moment based on the current moment data measured by the IMU in the mobile device includes: Using the first motion state model to predict the error state of the current moment data measured by the IMU in the mobile device, to obtain the actual error state data of the IMU at the current moment; Using the first motion state model to predict the error state of the current moment data measured by the IMU in the mobile device, to obtain the estimated error state data of the IMU at the current moment; Determine the data difference between the actual error state data and the estimated error state data of the IMU at the current moment as the initial posture error data of the IMU at the current moment.

7. The method according to claim 1, characterized in that The determining whether the current frame image loses the target feature point based on the image representation data of the current frame image and the image representation data of the previous frame image comprises: Matching the image representation data of the current frame image with the image representation data of the previous frame image; If the target feature point exists in the previous frame image through matching, and the target feature point does not exist in the current frame image, then it is determined that the current frame image loses the target feature point; If the matching shows that the target feature point exists in the previous frame image and the target feature point still exists in the current frame image, it is determined that the target feature point is not lost in the current frame image.

8. A posture estimation device combining a thermal infrared camera and an IMU, characterized in that: include: A first acquisition module is used to acquire image representation data of a current frame image and image representation data of a previous frame image acquired by a thermal infrared camera in a mobile device, wherein the image representation data of the current frame image is environmental data acquired by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is environmental data acquired by the thermal infrared camera at the previous moment; A determination module, configured to determine whether the current frame image loses a target feature point based on the image representation data of the current frame image and the image representation data of the previous frame image, wherein the target feature point is a feature point included in the previous frame image but not included in the current frame image; A second acquisition module is used for acquiring the world coordinate position of the target feature point when it is in the previous frame image if it is determined that the current frame image loses the target feature point based on the image representation data of the current frame image and the image representation data of the previous frame image; A third acquisition module is used to obtain the current moment data measured by the IMU in the mobile device; A fusion module, used for performing multi-state constrained Kalman filtering fusion on the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point when it is in the previous frame image, to obtain target attitude error data, so as to determine the current estimated posture of the mobile device according to the target attitude error data and the current moment data measured by the IMU in the mobile device; Among them, the multi-state constrained Kalman filter fusion is used to propagate the error state according to the measurement data of the IMU, so as to fuse the collected data of the thermal infrared camera to obtain the target attitude error data.

9. A computer device, characterized in that: The method comprises a memory and a processor, wherein a computer program is stored in the memory, and when the processor executes the computer program, a method for estimating a posture of a thermal infrared camera combined with an IMU as claimed in any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for estimating the position and posture of the thermal infrared camera combined with the IMU as described in any one of claims 1 to 7 is implemented.

Images