A joint initialization method for intelligent body pose estimator

Abstract

The invention discloses a joint initialization method for an intelligent body posture estimator, the steps of which include: first, constructing an intelligent body state vector, the state vector including the current orientation, position, speed, gyroscope and accelerometer bias, GNSS receiver bias and bias rate, visual feature depth, and bias information of an odometer coordinate system and a world coordinate system of the intelligent body, and preprocessing the measurement data of an onboard sensor of the intelligent body; then, solving the intelligent body posture under pure visual conditions by using a PnP algorithm, and constructing the joint initialization of vision and IMU as a least squares optimization problem, and completing the joint initialization of vision and IMU by introducing vision and IMU prior information; finally, solving the rough position of the intelligent body in an earth-centered earth-fixed coordinate system by using an SPP algorithm, and constructing a probability factor graph model of GNSS pseudorange, Doppler frequency shift and receiver clock bias, so as to obtain all initial values ​​of the intelligent body posture estimator.

Description

Technical Field

[0001] The present invention relates to a multi-sensor fusion pose estimator technology, and in particular to a joint initialization method for an intelligent body pose estimator. Background Art

[0002] Compared with the intelligent body pose estimator based on a single sensor, the multi-sensor fusion pose estimation technology can make full use of the complementary characteristics between different types of sensors to obtain more accurate and robust intelligent body pose estimation results. Stereo cameras and inertial measurement units (IMUs) can provide intelligent bodies with centimeter-level accuracy in the robot coordinate system, but the pose in the local coordinate system will drift with the movement of the intelligent body. The Global Navigation Satellite System (GNSS) can provide drift-free position information for intelligent bodies and has been widely used in various mobile robot navigation tasks. However, the intelligent body navigation method based on GNSS is difficult to use indoors and is easily affected by noise and multipath effects, resulting in only meter-level positioning accuracy. The intelligent body pose estimation algorithm based on the tight coupling of GNSS / IMU / visual information can make full use of the respective advantages of navigation satellites, accelerometers, gyroscopes, and airborne cameras to obtain accurate, drift-free, and smooth intelligent body pose estimation.

[0003] Although the intelligent body pose estimation algorithm based on the tight coupling of GNSS / IMU / visual information has a high performance advantage, due to the nonlinear nature of the tightly coupled algorithm, the performance of the pose estimator is heavily dependent on the initial value of the system. The initial value of the system with large errors will cause the intelligent body pose estimator to diverge, or even completely wrong pose estimation results. In the navigation system based on the fusion of local pose sensors and global pose sensors, since the transformation matrix between the local coordinate system and the global coordinate system is a dynamically changing variable, the transformation matrix must be corrected in real time with the movement of the intelligent body. This leads to the fact that the intelligent body pose estimator based on the tight coupling of GNSS / IMU / visual information must be jointly initialized by online initialization. Therefore, how to quickly, accurately and robustly obtain the initial value of the intelligent body pose estimator has become a difficult problem that needs to be solved in the field of intelligent robot navigation. Summary of the invention

[0004] Purpose of the invention: The purpose of the present invention is to provide a joint initialization method for an intelligent body pose estimator to improve the accuracy of initial value estimation of the pose estimator and to be able to correct the initial value of the system in real time.

[0005] Technical solution: The present invention provides a joint initialization method for an intelligent body pose estimator, wherein the intelligent body pose estimator includes a camera, an IMU and a GNSS receiver integrated on the intelligent body, and the joint initialization method includes the following steps:

[0006] S1. Preprocessing the IMU raw measurement data: constructing an IMU measurement model in the robot coordinate system, obtaining the IMU raw measurement data, pre-integrating the IMU raw measurement data in the robot coordinate system, and correcting the IMU pre-integration item;

[0007] S2. Preprocess the visual information: extract the Shi-Tomasi corner features of the first frame, and use this as a basis to track the corner features of subsequent image frames, obtain the relative relationship between the corner features between frames, calculate the relative motion relationship between two adjacent image frames, and optimize the reprojection error of all corner features in the sliding window, so as to obtain the initial posture of the intelligent body;

[0008] S3, vision and IMU joint initialization: The initial posture of the intelligent body corresponding to each image frame obtained in step S2 is substituted into the gyroscope bias cost function to obtain the gyroscope bias, and the intelligent body state vector χ is constructed. The intelligent body observation model is constructed as a least squares problem, and the intelligent body state vector χ is substituted into the intelligent body observation model to solve the current intelligent body orientation, speed, position and gravity acceleration information of each image frame in the sliding window under the robot coordinate system, and obtain the rotation matrix between the robot coordinate system r and the odometer coordinate system o. Using the rotation matrix Project the velocity and position information in the agent state vector χ to the odometer coordinate system to obtain the IMU / vision joint trajectory;

[0009] S4. Use the pseudo-range information in the original GNSS signal to roughly solve the position of the intelligent agent: Construct a distance model between the navigation satellite and the GNSS receiver based on the pseudo-range information in the original GNSS signal, calculate the longitude and latitude of the intelligent agent in the geocentric earth-fixed coordinate system, and obtain the rotation matrix between the geocentric earth-fixed coordinate system and the world coordinate system.

[0010] S5. Align the agent's odometer coordinate system with the world coordinate system to obtain the transformation matrix between the odometer coordinate system and the world coordinate system

[0011] S6. Joint initialization of GNSS / IMU / vision information: Obtain the agent’s GNSS trajectory, align the IMU / vision joint trajectory with the agent’s GNSS trajectory in the Earth-centered Earth-fixed coordinate system, and complete the joint initialization of the agent’s pose estimator.

