A method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion

Abstract

The present invention discloses a method for estimating the pose of an intelligent body based on GNSS / IMU / optical flow information fusion, and the steps are as follows: first, preprocessing the raw data of the intelligent body's onboard sensors, including IMU pre-integration, visual feature extraction and tracking, and GNSS signal filtering; then, constructing the visual, IMU, GNSS cost functions and the intelligent body state vector respectively, and jointly initializing the GNSS / IMU / optical flow information to obtain all the initial values ​​of the intelligent body pose estimator; finally, constructing the intelligent body pose solution process as a state estimation problem, and designing the corresponding probability factor graph model and marginalization strategy to solve the intelligent body pose in a graph optimization manner. Compared with the existing robot pose estimation method, the present invention has higher computational efficiency and adaptability, and can accurately, robustly, and drift-freely obtain the intelligent body pose information.

Description

Technical Field

[0001] The present invention relates to intelligent body posture estimation technology, and in particular to an intelligent body posture estimation method based on GNSS / IMU / optical flow information fusion. Background Art

[0002] An intelligent agent is an intelligent robot that can sense its surrounding environment with the help of sensors and act on the environment with the help of actuators. At present, the autonomous navigation ability of an intelligent agent has become an important indicator to measure its intelligence level. The difficulty in solving the problem of autonomous navigation of intelligent agents is how to obtain the current position and posture of the intelligent agent in real time and accurately. Compared with the intelligent agent 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 agent pose estimation results. Stereo cameras and inertial measurement units (IMUs) can provide intelligent agents with centimeter-level accuracy in the body coordinate system, but the pose in the local coordinate system will drift with the movement of the intelligent agent. The global navigation satellite system (GNSS) can provide drift-free position information for intelligent agents and has been widely used in various mobile robot navigation tasks. However, the intelligent agent navigation method based on GNSS is difficult to use indoors and is easily affected by noise and multipath effects, resulting in its only meter-level positioning accuracy. In order to make full use of the complementary characteristics of different sensors, the intelligent body pose estimation algorithm based on GNSS / IMU / optical flow information fusion can utilize the respective advantages of navigation satellites, accelerometers, gyroscopes, and stereo cameras to obtain accurate and drift-free intelligent body pose estimation.

[0003] However, the pose estimation method based on GNSS / IMU / optical flow information fusion will face the following problems: First, the output frequencies of different sensors are not the same (GNSS receiver is about 10HZ; camera is about 30HZ; IMU is about 250HZ), how to synchronize and align the raw measurement data from different sensors will be a difficult problem; second, visual / inertial odometers can usually achieve centimeter-level positioning accuracy, while GNSS positioning accuracy is two orders of magnitude lower than visual / inertial odometers; finally, when a sensor in the pose estimation system suddenly fails and then works normally again, how can the pose estimator quickly recover to its previous state. Therefore, how to use GNSS / IMU / optical flow information to quickly, accurately, and drift-freely obtain the pose of the intelligent body 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 an intelligent body pose estimation method based on GNSS / IMU / optical flow information fusion.

[0005] Technical solution: The present invention provides an intelligent body posture estimation method based on GNSS / IMU / optical flow information fusion, comprising the following steps:

[0006] S1. Preprocess the raw measurement data of the intelligent agent, including Shi-Tomasi corner feature extraction on the camera's RGB output and KLT optical flow tracking on subsequent RGB video frames; pre-integrate the raw measurement data of the inertial measurement unit (IMU) in the robot coordinate system; filter the captured GNSS raw signal;

[0007] S2, joint initialization of GNSS / IMU / optical flow information for pose estimator: Construct the state vector χ of the intelligent body, use the PnP algorithm and BA algorithm to solve the initial position and initial orientation of the intelligent body, and use the SPP algorithm to roughly solve the longitude and latitude of the intelligent body; align the initial position and longitude and latitude of the intelligent body, obtain the transformation matrix between the odometer coordinate system and the world coordinate system, and complete the joint initialization of GNSS / IMU / optical flow information;

[0008] S3, construct visual constraint factor: construct the camera projection model as a pinhole camera model, establish the reprojection function of the spatial feature point l from video frame i to video frame j, and construct the visual error factor term as the feature point reprojection residual;

[0009] S4. Constructing IMU constraint factors: The IMU constraint factors include gyroscope error factor constraint items and accelerometer error factor constraint items, wherein the gyroscope error factor constraint items are constructed as rotation residuals based on quaternion outer products, and the accelerometer error factor constraint items are constructed as velocity bias residuals and displacement bias residuals, respectively;

[0010] S5. Construct GNSS constraint factors: GNSS constraint factors include GNSS pseudorange constraint factors, GNSS Doppler frequency shift constraint factors and receiver clock bias constraint factors; wherein, the GNSS pseudorange constraint factor is constructed as the residual between the real pseudorange and the pseudorange measured by the receiver, the GNSS Doppler frequency shift constraint factor is constructed as the residual between the real carrier Doppler frequency shift and the Doppler frequency shift measurement value, and the receiver clock bias constraint factor is constructed as the residual between the real receiver clock bias at a certain moment and the clock bias measurement value at that moment;

[0011] S6. Solve the intelligent body posture information by probabilistic factor graph optimization.

[0012] Furthermore, step S2 is specifically as follows:

[0013] S201, constructing an agent state vector x, wherein the state vector includes the current orientation of the agent speed Location Gyroscope bias and accelerometer bias GNSS receiver clock bias δt and bias rate δt′, visual feature point depth d1, d2, ..., d m and the Z-axis offset ψ between the odometer coordinate system and the world coordinate system, the agent state vector χ is expressed as:

[0014]

