A dual-motion platform posture measurement method based on visual inertia

Abstract

The present application belongs to the field of posture measurement technology, and relates to a dual-motion platform posture measurement method based on visual inertia, including: obtaining a measurement platform and a target platform, and constructing a dual-motion platform; a visual inertial device is provided on the measurement platform, and a cooperation mark is provided on the target platform; the motion state of the target platform and the motion state of the measurement platform are predicted, and a priori estimate of the motion state of the target platform, a priori estimate of the uncertainty of the motion state of the target platform, a priori estimate of the motion state of the measurement platform, and a priori estimate of the uncertainty of the motion state of the measurement platform are obtained; an image of the target platform captured by the visual inertial device is obtained, and the sub-pixel image coordinates of the cooperation mark are obtained; the motion state of the target platform and the motion state of the measurement platform are updated respectively, and the posture of the target platform and the posture of the measurement platform are obtained. The present application can measure the posture of the target platform and the measurement platform in the autonomous interactive guidance of the dual-motion platform.

Description

Technical Field

[0001] The present application relates to the technical field of posture measurement, and in particular to a dual-motion platform posture measurement method based on vision-inertia. Background Art

[0002] During the autonomous rendezvous of dual-motion platforms, real-time and high-precision measurement of platform posture parameters is an important basis for the design of guidance and control laws.

[0003] In the existing technology, measurement methods are mainly based on radio equipment, global satellite positioning systems, etc. Such methods rely on data links between platforms, are easily affected by electromagnetic interference, have difficulty adapting to complex electromagnetic environments, and cannot achieve fully autonomous interactive guidance.

[0004] Due to the characteristics of passive measurement, monocular vision measurement does not require data link support, is not affected by electromagnetic interference, and can achieve fully autonomous relative posture measurement. In addition, it has the characteristics of small size, light weight, and low power consumption, and has gradually become one of the important technical solutions for autonomous interactive guidance.

[0005] However, due to factors such as monocular scale ambiguity, complex lighting, and limited computing power of embedded platforms, the posture measurement method based on monocular vision still faces prominent problems such as low accuracy, poor robustness, and low measurement frequency, making it difficult to provide reliable guidance information; in addition, the measurement of the motion state of the target platform in the autonomous interaction of dual-motion platforms is an important factor affecting the success rate of interactive guidance, while the visual method can only obtain the relative posture relationship between platforms and cannot estimate the motion state of the target platform. Summary of the invention

[0006] Based on this, it is necessary to provide a dual-motion platform posture measurement method based on visual inertia to address the above technical problems, which can solve the posture of the measurement platform and the target platform in real time and with high precision during the autonomous interactive guidance of the dual-motion platform.

[0007] A dual-motion platform posture measurement method based on visual inertia, comprising:

[0008] Acquire a measurement platform and a target platform to construct a dual-motion platform; wherein the measurement platform is provided with a visual inertial device, and the target platform is provided with a cooperation mark;

[0009] The constant velocity model and the inertial navigation mechanical arrangement are used to predict the motion state of the target platform and the motion state of the measuring platform respectively, and the error state propagation method is used to obtain the prior estimation of the motion state of the target platform, the prior estimation of the uncertainty of the motion state of the target platform, the prior estimation of the motion state of the measuring platform and the prior estimation of the uncertainty of the motion state of the measuring platform respectively;

[0010] Acquire the target platform image captured by the visual inertial device, and process it to obtain the sub-pixel image coordinates of the cooperation mark on the target platform;

[0011] According to the a priori estimate of the motion state of the measurement platform, the a priori estimate of the motion state of the target platform, the a priori estimate of the uncertainty of the motion state of the target platform, and the sub-pixel image coordinates of the cooperation mark on the target platform, the motion state of the target platform is updated to obtain a posterior estimate of the motion state of the target platform and a posterior estimate of the uncertainty of the motion state of the target platform, and the position state and posture state in the posterior estimate of the motion state of the target platform are used as the position and posture of the target platform;

[0012] According to the prior estimate of the motion state of the measuring platform, the prior estimate of the uncertainty of the motion state of the measuring platform, the sub-pixel image coordinates of the cooperation mark on the target platform and the posterior estimate of the motion state of the target platform, the motion state of the measuring platform is updated to obtain the posterior estimate of the motion state of the measuring platform, and the position state and posture state in the posterior estimate of the motion state of the measuring platform are used as the pose of the measuring platform.

[0013] In one embodiment, a constant velocity model and an inertial navigation mechanical arrangement are used to predict the motion state of the target platform and the motion state of the measuring platform respectively, and an error state propagation method is used to obtain a priori estimation of the motion state of the target platform, a priori estimation of the uncertainty of the motion state of the target platform, a priori estimation of the motion state of the measuring platform, and a priori estimation of the uncertainty of the motion state of the measuring platform respectively, including:

[0014] A constant velocity model is used to predict the motion state of the target platform, and an error state propagation method is used to obtain a priori estimation of the motion state of the target platform and a priori estimation of the uncertainty of the motion state of the target platform;

[0015] The motion state of the measuring platform is predicted by adopting inertial navigation mechanical arrangement, and the error state propagation method is used to obtain a priori estimation of the motion state of the measuring platform and a priori estimation of the uncertainty of the motion state of the measuring platform.

[0016] In one embodiment, a constant velocity model is used to predict the motion state of the target platform, and an error state propagation method is used to obtain a priori estimation of the motion state of the target platform and a priori estimation of the uncertainty of the motion state of the target platform, including:

[0017] The target platform is modeled by using a constant velocity model to obtain a motion state differential equation of the target platform;

[0018] Integrating the motion state differential equation of the target platform and ignoring the noise term to obtain a priori estimation of the motion state of the target platform;

[0019] According to the motion state differential equation of the target platform, the motion error state differential equation of the target platform is obtained; the motion error state differential equation of the target platform is integrated to obtain the error state propagation equation of the target platform; according to the error state propagation equation of the target platform, a priori estimate of the motion state uncertainty of the target platform is obtained.

[0020] In one embodiment, an inertial navigation mechanical arrangement is used to predict the motion state of the measurement platform, and an error state propagation method is used to obtain a priori estimation of the motion state of the measurement platform and a priori estimation of the uncertainty of the motion state of the measurement platform, including:

[0021] According to the working principle of the inertial measurement unit in the visual inertial device, the motion state differential equation of the measurement platform is obtained;

[0022] Assuming that the angular velocity of the measurement platform changes linearly between adjacent moments, the prior estimation of the attitude state of the measurement platform is obtained according to the Bortz equation; assuming that the acceleration of the measurement platform changes linearly between adjacent moments, the prior estimation of the velocity state of the measurement platform is obtained according to the inertial navigation mechanical arrangement algorithm; based on the prior estimation of the attitude state of the measurement platform and the prior estimation of the velocity state of the measurement platform, the prior estimation of the position state of the measurement platform is obtained by using the median integral method; based on the prior estimation of the attitude state of the measurement platform, the prior estimation of the velocity state of the measurement platform and the prior estimation of the position state of the measurement platform, the prior estimation of the motion state of the measurement platform is obtained;

[0023] According to the motion state differential equation of the measuring platform, the motion error state differential equation of the measuring platform is obtained; the motion error state differential equation of the measuring platform is integrated to obtain the error state propagation equation of the measuring platform; according to the error state propagation equation of the measuring platform, the prior estimation of the motion state uncertainty of the measuring platform is obtained.

[0024] In one embodiment, an image of a target platform captured by a visual inertial device is acquired and processed to obtain sub-pixel image coordinates of a cooperation mark on the target platform, including:

[0025] Acquire a target platform image captured by a visual inertial device, and perform automatic threshold segmentation and binarization on the target platform image to obtain a plurality of partitions containing cooperation marks;

[0026] A region growing algorithm is used to process multiple partitions to obtain multiple potential closed regions of the cooperation mark; the number of pixels in each closed region is counted, and all closed regions are screened according to the imaging area of ​​the cooperation mark to obtain multiple candidate regions;

[0027] Using multiple regularized negative Laplacian operators of different scales, convolve all candidate regions to obtain multiple image convolution response values ​​corresponding to each pixel in all candidate regions, and take the maximum value of multiple image convolution response values ​​corresponding to a pixel as the response value of the pixel;

[0028] According to all the response values, the imaging area of ​​each cooperation mark is obtained, and the sub-pixel image coordinates of the cooperation mark on the target platform are obtained.

[0029] In one embodiment, obtaining the imaging area of ​​each cooperation mark according to all response values ​​and obtaining the sub-pixel image coordinates of the cooperation mark on the target platform includes:

[0030] The pixel position corresponding to the maximum value of all response values ​​is taken as the position of the first cooperation mark, and the first circle is drawn with the position of the first cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the first cooperation mark as the radius. The area enclosed by the first circle is the imaging area of ​​the first cooperation mark.