[0012] Further, in step S1, the IMU measurement model includes a gyroscope measurement model and an accelerometer measurement model, the IMU original measurement data includes a gyroscope bias, a gyroscope noise, the earth's gravity acceleration, an accelerometer bias, and an accelerometer noise, the gyroscope measurement model measures the gyroscope bias and the gyroscope noise, and the accelerometer measurement model measures the earth's gravity acceleration, the accelerometer bias, and the accelerometer noise;

[0013] The video output interval of the onboard camera is used as the integration time difference to pre-integrate the IMU raw measurement data in the robot coordinate system, t k Time to t k+1 The IMU pre-integration term at time is expressed as:

[0014]

[0015] In the formula is the pre-integrated term of the IMU orientation, is the pre-integral term of IMU velocity, is the pre-integration term of the IMU position; is the measurement value of the IMU orientation pre-integration term, is the measured value of the IMU velocity pre-integration term, is the measured value of the IMU position pre-integration term; △b ωtk and △b atk They represent the initial values ​​of the gyroscope bias and accelerometer bias, Represents the Jacobian matrix When the gyroscope or accelerometer bias changes, the above first-order Jacobian approximation can be used to replace the IMU pre-integration term without re-integration.

[0016] Furthermore, in step S2, the distribution of corner point features is constrained so that the pixel interval between adjacent features is greater than or equal to a preset threshold, and KLT sparse optical flow tracking is used for tracking subsequent image frames;

[0017] The threshold D of the sliding window is set, the PnP algorithm is used to calculate the relative motion relationship between two adjacent image frames, and the BA algorithm is used to optimize the reprojection error of all corner point features in the sliding window to obtain the initial pose of the intelligent body, including the initial position and initial orientation of the intelligent body.

[0018] Furthermore, when the corner point features tracked in step S2 are less than a set threshold, step S2 is re-executed to pre-process the visual information.

[0019] Furthermore, the agent state vector x in step S3 includes the agent's current orientation, speed, position, gyroscope bias and accelerometer bias, GNSS receiver bias and bias rate, visual feature depth, and Z-axis yaw angle bias between the odometer coordinate system and the world coordinate system, expressed as:

[0020]

[0021] Where n represents the size of the sliding window, δt, δt′, ψ are the agent’s orientation, velocity, position, gyroscope bias, accelerometer bias, GNSS receiver bias and bias rate, and the Z-axis yaw angle bias between the odometer coordinate system and the world coordinate system, respectively. d1, d2, ..., d m is the visual feature depth;

[0022] The current state information of each image frame in the sliding window in the robot coordinate system includes the agent's orientation, speed, position and gravitational acceleration.

[0023] Furthermore, in step S4, the GNSS original signal is first filtered to remove satellite signals with too low elevation angles, and then a distance model between the navigation satellite and the GNSS receiver is constructed based on the pseudo-range information in the GNSS original signal. The distance model expression is:

[0024]

[0025] in, and are the positions of the GNSS receiver and navigation satellite in the geocentric inertial coordinate system, is the measured value of the pseudorange information in the GNSS original signal, c is the propagation speed of light in vacuum, δt r and δt s are the clock bias of the GNSS receiver and the navigation satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by multipath effect, n pr is the noise of pseudorange information;

[0026] The distance between the GNSS receiver and the navigation satellite is solved according to the distance model, and then the longitude and latitude of the intelligent body in the Earth-centered Earth-fixed coordinate system is obtained using the single-point positioning algorithm without prior information of vision and IMU.

[0027] Furthermore, step S5 is specifically as follows:

[0028] Construct a probabilistic factor graph model of Doppler frequency shift in the original GNSS signal.

[0029]

[0030] In the above formula,

[0031]

[0032] in, is the observation equation, χ dp is the subvector related to Doppler frequency shift in the agent state vector, wl is the carrier wavelength, is the direction vector between the satellite and the receiver, and are the speeds of the receiver and satellite in the geocentric inertial coordinate system, and are the speeds of the receiver and satellite in the Earth-centered Earth-fixed coordinate system, R represents the coordinate system error caused by the rotation of the Earth, and are the clock bias rates of the receiver and satellite, is the Doppler frequency shift measurement value, ω earth is the Earth's rotation angular velocity, Represents the signal propagation time from the satellite to the receiver.

[0033] The agent speed obtained in step S3 The probabilistic factor graph model of the Doppler frequency shift in the GNSS original signal is introduced into the latitude and longitude of the intelligent agent roughly solved in step S4 to solve the Z-axis yaw angle bias ψ between the odometer coordinate system and the world coordinate system and the GNSS receiver clock bias drift rate δt ′ ; Align the odometer coordinate system o with the world coordinate system w to obtain the transformation matrix between the odometer coordinate system and the world coordinate system

[0034] Furthermore, step S6 is specifically as follows:

[0035] Construct a probabilistic factor graph model of GNSS pseudorange information and receiver clock bias,

[0036]

[0037] In the above formula:

[0038]

[0039] in, and are the positions of the receiver and the satellite in the geocentric inertial coordinate system, and are the positions of the receiver and the satellite in the Earth-centered Earth-fixed coordinate system, c is the speed of light in a vacuum, and are the clock bias of the receiver and the satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by the multipath effect, is the pseudorange information measurement value.

[0040] The agent's orientation, velocity, position and gravity acceleration information obtained in step S3 are introduced into the probability factor graph model of GNSS pseudorange information and receiver clock bias, and the precise receiver clock bias and agent position are solved to obtain the agent's GNSS trajectory;

[0041] In the Earth-centered Earth-fixed coordinate system, the IMU / vision joint trajectory obtained in step S3 is aligned with the agent GNSS trajectory to complete the initialization of the agent pose estimator.