[0015] S202: Calculate the relative motion relationship between two adjacent image frames using the PnP algorithm, and optimize the reprojection error of all corner point features in the sliding window using the BA algorithm to obtain the initial position of the intelligent body. and initial orientation At the same time, the SPP algorithm is used to roughly solve the longitude and latitude of the intelligent agent;

[0016] S203: Align the initial position of the agent with the longitude and latitude to obtain the transformation matrix between the odometer coordinate system and the world coordinate system Complete the joint initialization of GNSS / IMU / optical flow information.

[0017] Furthermore, in step S3, the specific method of constructing the visual error factor term as the feature point reprojection residual is: project the spatial feature point l from the pixel coordinate system of video frame i to the world coordinate system, and then project it from the world coordinate system to the pixel coordinate system of video frame j, and then solve the residual between the measured position and the actual position of the feature point l in video frame j.

[0018] Furthermore, step S4 is specifically as follows:

[0019] S401, constructing the gyroscope error factor constraint term as a rotation residual based on the quaternion outer product, and performing quaternion multiplication with the real rotation angle of the agent in two consecutive video frames and the inverse of the gyroscope pre-integration term, that is, obtaining the gyroscope measurement residual;

[0020] S402, constructing the accelerometer error factor constraint items into velocity bias residual and displacement bias residual respectively; the velocity bias residual is obtained by subtracting the real velocity bias of the agent in two consecutive video frames from the velocity pre-integration item of the accelerometer; the displacement bias residual is obtained by subtracting the real displacement bias of the agent in two consecutive video frames from the displacement pre-integration item of the accelerometer;

[0021] S403, the gyroscope bias and accelerometer bias error factor terms are obtained by subtracting the bias between two consecutive video frames;

[0022] S404, combining step S401, step S402 and step S403 to complete the construction of the IMU constraint factor;

[0023] The constructed IMU constraint factors are:

[0024]

[0025] Among them, χ I represents the sub-vector related to IMU in the agent state vector χ, is the pre-integrated term of the IMU orientation, is the pre-integrated term of the 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 ω and δb a are the residuals of the gyroscope bias and accelerometer bias, respectively, Represents t k The rotation between the world coordinate system and the robot coordinate system at that moment, They are respectively t in the world coordinate system k+1 The direction, speed and position of the agent at each moment, t k The transformation matrix between the world coordinate system and the robot coordinate system at this moment, symbol represents quaternion multiplication, and t k+1 The gyroscope bias and accelerometer bias at the moment, and t k The gyroscope bias and accelerometer bias at the moment.

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

[0027] S501, establish a GNSS pseudorange measurement model, and use the pseudorange measurement model to solve the pseudorange information in the real GNSS original signal. The GNSS pseudorange constraint factor is constructed as the residual between the real pseudorange and the pseudorange measured by the receiver, expressed as:

[0028]

[0029] in, represents the observation equation between the satellite and the receiver, χ pr represents the subvector related to the GNSS pseudorange in the agent state vector χ, and are the positions of the receiver and the satellite in the geocentric inertial coordinate system, c is the speed of light in a vacuum, and are the clock biases of the receiver and satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by the multipath effect, represents the measured value of pseudorange;

[0030] S502: Establish a GNSS Doppler frequency shift measurement model, and use the Doppler frequency shift measurement model to solve the real GNSS Doppler frequency shift information. Then, the GNSS Doppler frequency shift constraint factor is constructed as the residual between the real carrier Doppler frequency shift and the Doppler frequency shift measurement value, which is expressed as:

[0031]

[0032] in, represents the observation equation between the satellite and the receiver, χ dp represents the subvector related to the GNSS 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, c is the speed of light in a vacuum, and are the clock bias rates of the receiver and satellite, is the Doppler shift measurement value;

[0033] S503: Establish a GNSS receiver clock bias measurement model. The receiver clock bias constraint factor is constructed as the residual between the actual receiver clock bias at a certain moment and the clock bias measurement value at that moment, expressed as:

[0034]

[0035] in, is the receiver clock bias observation model, χ τ represents the subvector in the agent state vector χ related to the receiver clock bias, and t k and t k-1 The receiver clock bias at time , t k-1 The receiver clock bias rate at time ;

[0036] S504, combining step S501, step S502 and step S503 to complete the construction of the GNSS constraint factor.

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

[0038] S601, the intelligent body posture solution process is regarded as a state estimation problem, and the intelligent body posture solution process becomes the maximum a posteriori estimation of the state vector;

[0039] S602, construct a pose estimator factor graph model with tight coupling of GNSS / IMU / optical flow information, expressed as:

[0040]

[0041] Where χ is the agent state vector, P(χ|z) is the maximum posterior probability of the agent state vector, z is the overall agent state measurement model, and e p is the system prior information, H p represents the agent's prior pose matrix, n is the number of agent state vectors in the sliding window, and the symbol E represents the visual constraint factor E V , IMU constraint factor E I , GNSS constraint factor E pr 、E dp 、E τ The sum of k is the k-th observation information of the agent;

[0042] S603, designing an observation data marginalization strategy to remove redundant optical flow, IMU, and GNSS information in the sliding window;

[0043] S604, solving the pose estimator factor graph model of the tightly coupled GNSS / IMU / optical flow information to obtain the agent state vector.

[0044] Furthermore, the construction process of the hardware equipment used in this method is:

[0045] (1) NVIDIA Jetson Xavier NX is used as the onboard embedded computing platform of the intelligent agent, and the ROSMelodic robot operating system and pose estimator related operating environment are installed;

[0046] (2) Using Intel RealSense D435i as an airborne stereo camera, calibrate the three lenses of the camera and the built-in BMI055 inertial measurement unit, and obtain the camera's intrinsic parameter matrix K and the transformation matrix T between the camera and the built-in BMI055. c r ;

