Thermal infrared camera and IMU (Inertial Measurement Unit) combined pose estimation method, device, equipment and medium

Abstract

The invention provides a thermal infrared camera and IMU combined pose estimation method and device, equipment and a medium. The method comprises the steps of 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 mobile equipment; 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; if it is determined that the current frame of image loses the target feature point, obtaining a world coordinate position of the target feature point when the target feature point is in the previous frame of image; acquiring current moment data measured by an IMU (Inertial Measurement Unit) in the mobile equipment; and performing multi-state constraint Kalman filtering fusion on the current moment data measured by the IMU in the mobile equipment and the world coordinate position when the target feature point is located in the previous frame of image to obtain target attitude error data, so as to determine the current estimated pose of the mobile equipment according to the target attitude error data and the current moment data measured by the IMU in the mobile equipment. Therefore, the pose estimation accuracy is effectively improved.

Description

Technical Field

[0001] Embodiments of the present disclosure relate to the technical field of posture estimation, and in particular, to a posture estimation method, apparatus, device and medium applicable to a combination of a thermal infrared camera and an IMU. Background Art

[0002] Visual inertial odometry (VIO) technology is widely used in a variety of fields to perform complex and dangerous tasks instead of humans, such as industrial inspection, remote sensing, search and rescue. Due to its flexibility to minimize mission risks, there is an increasing demand for the ability to safely and reliably achieve perception and positioning in more challenging environments, such as fire scenes, underground environments, and environments where GPS is denied.

[0003] In the related art, when positioning a mobile device (such as a robot), the position estimation is 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 nighttime operation, they still cannot adapt to other visually degraded environmental scenes, such as scenes with obstructions in the air such as smoke, fog, and dust, and scenes with poor lighting conditions such as snowy days and darkness. The data information collected by the visible spectrum camera may be significantly reduced.

[0004] However, using the existing method, the accuracy of pose estimation is not high. Summary of the invention

[0005] The embodiments described herein provide a method, apparatus, device and medium for posture estimation combining a thermal infrared camera with an IMU, which overcomes the above-mentioned problems.

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

[0007] 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;

[0008] 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;

[0009] 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;

[0010] Obtaining current time data measured by the IMU in the mobile device;

[0011] 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;

[0012] 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.

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

[0014] 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;

[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, wherein the target feature point is a feature point included in the previous frame image but not included in the current frame image;

[0016] 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;

[0017] A third acquisition module is used to obtain the current moment data measured by the IMU in the mobile device;

[0018] 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;

[0019] 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.

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

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

[0022] The method for posture estimation combining a thermal infrared camera and an IMU provided in an embodiment of the present application obtains 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, wherein the image representation data of the current frame image is environmental data collected by the thermal infrared camera at the current moment, and the image representation data of the previous frame image is 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 loses target feature points, wherein the target feature points are feature points included in the previous frame image but not included in the current frame image; if based on the image representation data of the current frame image and the image representation data of the previous frame image If the target feature point is lost in the current frame image based on the data, the world coordinate position of the target feature point 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 point in the previous frame image are fused by multi-state constrained Kalman filtering to obtain the target attitude error data, so as to determine the current estimated posture of the mobile device based on the target attitude error data and the current moment data measured by the IMU in the mobile device; wherein, the multi-state constrained Kalman filtering fusion is used to propagate the error state based on 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. 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 can be achieved, and the accuracy of posture estimation of the mobile device can be effectively improved.

[0023] The above description is only an overview of the technical solution of the embodiment of the present application. In order to more clearly understand the technical means of the embodiment of the present application, it can be implemented in accordance with the contents of the specification. In order to make the above and other purposes, features and advantages of the embodiment of the present application more obvious and easy to understand, the specific implementation methods of the present application are listed below. BRIEF 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 noted that the drawings described below only relate to some embodiments of the present disclosure, but are not intended to limit the present disclosure, wherein:

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

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

[0027] Figure 3 It is a structural schematic 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 DESCRIPTION

[0029] In order to make the purpose, technical solution and advantages of the embodiments of the present disclosure clearer, the technical solution of the embodiments of the present disclosure will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present disclosure, rather than all of the embodiments. Based on the described embodiments of the present disclosure, all other embodiments obtained by those skilled in the art without creative work 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 a person skilled in the art to which the subject matter of the present disclosure belongs. It will be further understood that terms such as those defined in commonly used dictionaries should 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 otherwise explicitly defined herein. As used herein, a statement that two or more parts are "connected" or "coupled" together shall mean that the parts are joined together directly or through one or more intermediate components.

[0031] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase "embodiments" in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0032] The term "and / or" in this article is only a description of the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can mean: A exists, A and B exist at the same time, and B exists. In addition, the character " / " in this article 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, "plurality" means more than two (including two), and similarly, "multiple groups" means more than two groups (including two).