[0031] After excluding the imaging area of ​​the first cooperation mark, the pixel position corresponding to the maximum value of all response values ​​is taken as the position of the second cooperation mark, and a second circle is drawn with the position of the second cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the second cooperation mark as the radius. The area enclosed by the second circle is the imaging area of ​​the second cooperation mark;

[0032] Traverse each cooperation mark until the imaging area of ​​each cooperation mark is obtained;

[0033] Perform quadratic surface fitting on the response value of each pixel in an imaging area to obtain a quadratic surface equation; calculate the partial derivative of the quadratic surface equation and set it to zero to construct a homogeneous linear equation group; solve the homogeneous linear equation group to obtain the peak point of the quadratic surface equation, and use the coordinates of the peak point as the coordinates of the cooperation mark corresponding to the imaging area; traverse each imaging area to obtain the sub-pixel image coordinates of the cooperation mark on the target platform.

[0034] In one embodiment, the motion state of the target platform is updated according to the a priori estimate of the motion state of the measurement platform, the a priori estimate of the motion state of the target platform, the a priori estimate of the uncertainty of the motion state of the target platform, and the sub-pixel image coordinates of the cooperation mark on the target platform, to obtain a posterior estimate of the motion state of the target platform and a posterior estimate of the uncertainty of the motion state of the target platform, and the position state and posture state in the posterior estimate of the motion state of the target platform are used as the pose of the target platform, including:

[0035] The sub-pixel image coordinates of all the cooperation marks on the target platform are used to form a vector to obtain an observation vector;

[0036] According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, a first observation constraint is constructed, and a first observation constraint vector is obtained; according to the first observation constraint, a Jacobian matrix of the first observation constraint vector relative to the motion error state of the target platform is obtained;

[0037] The error state Kalman filter theory is adopted to obtain the target platform Kalman gain matrix according to the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state and the prior estimation of the target platform motion state uncertainty.

[0038] Using the error state Kalman filter theory, according to the target platform Kalman gain matrix and the observation vector, the posterior estimation of the motion error state of the target platform is obtained; according to the posterior estimation of the motion error state of the target platform, the posterior estimation of the motion state of the target platform is obtained;

[0039] The error state Kalman filter theory is used to obtain the posterior estimation of the target platform's motion state uncertainty according to the target platform's Kalman gain matrix.

[0040] The position state and the attitude state in the posterior estimation of the motion state of the target platform are used as the position and posture of the target platform.

[0041] In one embodiment, a first observation constraint is constructed according to a priori estimation of the motion state of the target platform and a priori estimation of the motion state of the measurement platform, and a first observation constraint vector is obtained; and a Jacobian matrix of the first observation constraint vector relative to the motion error state of the target platform is obtained according to the first observation constraint, including:

[0042] According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, the first observation constraint is constructed, and according to the pinhole imaging model, the coordinates of the cooperation mark are converted from the target platform coordinate system to the camera system to obtain the first observation constraint vector;

[0043] According to the first observation constraint, the Jacobi matrix of the first observation constraint vector relative to the target platform motion error state is obtained; wherein the Jacobi matrix of the first observation constraint vector relative to the target platform motion error state includes: the Jacobi matrix of the first observation constraint vector relative to the target platform motion state and the Jacobi matrix of the target platform motion state relative to the target platform motion error state.

[0044] In one embodiment, the motion state of the measuring platform is updated according to the a priori estimate of the motion state of the measuring platform, the a priori estimate of the uncertainty of the motion state of the measuring platform, the sub-pixel image coordinates of the cooperation mark on the target platform, and the posterior estimate of the motion state of the target platform to obtain the posterior estimate of the motion state of the measuring platform, and the position state and posture state in the posterior estimate of the motion state of the measuring platform are used as the pose of the measuring platform, including:

[0045] The sub-pixel image coordinates of all the cooperation marks on the target platform are used to form a vector to obtain an observation vector;

[0046] According to the a priori estimate of the motion state of the measurement platform and the a posteriori estimate of the motion state of the target platform, a second observation constraint is constructed, and a second observation constraint vector is obtained; according to the second observation constraint, a Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform is obtained;

[0047] Adopting the error state Kalman filter theory, the Kalman gain matrix of the measurement platform is obtained according to the Jacobian matrix of the second observation constraint vector relative to the error state of the measurement platform motion and the prior estimation of the uncertainty of the motion state of the measurement platform;

[0048] Using the error state Kalman filter theory, according to the Kalman gain matrix of the measurement platform and the observation vector, the posterior estimation of the motion error state of the measurement platform is obtained; according to the posterior estimation of the motion error state of the measurement platform, the posterior estimation of the motion state of the measurement platform is obtained;

[0049] The position state and the attitude state in the posterior estimation of the motion state of the measuring platform are used as the position and attitude of the measuring platform.

[0050] In one embodiment, a second observation constraint is constructed based on a priori estimation of the motion state of the measurement platform and a posteriori estimation of the motion state of the target platform, and a second observation constraint vector is obtained; and a Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform is obtained based on the second observation constraint, including:

[0051] According to the prior estimation of the motion state of the measuring platform and the a posteriori estimation of the motion state of the target platform, a second observation constraint is constructed, and according to the pinhole imaging model, a second observation constraint vector is obtained;

[0052] According to the second observation constraint, the Jacobi matrix of the second observation constraint vector relative to the motion error state of the measuring platform is obtained; wherein the Jacobi matrix of the second observation constraint vector relative to the motion error state of the measuring platform includes: the Jacobi matrix of the second observation constraint vector relative to the motion state of the measuring platform and the Jacobi matrix of the motion state of the measuring platform relative to the motion error state of the measuring platform.

[0053] The above-mentioned dual-motion platform posture measurement method based on visual inertia utilizes the image information of the infrared cooperation marker light on the target platform collected by the monocular infrared camera on the measuring platform and the fusion of the inertial navigation information to realize the efficient and high-precision measurement of the posture parameters (6-dimensional relative posture and 6-dimensional target platform posture) between platforms during the autonomous interaction of the dual-motion platform. The measurement process is completely autonomous and does not require data link support between platforms. The measurement results can be output at a high frequency. Compared with monocular posture measurement, the accuracy and continuity of the measurement results are greatly improved. This application designs dual-channel motion state estimation of the measuring platform and the target platform, and proposes an error state update strategy, which realizes the effective fusion of inertial measurement data and monocular vision measurement data between the dual-motion platforms, and can realize the estimation of relative posture parameters between platforms and the motion state of the dual platforms (including: posture parameter estimation of the measuring platform and posture parameter estimation of the target platform), which has broad application prospects in the fields of visual inertial navigation, monocular vision measurement, and posture estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 1 is a flow chart of a dual-motion platform posture measurement method based on visual inertia in one embodiment;

[0055] Figure 2 A time correspondence diagram of an image sequence and an IMU sequence in one embodiment;

[0056] Figure 3 A schematic diagram of a scene of a dual-motion platform posture measurement method based on visual inertia in one embodiment;

[0057] Figure 4 Schematic diagram of the architecture of a dual-motion platform posture measurement method based on visual inertia in one embodiment;

[0058] Figure 5 It is a structural block diagram of a dual-motion platform posture measurement device based on visual inertia in one embodiment;

[0059] Figure 6 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0061] In addition, the descriptions of "first", "second", etc. in this application are only for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of this application, "multiple groups" means at least two groups, such as two groups, three groups, etc., unless otherwise clearly and specifically defined.

[0062] In this application, unless otherwise clearly specified and limited, the terms "connection", "fixation", etc. should be understood in a broad sense. For example, "fixation" can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection, an electrical connection, a physical connection, or a wireless communication connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0063] In addition, the technical solutions between the various embodiments of the present application can be combined with each other, but it must be based on the fact that ordinary technicians in the field can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0064] This application provides a dual-motion platform posture measurement method based on visual inertia, such as Figure 1 The flowchart shown, in one embodiment, includes:

[0065] Step 101, obtaining a measurement platform and a target platform to construct a dual-motion platform; wherein the measurement platform is provided with a visual inertial device, and the target platform is provided with a cooperation mark.

[0066] Specifically, the measurement platform may be an aircraft, the visual inertial equipment may include a visual sensor (such as a monocular infrared camera) and an inertial measurement unit (IMU), the target platform may be a ship, and the cooperation mark may include infrared cooperation mark lights (no less than 4 lights).

[0067] In this step, the measurement platform, the target platform, the visual inertial device and the cooperation mark are all existing technologies and will not be described in detail here.

[0068] Step 102, using a constant velocity model and an inertial navigation mechanical arrangement, respectively predicting the motion state of the target platform and the motion state of the measurement platform, and using an error state propagation method, respectively obtaining a priori estimate of the motion state of the target platform, a priori estimate of the uncertainty of the motion state of the target platform, a priori estimate of the motion state of the measurement platform, and a priori estimate of the uncertainty of the motion state of the measurement platform.

[0069] Specifically:

[0070] The constant velocity model is used to predict the motion state of the target platform, and the error state propagation method is used to obtain the prior estimation of the motion state of the target platform and the prior estimation of the uncertainty of the motion state of the target platform.

[0071] The inertial navigation mechanical arrangement is adopted to predict the motion state of the measurement platform, and the error state propagation method is used to obtain the prior estimation of the motion state of the measurement platform and the prior estimation of the uncertainty of the motion state of the measurement platform.

[0072] More specifically:

[0073] The target platform is modeled using a constant velocity model to obtain the motion state differential equation of the target platform;

[0074] Integrate the differential equation of the motion state of the target platform and ignore the noise term to obtain a priori estimate of the motion state of the target platform;

[0075] According to the motion state differential equation of the target platform, the motion error state differential equation of the target platform is obtained; the motion error state differential equation of the target platform is integrated to obtain the error state propagation equation of the target platform; according to the error state propagation equation of the target platform, a priori estimation of the motion state uncertainty of the target platform is obtained.

[0076] According to the working principle of the inertial measurement unit in the visual inertial device, the motion state differential equation of the measurement platform is obtained;

[0077] Assuming that the angular velocity of the measurement platform changes linearly between adjacent moments, the prior estimation of the attitude state of the measurement platform is obtained according to the Bortz equation; assuming that the acceleration of the measurement platform changes linearly between adjacent moments, the prior estimation of the velocity state of the measurement platform is obtained according to the inertial navigation mechanical arrangement algorithm; based on the prior estimation of the attitude state of the measurement platform and the prior estimation of the velocity state of the measurement platform, the prior estimation of the position state of the measurement platform is obtained by using the median integral method; based on the prior estimation of the attitude state of the measurement platform, the prior estimation of the velocity state of the measurement platform and the prior estimation of the position state of the measurement platform, the prior estimation of the motion state of the measurement platform is obtained;

[0078] According to the motion state differential equation of the measuring platform, the motion error state differential equation of the measuring platform is obtained; the motion error state differential equation of the measuring platform is integrated to obtain the error state propagation equation of the measuring platform; according to the error state propagation equation of the measuring platform, the prior estimation of the motion state uncertainty of the measuring platform is obtained.

[0079] More specifically:

[0080] The motion state of the target platform is defined as the position state, velocity state and attitude state of the target platform, which are respectively the position of the origin of the target platform coordinate system in the world system, the velocity of the origin of the target platform coordinate system in the world system, and the rotation quaternion from the target platform coordinate system to the world system. The three together constitute the target platform motion state vector:

[0081] ;

[0082] In the formula, is the target platform motion state vector, is the position of the origin of the target platform coordinate system in the world system, is the velocity of the origin of the target platform coordinate system in the world system, The rotation quaternion from the target platform coordinate system to the world system;

[0083] The motion error state of the target platform is defined as:

[0084] ;

[0085] In the formula, is the motion error state vector of the target platform, is the target platform position error, is the target platform velocity error, is the equivalent rotation vector corresponding to the target platform attitude error;

[0086] Among them, the relationship between the equivalent rotation vector corresponding to the target platform attitude error and the quaternion attitude error is:

[0087] ;

[0088] In the formula, is the quaternion attitude error;

[0089] The motion state of the measurement platform is defined as the position state, velocity state and attitude state of the measurement platform, which are respectively the position of the origin of the IMU coordinate system in the world system, the velocity of the origin of the IMU coordinate system in the world system, and the rotation quaternion from the IMU coordinate system to the world system. The three together constitute the motion state vector of the measurement platform:

[0090] ;

[0091] In the formula, To measure the platform motion state vector, is the position of the origin of the IMU coordinate system in the world system, is the velocity of the origin of the IMU coordinate system in the world system, is the rotation quaternion from the IMU coordinate system to the world system;

[0092] The motion error state of the measurement platform is defined as:

[0093] ;

[0094] In the formula, To measure the platform motion error state, To measure the platform position error state, To measure the platform velocity error state, is the equivalent rotation vector corresponding to the attitude error state of the measurement platform;

[0095] Among them, the relationship between the equivalent rotation vector corresponding to the measurement platform attitude error and the quaternion attitude error is:

[0096] ;

[0097] In the formula, is the quaternion attitude error;

[0098] The target platform is modeled using a constant velocity model, and the motion state differential equation of the target platform is obtained:

[0099] ;

[0100] in, , The following distribution is satisfied:

[0101] ;

[0102] In the formula, is the first-order derivative of the target platform position state with respect to time, is the first-order derivative of the target platform velocity state with respect to time, is the first-order derivative of the target platform attitude state with respect to time; is quaternion multiplication; is the target platform acceleration, modeled as zero-mean Gaussian noise; is the angular velocity of the target platform, modeled as zero-mean Gaussian noise; is a Gaussian distribution; is the target platform acceleration noise covariance matrix, setting parameters for the constant velocity motion model; is the target platform angular velocity noise covariance matrix, setting parameters for the constant velocity motion model;

[0103] Integrate the differential equation of the motion state of the target platform and ignore the noise term to obtain the prior estimate of the motion state of the target platform in discrete time:

[0104] ;

[0105] In the formula, the subscript Indicates The time corresponding to the frame IMU data, referred to as time; subscript Indicates The time corresponding to the frame IMU data, referred to as time; for The prior estimate of the target platform position state at the moment, for The posterior estimation of the position state of the target platform at the moment, for The prior estimate of the target platform velocity state at time instant, for The posterior estimation of the target platform velocity state at time instant, for Prior estimation of the target platform attitude state at the moment, for Posterior estimation of the target platform attitude state at every moment;

[0106] According to the motion state differential equation of the target platform, the motion error state differential equation of the target platform is obtained:

[0107] ;

[0108] In the formula, is the first-order derivative of the target platform position error state with respect to time, is the first-order derivative of the target platform velocity error state with respect to time, is the first-order derivative of the target platform attitude error state with respect to time;

[0109] By integrating the motion error state differential equation of the target platform, the error state propagation equation of the target platform in discrete time is obtained:

[0110] ;

[0111] in,

[0112] ;

[0113] ;

[0114] In the formula, for The motion error state of the target platform at the moment, for The error state transfer matrix of the target platform at time, for The motion error state of the target platform at the moment, for The noise term of the error state propagation equation of the target platform at time t; for The identity matrix, for Zero matrix, for Moment and the time interval between moments; for Zero vector, is the Gaussian white noise of the target platform acceleration The value of the moment, is the target platform angular velocity Gaussian white noise in The value of the moment;

[0115] According to the error state propagation equation of the target platform and considering the characteristics of Gaussian white noise, the uncertainty prior estimate of the motion state of the target platform in discrete time (i.e., the prior estimate of the covariance matrix of the motion state of the target platform) is obtained:

[0116] ;

[0117] in,

[0118] ;

[0119] In the formula, for The prior estimate of the covariance matrix of the motion state of the target platform at the moment, for The posterior estimation of the covariance matrix of the motion state of the target platform at the moment, for The transpose of for The noise covariance matrix of the platform motion measured at each moment.

[0120] According to the working principle of the inertial measurement unit in the visual inertial device, the motion state differential equation of the measurement platform is obtained by using the accelerometer and gyroscope measurement data:

[0121] ;

[0122] in, , The following distribution is satisfied:

[0123] ;

[0124] In the formula, To measure the first-order derivative of the platform position state with respect to time, To measure the first-order derivative of the platform velocity state with respect to time, To measure the first-order derivative of the platform attitude state with respect to time, is the rotation matrix corresponding to the attitude state of the measurement platform, is the accelerometer measurement value, is the accelerometer bias, is the Gaussian white noise of the accelerometer zero bias, is the gyroscope angular velocity measurement, is the gyroscope bias, is the Gaussian white noise of the gyroscope zero bias, is the gravity vector, is quaternion multiplication; is the covariance matrix of the zero-bias Gaussian white noise of the accelerometer, and is the internal parameter of the inertial measurement unit calibrated in advance; is the covariance matrix of the gyroscope zero-bias Gaussian white noise, and is the pre-calibrated internal parameter of the inertial measurement unit;

[0125] Assume that the angular velocity of the measuring platform is Moment and The linear change between the time and the moment can obtain the prior estimation of the attitude state of the measurement platform in discrete time:

[0126] ;

[0127] According to the Bortz equation, we get:

[0128] ;

[0129] in,

[0130] ;

[0131] ;

[0132] In the formula, for The prior estimate of the attitude state of the measurement platform at all times, for The posterior estimation of the attitude state of the platform is measured at all times. For Time has come The equivalent rotation vector that measures the change in platform attitude at all times, For vector Mould length; for Gyroscope measurement value at the moment, for Gyroscope measurement value at the moment;