[0047] (3) The Ublox NEO-M8T module is used as the GNSS signal receiver and connected to the UART port of the NVIDIA Jetson Xavier NX using the TTL level standard.

[0048] Furthermore, the hardware connection and synchronization process between the airborne camera, IMU, and GNSS receiver is as follows:

[0049] The Intel RealSense D435i stereo camera and BMI055 inertial measurement unit are connected to the NVIDIA Jetson Xavier NX development board via USB3.0, and the Ublox NEO-M8T module is connected to the UART interface of the NVIDIA Jetson Xavier NX development board. The sensors are synchronized by hardware through the PPS signal of the GNSS.

[0050] Furthermore, the GPU module of NVIDIA Jetson Xavier NX completes visual feature extraction and optical flow tracking, and the onboard CPU performs pre-integration and filtering on the raw data of the inertial measurement unit and the raw GNSS signal respectively.

[0051] The present invention provides an intelligent body posture estimation system based on GNSS / IMU / optical flow information fusion, comprising:

[0052] The data preprocessing module is used to preprocess the raw measurement data of the intelligent agent, including Shi-Tomasi corner feature extraction on the camera's RGB output and KLT optical flow tracking on subsequent RGB video frames; pre-integrate the raw measurement data of the inertial measurement unit IMU in the robot coordinate system; and filter the captured GNSS raw signal;

[0053] Joint initialization module, used to jointly initialize GNSS / IMU / optical flow information;

[0054] The visual constraint factor construction module is used to construct the camera projection model as a pinhole camera model, establish the reprojection function of the spatial feature point l from the video frame i to the video frame j, and construct the visual error factor term as the feature point reprojection residual;

[0055] An IMU constraint factor construction module is used to construct the gyroscope error factor constraint term as a rotation residual based on the quaternion outer product, and to construct the accelerometer error factor constraint term as a velocity bias residual and a displacement bias residual respectively;

[0056] A GNSS constraint factor construction module is used to construct the GNSS pseudorange constraint factor as the residual between the real pseudorange and the pseudorange measured by the receiver, the GNSS Doppler shift constraint factor as the residual between the real carrier Doppler shift and the Doppler shift measurement value, and the receiver clock bias constraint factor as the residual between the real receiver clock bias at a certain moment and the clock bias measurement value at that moment;

[0057] The intelligent body posture information solving module is used to solve the intelligent body posture information by probabilistic factor graph optimization.

[0058] Through the above-mentioned intelligent body pose estimation method, it can be known that the present invention is an intelligent body pose estimation method based on the tight coupling of GNSS / IMU / optical flow information. First, the intelligent body airborne sensor data is preprocessed separately, including IMU pre-integration, corner feature extraction and tracking, GNSS signal filtering and other processes; then, the visual, IMU, and GNSS factor graph constraints are constructed separately, and the GNSS / IMU / optical flow information is jointly initialized; finally, the intelligent body pose solution process is constructed as a state estimation problem, and the corresponding probabilistic factor graph model is designed to solve the intelligent body pose in a graph optimization manner.

[0059] Beneficial effects: Compared with the prior art, the intelligent body pose estimation method based on GNSS / IMU / optical flow information fusion of the present invention has higher computational efficiency and adaptability, and makes full use of the unified computing device architecture (CUDA) in the NVIDIA Jetson Xavier NX airborne computing platform for data parallel processing, which can achieve an intelligent body pose update rate of more than 200HZ, greatly improving the real-time performance of the intelligent body pose estimator. Experimental results show that the pose estimation error of the method proposed in the present invention is reduced by 11.7% compared with VINS-Fusion, and the calculation speed is increased by 1.5 times compared with VINS-Fusion. Thanks to the tight coupling of GNSS / IMU / optical flow information, when encountering abnormal data of an airborne sensor (such as: entering indoors from outdoor, the camera is blocked, etc.), the joint pose estimator can still use the sensor in normal operation to solve the intelligent body pose, and re-initialize the joint when the abnormal sensor returns to normal, so as to obtain accurate, robust, and drift-free intelligent body pose information, while other existing pose estimation methods of the same type cannot achieve the same robustness as the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 It is a flowchart of the intelligent body pose estimation method based on GNSS / IMU / optical flow information fusion;

[0061] Figure 2 This is a schematic diagram of the hardware connection of the intelligent body pose estimator;

[0062] Figure 3 It is a schematic diagram of the coordinate system transformation relationship of the present invention;

[0063] Figure 4 A graphic illustration of the relationship between the Earth-centered, Earth-fixed coordinate system, the local world coordinate system, and the odometry coordinate system. DETAILED DESCRIPTION

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

[0065] In view of the problems of poor robustness and non-smooth trajectory estimation of traditional multi-sensor fusion pose estimators, the present invention provides a method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion. First, the raw data of the intelligent body's onboard sensors are preprocessed separately, including visual feature extraction and tracking, IMU pre-integration, GNSS raw signal filtering and other processes; then, the intelligent body state vector and visual, IMU, and GNSS cost functions are constructed respectively, and the GNSS / IMU / optical flow information is jointly initialized to obtain all the initial values ​​of the intelligent body pose estimator; finally, the intelligent body pose solution process is constructed as a state estimation problem, and the corresponding probabilistic factor graph model and marginalization strategy are designed to solve the intelligent body pose in a graph optimization manner, and finally achieve fast, accurate, robust, and drift-free intelligent body pose estimation.

[0066] The hardware device connection diagram of the intelligent body pose estimator is as follows Figure 1 As shown in the figure, the hardware device deployment of the intelligent body pose estimator is:

[0067] The NVIDIA Jetson Xavier NX development board was selected as the onboard embedded computing platform for the intelligent agent, and the ROS Melodic robot operating system and C++ and Python related development environments were installed. Among them, the GPU version of OpenCV3.4.1 was used for corner feature extraction and optical flow tracking, Eigen 3.3 was used for matrix operations, and Ceres1.14 was used in the factor graph optimization process to solve the nonlinear least squares problem.

[0068] Intel Realsense D435i is selected as the onboard stereo camera to capture visual and IMU measurement information, and is connected to the NVIDIA Jetson Xavier NX development board via USB3.0. The RGB lens and two binocular lenses of the Intel Realsense D435i camera are calibrated separately to obtain the intrinsic parameter matrix K of each camera, and then each lens is jointly calibrated with the built-in BMI055 inertial measurement unit of the camera to obtain the transformation matrix T from the camera to the IMU c r .

[0069] Ublox NEO-M8T is selected as the main chip of the GNSS signal receiver, and is connected to the UART interface of the NVIDIA Jetson Xavier NX with the TTL level standard. The PPS signal is connected to the GPIO interface of the NVIDIA Jetson Xavier NX development board.

[0070] The hardware connection and synchronization process between airborne sensors is as follows:

[0071] The Intel RealSense D435i stereo camera and BMI055 inertial measurement unit are connected to the NVIDIA Jetson Xavier NX development board via USB3.0, and the Ublox NEO-M8T module is connected to the UART interface of the NVIDIA Jetson Xavier NX development board. The stereo camera, inertial measurement unit, and GNSS receiver are hardware synchronized through the GNSS PPS signal.

[0072] The intelligent body pose estimator includes a stereo camera, IMU and GNSS receiver integrated on the intelligent body. The stereo 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.

[0073] like Figure 2 As shown, a method for intelligent body posture estimation based on GNSS / IMU / optical flow information fusion of the present invention comprises the following steps:

[0074] S1. Preprocess the raw measurement data of the stereo camera, IMU, and GNSS receiver, including:

[0075] S101, perform Shi-Tomasi corner feature extraction on the RGB output of the Intel Realsense D435i camera and perform KLT optical flow tracking on subsequent RGB video frames;

[0076] S102, constructing an IMU measurement model, and pre-integrating the raw measurement data of the BMI055 inertial measurement unit in the robot coordinate system;

[0077] S103, filtering the GNSS raw signal captured by Ublox NEO-M8T to remove satellites with unstable signals and low elevation angles.

[0078] The visual feature extraction (i.e., corner feature extraction) and optical flow tracking process in step S101 can be completed by the GPU module of NVIDIA Jetson Xavier NX, and steps S102 and S103 are processed by the onboard CPU.

[0079] Specific:

[0080] The Shi-Tomasi corner features of the RGB output image of the Intel Realsense D435i camera are extracted, and the interval between adjacent features is constrained to 25-100 pixels. 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 feature points are less than 200, the Shi-Tomasi corner features are re-extracted. The Shi-Tomasi corner feature extraction and KLT optical flow tracking process are solved by the GPU module of the NVIDIA Jetson Xavier NX development board.

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

[0082]

[0083] in, and They represent the measurement values ​​of the gyroscope and accelerometer at time t, respectively, with the current robot coordinate system 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.

[0084] Assume that the motion time of the agent in two consecutive video frames is t k and t k+1 , converting the reference coordinate system from the local world coordinate system (w) to the robot coordinate system (r), the agent's orientation (o), velocity (v), and position (p) can be expressed as follows:

[0085]

[0086] in, They are respectively t in the world coordinate system (w) k+1 The direction, speed and position of the agent at each moment, t k The direction, speed and position of the agent at each moment, 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 this moment, symbol Represents quaternion multiplication. The IMU pre-integration term can be expressed as:

[0087]

[0088] in, is the pre-integrated term of the IMU orientation, is the pre-integrated term of the 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 this moment, and are the measurements of the gyroscope and accelerometer, respectively.

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

[0090]

[0091] in, 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, Respectively represent and 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.

[0092] Finally, the original GNSS signal captured by the Ublox NEO-M8T receiver is filtered to remove satellites with unstable signals and low elevation angles, thereby reducing the impact of multipath effects during signal propagation.

[0093] S2, GNSS / IMU / optical flow information joint initialization; including:

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

[0095] S202, using the PnP algorithm and the BA algorithm to solve the initial position and initial orientation of the intelligent body, and using the SPP algorithm to roughly solve the longitude and latitude of the intelligent body;

[0096] S203: Align the initial position of the agent with the longitude and latitude to obtain the transformation matrix between the odometer coordinate system and the world coordinate system

[0097] Specific:

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

[0099]

[0100] The agent state vector contains the orientation of the agent in the local world coordinate system speed Location Gyroscope bias and accelerometer bias GNSS receiver clock bias δt and bias rate δt′, visual feature point depth d1, d2, ..., d m and the Z-axis offset ψ between the odometer coordinate system and the world coordinate system, where n is the sliding window size.

[0101] 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 position of the intelligent body. and initial orientation The single point positioning (SPP) algorithm is performed on the pseudorange information in the GNSS raw signal to calculate the longitude and latitude of the intelligent body in the Earth-centered Earth-fixed (ECEF) coordinate system. Then the initial position of the intelligent body and the longitude and latitude are aligned to obtain the transformation matrix between the odometer coordinate system and the world coordinate system. Complete the joint initialization of GNSS / IMU / optical flow information. After the initialization is completed, the initial position, orientation, and transformation matrix between the odometer coordinate system and the world coordinate system of the intelligent body can be obtained.