[0034] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution 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 flow chart of a method for posture estimation combining a thermal infrared camera and an IMU provided in an embodiment of the present disclosure, such as Figure 1 As shown in the figure, the specific process of the pose estimation method combining the thermal infrared camera and the IMU includes:

[0036] S110, obtaining 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.

[0037] 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 , determining 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.

[0040] The target feature point is a feature point included in the previous frame image but not included in the current frame image.

[0041] In some embodiments, 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 includes:

[0042] Match the image representation data of the current frame image with the image representation data of the previous frame image; if the match shows that the target feature point exists in the previous frame image and does not exist in the current frame image, then determine that the current frame image loses the target feature point; if the match shows that the target feature point exists in the previous frame image and still exists in the current frame image, then determine that the current frame image does not lose the target feature point.

[0043] Among them, the image representation data of the current frame image and the image representation data of the previous frame image can be matched to identify whether the feature points contained in the current frame image are the same as the feature points contained in the previous frame image. Thus, it is effectively determined whether the current frame image is missing the target feature point.

[0044] S130: 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.

[0045] Among them, the image coordinates of the target feature points in the image representation data of the previous image can be triangulated in combination with the camera posture to obtain the position coordinates of the target feature points in the world coordinate system corresponding to the previous frame of the image, that is, the world coordinate position.

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

[0047] 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 at the current moment measured by the IMU during the movement of the mobile device.

[0048] S150, 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 in the previous frame image to obtain target attitude error data.

[0049] Among them, multi-state constrained Kalman filtering 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 in the previous frame image to obtain the target attitude error data, so as to determine the current estimated position of the mobile device based on 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 propagate the error state according to the IMU measurement data, so as to fuse the collected data of the thermal infrared camera to obtain the target attitude error data. That is, the error state of the IMU at the current moment and the system covariance are updated according to the IMU measurement data, and the Kalman gain is calculated according to the covariance to calculate the error state of the IMU at the next moment according to the IMU measurement data at the next moment, so as to propagate the error state.

[0051] In some embodiments, the current moment data measured by the IMU in the mobile device and the world coordinate position of the target feature point in the previous frame image are fused by multi-state constrained Kalman filtering to obtain the target attitude error data, including:

[0052] Based on the current moment data measured by the IMU in the mobile device, the initial posture error data of the IMU at the current moment is predicted; based on the covariance matrix data corresponding to the initial posture error data, the Kalman gain is determined; based on the Kalman gain and the world coordinate position of the target feature point in the previous frame image, the initial posture error data is updated to obtain the target posture error data.

[0053] The initial attitude error data is used to describe the current error estimation state of the IMU, that is, the error data of the posture information of the mobile device estimated by the IMU, which may specifically 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 posture error data of the IMU at the current moment includes:

[0055] The first motion state model is used to predict the error state of the current moment data measured by the IMU in the mobile device, and the actual error state data of the IMU at the current moment is obtained; the first motion state model is used to predict the error state of the current moment data measured by the IMU in the mobile device, and the estimated error state data of the IMU at the current moment is obtained; the data difference between the actual error state data and the estimated error state data of the IMU at the current moment is determined as the initial posture error data of the IMU at the current moment.

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

[0057] For example, the IMU real state motion model state vector X I It is expressed as shown in formula (1).

[0058]

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

[0060] Considering that the features are 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 formula (2), w(t)=[w x w y w z ] T It represents the angular velocity in the IMU system, represented by {I}, G a I (t) represents the IMU acceleration, expressed in {G}, n wg (t) and n wa (t) is the Gaussian white noise process driving the IMU bias, and is an antisymmetric matrix.

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

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

[0065]

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

[0067]

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

[0069]

[0070] In formula (6),

[0071] Therefore, the above true state is “differed” from the estimated state, and the IMU error state motion model is defined as shown in formula (7).