[0133] Assume that the acceleration of the measuring platform is at adjacent moments ( Moment and The velocity state of the measuring platform in discrete time is estimated based on the inertial navigation mechanical arrangement algorithm:

[0134] ;

[0135] in,

[0136] ;

[0137] In the formula, for The prior estimate of the velocity state of the platform is measured at every moment, for The a posteriori estimate of the velocity state of the platform is measured at all times. for The rotation matrix corresponding to the posterior estimation of the attitude state of the measurement platform at all times, for Accelerometer measurement value at the moment, for Accelerometer measurement value at the moment, for Gyroscope angular velocity measurement value at the moment, for Gyroscope angular velocity measurement value at the moment;

[0138] According to the prior estimation of the attitude state of the measurement platform and the prior estimation of the velocity state of the measurement platform, the median integration method is used to obtain the prior estimation of the position state of the measurement platform:

[0139] ;

[0140] In the formula, for The prior estimate of the position state of the measurement platform at each moment, for The a posteriori estimate of the position state of the measurement platform at each moment, for The rotation matrix corresponding to the prior estimate of the attitude state of the measurement platform at all times;

[0141] According to the prior estimation of the attitude state of the measuring platform, the prior estimation of the velocity state of the measuring platform and the prior estimation of the position state of the measuring platform, a prior estimation of the motion state of the measuring platform is obtained;

[0142] According to the motion state differential equation of the measuring platform, the motion error state differential equation of the measuring platform is obtained:

[0143] ;

[0144] In the formula, is the first-order derivative of the measurement platform position error state with respect to time, is the first-order derivative of the platform velocity error state with respect to time, is the first-order derivative of the measured platform attitude error state with respect to time, For vector The corresponding antisymmetric matrix;

[0145] By integrating the motion error state differential equation of the measurement platform, the error state propagation equation of the measurement platform in discrete time is obtained:

[0146] ;

[0147] in,

[0148] ;

[0149] ;

[0150] ;

[0151] In the formula, for Measure the motion error state of the platform at all times. for The error state transfer matrix of the platform is measured at every moment. for Measure the motion error state of the platform at all times. for The noise term of the platform error state propagation equation is measured at all times, is the equivalent rotation vector The transformation to the rotation matrix, is the accelerometer zero bias Gaussian white noise The value of the moment, is the gyroscope zero bias Gaussian white noise in The value of the moment;

[0152] According to the error state propagation equation of the measurement platform and considering the characteristics of Gaussian white noise, the uncertainty prior estimate of the motion state of the measurement platform (i.e., the prior estimate of the covariance matrix of the motion state of the measurement platform) is obtained:

[0153] ;

[0154] in,

[0155] ;

[0156] In the formula, for The prior estimate of the covariance matrix of the motion state of the measurement platform at all times, for The posterior estimation of the covariance matrix of the motion state of the measurement platform at all times, for The transpose of for The noise covariance matrix of the platform motion measured at each moment.

[0157] In this step, the state of the target platform and the state of the measurement platform are predicted.

[0158] It should be noted that inertial navigation mechanical arrangement refers to the use of IMU measurement data, combined with the noise distribution of the accelerometer and the noise distribution of the gyroscope, to integrate the motion state differential equation to obtain a priori estimation of the motion state of the measurement platform.

[0159] Step 103 , obtaining the target platform image captured by the visual inertial device and processing it to obtain the sub-pixel image coordinates of the cooperation mark on the target platform.

[0160] Specifically:

[0161] Obtain the target platform image captured by the visual inertial device, and perform automatic threshold segmentation and binarization on the target platform image to obtain multiple partitions containing cooperation marks;

[0162] A region growing algorithm is used to process multiple partitions to obtain multiple potential closed regions of the cooperation mark; the number of pixels in each closed region is counted, and all closed regions are screened according to the imaging area of ​​the cooperation mark to obtain multiple candidate regions;

[0163] Using multiple regularized negative Laplacian operators of different scales, convolve all candidate regions to obtain multiple image convolution response values ​​corresponding to each pixel in all candidate regions, and take the maximum value of multiple image convolution response values ​​corresponding to a pixel as the response value of the pixel;

[0164] According to all the response values, the imaging area of ​​each cooperation mark is obtained, and the sub-pixel image coordinates of the cooperation mark on the target platform are obtained.

[0165] More specifically:

[0166] Obtain the target platform image captured by the visual inertial device, and perform automatic threshold segmentation and binarization on the target platform image to obtain multiple partitions containing cooperation marks;

[0167] A region growing algorithm is used to process multiple partitions to obtain multiple potential closed regions of the cooperation mark; the number of pixels in each closed region is counted, and all closed regions are screened according to the imaging area of ​​the cooperation mark to obtain multiple candidate regions;

[0168] Using multiple regularized negative Laplacian operators of different scales, convolve all candidate regions to obtain multiple image convolution response values ​​corresponding to each pixel in all candidate regions, and take the maximum value of multiple image convolution response values ​​corresponding to a pixel as the response value of the pixel;

[0169] The pixel position corresponding to the maximum value of all response values ​​is taken as the position of the first cooperation mark, and the first circle is made with the position of the first cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the first cooperation mark as the radius, and the area surrounded by the first circle is the imaging area of ​​the first cooperation mark; after excluding the imaging area of ​​the first cooperation mark, the pixel position corresponding to the maximum value of all response values ​​is taken as the position of the second cooperation mark, and the second circle is made with the position of the second cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the second cooperation mark as the radius, and the area surrounded by the second circle is the imaging area of ​​the second cooperation mark; traverse each cooperation mark until the imaging area of ​​each cooperation mark is obtained;

[0170] Perform quadratic surface fitting on the response value of each pixel in an imaging area to obtain the quadratic surface equation; calculate the partial derivative of the quadratic surface equation and set it to zero to construct a homogeneous linear equation system; solve the homogeneous linear equation system to obtain the peak point of the quadratic surface equation, and use the coordinates of the peak point as the coordinates of the cooperation mark corresponding to the imaging area; traverse each imaging area to obtain the sub-pixel image coordinates of the cooperation mark on the target platform.

[0171] More specifically:

[0172] Obtain the target platform image captured by the monocular infrared camera of the visual inertial device, and perform automatic threshold segmentation and binarization on the target platform image to obtain multiple partitions containing cooperation marks;

[0173] A region growing algorithm is used to process multiple partitions to obtain multiple potential closed regions of the cooperation mark; the number of pixels in each closed region is counted, and all closed regions are screened according to the imaging area of ​​the cooperation mark, and regions that do not meet the imaging area are deleted to obtain multiple candidate regions;

[0174] Use multiple regularized negative Laplacian operators of different scales to convolve all candidate areas, obtain multiple image convolution response values ​​corresponding to each pixel in all candidate areas, take the maximum value of multiple image convolution response values ​​corresponding to a pixel as the response value of the pixel (that is, for any pixel in the candidate area, take the maximum value of the convolution response values ​​at multiple scales as the response value of the pixel), and obtain the response values ​​of all pixels; where the regularized negative Laplacian operator is:

[0175] ;

[0176] In the formula, is the response value of the Normalized Negative Laplacian of Gaussian (NNLOG) operator, is the two-dimensional point coordinate of the convolution kernel, is the NNLOG operator scale;

[0177] The pixel position corresponding to the maximum value of the response value of all pixels is taken as the position of the first cooperation mark, and the first circle is made with the position of the first cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the first cooperation mark as the radius, and the area surrounded by the first circle is the imaging area of ​​the first cooperation mark; after excluding the imaging area of ​​the first cooperation mark, in the remaining candidate areas, the pixel position corresponding to the maximum value of all response values ​​is taken as the position of the second cooperation mark, and the second circle is made with the position of the second cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the second cooperation mark as the radius, and the area surrounded by the second circle is the imaging area of ​​the second cooperation mark; and so on, traverse each cooperation mark until the imaging area of ​​each cooperation mark is obtained;

[0178] Perform quadratic surface fitting on the response value of each pixel in an imaging area to obtain the quadratic surface equation:

[0179] ;

[0180] In the formula, is the quadratic surface equation, is the two-dimensional coordinate of the pixel point in the imaging area; , , , , , The coefficients to be solved for the quadratic surface equation are obtained by selecting more than 6 pixel points in the imaging area including the center of the infrared cooperation sign light and constructing a non-homogeneous linear equation system;

[0181] After taking the partial derivative of the quadratic surface equation and setting it to zero, we can construct a homogeneous linear equation system:

[0182] ;

[0183] In the formula, is the quadratic surface equation;

[0184] Solve the homogeneous linear equations to obtain the peak point of the quadratic surface equation, and use the coordinates of the peak point as the sub-pixel image coordinates of the cooperation mark in the imaging area:

[0185] ;

[0186] In the formula, is the two-dimensional coordinate of the pixel point in the imaging area;

[0187] Each imaging region is traversed to obtain the sub-pixel image coordinates of the cooperation mark on the target platform.

[0188] In this step, the sub-pixel image coordinates of the cooperation mark are obtained according to the target platform image to update the states of the target platform and the measurement platform (including the motion state and the motion state uncertainty).

[0189] It should be noted that automatic threshold segmentation binarization, region growing algorithm and convolution are all existing technologies and will not be described in detail here.

[0190] Step 104, based on the prior estimate of the motion state of the measurement platform, the prior estimate of the motion state of the target platform, the prior estimate of the motion state uncertainty of the target platform, and the sub-pixel image coordinates of the cooperation mark on the target platform, the motion state of the target platform is updated to obtain a posterior estimate of the motion state of the target platform and a posterior estimate of the motion state uncertainty of the target platform, and the position state and posture state in the posterior estimate of the motion state of the target platform are used as the position and posture of the target platform.

[0191] Specifically:

[0192] The sub-pixel image coordinates of all cooperation marks on the target platform are used to form a vector to obtain an observation vector;

[0193] According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, a first observation constraint is constructed, and a first observation constraint vector is obtained; according to the first observation constraint, a Jacobian matrix of the first observation constraint vector relative to the motion error state of the target platform is obtained;

[0194] The error state Kalman filter theory is adopted to obtain the target platform Kalman gain matrix according to the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state and the prior estimation of the target platform motion state uncertainty.

[0195] Using the error state Kalman filter theory, according to the target platform Kalman gain matrix and the observation vector, the posterior estimation of the motion error state of the target platform is obtained; according to the posterior estimation of the motion error state of the target platform, the posterior estimation of the motion state of the target platform is obtained;

[0196] The error state Kalman filter theory is used to obtain the posterior estimation of the target platform's motion state uncertainty according to the target platform's Kalman gain matrix.

[0197] The position state and attitude state in the posterior estimation of the motion state of the target platform are used as the position and posture of the target platform.

[0198] More specifically:

[0199] The sub-pixel image coordinates of all cooperation marks on the target platform are used to form a vector to obtain an observation vector;

[0200] According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, the first observation constraint is constructed, and according to the pinhole imaging model, the coordinates of the cooperation mark are converted from the target platform coordinate system to the camera system to obtain the first observation constraint vector;

[0201] According to the first observation constraint, a Jacobian matrix of the first observation constraint vector relative to the target platform motion error state is obtained; wherein the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state includes: a Jacobian matrix of the first observation constraint vector relative to the target platform motion state and a Jacobian matrix of the target platform motion state relative to the target platform motion error state;

[0202] The error state Kalman filter theory is adopted to obtain the target platform Kalman gain matrix according to the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state and the prior estimation of the target platform motion state uncertainty.

[0203] Using the error state Kalman filter theory, according to the target platform Kalman gain matrix and the observation vector, the posterior estimation of the motion error state of the target platform is obtained; according to the posterior estimation of the motion error state of the target platform, the posterior estimation of the motion state of the target platform is obtained;

[0204] The error state Kalman filter theory is used to obtain the posterior estimation of the target platform's motion state uncertainty according to the target platform's Kalman gain matrix.

[0205] The position state and attitude state in the posterior estimation of the motion state of the target platform are used as the position and posture of the target platform.

[0206] More specifically:

[0207] The sub-pixel image coordinates of all cooperative signs on the target platform form a vector to obtain the observation vector:

[0208] ;

[0209] in,

[0210] ;

[0211] In the formula, is the observation vector, For the The sub-pixel image coordinates of the cooperation logo, The number of cooperation marks;

[0212] According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, the first observation constraint is constructed:

[0213] ;

[0214] in,

[0215] ;

[0216] In the formula, For the The projection coordinates of the infrared cooperation marker lights on the image; is the observation equation, which means exist and The process of projecting onto the image, For the target platform Coordinates of infrared cooperation marker lights, To measure the prior estimate of the motion state of the platform, A priori estimate of the motion state of the target platform; express exist and The coordinate system of the target platform is transformed into the camera system during the projection onto the image; For the The three-dimensional coordinates of the infrared cooperation marker lights under the camera system; is the rotation extrinsic parameter between the IMU coordinate system and the camera system, which is a known constant calibrated in advance; The rotation matrix form of the prior estimation of the attitude state of the measurement platform; The rotation matrix form of the prior estimation of the attitude state of the target platform; A priori estimate of the position state of the target platform; A priori estimate of the position state of the measurement platform; is the translation vector from the IMU coordinate system to the camera system, which is a known constant calibrated in advance;

[0217] but, express The process of projecting onto an image, and and The following constraints exist:

[0218] ;

[0219] In the formula, The main point of the camera is The coordinates in the direction, The main point of the camera is The coordinates in the direction, for The equivalent focal length in the direction, for The equivalent focal length in the direction, ;

[0220] According to the pinhole imaging model, the coordinates of the cooperation mark are converted from the target platform coordinate system to the camera system, and all infrared cooperation mark lights are placed in , The projection coordinates under form a vector, that is, the first observation constraint vector is obtained:

[0221] ;

[0222] In the formula, is the first observation constraint vector;

[0223] According to the first observation constraint, the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state is obtained:

[0224] ;

[0225] In the formula, is the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state;

[0226] The Jacobian matrix of the first observation constraint vector relative to the target platform motion error state includes: the Jacobian matrix of the first observation constraint vector relative to the target platform motion state and the Jacobian matrix of the target platform motion state relative to the target platform motion error state;

[0227] Among them, the Jacobian matrix of the first observation constraint vector relative to the motion state of the target platform is:

[0228] ;

[0229] According to the pinhole imaging model, Matrix Block ,have:

[0230] ;

[0231] According to the pinhole imaging model and the conversion process from the target platform coordinate system to the camera system, The calculation process of the three matrix blocks in the calculation can be expressed as:

[0232] ;

[0233] In the formula, is the Jacobian matrix of the first observation constraint vector relative to the motion state of the target platform;

[0234] According to the error theory, the Jacobian matrix of the target platform motion state relative to the target platform motion error state is:

[0235] ;

[0236] in,

[0237] ;

[0238] In the formula, is the Jacobian matrix of the target platform motion state relative to the target platform motion error state; Lie Group Equivalent rotation vector Right Jacobian matrix; Prior estimation of the target platform attitude state The corresponding equivalent rotation vector;

[0239] Using the error state Kalman filter theory, the target platform Kalman gain matrix is ​​obtained according to the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state and the prior estimate of the target platform motion state uncertainty:

[0240] ;

[0241] In the formula, is the image frame number, such as Figure 2 The time correspondence diagram of the image sequence and IMU sequence shown; is the target platform Kalman gain matrix, For the The frame image corresponds to the time (with The uncertainty of the motion state of the target platform is estimated a priori (corresponding to the moment). for The transpose of for The Jacobian matrix of the first observation constraint vector relative to the target platform motion error state at time, is the observation vector noise covariance matrix, and is a known constant set;

[0242] Using the error state Kalman filter theory, according to the target platform Kalman gain matrix and observation vector, the posterior estimation of the motion error state of the target platform is obtained:

[0243] ;

[0244] In the formula, For the Posterior estimation of the motion error state of the target platform at the corresponding moment of the frame image; For the A posteriori estimation of the position error state of the target platform at the corresponding moment of the frame image; For the Posterior estimation of the velocity error state of the target platform at the time corresponding to the frame image; For the Posterior estimation of the rotation error state of the target platform at the time corresponding to the frame image; For the An observation vector composed of the image coordinates of sub-pixels of all infrared cooperation sign lights detected in the frame image; For the The frame image corresponds to the moment in motion , The observation constraint vector under ; For the The a priori estimation of the motion state of the measurement platform at the corresponding moment of the frame image is obtained by the arrangement of the inertial navigation machinery; For the The prior estimation of the motion state of the target platform at the corresponding moment of the frame image is obtained by recursion of the constant speed motion model;

[0245] According to the posterior estimation of the motion error state of the target platform, the posterior estimation of the motion state of the target platform is obtained:

[0246] ;

[0247] In the formula, For the The posterior estimation of the motion state of the target platform at the corresponding moment of the frame image, For the The position state posterior estimation in the posterior estimation of the motion state of the target platform at the corresponding moment of the frame image, For the The velocity state posterior estimation in the posterior estimation of the motion state of the target platform at the corresponding moment of the frame image, For the The a posteriori estimation of the attitude state in the a posteriori estimation of the motion state of the target platform at the corresponding moment of the frame image, For the The prior estimation of the position state of the target platform at the corresponding moment of the frame image, For the The prior estimation of the target platform velocity state at the corresponding moment of the frame image, For the A priori estimation of the target platform's attitude state at the corresponding moment of the frame image;

[0248] Using the error state Kalman filter theory and the target platform Kalman gain matrix, the posterior estimate of the target platform's motion state uncertainty is obtained:

[0249] ;

[0250] In the formula, For the The uncertainty posterior estimation of the motion state of the target platform at the corresponding moment of the frame image, for The identity matrix, For the The uncertainty prior estimation of the motion state of the target platform at the corresponding moment of the frame image is obtained by error state propagation;

[0251] The position state and attitude state in the posterior estimation of the motion state of the target platform are used as the position and posture of the target platform.

[0252] In this step, the state of the target platform is updated, and the obtained posterior estimate of the uncertainty of the motion state of the target platform is used as one of the input parameters of the next measurement.

[0253] Step 105, based on the prior estimate of the motion state of the measurement platform, the prior estimate of the uncertainty of the motion state of the measurement platform, the sub-pixel image coordinates of the cooperation mark on the target platform, and the posterior estimate of the motion state of the target platform, the motion state of the measurement platform is updated to obtain the posterior estimate of the motion state of the measurement platform, and the position state and posture state in the posterior estimate of the motion state of the measurement platform are used as the position and posture of the measurement platform.

[0254] Specifically:

[0255] The sub-pixel image coordinates of all cooperation marks on the target platform are used to form a vector to obtain an observation vector;

[0256] According to the a priori estimate of the motion state of the measurement platform and the a posteriori estimate of the motion state of the target platform, a second observation constraint is constructed, and a second observation constraint vector is obtained; according to the second observation constraint, a Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform is obtained;

[0257] Adopting the error state Kalman filter theory, the Kalman gain matrix of the measurement platform is obtained according to the Jacobian matrix of the second observation constraint vector relative to the error state of the measurement platform motion and the prior estimation of the uncertainty of the motion state of the measurement platform;

[0258] Using the error state Kalman filter theory, according to the Kalman gain matrix of the measurement platform and the observation vector, the posterior estimation of the motion error state of the measurement platform is obtained; according to the posterior estimation of the motion error state of the measurement platform, the posterior estimation of the motion state of the measurement platform is obtained;

[0259] The position state and attitude state in the posterior estimation of the motion state of the measurement platform are used as the position and posture of the measurement platform.

[0260] More specifically:

[0261] The sub-pixel image coordinates of all cooperation marks on the target platform are used to form a vector to obtain an observation vector;

[0262] According to the prior estimation of the motion state of the measuring platform and the a posteriori estimation of the motion state of the target platform, a second observation constraint is constructed, and according to the pinhole imaging model, a second observation constraint vector is obtained;

[0263] According to the second observation constraint, a Jacobi matrix of the second observation constraint vector relative to the motion error state of the measuring platform is obtained; wherein the Jacobi matrix of the second observation constraint vector relative to the motion error state of the measuring platform includes: a Jacobi matrix of the second observation constraint vector relative to the motion state of the measuring platform and a Jacobi matrix of the motion state of the measuring platform relative to the motion error state of the measuring platform;

[0264] Adopting the error state Kalman filter theory, the Kalman gain matrix of the measurement platform is obtained according to the Jacobian matrix of the second observation constraint vector relative to the error state of the measurement platform motion and the prior estimation of the uncertainty of the motion state of the measurement platform;

[0265] Using the error state Kalman filter theory, according to the Kalman gain matrix of the measurement platform and the observation vector, the posterior estimation of the motion error state of the measurement platform is obtained; according to the posterior estimation of the motion error state of the measurement platform, the posterior estimation of the motion state of the measurement platform is obtained;

[0266] The error state Kalman filter theory is used to obtain the a posteriori estimate of the uncertainty of the motion state of the measurement platform according to the Kalman gain matrix of the measurement platform.

[0267] The position state and attitude state in the posterior estimation of the motion state of the measurement platform are used as the position and posture of the measurement platform.

[0268] More specifically:

[0269] The sub-pixel image coordinates of all cooperative signs on the target platform form a vector to obtain the observation vector:

[0270] ;

[0271] in,

[0272] ;

[0273] In the formula, is the observation vector, For the The sub-pixel image coordinates of the cooperation logo, The number of cooperation marks;

[0274] According to the a priori estimate of the motion state of the measurement platform and the a posteriori estimate of the motion state of the target platform, the second observation constraint is constructed:

[0275] ;

[0276] in,

[0277] ;

[0278] In the formula, For the Infrared cooperation sign lights , The coordinates projected onto the image; is the observation equation, which means exist and The process of projecting onto the image, To measure the prior estimate of the motion state of the platform, is the posterior estimate of the motion state of the target platform, For the target platform Coordinates of infrared cooperation marker lights; for exist and The coordinate system of the target platform is transformed into the camera system during the projection onto the image; For the status , Next The coordinates of the infrared cooperation marker lights under the camera system; is the rotation extrinsic parameter between the IMU coordinate system and the camera system, which is a known constant calibrated in advance; The rotation matrix corresponding to the prior estimation of the attitude state of the measurement platform; is the rotation matrix form of the posterior estimation of the target platform attitude state; A posterior estimate of the position state of the target platform; A priori estimate of the position state of the measurement platform;

[0279] but, express The process of projecting onto the image, according to the pinhole imaging model, and The following constraints exist:

[0280] ;

[0281] In the formula, The main point of the camera is The coordinates in the direction, The main point of the camera is The coordinates in the direction, for The equivalent focal length in the direction, for The equivalent focal length in the direction, ;

[0282] According to the pinhole imaging model, all infrared cooperation marker lights are , The projection coordinates under form a vector, that is, the second observation constraint vector is obtained:

[0283] ;

[0284] In the formula, is the second observation constraint vector;

[0285] According to the second observation constraint, the Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform is obtained:

[0286] ;

[0287] In the formula, is the Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform;

[0288] The Jacobian matrix of the second observation constraint vector relative to the measurement platform motion error state includes: the Jacobian matrix of the second observation constraint vector relative to the measurement platform motion state and the Jacobian matrix of the measurement platform motion state relative to the measurement platform motion error state;

[0289] Among them, the Jacobian matrix of the second observation constraint vector relative to the motion state of the measurement platform is:

[0290] ;

[0291] According to the pinhole imaging model, Matrix Block ,have:

[0292] ;

[0293] According to the pinhole imaging model and observation constraints, the calculation process of the three matrix blocks in the above formula can be expressed as:

[0294] ;

[0295] In the formula, is the Jacobian matrix of the second observation constraint vector relative to the motion state of the measurement platform;

[0296] The Jacobian matrix of the motion state of the measurement platform relative to the motion error state of the measurement platform is:

[0297] ;

[0298] In the formula, is the Jacobian matrix of the motion state of the measurement platform relative to the motion error state of the measurement platform, Prior estimation of the attitude state of the measurement platform The corresponding equivalent rotation vector;

[0299] Using the error state Kalman filter theory, the measurement platform Kalman gain matrix is ​​obtained according to the Jacobian matrix of the second observation constraint vector relative to the measurement platform motion error state and the prior estimate of the measurement platform motion state uncertainty:

[0300] ;

[0301] In the formula, is the Kalman gain matrix of the measurement platform; For the At the corresponding moment of the frame image, the a priori estimate of the uncertainty of the motion state of the measurement platform is obtained by error state propagation; for The transpose of for The Jacobian matrix of the second observation constraint vector at time instant relative to the motion error state of the measurement platform; is the observation vector noise covariance matrix, and is a known constant set;

[0302] Using the error state Kalman filter theory, according to the measurement platform Kalman gain matrix and observation vector, the posterior estimation of the motion error state of the measurement platform is obtained:

[0303] ;

[0304] In the formula, A posteriori estimate of the motion error state of the measurement platform; For the The position error of the measurement platform is estimated a posteriori at the corresponding moment of the frame image; For the The velocity error of the measurement platform is estimated a posteriori at the corresponding moment of the frame image; For the The rotation error of the measurement platform is estimated a posteriori at the corresponding moment of the frame image; For the An observation vector composed of the image coordinates of sub-pixels of all infrared cooperation sign lights detected in the frame image; For the The frame image corresponds to the moment, and The observation constraint vector under ; For the A priori estimation of the motion state of the measurement platform at the corresponding moment of the frame image; For the Posterior estimation of the motion state of the target platform at the corresponding moment of the frame image;

[0305] According to the posterior estimation of the motion error state of the measurement platform, the posterior estimation of the motion state of the measurement platform is obtained:

[0306] ;

[0307] In the formula, To measure the a posteriori estimate of the motion state of the platform; A posterior estimation of the position state in the posterior estimation of the motion state of the measurement platform; A posteriori estimation of the velocity state in the posteriori estimation of the motion state of the measurement platform; A posteriori estimation of the attitude state in the posteriori estimation of the motion state of the measurement platform; For the The a priori estimate of the position state of the measurement platform at the corresponding moment of the frame image is obtained through the arrangement of inertial navigation machinery; For the The a priori estimate of the velocity state of the measurement platform at the corresponding moment of the frame image is obtained through the arrangement of inertial navigation machinery; For the The a priori estimate of the attitude state of the measurement platform at the corresponding moment of the frame image is obtained through the arrangement of inertial navigation machinery;

[0308] Using the error state Kalman filter theory and the Kalman gain matrix of the measurement platform, the posterior estimate of the uncertainty of the motion state of the measurement platform is obtained:

[0309] ;

[0310] In the formula, For the A posteriori estimation of uncertainty in the motion state of the measurement platform at the moment corresponding to the frame image; For the A priori estimation of the uncertainty of the motion state of the measurement platform at the corresponding moment of the frame image;

[0311] The position state and attitude state in the posterior estimation of the motion state of the measurement platform are used as the position and posture of the measurement platform.

[0312] In this step, the state of the measurement platform is updated, and the obtained a posteriori estimate of the uncertainty of the motion state of the measurement platform is used as one of the input parameters for calculating the a priori estimate of uncertainty during the next measurement.

[0313] In this embodiment, if Figure 3 and Figure 4 As shown in the figure, the measuring platform is equipped with visual-inertial devices (monocular infrared camera and micro-electromechanical inertial measurement unit) with known external parameters, and the target platform is equipped with a certain number (not less than 4) of infrared cooperation marker lights; the monocular infrared camera is used as the visual front end, and the precise image coordinates of the infrared cooperation marker lights are obtained by the NNLOG algorithm, and the dual-channel motion state estimator based on the error state Kalman filter is used as the back end; the motion priors of the measuring platform and the target platform are obtained by the inertial navigation mechanical arrangement and the constant velocity model respectively; the image coordinates of the infrared cooperation marker lights are used as the observation constraints, and the motion state of the target platform and the motion state of the observation platform are updated in sequence in an asynchronous update manner, thereby obtaining the 6-dimensional pose (3-dimensional rotation and 3-dimensional translation) of the measuring platform and the 6-dimensional pose (3-dimensional rotation and 3-dimensional translation) of the target platform.

[0314] The above-mentioned dual-motion platform posture measurement method based on visual inertia utilizes the image information of the infrared cooperation marker light on the target platform collected by the monocular infrared camera on the measuring platform and the fusion of the inertial navigation information to realize the efficient and high-precision measurement of the posture parameters (6-dimensional relative posture and 6-dimensional target platform posture) between platforms during the autonomous interaction of the dual-motion platform. The measurement process is completely autonomous and does not require data link support between platforms. The measurement results can be output at a high frequency. Compared with monocular posture measurement, the accuracy and continuity of the measurement results are greatly improved. This application designs dual-channel motion state estimation of the measuring platform and the target platform, and proposes an error state update strategy, which realizes the effective fusion of inertial measurement data and monocular vision measurement data between the dual-motion platforms, and can realize the estimation of relative posture parameters between platforms and the motion state of the dual platforms (including: posture parameter estimation of the measuring platform and posture parameter estimation of the target platform), which has broad application prospects in the fields of visual inertial navigation, monocular vision measurement, and posture estimation.

[0315] It should be understood that although Figure 1 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0316] The present application also provides a dual-motion platform posture measurement device based on visual inertia, such as Figure 5 As shown, in one embodiment, it includes: a first module 501, a second module 502, a third module 503, a fourth module 504 and a fifth module 505, wherein:

[0317] The first module 501 is used to obtain a measuring platform and a target platform to construct a dual-motion platform; wherein the measuring platform is provided with a visual inertial device, and the target platform is provided with a cooperation mark;

[0318] The second module 502 is used to use a constant velocity model and an inertial navigation mechanical arrangement to predict the motion state of the target platform and the motion state of the measurement platform respectively, and use an error state propagation method to obtain a priori estimation of the motion state of the target platform, a priori estimation of the uncertainty of the motion state of the target platform, a priori estimation of the motion state of the measurement platform, and a priori estimation of the uncertainty of the motion state of the measurement platform respectively;

[0319] The third module 503 is used to obtain the target platform image collected by the visual inertial device and process it to obtain the sub-pixel image coordinates of the cooperation mark on the target platform;

[0320] The fourth module 504 is used to update the motion state of the target platform according to the a priori estimate of the motion state of the measurement platform, the a priori estimate of the motion state of the target platform, the a priori estimate of the uncertainty of the motion state of the target platform, and the sub-pixel image coordinates of the cooperation mark on the target platform, to obtain a posterior estimate of the motion state of the target platform and a posterior estimate of the uncertainty of the motion state of the target platform, and use the position state and posture state in the posterior estimate of the motion state of the target platform as the position and posture of the target platform;

[0321] The fifth module 505 is used to update the motion state of the measurement platform according to the prior estimate of the motion state of the measurement platform, the prior estimate of the uncertainty of the motion state of the measurement platform, the sub-pixel image coordinates of the cooperation mark on the target platform, and the posterior estimate of the motion state of the target platform, to obtain the posterior estimate of the motion state of the measurement platform, and use the position state and posture state in the posterior estimate of the motion state of the measurement platform as the pose of the measurement platform.

[0322] For the specific definition of the dual-motion platform posture measurement device based on visual inertia, please refer to the definition of the dual-motion platform posture measurement method based on visual inertia above, which will not be repeated here. Each module in the above device can be implemented in whole or in part by software, hardware and a combination thereof. Each of the above modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory in the computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0323] In one embodiment, a computer device is provided. The computer device may be a terminal, and its internal structure diagram may be as follows: Figure 6As shown. The computer device includes a processor, a memory, a network interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a dual-motion platform posture measurement method based on visual inertia is implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covered on the display screen, or a button, trackball or touchpad set on the computer device shell, or an external keyboard, touchpad or mouse, etc.