[0042] An intelligent body pose estimator joint initialization system of the present invention comprises:

[0043] The data preprocessing module is used to pre-integrate the IMU raw measurement data in the robot coordinate system and correct the IMU pre-integration items; extract the Shi-Tomasi corner point features of the first frame of the video data collected by the camera, and track the subsequent image frames based on this, obtain the relative relationship between the corner point features between frames, calculate the relative motion relationship between two adjacent image frames, optimize the reprojection error of all corner point features in the sliding window, and obtain the initial position and posture of the camera;

[0044] The vision and IMU joint initialization module brings the initial camera pose corresponding to each image frame into the gyroscope bias cost function to obtain the gyroscope bias, constructs the agent state vector χ, constructs the agent observation model as a least squares problem, and brings the agent state vector into the least squares problem to solve the current agent state information of each image frame in the sliding window under the robot coordinate system, and obtains the rotation matrix between the robot coordinate system r and the odometer coordinate system o Using the rotation matrix Project the agent's orientation, velocity, position, and gravity acceleration information in the agent's state vector χ to the odometer coordinate system to obtain the IMU / vision joint trajectory;

[0045] The rough solution module of the intelligent agent position uses the pseudo-range information in the original GNSS signal to build a distance model between the navigation satellite and the GNSS receiver, calculates the longitude and latitude of the intelligent agent in the Earth-centered Earth-fixed coordinate system, and obtains the rotation matrix between the Earth-centered Earth-fixed coordinate system and the world coordinate system.

[0046] The coordinate system alignment module is used to align the agent's odometer coordinate system with the world coordinate system and obtain the transformation matrix between the odometer coordinate system and the world coordinate system.

[0047] The GNSS / IMU / vision information joint initialization module obtains the agent GNSS trajectory, aligns the IMU / vision joint trajectory with the agent GNSS trajectory in the Earth-centered Earth-fixed coordinate system, and completes the joint initialization of the agent pose estimator.

[0048] A storage medium of the present invention stores a computer program, which, when executed by at least one processor, implements the steps of the above-mentioned method for joint initialization of an intelligent body pose estimator.

[0049] Through the above-mentioned pose estimator initialization scheme, it can be known that the present invention discloses a GNSS / IMU / visual tightly coupled intelligent body pose estimator joint initialization method based on nonlinear optimization. First, the motion trajectory of the camera is solved by the motion recovery structure algorithm (SFM), and the IMU data is pre-integrated to initialize the IMU / visual tightly coupled intelligent body pose estimator; then, the latitude and longitude of the intelligent body in the geocentric earth-fixed coordinate system are solved by the single point positioning algorithm (SPP), and the transformation matrix between the intelligent body odometer coordinate system and the world coordinate system is solved in a nonlinear optimization manner under the condition that the IMU / visual pose estimator is used as prior information. Finally, the precise position and posture of the agent in the global coordinate system are corrected by probabilistic factor graph optimization, and finally the joint initialization of the agent's pose estimator is achieved.

[0050] Beneficial effects: Compared with the prior art, the technical effects of the present invention are as follows: the present invention integrates GNSS, IMU and visual information, has stronger robustness, and can effectively deal with the noise and multipath effects in the sensor receiving signal. In the absence of GNSS signals, the IMU and visual information can be used to complete the temporary initialization of the pose estimator, so that the intelligent body pose estimator can provide local pose information in a short time. When the receiver captures the GNSS signal, the previous initialization result is corrected to obtain accurate, smooth, and drift-free intelligent body pose information. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flow chart of the method of the present invention;

[0052] Figure 2 It is a schematic diagram of the coordinate system transformation relationship of the present invention. DETAILED DESCRIPTION

[0053] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0054] Aiming at the problem that the initial value estimation accuracy of the traditional multi-sensor fusion pose estimator initialization method is low and the initial value of the system cannot be corrected in real time, the present invention proposes a joint initialization method of GNSS / IMU / visual tightly coupled intelligent body pose estimator based on nonlinear optimization. In the system initialization stage, the motion trajectory of the camera is first solved by the motion recovery structure algorithm (SFM), and the IMU data is pre-integrated to initialize the IMU / visual tightly coupled intelligent body odometer; then the rough position of the intelligent body in the earth-centered earth-fixed coordinate system is solved by the single point positioning algorithm (SPP), and the transformation matrix between the odometer coordinate system and the world coordinate system is solved by nonlinear optimization under the condition that the IMU / visual odometer is used as prior information; finally, the precise pose of the intelligent body in the world coordinate system is corrected by the probability factor graph optimization method, and finally the joint initialization of the intelligent body pose estimator is realized.

[0055] The intelligent body pose estimator includes a camera, IMU and GNSS receiver integrated on the intelligent body. The camera is used to collect the visual information of the intelligent body; the IMU includes a gyroscope and an accelerometer. The gyroscope is used to measure the angular velocity information of the intelligent body, and the accelerometer is used to measure the acceleration of the intelligent body and the acceleration of the earth's gravity; the GNSS receiver is used to measure the pseudorange, Doppler frequency shift and clock bias information of the intelligent body.

[0056] like Figure 1 As shown, a joint initialization method of an intelligent body pose estimator of the present invention comprises the following specific steps:

[0057] S1. Preprocess the IMU raw measurement data, including:

[0058] S101, constructing an IMU measurement model in a robot coordinate system, wherein the IMU measurement model includes a gyroscope measurement model and an accelerometer measurement model, wherein the gyroscope measurement model includes a gyroscope bias and a gyroscope noise, and wherein the accelerometer measurement model includes earth gravity acceleration, accelerometer bias, and accelerometer noise;