[0102] S3. Construct visual constraint factors, including:

[0103] S301. Construct the camera model of Intel Realsense D435i into a pinhole camera model;

[0104] S302, establishing a reprojection function of the spatial feature point l from the video frame i to the video frame j;

[0105] S303, constructing the visual error factor term as the feature point reprojection residual, that is, projecting the spatial feature point l from the pixel coordinate system of video frame i to the world coordinate system, and then projecting from the world coordinate system to the pixel coordinate system of video frame j, and then solving the residual between the measured position and the actual position of the spatial feature point l in video frame j.

[0106] Specific:

[0107] The imaging model of the Intel RealSense D435i stereo camera is constructed as a pinhole camera model. It can be assumed that the homogeneous coordinates of the spatial feature point l in the image in the world coordinate system are:

[0108]

[0109] Among them, X l , Y l , Z l are the coordinate values ​​of feature point l respectively, then the homogeneous coordinates of feature point l in the pixel plane of video frame i can be expressed as:

[0110]

[0111] in, are the X and Y coordinates of the pixel plane of video frame i, respectively. The projection model of the Intel RealSense D435i stereo camera can be expressed as:

[0112]

[0113] in, is the transformation matrix between the robot coordinate system and the camera coordinate system, is the transformation matrix between the world coordinate system and the robot coordinate system at time i, n c is the camera imaging noise, and K is the camera intrinsic parameter matrix:

[0114]

[0115] The elements of the intrinsic parameter matrix K have been obtained by the camera calibration process, f x and f y is the scale scaling amount, c x and c y is the translation amount, and the reprojection function of the spatial feature point l from video frame i to video frame j can be expressed as:

[0116]

[0117] in, represents the measured position of spatial feature point l in video frame j, represents the actual position of the spatial feature point l in the video frame i, is the transformation matrix between the world coordinate system and the robot coordinate system at time j, is the transformation matrix between the robot coordinate system and the world coordinate system at time i, is the transformation matrix between the camera coordinate system and the world coordinate system, is the transformation matrix between the camera coordinate system and the pixel coordinate system;

[0118]

[0119] in are the X and Y coordinates of feature point l in the pixel plane of video frame i, is the spatial feature point l relative to the camera c i Depth.

[0120] Then the visual constraint factor can be expressed as the actual position of the spatial feature point l in the video frame j With measurement position The residual of:

[0121]

[0122] Among them, χ V Represents the subvector related to visual information in the agent state vector χ.

[0123] S4. Construct IMU constraint factors, including:

[0124] S401, constructing the gyroscope error factor constraint term as a rotation residual based on the quaternion outer product, and performing quaternion multiplication with the real rotation angle of the agent in two consecutive video frames and the inverse of the gyroscope pre-integration term to obtain the gyroscope measurement residual;

[0125] S402, constructing the accelerometer error factor constraint items into velocity bias residual and displacement bias residual respectively; the velocity bias residual is obtained by subtracting the real velocity bias of the agent in two consecutive video frames from the velocity pre-integration item of the accelerometer; the displacement bias residual is obtained by subtracting the real displacement bias of the agent in two consecutive video frames from the displacement pre-integration item of the accelerometer;

[0126] S403, the gyroscope bias and accelerometer bias error factor terms are obtained by subtracting the bias between two consecutive video frames;

[0127] S404, by combining the steps S401, S402 and S403, the construction of the IMU constraint factor can be completed.

[0128] Specific:

[0129] The gyroscope error factor constraint term is constructed as a rotation residual based on the quaternion outer product, and the accelerometer error factor constraint term is constructed as a velocity residual and a displacement residual, respectively. The gyroscope bias and accelerometer bias error factor terms are obtained by subtracting the bias between two consecutive video frames. Then the IMU constraint factor can be constructed as follows:

[0130]

[0131] Among them, χ I represents the sub-vector related to IMU in the agent state vector χ, is the pre-integrated term of the IMU orientation, is the pre-integrated term of the 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, which can be obtained by step S1, δb ω and δb a are the residuals of the gyroscope bias and accelerometer bias, respectively, Represents t k The rotation between the world coordinate system and the robot coordinate system at that moment, They are respectively t in the world coordinate system (w) k+1 The direction, speed and position of the agent at each moment, 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 this moment, symbol represents quaternion multiplication, and t k+1 The gyroscope bias and accelerometer bias at the moment, and t k The gyroscope bias and accelerometer bias at the moment.

[0132] S5. Construct GNSS constraint factors, including:

[0133] S501: Establishing a GNSS pseudorange measurement model, and using the pseudorange measurement model to solve the real GNSS pseudorange information, the GNSS pseudorange constraint factor can be constructed as the residual between the real pseudorange and the pseudorange measured by the receiver;

[0134] S502: Establishing a GNSS Doppler frequency shift measurement model, and using the Doppler frequency shift measurement model to solve the real GNSS Doppler frequency shift information, so that the GNSS Doppler frequency shift constraint factor can be constructed as the residual between the real carrier Doppler frequency shift and the Doppler frequency shift measurement value;

[0135] S503: Establishing a GNSS receiver clock bias measurement model, the receiver clock bias constraint factor can be constructed as a residual between the actual receiver clock bias at a certain moment and the clock bias measurement value at that moment;

[0136] S504: The construction of the GNSS constraint factor can be completed by combining the steps S501, S502 and S503.

[0137] Specific:

[0138] The GNSS constraint factor term in the agent probability factor graph model is composed of pseudorange constraint factor, Doppler frequency shift constraint factor and receiver clock bias constraint factor. Among them, the pseudorange measurement model between the Ublox NEO-M8T receiver on the agent and the navigation satellite can be expressed as:

[0139]