[0324] Those skilled in the art will understand that Figure 6 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0325] In one embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the method in the above embodiment when executing the computer program.

[0326] In one embodiment, 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 method in the above embodiment are implemented.

[0327] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0328] The contents not described in detail in this specification belong to the prior art known to professional and technical personnel in this field.

[0329] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0330] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A dual-motion platform posture measurement method based on visual inertia, characterized in that: include: Acquire a measurement platform and a target platform to construct a dual-motion platform; wherein the measurement platform is provided with a visual inertial device, and the target platform is provided with a cooperation mark; The constant velocity model and the inertial navigation mechanical arrangement are used to predict the motion state of the target platform and the motion state of the measuring platform respectively, and the error state propagation method is used to obtain the prior estimation of the motion state of the target platform, the prior estimation of the uncertainty of the motion state of the target platform, the prior estimation of the motion state of the measuring platform and the prior estimation of the uncertainty of the motion state of the measuring platform respectively; Acquire the target platform image captured by the visual inertial device, and process it to obtain the sub-pixel image coordinates of the cooperation mark on the target platform; According to the a priori estimate of the motion state of the measurement platform, the a priori estimate of the motion state of the target platform, the a priori estimate of the uncertainty of the motion state of the target platform, and the sub-pixel image coordinates of the cooperation mark on the target platform, the motion state of the target platform is updated to obtain a posterior estimate of the motion state of the target platform and a posterior estimate of the uncertainty of the motion state of the target platform, and the position state and posture state in the posterior estimate of the motion state of the target platform are used as the position and posture of the target platform; According to the prior estimate of the motion state of the measuring platform, the prior estimate of the uncertainty of the motion state of the measuring platform, the sub-pixel image coordinates of the cooperation mark on the target platform and the posterior estimate of the motion state of the target platform, the motion state of the measuring platform is updated to obtain the posterior estimate of the motion state of the measuring platform, and the position state and posture state in the posterior estimate of the motion state of the measuring platform are used as the pose of the measuring platform.