[0059] S102, using the video output interval of the onboard camera as the integration time difference, pre-integrating the IMU raw measurement data in the robot coordinate system;

[0060] S103, solving the covariance matrix and the first-order Jacobian matrix of the IMU pre-integration term, and when the IMU bias (including the gyroscope bias and the accelerometer bias) changes, correcting the IMU pre-integration term according to the first-order Jacobian approximation of the IMU bias term.

[0061] Specific:

[0062] The IMU raw measurement data includes accelerometer measurement data and gyroscope measurement data. The measurement values ​​are affected by acceleration bias and gyroscope bias respectively. The raw measurement values ​​of gyroscope and accelerometer can be constructed by the following formula:

[0063]

[0064] in, and They represent the original measurement values ​​of the gyroscope and accelerometer at time t, respectively, with the current robot coordinate system (r) as the reference system. and represents the true value of the gyroscope and accelerometer at time t, and are the gyroscope bias and accelerometer bias, respectively. and are the gyroscope noise and accelerometer noise respectively, is the rotation matrix between the world coordinate system and the robot coordinate system, g w is the gravity acceleration. The gyroscope and accelerometer noises are Gaussian white noises, the gyroscope bias and accelerometer bias obey Brownian motion, and their derivatives obey Gaussian distribution.

[0065] Assume that the motion time of the agent in two consecutive video frames is t k and t k+1 , then in the world coordinate system (w) t k+1 The direction of the agent at each moment speed and location It can be expressed by the following formula:

[0066]

[0067] in, They are respectively t in the world coordinate system (w) k The direction, speed and position of the agent at each moment, is the change value of the agent's orientation angle, is the Earth's gravitational acceleration, is the transformation matrix between the robot coordinate system and the world coordinate system, symbol Represents quaternion multiplication.

[0068]

[0069] By converting the reference coordinate system from the world coordinate system (w) to the robot coordinate system (r), the above formula (3) can be rewritten as:

[0070]

[0071] in, Represents t k The rotation between the world coordinate system and the robot coordinate system at that moment, t k The transformation matrix between the world coordinate system and the robot coordinate system at the moment, the IMU pre-integration term is as follows:

[0072]

[0073] in, is the pre-integrated term of the IMU orientation, is the pre-integral term of IMU velocity, is the pre-integration term of the IMU position, From time t to t k The change value of the agent's orientation in the robot coordinate system at all times, From time t to t k The transformation matrix of the agent itself at the moment. Considering the discreteness of the IMU pre-integration term, is a unit quaternion, indicating no rotation, and is equal to 0. and The mean of is 0 and can be ignored, then t k to k+1 The estimated value of the IMU pre-integration term at time can be expressed as follows:

[0074]

[0075] Among them, i represents the time from time t k to k+1 Any discrete moment between, △t represents the time interval from time i to i+1, From time t k The transformation matrix of the agent between i and i, is time t k The measured value of the IMU orientation pre-integration term between i and i+1, is time t k The measured value of the IMU orientation pre-integration term between i and i, is time t k The measured value of the IMU velocity pre-integral term between i and i+1, is time t k The measured value of the IMU velocity pre-integration term between i and i, is time t k The measured value of the IMU position pre-integration term between i and i+1, is time t k The measured value of the IMU position pre-integration term between i and i, and Respectively represent the measurement values ​​of the gyroscope and accelerometer at time i, and are the gyroscope bias and accelerometer bias at time i respectively.

[0076] Then the first-order Jacobian approximation of the IMU pre-integration term can be expressed by the following formula:

[0077]

[0078] In the formula is the measurement value of the IMU orientation pre-integration term, is the measured value of the IMU velocity pre-integration term, is the measured value of the IMU position pre-integration term, and They represent the initial values ​​of the gyroscope bias and accelerometer bias, Represents the Jacobian matrix The corresponding sub-matrix. When the gyroscope or accelerometer bias changes, the above first-order Jacobian approximation can be used to replace the IMU pre-integration term without re-integration.

[0079] S2. Preprocessing of visual information, including:

[0080] S201, extracting Shi-Tomasi corner features of the first frame image, and setting the pixel interval range between adjacent corner features to 25-100 pixels;

[0081] S202, based on the Shi-Tomasi corner point features extracted from the first frame image, performing KLT sparse optical flow tracking on subsequent image frames to obtain the relative relationship between the corner point features between frames;

[0082] If the tracked corner feature is less than the preset threshold, step S201 is executed again;

[0083] S203, setting the threshold of the sliding window, and using the PnP algorithm to calculate the relative motion relationship between two adjacent image frames; then using the BA algorithm to optimize the reprojection error of all corner point features in the sliding window, so as to obtain the initial posture of the intelligent body, that is, the initial position and initial orientation of the intelligent body.

[0084] Specific:

[0085] First, the Shi-Tomasi corner features of the first frame image are extracted, and the distribution of the corner features is constrained to set the interval range between adjacent corner features to 25-100 pixels, so that the feature distribution is more uniform, which is conducive to the subsequent PnP algorithm solution. Then, based on the Shi-Tomasi corner features extracted from the first frame image, KLT sparse optical flow tracking is performed on subsequent image frames to obtain the inter-frame relative relationship of the corner features. When the tracked corner features are less than 200, the Shi-Tomasi corner features are re-extracted. Finally, the threshold of the sliding window is set to 5-15, and the PnP algorithm is used to calculate the relative motion relationship between two adjacent image frames. At the same time, the BA algorithm is used to optimize the reprojection error of all corner features in the sliding window to obtain the initial position of the intelligent body. and initial orientation This completes the visual initialization part of the intelligent body pose estimator.

[0086] S3, vision and IMU joint initialization, including:

[0087] S301, according to step S2, the initial position and initial orientation of the intelligent body corresponding to each image frame can be preliminarily obtained, and the initial position and initial orientation are brought into the gyroscope bias cost function to obtain the gyroscope bias;

[0088] S302, constructing an agent state vector x, wherein the state vector includes the agent's current orientation, speed, position, gyroscope bias and accelerometer bias, GNSS receiver bias and bias rate, visual feature depth, and Z-axis yaw angle bias between the odometer coordinate system and the world coordinate system;

[0089] S303, constructing the joint initialization process of vision and IMU as a least squares problem, and bringing in the state vector described in step S302, solving the current orientation, speed, position and gravitational acceleration of each image frame in the sliding window in the robot coordinate system;

[0090] S304: After step S303 is completed, the rotation matrix between the robot coordinate system (r) and the odometer coordinate system (o) can be obtained. This rotation matrix can be used to project other variables in the agent state vector to the odometer coordinate system, thus completing the joint initialization of vision and IMU.

[0091] Specific:

[0092] First, the agent state vector χ is constructed as follows:

[0093]

[0094] Where n represents the size of the sliding window, and the agent state vector contains the orientation of the agent in the world coordinate system (w) speed Location Gyroscope bias and accelerometer bias GNSS receiver bias δt and bias rate δt′, visual feature depth d1,d2,...,d m And the Z-axis yaw angle offset ψ between the odometer coordinate system and the world coordinate system.

[0095] The initial position of the agent corresponding to any two consecutive frames in the sliding window can be obtained from step S2. and initial orientation Using this initial posture information as a priori to estimate the initial value of the gyroscope bias, the initial value of the gyroscope bias △b can be ω The optimization problem is constructed as follows:

[0096]

[0097] In the formula, L is all video frames in the sliding window, From t0 to t k The change in the agent's orientation obtained from visual information during the time period, From t0 to t k+1 The change in the direction of the agent obtained from the visual information during the time period can be used to estimate the initial value of the gyroscope bias by solving the above formula.

[0098] After obtaining the initial value of the gyroscope bias, construct the subvector χ of the agent state vector χ VI In the following form:

[0099]

[0100] Subvector χ VI It includes the agent's orientation, speed, position, gyroscope and accelerometer bias, and visual feature depth. Then, the agent coordinate system is used as the reference system, and the joint initialization process of vision and IMU is constructed as the following least squares problem:

[0101]

[0102] in, is the linear observation of the agent, The matrix is ​​the prior pose information obtained by the camera.

[0103] Substituting the initial position and initial orientation of the agent obtained by the onboard camera in step S2 into the above formula, the agent state subvector χ can be obtained. VI .

[0104] Finally, the reference system is converted back to the world coordinate system to obtain the vision / IMU joint position and orientation, thereby completing the joint initialization of vision and IMU.

[0105] S4: Use the SPP algorithm to roughly solve the position of the agent, including:

[0106] S401, filtering the original GNSS signal to remove satellite signals with too low elevation angles and reduce the impact of multipath effects;

[0107] S402, constructing a distance model between the navigation satellite and the GNSS receiver according to the pseudo-range information in the GNSS original signal;

[0108] S403: In the absence of vision and IMU prior information, the single point positioning (SPP) algorithm is performed on the pseudo-range information in the GNSS original signal to calculate the longitude and latitude of the intelligent agent in the Earth-centered Earth-fixed (ECEF) coordinate system (i.e., the rough position of the intelligent agent), and at the same time obtain the rotation matrix between the Earth-centered Earth-fixed coordinate system and the world coordinate system.

[0109] Specific:

[0110] Firstly, the original GNSS signal is filtered to remove satellites with elevation angles lower than 30 degrees to reduce the impact of multipath effect during signal propagation. The ionospheric delay and tropospheric delay are constructed as Krobucher model and Sastamonen model, respectively.

[0111] Then, the distance model between the navigation satellite and the GNSS receiver is constructed in the following form:

[0112]

[0113] in, and are the positions of the GNSS receiver and navigation satellite in the Earth-centered Inertial (ECI) coordinate system, is the measured value of GNSS pseudorange information, c is the propagation speed of light in vacuum, δt r and δt s are the clock bias of the receiver and the satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by multipath effect, n pr is the noise of pseudorange information.

[0114] Finally, the distance between the GNSS receiver and the navigation satellite is solved according to the above formula, and then the longitude and latitude of the intelligent agent in the Earth-centered Earth-fixed (ECEF) coordinate system (i.e., the rough position of the intelligent agent) is obtained by the single point positioning (SPP) algorithm without prior information of vision and IMU, and the position of the intelligent agent in the Earth-centered Earth-fixed coordinate system (e) is roughly solved.

[0115] Once the latitude and longitude of the agent are obtained, the rotation matrix between the Earth-centered Earth-fixed coordinate system and the world coordinate system can be obtained:

[0116]

[0117] Among them, λ represents longitude, Represents dimension.

[0118] S5: Solve the transformation matrix between the world coordinate system (w) and the agent odometry coordinate system (o), including:

[0119] S501, constructing a probability factor graph model of GNSS Doppler frequency shift (as shown in formula (15)), the factors in the factor graph include the Z-axis angle bias ψ and the clock bias drift rate δt′ between the intelligent agent odometer coordinate system (o) and the world coordinate system (w);

[0120] S502, bringing the agent speed obtained in step S3 and the agent longitude and latitude obtained in step S4 into the factor graph model in step S501, solving the Z-axis yaw angle bias ψ and the receiver clock bias drift rate δt′ between the odometer coordinate system and the world coordinate system;