[0072]

[0073] The difference of quaternion is different from the ordinary subtraction, so the error quaternion is introduced to represent the rotation error between the real and estimated postures, as shown in formula (8).

[0074]

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

[0076]

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

[0078]

[0079] In formula (10), is the system noise; vector n g and n a represents the measurement noise of the gyroscope and accelerometer (in Gauss), n wg and n wa is the random walk rate of the gyroscope and accelerometer bias; F is the continuous-time error state transfer matrix, as shown in formula (11); G is the input noise matrix, as shown in formula (12).

[0080]

[0081]

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

[0083] In order to process the discrete-time IMU data, the high-order Runge-Kutta numerical integration method is used to discretize the continuous-time dynamic state equation (2), so as to predict the state estimate 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 transfer matrix Φ is required. k :=Φ(t k+1 ,t k ) in order to transfer the error covariance from time t k Propagate to t k+1 According to linear system theory, the error state propagation equation The discretization form of is shown in formula (13).

[0085]

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

[0087]

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

[0089]

[0090] make: The discretized equation is shown in formula (16).

[0091]

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

[0093]

[0094] In formula (17),

[0095] Considering the impact of IMU error state changes 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 IMU error state predicted at time k+1 is shown in formula (19).

[0098]

[0099] The covariance matrix of the prediction 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, the method further includes:

[0102] Obtain the sensor posture corresponding to the IMU at the current moment; determine the camera posture corresponding to the thermal infrared camera at the current moment 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, perform matrix data amplification on the covariance matrix data corresponding to the initial posture error data.