2. The dual-motion platform posture measurement method based on visual inertia according to claim 1 is characterized in that: The constant velocity model and the inertial navigation mechanical arrangement are used to predict the motion state of the target platform and the motion state of the measurement platform respectively, and the error state propagation method is used to obtain the a priori estimate of the motion state of the target platform, the a priori estimate of the uncertainty of the motion state of the target platform, the a priori estimate of the motion state of the measurement platform and the a priori estimate of the uncertainty of the motion state of the measurement platform respectively, including: A constant velocity model is used to predict the motion state of the target platform, and an error state propagation method is used to obtain a priori estimation of the motion state of the target platform and a priori estimation of the uncertainty of the motion state of the target platform; The motion state of the measuring platform is predicted by adopting inertial navigation mechanical arrangement, and the error state propagation method is used to obtain a priori estimation of the motion state of the measuring platform and a priori estimation of the uncertainty of the motion state of the measuring platform.

3. The dual-motion platform posture measurement method based on visual inertia according to claim 2 is characterized in that: The constant velocity model is used to predict the motion state of the target platform, and the error state propagation method is used to obtain a priori estimation of the motion state of the target platform and a priori estimation of the uncertainty of the motion state of the target platform, including: The target platform is modeled by using a constant velocity model to obtain a motion state differential equation of the target platform; Integrating the motion state differential equation of the target platform and ignoring the noise term to obtain a priori estimation of the motion state of the target platform; According to the motion state differential equation of the target platform, the motion error state differential equation of the target platform is obtained; the motion error state differential equation of the target platform is integrated to obtain the error state propagation equation of the target platform; according to the error state propagation equation of the target platform, a priori estimate of the motion state uncertainty of the target platform is obtained.