[0121] S503: Align the odometer coordinate system (o) with the world coordinate system (w) to obtain a transformation matrix between the odometer coordinate system and the world coordinate system.

[0122] Specific:

[0123] The present invention aligns the origin and Z-axis of the world coordinate system with the intelligent body odometer coordinate system, that is, the origins and Z-axes of the two coordinate systems completely coincide, and the two coordinate systems differ only by a yaw angle offset ψ around the Z-axis.

[0124] First, the subvector related to the Doppler frequency shift in the agent state vector χ is constructed as:

[0125]

[0126] Then the Doppler frequency shift error E in the original GNSS signal is dp It can be constructed as follows:

[0127]

[0128] In the above formula,

[0129]

[0130] in, is the observation equation, χ dp is the subvector related to Doppler frequency shift in the agent state vector, wl is the carrier wavelength, is the direction vector between the satellite and the receiver, and are the speeds of the receiver and satellite in the geocentric inertial coordinate system, and are the speeds of the receiver and satellite in the Earth-centered Earth-fixed coordinate system, R represents the coordinate system error caused by the rotation of the Earth, c is the propagation speed of light in a vacuum, and are the clock bias rates of the receiver and satellite, is the Doppler frequency shift measurement value, ω earth is the Earth's rotation angular velocity, Represents the signal propagation time from the satellite to the receiver.

[0131] The Z-axis initial angle deviation ψ between the odometer coordinate system and the world coordinate system and the receiver clock bias drift rate can be formulated as the following optimization problem:

[0132]

[0133] Where n is the number of agent state vectors in the sliding window, and m is the number of captured satellites. Substituting the agent state information obtained in step S3 and step S4 into formula (17) as a priori, ψ and δt′ can be solved.

[0134] S6: Joint initialization of GNSS / IMU / visual information, including:

[0135] S601, constructing a probability factor graph model of GNSS pseudorange information and receiver clock bias (as shown in formula (19)), wherein the factors in the factor graph include the receiver clock bias and the longitude and latitude of the intelligent agent obtained in step S4;

[0136] S602, bringing the vision / IMU joint intelligent body orientation and position obtained in step S3 into the factor graph model in step S601 to solve the receiver clock bias and the precise position of the intelligent body;

[0137] S603. In the Earth-centered Earth-fixed coordinate system, align the IMU / vision combined intelligent body position completed in step S3 with the precise position of the intelligent body obtained in step S602 to complete the initialization of the intelligent body pose estimator.

[0138] Specific:

[0139] First, the subvector related to the pseudorange information in the agent state vector χ is constructed as:

[0140]

[0141] Then the pseudorange error in the GNSS original signal can be constructed as follows:

[0142]

[0143] In the above formula:

[0144]

[0145] in, and are the positions of the receiver and the satellite in the geocentric inertial coordinate system, and are the positions in the Earth-centered Earth-fixed coordinate system, c is the speed of light in a vacuum, and are the clock bias of the receiver and the satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by the multipath effect, is the pseudorange information measurement value.

[0146] Then, t k to k+1 The internal receiver clock offset error is constructed as follows:

[0147]

[0148] in t k-1 The receiver clock bias rate at time . Then the precise position of the agent can be formulated as the following least squares optimization problem:

[0149]

[0150] Substituting the prior information obtained in step S3 and step S5 into formula (22) can obtain the precise position and posture of the intelligent agent.

[0151] After step S6 is completed, all initial values ​​of the intelligent body pose estimator can be obtained.

[0152] An intelligent body pose estimator joint initialization system of the present invention comprises:

[0153] The data preprocessing module is used to pre-integrate the IMU raw measurement data in the robot coordinate system and correct the IMU pre-integration items; extract the Shi-Tomasi corner point features of the first frame of the video data collected by the camera, and track the subsequent image frames based on this, obtain the relative relationship between the corner point features between frames, calculate the relative motion relationship between two adjacent image frames, optimize the reprojection error of all corner point features in the sliding window, and obtain the initial position and posture of the camera;

[0154] The vision and IMU joint initialization module brings the initial camera pose corresponding to each image frame into the gyroscope bias cost function to obtain the gyroscope bias, constructs the agent state vector χ, constructs the agent observation model as a least squares problem, and brings the agent state vector into the least squares problem to solve the current agent state information of each image frame in the sliding window under the robot coordinate system, and obtains the rotation matrix between the robot coordinate system r and the odometer coordinate system o Using the rotation matrix Project the agent's orientation, velocity, position, and gravity acceleration information in the agent's state vector χ to the odometer coordinate system to obtain the IMU / vision joint trajectory;

[0155] The rough solution module of the intelligent agent position uses the pseudo-range information in the original GNSS signal to build a distance model between the navigation satellite and the GNSS receiver, calculates the longitude and latitude of the intelligent agent in the Earth-centered Earth-fixed coordinate system, and obtains the rotation matrix between the Earth-centered Earth-fixed coordinate system and the world coordinate system.

[0156] The coordinate system alignment module is used to align the agent's odometer coordinate system with the world coordinate system and obtain the transformation matrix between the odometer coordinate system and the world coordinate system.

[0157] The GNSS / IMU / vision information joint initialization module obtains the agent GNSS trajectory, aligns the IMU / vision joint trajectory with the agent GNSS trajectory in the Earth-centered Earth-fixed coordinate system, and completes the joint initialization of the agent pose estimator.

[0158] A storage medium of the present invention stores a computer program, which, when executed by at least one processor, implements the steps of a joint initialization method for an intelligent body pose estimator and can achieve the technical effect of the above method.

[0159] The above are only preferred implementations of the present invention. The protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention.

Claims