[0103] Among them, the error state vector of the MSCKF (based on improved multi-state constrained Kalman filter 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, the IMU state is predicted and the covariance matrix of the error state is calculated; when there is an image coming in, the current camera posture is calculated based on the relative external parameters of the camera and IMU, and the latest camera state is added to the system state vector to expand the covariance matrix of the error state.

[0106] For example, when a new image is recorded, the camera's attitude estimate is calculated based on the IMU attitude estimate and the camera's external parameters, and the calculation formula is shown in formula (22).

[0107]

[0108] Set the current camera posture is added to the state vector, and the error state covariance matrix of the EKF is correspondingly expanded as shown in formula (23).

[0109]

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

[0111]

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

[0113] 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, the world coordinate position of the camera feature point is determined; based on the world coordinate position of the camera feature point, the camera residual item data is determined; based on the camera residual item data and the Kalman gain, the state update data is determined; based on the state update data and the initial attitude error data, the target attitude error data is determined.

[0114] For example, consider a set of M j Camera pose The observed single feature f j In this case, a camera measurement model is proposed. j The observed values ​​can be described by the model shown in formula (25).

[0115]

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

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

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

[0119]

[0120] In formula (26), Represents the position of the 3D feature point in the world coordinate system. Since this is unknown, the least squares method can be used to obtain the estimated value of the feature point position using the filter estimation of the measured value and the camera posture at the corresponding moment

[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 camera pose and features, the residual of formula (27) can be approximated as shown in formula (28).

[0125]

[0126] In formula (27), and The measured values ​​are The Jacobian matrix with respect to the state vector and feature position; correspondingly, is the position estimate f j The error is obtained by superimposing all M j The measured residual can be obtained from formula (29).

[0127]

[0128] In formula (29), r (j) , n (j) Is an element A block vector or matrix of .

[0129] In some embodiments, determining camera residual item data based on the world coordinate position of the camera feature point includes:

[0130] Based on the world coordinate position of the camera feature point, initial residual item data is determined; and the related item data corresponding to the world coordinate position of the camera feature point is eliminated from the initial residual item data to obtain the camera residual item 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, in order to overcome this problem, a new residual term is defined (i.e. camera residual data), r (j) Projection to Matrix The left null space of , can be obtained as shown in formula (30).

[0132]

[0133] Because 2M j ×3 matrix The column is full rank, and its left null space should be 2M j -3 dimensions, therefore, 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 on this basis. The above residual formula defines a linear constraint between all camera poses that observe the feature point, expressing the measurement value is the camera pose M j Given all available information, the resulting EKF update is optimal, except for the inaccuracies caused by linearization.

[0134] EKF updates are triggered by one of two events: ① When a feature tracked in multiple images can no longer be detected, feature point triangulation is performed to process all measurements of the feature. This is most common because the feature moves out of the camera's field of view. ② When recording a new image frame, the state vector contains a copy of the current camera pose estimate. If the maximum number of camera poses N is exceeded, max , then the old camera pose must be deleted. Before deleting the state, all feature observations at the corresponding moment will be used to provide positioning data.

[0135] Since the feature measurements are statistically independent, the noise vector are uncorrelated. Therefore, the covariance matrix of the noise vector is equal to The residual term r o The dimension is One problem that arises in practice is that d may be a very large number. In order to reduce the computational complexity of the EKF update process, the multi-state constrained Kalman filter fusion algorithm is used to update H X Perform QR decomposition. The specific expression is shown in formula (31).

[0136]

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

[0138]

[0139] It can be clearly seen that by o Projection to H X The basis vectors of the range retain all useful information in the measurement It is just a 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 EKF update, as shown in formula (33).

[0140]

[0141] In formula (33), is a noise vector, whose covariance matrix is ​​equal to Where r is the number of columns of Q1. So the Kalman gain calculated by EKF update is as shown in formula (34).

[0142]

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

[0144]

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

[0146] P k+1|k+1 =(Ι ξ -KT H ) k+1|k (Ι ξ -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 Same matrix T H Similarly, its computational complexity is O(r 2 d). On the one hand, the update of the covariance matrix involves the product of square matrices of dimension ξ, 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 is used o , without projecting it onto T X In the range of , the computational cost of calculating the Kalman gain is O(d 3 ). Since d>>ξ,r is usually typical, the residual r is used n The result will greatly save resources in computing cost.

[0148] In this embodiment, 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 are obtained, the image representation data of the current frame image being environmental data captured by the thermal infrared camera at the current moment, and the image representation data of the previous frame image being 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, it is determined whether the current frame image loses a target feature point, the target feature point being 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 a target feature point, then the target feature point is determined to be a feature point included in the previous frame image but not .... If the target feature point is lost, the world coordinate position of the target feature point 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 point in the previous frame image are fused by multi-state constrained Kalman filtering to obtain the 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; wherein, the multi-state constrained Kalman filtering 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. 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 can be achieved, and the accuracy of posture estimation of the mobile device can be effectively improved.

[0149] In this embodiment, the performance of the visual inertial odometry (VIO) of the visible light camera is reduced in special environmental scenes, resulting in unreliable motion estimation of the robot. The visible light camera in the system is replaced by a thermal infrared camera. Compared with the visible light camera, the thermal infrared camera has better robustness to lighting changes and obstructions such as dust and smoke. In the visually degraded environmental scenes, the thermal infrared camera odometry is an ideal choice for achieving better robust posture estimation. The multi-sensor fusion of thermal infrared images and inertial measurement units is used to achieve posture state estimation for mobile robots. However, unlike visible light images, the contrast of thermal infrared images is related to the radiation energy in the environment, more specifically, it is determined by the difference in infrared emissivity.

[0150] This embodiment uses inertial information to predict the robot state and covariance, and tracks the features of two frames of images. The feature points are not added to the state vector, and the pose information of the frame is added to the state vector only when new image information arrives. The state vector is a sliding window containing inertial information and several image pose information that form geometric constraints. When certain conditions are met, it is marginalized, the old state is eliminated, the new state is retained, and the size of the sliding window is kept within a certain limit; when the feature point that has been tracked cannot be observed or the size of the sliding window reaches a certain threshold, the position of the feature point is calculated using triangulation and iterative optimization algorithms to obtain an accurate three-dimensional space point position, and the geometric constraints of the pose in the sliding window are used to estimate the state vector of the system through the Kalman filter using the feature point information.

[0151] Figure 2 A structural schematic diagram of a posture estimation device combining a thermal infrared camera and an IMU is provided in this embodiment. The posture 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 used to 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.

[0153] The determination module 220 is used to determine whether 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. The target feature points are feature points included in the previous frame image but not included in the current frame image.

[0154] The second acquisition module 230 is used to acquire the world coordinate position of the target feature point 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.

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

[0156] The fusion module 250 is used to perform 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 in the previous frame image to obtain the target attitude error data, so as to determine the current estimated posture of the mobile device based on the target attitude error data and the current moment data measured by the IMU in the mobile device.

[0157] Among them, multi-state constrained Kalman filter fusion is used to propagate error states based on 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.

[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 posture error data of the IMU at the current moment based on the current moment data measured by the IMU in the mobile device, and the initial posture 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 posture error data.

[0161] The updating unit is used to update 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 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 obtain the sensor posture corresponding to the IMU at the current moment.

[0164] The second determination unit is used to 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.

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

[0166] In this embodiment, the updating unit is optionally configured to:

[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, the world coordinate position of the camera feature point is determined; based on the world coordinate position of the camera feature point, the camera residual item data is determined; based on the camera residual item data and the Kalman gain, the state update data is determined; based on the state update data and the initial attitude error data, the target attitude error data is determined.

[0168] In this embodiment, the updating unit is optionally configured to:

[0169] Based on the world coordinate position of the camera feature point, initial residual item data is determined; and the related item data corresponding to the world coordinate position of the camera feature point is eliminated from the initial residual item data to obtain the camera residual item data.

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

[0171] The first motion state model is used to predict the error state of the current moment data measured by the IMU in the mobile device, and the actual error state data of the IMU at the current moment is obtained; the first motion state model is used to predict the error state of the current moment data measured by the IMU in the mobile device, and the estimated error state data of the IMU at the current moment is obtained; the data difference between the actual error state data and the estimated error state data of the IMU at the current moment is determined as the initial posture 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 with the image representation data of the previous frame image; if the match shows that the target feature point exists in the previous frame image and does not exist in the current frame image, then determine that the current frame image loses the target feature point; if the match shows that the target feature point exists in the previous frame image and still exists in the current frame image, then determine that the current frame image does not lose the target feature point.

[0174] The pose estimation device combining a thermal infrared camera and an IMU provided in the present disclosure can execute the above method embodiments. Its specific implementation principles and technical effects can be found in the above method embodiments, and the present disclosure will not repeat them here.

[0175] The present application also provides a computer device. Figure 3 , Figure 3 This is a 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 connected to each other through a system bus. It should be noted that the figure only shows a computer device with a memory 310 and a processor 320, but it should be understood that it is not required to implement all the components shown, and more or fewer components can be implemented instead. Among them, those skilled in the art 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 (Application Specific Integrated Circuit, ASIC), programmable gate arrays (Field-Programmable Gate Array, FPGA), digital processors (Digital Signal Processor, DSP), embedded devices, etc.

[0177] Computer devices can be computing devices such as desktop computers, notebooks, PDAs, and cloud servers. Computer devices can interact with users through keyboards, mice, remote controls, touch pads, or voice control devices.

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

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

[0180] In this article, the bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. The bus system can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, only one 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 also provides a computer-readable medium, which may be a computer-readable signal medium or a computer-readable medium. A processor in a computer reads a computer-readable program code stored in the computer-readable medium, so that the processor can execute the functional actions specified in each step or a combination of steps in the above method; and generate a device for implementing the functional actions specified in each block or a combination of blocks in the block diagram.

[0182] Computer-readable media include but are not limited to electronic, magnetic, optical, electromagnetic, infrared memory or semiconductor systems, devices or apparatuses, or any appropriate combination of the foregoing, the memory is used to store program codes or instructions, the program codes include computer operating instructions, and the processor is used to execute the program codes or instructions of the above methods stored in the memory.

[0183] For the definitions of memory and processor, please refer to the description of the aforementioned computer device embodiment and will not be repeated here.

[0184] In the several embodiments provided in 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 schematic, for example, the division of modules or units is only a logical function division, and there may be other division methods in actual implementation, such as 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 mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0185] Each functional unit or module in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional unit.

[0186] If the 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 the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of each embodiment method of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), disk or optical disk and other media that can store program code.

[0187] In the claims, any reference symbols placed between brackets shall not be construed as limiting the claims. The word "comprising" described in the present application does not exclude the presence of elements or steps not listed in the claims. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The present application can be implemented with the aid of hardware comprising several different elements and with the aid of a suitably programmed computer. In a unit claim that lists a number of devices, several units of these devices may be embodied by the same hardware item. The use of first, second, and third, etc. does not indicate any order, and these words may be interpreted as names. The steps in the above embodiments, unless otherwise specified, should not be understood as limitations on the order of execution.

[0188] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present 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