4. The dual-motion platform posture measurement method based on visual inertia according to claim 3 is characterized in that: The motion state of the measurement platform is predicted by adopting inertial navigation mechanical arrangement, and the error state propagation method is used to obtain a priori estimation of the motion state of the measurement platform and a priori estimation of the uncertainty of the motion state of the measurement platform, including: According to the working principle of the inertial measurement unit in the visual inertial device, the motion state differential equation of the measurement platform is obtained; Assuming that the angular velocity of the measurement platform changes linearly between adjacent moments, the prior estimation of the attitude state of the measurement platform is obtained according to the Bortz equation; assuming that the acceleration of the measurement platform changes linearly between adjacent moments, the prior estimation of the velocity state of the measurement platform is obtained according to the inertial navigation mechanical arrangement algorithm; based on the prior estimation of the attitude state of the measurement platform and the prior estimation of the velocity state of the measurement platform, the prior estimation of the position state of the measurement platform is obtained by using the median integral method; based on the prior estimation of the attitude state of the measurement platform, the prior estimation of the velocity state of the measurement platform and the prior estimation of the position state of the measurement platform, the prior estimation of the motion state of the measurement platform is obtained; According to the motion state differential equation of the measuring platform, the motion error state differential equation of the measuring platform is obtained; the motion error state differential equation of the measuring platform is integrated to obtain the error state propagation equation of the measuring platform; according to the error state propagation equation of the measuring platform, the prior estimation of the motion state uncertainty of the measuring platform is obtained.