1. A joint initialization method for an intelligent body pose estimator, characterized in that: The intelligent body pose estimator includes a camera, an IMU and a GNSS receiver integrated on the intelligent body, and its joint initialization method includes the following steps: S1. Preprocessing the IMU raw measurement data: constructing an IMU measurement model in the robot coordinate system, obtaining the IMU raw measurement data, pre-integrating the IMU raw measurement data in the robot coordinate system, and correcting the IMU pre-integration item; S2. Preprocess the visual information: extract the Shi-Tomasi corner features of the first frame, and use this as a basis to track the corner features of subsequent image frames, obtain the relative relationship between the corner features between frames, calculate the relative motion relationship between two adjacent image frames, and optimize the reprojection error of all corner features in the sliding window, so as to obtain the initial posture of the intelligent body; S3, vision and IMU joint initialization: The initial posture of the intelligent body corresponding to each image frame obtained in step S2 is substituted into the gyroscope bias cost function to obtain the gyroscope bias, and the intelligent body state vector χ is constructed. The intelligent body observation model is constructed as a least squares problem, and the intelligent body state vector χ is substituted into the intelligent body observation model to solve the current intelligent body orientation, speed, position and gravity acceleration information of each image frame in the sliding window under the robot coordinate system, and obtain the rotation matrix between the robot coordinate system r and the odometer coordinate system o. Using the rotation matrix Project the velocity and position information in the agent state vector χ to the odometer coordinate system to obtain the IMU / vision joint trajectory; S4. Use the pseudo-range information in the original GNSS signal to roughly solve the position of the intelligent agent: Construct a distance model between the navigation satellite and the GNSS receiver based on the pseudo-range information in the original GNSS signal, calculate the longitude and latitude of the intelligent agent in the geocentric earth-fixed coordinate system, and obtain the rotation matrix between the geocentric earth-fixed coordinate system and the world coordinate system. S5. Align the agent's odometer coordinate system with the world coordinate system to obtain the transformation matrix between the odometer coordinate system and the world coordinate system S6, GNSS / IMU / visual information joint initialization: Get the agent GNSS trajectory, align the IMU / visual joint trajectory with the agent GNSS trajectory in the Earth-centered Earth-fixed coordinate system, and complete the joint initialization of the agent pose estimator; specifically: Construct a probabilistic factor graph model of GNSS pseudorange information and receiver clock bias, in, is the observation equation, χ pr is the subvector related to pseudorange information in the agent state vector χ; In the above formula: in, and are the positions of the receiver and the satellite in the geocentric inertial coordinate system, and are the positions of the receiver and the satellite in the Earth-centered Earth-fixed coordinate system, c is the speed of light in a vacuum, and are the clock bias of the receiver and the satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by the multipath effect, is the pseudorange information measurement value, ω earth is the angular velocity of the Earth's rotation, Represents the signal propagation time from the satellite to the receiver; The agent's orientation, velocity, position and gravity acceleration information obtained in step S3 are introduced into the probability factor graph model of GNSS pseudorange information and receiver clock bias, and the precise receiver clock bias and agent position are solved to obtain the agent's GNSS trajectory; In the Earth-centered Earth-fixed coordinate system, the IMU / vision joint trajectory obtained in step S3 is aligned with the agent GNSS trajectory to complete the initialization of the agent pose estimator.

2. The method for joint initialization of an intelligent body pose estimator according to claim 1, characterized in that: In step S1, the IMU measurement model includes a gyroscope measurement model and an accelerometer measurement model, the IMU original measurement data includes a gyroscope bias, a gyroscope noise, the earth's gravity acceleration, an accelerometer bias, and an accelerometer noise, the gyroscope measurement model measures the gyroscope bias and the gyroscope noise, and the accelerometer measurement model measures the earth's gravity acceleration, the accelerometer bias, and the accelerometer noise; The video output interval of the onboard camera is used as the integration time difference to pre-integrate the IMU raw measurement data in the robot coordinate system, t k Time to t k+1 The IMU pre-integration term at time is expressed as: In the formula is the pre-integrated term of the IMU orientation, is the pre-integral term of IMU velocity, is the pre-integration term of the IMU position; is the measurement value of the IMU orientation pre-integration term, is the measured value of the IMU velocity pre-integration term, is the measured value of the IMU position pre-integration item; and They represent the initial values ​​of the gyroscope bias and accelerometer bias, Represents the Jacobian matrix The corresponding sub-matrix; when the gyroscope or accelerometer bias changes, the above first-order Jacobian approximation can be used to replace the IMU pre-integration term without re-integration.

3. The method for joint initialization of an intelligent body pose estimator according to claim 1, characterized in that: In step S2, the distribution of corner point features is constrained so that the pixel interval between adjacent features is greater than or equal to a preset threshold, and KLT sparse optical flow tracking is used for tracking subsequent image frames; The threshold D of the sliding window is set, the PnP algorithm is used to calculate the relative motion relationship between two adjacent image frames, and the BA algorithm is used to optimize the reprojection error of all corner point features in the sliding window to obtain the initial pose of the intelligent body, including the initial position and initial orientation of the intelligent body.

4. The method for joint initialization of an intelligent body pose estimator according to claim 1, characterized in that: When the corner point features tracked in step S2 are less than the set threshold, step S2 is re-executed to pre-process the visual information.