[0140] In the above formula

[0141]

[0142] in, and are the positions of the Ublox NEO-M8T receiver and the navigation satellite in the Earth-centered Inertial (ECI) coordinate system, and are the positions in the Earth-centered Earth-fixed coordinate system, is the measured value of GNSS pseudorange, c is the propagation speed of light in vacuum, δt r and δt sare 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 the pseudorange signal, R represents the coordinate system error caused by the rotation of the earth, ω earth is the Earth's rotation angular velocity, Represents the signal propagation time from the satellite to the receiver.

[0143] The GNSS pseudorange constraint factor can be constructed as the residual between the true pseudorange and the pseudorange measured by the receiver:

[0144]

[0145] in, represents the observation equation between the satellite and the receiver, χ pr represents the subvector related to the GNSS pseudorange in the agent state vector χ, and are the clock biases of the receiver and satellite, respectively, Represents the measured value of the pseudorange.

[0146] The Doppler frequency shift measurement model between the Ublox NEO-M8T receiver onboard the intelligent agent and the navigation satellite can be expressed as:

[0147]

[0148] In the above formula

[0149]

[0150] in, is the Doppler shift measurement value of the receiver and the satellite, 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, ω earth is the Earth's rotation angular velocity, δt r ′ and δt s ′ are the clock bias rates of the receiver and satellite respectively, represents the signal propagation time from the satellite to the receiver, n dp is the Doppler noise.

[0151] The GNSS Doppler shift constraint factor can be constructed as the residual between the actual carrier Doppler shift and the Doppler shift measurement:

[0152]

[0153] in, represents the observation equation between the satellite and the receiver, χ dp represents the subvector related to the GNSS Doppler frequency shift in the agent state vector χ, is the direction vector between the satellite and the receiver, is the Doppler shift measurement value.

[0154] Now t k to k+1 The internal receiver clock bias constraint factor is constructed as follows:

[0155]

[0156] in, is the receiver clock bias observation model, χ τ represents the subvector related to the receiver clock bias in the agent state vector χ, and t k and t k-1 The receiver clock bias at time t k-1 The detailed coordinate system transformation relationship of the present invention is as follows: Figure 3 , Figure 4 shown.

[0157] The pseudorange constraint factor, the GNSS Doppler frequency shift constraint factor and the receiver clock bias constraint factor are combined to form the GNSS constraint factor item in the agent probabilistic factor graph model.

[0158] S6. Solving the intelligent body posture information, including:

[0159] S601: The intelligent body posture solution process is regarded as a state estimation problem, and the intelligent body posture solution process becomes the maximum a posteriori estimation of the state vector;

[0160] S602: Construct a factor graph model of the intelligent body pose estimator with tight coupling of GNSS / IMU / optical flow information;

[0161] S603: Design an observation data marginalization strategy to remove redundant optical flow, IMU, and GNSS information in the sliding window;

[0162] S604: Solve the agent state vector by factor graph optimization.

[0163] Specific:

[0164] The intelligent body pose solution process is regarded as a state estimation problem, and the optimal state of the intelligent body is the maximum a posteriori estimate of the state vector. Assuming that the measurement signals of the intelligent body's onboard camera, IMU, and GNSS receiver are independent of each other, and the measurement noise conforms to a Gaussian distribution with a mean of zero, the maximum a posteriori estimation problem can be equivalent to minimizing the sum of errors. Then the factor graph model of the intelligent body pose estimator with tightly coupled GNSS / IMU / optical flow information can be expressed as:

[0165]

[0166] Among them, P(χ|z) is the maximum posterior probability of the agent state vector, z is the total agent state measurement model, and e p is the system prior information, H p represents the agent's prior pose matrix, n is the number of agent state vectors in the sliding window, and the symbol E represents the visual constraint factor E in step S3. V , IMU constraint factor E in step S4 I , and the GNSS constraint factor E in step S5 pr 、E dp 、E τ The sum of k is the k-th observation information of the agent.

[0167] Finally, the agent state vector χ is solved by probabilistic factor graph optimization, that is, solving formula (21) can obtain the complete agent posture information.

[0168] An intelligent body pose estimation system based on GNSS / IMU / optical flow information fusion, comprising:

[0169] The data preprocessing module is used to preprocess the raw measurement data of the intelligent agent, including Shi-Tomasi corner feature extraction on the camera's RGB output and KLT optical flow tracking on subsequent RGB video frames; pre-integrate the raw measurement data of the inertial measurement unit IMU in the robot coordinate system; and filter the captured GNSS raw signal;

[0170] Joint initialization module, used to jointly initialize GNSS / IMU / optical flow information;

[0171] The visual constraint factor construction module is used to construct the camera projection model as a pinhole camera model, establish the reprojection function of the spatial feature point l from the video frame i to the video frame j, and construct the visual error factor term as the feature point reprojection residual;

[0172] An IMU constraint factor construction module is used to construct the gyroscope error factor constraint term as a rotation residual based on the quaternion outer product, and to construct the accelerometer error factor constraint term as a velocity bias residual and a displacement bias residual respectively;

[0173] A GNSS constraint factor construction module is used to construct the GNSS pseudorange constraint factor as the residual between the real pseudorange and the pseudorange measured by the receiver, the GNSS Doppler shift constraint factor as the residual between the real carrier Doppler shift and the Doppler shift measurement value, and the receiver clock bias constraint factor as the residual between the real receiver clock bias at a certain moment and the clock bias measurement value at that moment;

[0174] The intelligent body posture information solving module is used to solve the intelligent body posture information by probabilistic factor graph optimization.

Claims