5. A dual-motion platform posture measurement method based on visual inertia according to any one of claims 1 to 4, characterized in that: The target platform image captured by the visual inertial device is obtained and processed to obtain the sub-pixel image coordinates of the cooperation mark on the target platform, including: Acquire a target platform image captured by a visual inertial device, and perform automatic threshold segmentation and binarization on the target platform image to obtain a plurality of partitions containing cooperation marks; A region growing algorithm is used to process multiple partitions to obtain multiple potential closed regions of the cooperation mark; the number of pixels in each closed region is counted, and all closed regions are screened according to the imaging area of ​​the cooperation mark to obtain multiple candidate regions; Using multiple regularized negative Laplacian operators of different scales, convolve all candidate regions to obtain multiple image convolution response values ​​corresponding to each pixel in all candidate regions, and take the maximum value of multiple image convolution response values ​​corresponding to a pixel as the response value of the pixel; According to all the response values, the imaging area of ​​each cooperation mark is obtained, and the sub-pixel image coordinates of the cooperation mark on the target platform are obtained.

6. The dual-motion platform posture measurement method based on visual inertia according to claim 5 is characterized in that: According to all the response values, the imaging area of ​​each cooperation mark is obtained, and the sub-pixel image coordinates of the cooperation mark on the target platform are obtained, including: The pixel position corresponding to the maximum value of all response values ​​is taken as the position of the first cooperation mark, and the first circle is drawn with the position of the first cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the first cooperation mark as the radius. The area enclosed by the first circle is the imaging area of ​​the first cooperation mark. After excluding the imaging area of ​​the first cooperation mark, the pixel position corresponding to the maximum value of all response values ​​is taken as the position of the second cooperation mark, and a second circle is drawn with the position of the second cooperation mark as the center and the convolution kernel scale of the response value corresponding to the position of the second cooperation mark as the radius. The area enclosed by the second circle is the imaging area of ​​the second cooperation mark; Traverse each cooperation mark until the imaging area of ​​each cooperation mark is obtained; Perform quadratic surface fitting on the response value of each pixel in an imaging area to obtain a quadratic surface equation; calculate the partial derivative of the quadratic surface equation and set it to zero to construct a homogeneous linear equation group; solve the homogeneous linear equation group to obtain the peak point of the quadratic surface equation, and use the coordinates of the peak point as the coordinates of the cooperation mark corresponding to the imaging area; traverse each imaging area to obtain the sub-pixel image coordinates of the cooperation mark on the target platform.

7. A dual-motion platform posture measurement method based on visual inertia according to any one of claims 1 to 4, characterized in that: According to the a priori estimate of the motion state of the measurement platform, the a priori estimate of the motion state of the target platform, the a priori estimate of the uncertainty of the motion state of the target platform, and the sub-pixel image coordinates of the cooperation mark on the target platform, the motion state of the target platform is updated to obtain a posterior estimate of the motion state of the target platform and a posterior estimate of the uncertainty of the motion state of the target platform, and the position state and posture state in the posterior estimate of the motion state of the target platform are used as the pose of the target platform, including: The sub-pixel image coordinates of all the cooperation marks on the target platform are used to form a vector to obtain an observation vector; According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, a first observation constraint is constructed, and a first observation constraint vector is obtained; according to the first observation constraint, a Jacobian matrix of the first observation constraint vector relative to the motion error state of the target platform is obtained; The error state Kalman filter theory is adopted to obtain the target platform Kalman gain matrix according to the Jacobian matrix of the first observation constraint vector relative to the target platform motion error state and the prior estimation of the target platform motion state uncertainty. Using the error state Kalman filter theory, according to the target platform Kalman gain matrix and the observation vector, the posterior estimation of the motion error state of the target platform is obtained; according to the posterior estimation of the motion error state of the target platform, the posterior estimation of the motion state of the target platform is obtained; The error state Kalman filter theory is used to obtain the posterior estimation of the target platform's motion state uncertainty according to the target platform's Kalman gain matrix. The position state and the attitude state in the posterior estimation of the motion state of the target platform are used as the position and posture of the target platform.

8. The dual-motion platform posture measurement method based on visual inertia according to claim 7 is characterized in that: According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, a first observation constraint is constructed, and a first observation constraint vector is obtained; According to the first observation constraint, the Jacobian matrix of the first observation constraint vector relative to the motion error state of the target platform is obtained, including: According to the prior estimation of the motion state of the target platform and the prior estimation of the motion state of the measurement platform, the first observation constraint is constructed, and according to the pinhole imaging model, the coordinates of the cooperation mark are converted from the target platform coordinate system to the camera system to obtain the first observation constraint vector; According to the first observation constraint, the Jacobi matrix of the first observation constraint vector relative to the target platform motion error state is obtained; wherein the Jacobi matrix of the first observation constraint vector relative to the target platform motion error state includes: the Jacobi matrix of the first observation constraint vector relative to the target platform motion state and the Jacobi matrix of the target platform motion state relative to the target platform motion error state.

9. A dual-motion platform posture measurement method based on visual inertia according to any one of claims 1 to 4, characterized in that: The method updates the motion state of the measurement platform according to a priori estimation of the motion state of the measurement platform, a priori estimation of the uncertainty of the motion state of the measurement platform, the sub-pixel image coordinates of the cooperation mark on the target platform, and a posteriori estimation of the motion state of the target platform to obtain a posteriori estimation of the motion state of the measurement platform, and uses the position state and attitude state in the posteriori estimation of the motion state of the measurement platform as the position and posture of the measurement platform, including: The sub-pixel image coordinates of all the cooperation marks on the target platform are used to form a vector to obtain an observation vector; According to the a priori estimate of the motion state of the measurement platform and the a posteriori estimate of the motion state of the target platform, a second observation constraint is constructed, and a second observation constraint vector is obtained; according to the second observation constraint, a Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform is obtained; Adopting the error state Kalman filter theory, the Kalman gain matrix of the measurement platform is obtained according to the Jacobian matrix of the second observation constraint vector relative to the error state of the measurement platform motion and the prior estimation of the uncertainty of the motion state of the measurement platform; Using the error state Kalman filter theory, according to the Kalman gain matrix of the measurement platform and the observation vector, the posterior estimation of the motion error state of the measurement platform is obtained; according to the posterior estimation of the motion error state of the measurement platform, the posterior estimation of the motion state of the measurement platform is obtained; The position state and the attitude state in the posterior estimation of the motion state of the measuring platform are used as the position and attitude of the measuring platform.

10. The dual-motion platform posture measurement method based on visual inertia according to claim 9 is characterized in that: According to the a priori estimate of the motion state of the measurement platform and the a posteriori estimate of the motion state of the target platform, a second observation constraint is constructed, and a second observation constraint vector is obtained; According to the second observation constraint, the Jacobian matrix of the second observation constraint vector relative to the motion error state of the measurement platform is obtained, including: According to the prior estimation of the motion state of the measuring platform and the a posteriori estimation of the motion state of the target platform, a second observation constraint is constructed, and according to the pinhole imaging model, a second observation constraint vector is obtained; According to the second observation constraint, the Jacobi matrix of the second observation constraint vector relative to the motion error state of the measuring platform is obtained; wherein the Jacobi matrix of the second observation constraint vector relative to the motion error state of the measuring platform includes: the Jacobi matrix of the second observation constraint vector relative to the motion state of the measuring platform and the Jacobi matrix of the motion state of the measuring platform relative to the motion error state of the measuring platform.

Images