5. The method for joint initialization of an intelligent body pose estimator according to claim 1, characterized in that: The agent state vector x in step S3 includes the agent's current orientation, speed, position, gyroscope bias and accelerometer bias, GNSS receiver bias and bias rate, visual feature depth, and Z-axis yaw angle bias between the odometer coordinate system and the world coordinate system, expressed as: Where n represents the size of the sliding window, δt, δt′, ψ are the agent’s orientation, velocity, position, gyroscope bias, accelerometer bias, GNSS receiver bias and bias rate, and the Z-axis yaw angle bias between the odometer coordinate system and the world coordinate system, respectively. d1, d2, ..., d m is the visual feature depth; The current state information of each image frame in the sliding window in the robot coordinate system includes the agent's orientation, speed, position and gravitational acceleration.

6. The method for joint initialization of an intelligent body pose estimator according to claim 1, characterized in that: In step S4, the GNSS original signal is first filtered to remove satellite signals with too low elevation angles, and then a distance model between the navigation satellite and the GNSS receiver is constructed based on the pseudo-range information in the GNSS original signal. The distance model expression is: in, and are the positions of the GNSS receiver and navigation satellite in the geocentric inertial coordinate system, is the measured value of the pseudorange information in the GNSS original signal, c is the propagation speed of light in vacuum, δt r and δt s are the clock bias of the GNSS receiver and the navigation satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by multipath effect, n pr is the noise of pseudorange information; The distance between the GNSS receiver and the navigation satellite is solved according to the distance model, and then the longitude and latitude of the intelligent body in the Earth-centered Earth-fixed coordinate system is obtained using the single-point positioning algorithm without prior information of vision and IMU.

7. The method for joint initialization of an intelligent body pose estimator according to claim 1, characterized in that: Step S5 is specifically as follows: Construct a probabilistic factor graph model of Doppler frequency shift in the original GNSS signal. In the above formula, in, is the observation equation, χ dp is the subvector related to Doppler frequency shift in the agent state vector, wl is the carrier wavelength, is the direction vector between the satellite and the receiver, and are the speeds of the receiver and satellite in the geocentric inertial coordinate system, and are the speeds of the receiver and satellite in the Earth-centered Earth-fixed coordinate system, R represents the coordinate system error caused by the rotation of the Earth, and are the clock bias rates of the receiver and satellite, is the Doppler frequency shift measurement value, ω earth is the angular velocity of the Earth's rotation, Represents the signal propagation time from the satellite to the receiver; The agent speed obtained in step S3 The probabilistic factor graph model of the Doppler frequency shift in the GNSS original signal is introduced into the latitude and longitude of the intelligent agent roughly solved in step S4 to solve the Z-axis yaw angle bias ψ between the odometer coordinate system and the world coordinate system and the GNSS receiver clock bias drift rate δt ′ ; Align the odometer coordinate system o with the world coordinate system w to obtain the transformation matrix between the odometer coordinate system and the world coordinate system 8. A joint initialization system for an intelligent body pose estimator, characterized in that: include: The data preprocessing module is used to pre-integrate the IMU raw measurement data in the robot coordinate system and correct the IMU pre-integration items; The Shi-Tomasi corner features of the first frame of the video data collected by the camera are extracted, and the subsequent image frames are tracked based on this, the relative relationship between the corner features between frames is obtained, the relative motion relationship between two adjacent image frames is calculated, the reprojection error of all corner features in the sliding window is optimized, and the initial position and posture of the camera are obtained; The vision and IMU joint initialization module brings the initial camera pose corresponding to each image frame into the gyroscope bias cost function to obtain the gyroscope bias, constructs the agent state vector χ, constructs the agent observation model as a least squares problem, and brings the agent state vector into the least squares problem to solve the current agent state information of each image frame in the sliding window under the robot coordinate system, and obtains the rotation matrix between the robot coordinate system r and the odometer coordinate system o Using the rotation matrix Project the agent's orientation, velocity, position, and gravity acceleration information in the agent's state vector χ to the odometer coordinate system to obtain the IMU / vision joint trajectory; The rough solution module of the intelligent agent position uses the pseudo-range information in the original GNSS signal to build a distance model between the navigation satellite and the GNSS receiver, calculates the longitude and latitude of the intelligent agent in the Earth-centered Earth-fixed coordinate system, and obtains the rotation matrix between the Earth-centered Earth-fixed coordinate system and the world coordinate system. The coordinate system alignment module is used to align the agent's odometer coordinate system with the world coordinate system and obtain the transformation matrix between the odometer coordinate system and the world coordinate system. The GNSS / IMU / visual information joint initialization module obtains the agent GNSS trajectory, aligns the IMU / visual joint trajectory with the agent GNSS trajectory in the Earth-centered Earth-fixed coordinate system, and completes the joint initialization of the agent pose estimator; specifically: Construct a probabilistic factor graph model of GNSS pseudorange information and receiver clock bias, in, is the observation equation, χ pr is the subvector related to pseudorange information in the agent state vector χ; In the above formula: in, and are the positions of the receiver and the satellite in the geocentric inertial coordinate system, and are the positions of the receiver and the satellite in the Earth-centered Earth-fixed coordinate system, c is the speed of light in a vacuum, and are the clock bias of the receiver and the satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by the multipath effect, is the pseudorange information measurement value, ω earth is the Earth's rotation angular velocity, Represents the signal propagation time from the satellite to the receiver; The obtained agent orientation, velocity, position and gravity acceleration information are introduced into the probability factor graph model of GNSS pseudorange information and receiver clock bias to solve the precise receiver clock bias and agent position, and obtain the agent GNSS trajectory; In the Earth-centered Earth-fixed coordinate system, the obtained IMU / vision joint trajectory is aligned with the agent’s GNSS trajectory to complete the initialization of the agent’s pose estimator.

9. A storage medium, characterized in that: The storage medium stores a computer program, which, when executed by at least one processor, implements the steps of a joint initialization method for an intelligent body pose estimator as described in any one of claims 1 to 7.

Images