1. A method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion, characterized in that: The following steps are involved: S1. Preprocess the raw measurement data of the intelligent agent, including Shi-Tomasi corner feature extraction on the camera's RGB output and KLT optical flow tracking on subsequent RGB video frames; pre-integrate the raw measurement data of the inertial measurement unit (IMU) in the robot coordinate system; filter the captured GNSS raw signal; S2, joint initialization of GNSS / IMU / optical flow information for pose estimator: Construct the state vector χ of the intelligent body, use the PnP algorithm and BA algorithm to solve the initial position and initial orientation of the intelligent body, and use the SPP algorithm to roughly solve the longitude and latitude of the intelligent body; align the initial position and longitude and latitude of the intelligent body, obtain the transformation matrix between the odometer coordinate system and the world coordinate system, and complete the joint initialization of GNSS / IMU / optical flow information; S3, construct visual constraint factor: construct the camera projection model as a pinhole camera model, establish the reprojection function of the spatial feature point l from video frame i to video frame j, and construct the visual error factor term as the feature point reprojection residual; S4. Constructing IMU constraint factors: The IMU constraint factors include gyroscope error factor constraint items and accelerometer error factor constraint items, wherein the gyroscope error factor constraint items are constructed as rotation residuals based on quaternion outer products, and the accelerometer error factor constraint items are constructed as velocity bias residuals and displacement bias residuals, respectively; S5. Construct GNSS constraint factors: GNSS constraint factors include GNSS pseudorange constraint factors, GNSS Doppler frequency shift constraint factors and receiver clock bias constraint factors; wherein, the GNSS pseudorange constraint factor is constructed as the residual between the real pseudorange and the pseudorange measured by the receiver, the GNSS Doppler frequency shift constraint factor is constructed as the residual between the real carrier Doppler frequency shift and the Doppler frequency shift measurement value, and the receiver clock bias constraint factor is constructed as the residual between the real receiver clock bias at a certain moment and the clock bias measurement value at that moment; S6. Solve the posture information of the intelligent body by probabilistic factor graph optimization; specifically: S601, the intelligent body posture solution process is regarded as a state estimation problem, and the intelligent body posture solution process becomes the maximum a posteriori estimation of the state vector; S602, construct a pose estimator factor graph model with tight coupling of GNSS / IMU / optical flow information, expressed as: Where χ is the agent state vector, P(χ|z) is the maximum posterior probability of the agent state vector, z is the overall agent state measurement model, and e p is the system prior information, H p represents the agent's prior pose matrix, n is the number of agent state vectors in the sliding window, and the symbol E represents the visual constraint factor E V , IMU constraint factor E I , GNSS constraint factor E pr 、E dp 、E τ The sum of k is the k-th observation information of the agent; S603, designing an observation data marginalization strategy to remove redundant optical flow, IMU, and GNSS information in the sliding window; S604, solving the pose estimator factor graph model of the tightly coupled GNSS / IMU / optical flow information to obtain the agent state vector.

2. According to claim 1, a method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion, characterized in that: Step S2 is specifically as follows: S201, constructing an agent state vector x, wherein the state vector includes the current orientation of the agent speed Location Gyroscope bias and accelerometer bias GNSS receiver clock bias δt and bias rate δt′, visual feature point depth d1, d2, ..., d m and the Z-axis offset ψ between the odometer coordinate system and the world coordinate system, the agent state vector χ is expressed as: S202: Calculate the relative motion relationship between two adjacent image frames using the PnP algorithm, and optimize the reprojection error of all corner point features in the sliding window using the BA algorithm to obtain the initial position of the intelligent body. and initial orientation At the same time, the SPP algorithm is used to roughly solve the longitude and latitude of the intelligent agent; S203: Align the initial position of the agent with the longitude and latitude to obtain the transformation matrix between the odometer coordinate system and the world coordinate system Complete the joint initialization of GNSS / IMU / optical flow information.

3. The method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion according to claim 1, characterized in that: The specific method of constructing the visual error factor term as the feature point reprojection residual in step S3 is: project the spatial feature point l from the pixel coordinate system of video frame i to the world coordinate system, and then project it from the world coordinate system to the pixel coordinate system of video frame j, and then solve the residual between the measured position and the actual position of the feature point l in video frame j.

4. The method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion according to claim 1, characterized in that: Step S4 is specifically as follows: S401, constructing the gyroscope error factor constraint term as a rotation residual based on the quaternion outer product, and performing quaternion multiplication with the real rotation angle of the agent in two consecutive video frames and the inverse of the gyroscope pre-integration term, that is, obtaining the gyroscope measurement residual; S402, constructing the accelerometer error factor constraint term into a velocity bias residual and a displacement bias residual respectively; the velocity bias residual is obtained by subtracting the actual velocity bias of the agent in two consecutive video frames from the velocity pre-integral term of the accelerometer; The displacement bias residual is obtained by subtracting the real displacement bias of the agent in two consecutive video frames from the accelerometer displacement pre-integrated term; S403, the gyroscope bias and accelerometer bias error factor terms are obtained by subtracting the bias between two consecutive video frames; S404, combining step S401, step S402 and step S403 to complete the construction of the IMU constraint factor; The constructed IMU constraint factors are: Among them, χ I represents the subvector related to IMU in the agent state vector χ, 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 ω and δb a are the residuals of the gyroscope bias and accelerometer bias, respectively, Represents t k The rotation between the world coordinate system and the robot coordinate system at that moment, They are respectively t in the world coordinate system k+1 The direction, speed and position of the agent at each moment, t k The transformation matrix between the world coordinate system and the robot coordinate system at this moment, symbol represents quaternion multiplication, and t k+1 The gyroscope bias and accelerometer bias at the moment, and t k The gyroscope bias and accelerometer bias at the moment.

5. The method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion according to claim 1, characterized in that: Step S5 is specifically as follows: S501, establish a GNSS pseudorange measurement model, and use the pseudorange measurement model to solve the pseudorange information in the real GNSS original signal. The GNSS pseudorange constraint factor is constructed as the residual between the real pseudorange and the pseudorange measured by the receiver, expressed as: in, represents the observation equation between the satellite and the receiver, χ pr represents the subvector related to the GNSS pseudorange in the agent state vector χ, and are the positions of the receiver and the satellite in the geocentric inertial coordinate system, c is the speed of light in a vacuum, and are the clock biases of the receiver and satellite, △t tro and △t ion are the delays in the troposphere and ionosphere, △t mul is the delay caused by the multipath effect, represents the measured value of pseudorange; S502: Establish a GNSS Doppler frequency shift measurement model, and use the Doppler frequency shift measurement model to solve the real GNSS Doppler frequency shift information. Then, the GNSS Doppler frequency shift constraint factor is constructed as the residual between the real carrier Doppler frequency shift and the Doppler frequency shift measurement value, which is expressed as: in, represents the observation equation between the satellite and the receiver, χ dp represents the subvector related to the GNSS 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, c is the speed of light in a vacuum, and are the clock bias rates of the receiver and satellite, is the Doppler shift measurement value; S503: Establish a GNSS receiver clock bias measurement model. The receiver clock bias constraint factor is constructed as the residual between the actual receiver clock bias at a certain moment and the clock bias measurement value at that moment, expressed as: in, is the receiver clock bias observation model, χ τ represents the subvector in the agent state vector χ related to the receiver clock bias, and t k and t k-1 The receiver clock bias at time , t k-1 The receiver clock bias rate at time ; S504, combining step S501, step S502 and step S503 to complete the construction of the GNSS constraint factor.

6. The method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion according to claim 1, characterized in that: The construction process of the hardware equipment used in this method is as follows: (1) NVIDIA Jetson Xavier NX is used as the onboard embedded computing platform of the intelligent agent, and the ROSMelodic robot operating system and pose estimator related operating environment are installed; (2) Using Intel RealSense D435i as an airborne stereo camera, calibrate the three lenses of the camera and the built-in BMI055 inertial measurement unit, and obtain the camera's intrinsic parameter matrix K and the transformation matrix T between the camera and the built-in BMI055. c r ; (3) The Ublox NEO-M8T module is used as the GNSS signal receiver and connected to the UART port of the NVIDIA Jetson Xavier NX using the TTL level standard.

7. The method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion according to claim 6, characterized in that: The hardware connection and synchronization process between the airborne camera, IMU, and GNSS receiver is as follows: The Intel RealSense D435i stereo camera and BMI055 inertial measurement unit are connected to the NVIDIA Jetson Xavier NX development board via USB3.0, and the Ublox NEO-M8T module is connected to the UART interface of the NVIDIA Jetson Xavier NX development board. The sensors are synchronized by hardware through the PPS signal of the GNSS.

8. The method for intelligent body pose estimation based on GNSS / IMU / optical flow information fusion according to claim 6, characterized in that: The GPU module of NVIDIA Jetson Xavier NX performs visual feature extraction and optical flow tracking, and the onboard CPU performs pre-integration and filtering on the raw data of the inertial measurement unit and the raw GNSS signal.

9. An intelligent body pose estimation system based on GNSS / IMU / optical flow information fusion, characterized in that: include: The data preprocessing module is used to preprocess the raw measurement data of the intelligent agent, including Shi-Tomasi corner feature extraction on the camera's RGB output and KLT optical flow tracking on subsequent RGB video frames; pre-integrate the raw measurement data of the inertial measurement unit IMU in the robot coordinate system; and filter the captured GNSS raw signal; Joint initialization module, used to jointly initialize GNSS / IMU / optical flow information; The visual constraint factor construction module is used to construct the camera projection model as a pinhole camera model, establish the reprojection function of the spatial feature point l from the video frame i to the video frame j, and construct the visual error factor term as the feature point reprojection residual; An IMU constraint factor construction module is used to construct the gyroscope error factor constraint term as a rotation residual based on the quaternion outer product, and to construct the accelerometer error factor constraint term as a velocity bias residual and a displacement bias residual respectively; A GNSS constraint factor construction module is used to construct the GNSS pseudorange constraint factor as the residual between the real pseudorange and the pseudorange measured by the receiver, the GNSS Doppler shift constraint factor as the residual between the real carrier Doppler shift and the Doppler shift measurement value, and the receiver clock bias constraint factor as the residual between the real receiver clock bias at a certain moment and the clock bias measurement value at that moment; The intelligent body posture information solving module is used to solve the intelligent body posture information by means of probabilistic factor graph optimization; Specifically: S601, the intelligent body posture solution process is regarded as a state estimation problem, and the intelligent body posture solution process becomes the maximum a posteriori estimation of the state vector; S602, construct a pose estimator factor graph model with tight coupling of GNSS / IMU / optical flow information, expressed as: Where χ is the agent state vector, P(χ|z) is the maximum posterior probability of the agent state vector, z is the overall agent state measurement model, and e p is the system prior information, H p represents the agent's prior pose matrix, n is the number of agent state vectors in the sliding window, and the symbol E represents the visual constraint factor E V , IMU constraint factor E I , GNSS constraint factor E pr 、E dp 、E τ The sum of k is the k-th observation information of the agent; S603, designing an observation data marginalization strategy to remove redundant optical flow, IMU, and GNSS information in the sliding window; S604, solving the pose estimator factor graph model of the tightly coupled GNSS / IMU / optical flow information to obtain the agent state vector.

Images