Virtual Initialization Method for Monocular Vision Inertial Odometry Based on Velocity Input

Abstract

This invention discloses a virtual initialization method for monocular visual inertial odometry based on velocity input. This method is a virtual initialization process for monocular visual inertial odometry. It only requires inputting the initial velocity parameters of the motion state into the computer, and then using the monocular visual inertial odometry calculation method, it can complete the initialization solution process for the visual inertial odometry of any carrier under different motion states, thereby realizing the navigation and positioning function of the odometry calculation method. This invention can restore the initial value of the system in degenerate motion states where initialization cannot be completed on its own.

Description

Technical Field

[0001] This invention belongs to the field of positioning technology. Background Technology

[0002] Monocular vision-based inertial systems (VISs), with their simple structure, small footprint, and rich information acquisition capabilities, have become a hot topic in autonomous navigation research. Monocular vision-based inertial odometry, as a relatively mature navigation method, is widely used in fields such as unmanned vehicle and drone driving, logistics transportation, search and rescue, and infrastructure inspection. However, visual odometry methods typically struggle to complete the initialization process under degenerate motion conditions. Summary of the Invention

[0003] Purpose of the invention: In order to solve the problems existing in the prior art, the present invention provides a virtual initialization method for monocular vision inertial odometry based on velocity input.

[0004] Technical solution: This invention provides a virtual initialization method for monocular vision inertial odometry based on velocity input, specifically including the following steps:

[0005] Step 1: Set up the simulation platform, adaptively set the trajectory and landmark positions of the monocular vision inertial odometry virtual initialization process, and set the configuration parameter file for the virtual initialization process and the noise parameters of the inertial sensor;

[0006] Step 2: Generate the virtual initialization simulation data from Step 1 in the simulation software;

[0007] Step 3: Run the virtual initialization simulation data generated in Step 2 within the set virtual initialization time period to obtain the initial values ​​of the state variables of the simulated carrier. Complete the docking of the state variables of the simulated carrier with the state variables of the actual carrier at the end of the virtual initialization, thereby completing the virtual initialization of the monocular visual inertial odometry.

[0008] Furthermore, the virtual initialization process trajectory generated in step 1 satisfies the following two rules:

[0009] Rule 1: The initial velocity and attitude angle values ​​of the simulated carrier in the process trajectory are continuous values;

[0010] Rule 2: The first derivative of the initial velocity of the simulated carrier with respect to time and the first derivative of the attitude angle of the simulated carrier with respect to time are both continuous values.

[0011] Furthermore, the adaptive setting of the trajectory and landmark position for the monocular visual inertial odometry virtual initialization process in step 1 is specifically as follows:

[0012] Step 1.1: Initialize the excitation using sine and cosine motion on three axes to form a sine and cosine trajectory. The three axes are the north axis (y) pointing due north, the east axis (x) pointing due east, and the sky axis (z). The expressions for the simulated carrier's position p1(t), velocity v1(t), and acceleration a1(t) during sine and cosine motion are as follows:

[0013]

[0014]

[0015]

[0016] Where t represents the time variable, A x A y A z Let A and B represent the amplitudes of the sine and cosine motion trajectories along the three axes, respectively. K is the multiple of the angular velocity of the celestial axis motion, t1 represents the preset time for the sine motion, and π(·) is the direct proportional mapping relationship. x =A x0 ·π(V0),A y =A y0 ·π(V0),A z =A z0 ·π(V0), A x0 A y0 and A z0 All represent dimensionless values; where V0 represents the magnitude of the velocity at the initial moment during the actual motion of the carrier, that is, the instantaneous velocity of the actual carrier at the current moment after the virtual initialization calculation is completed;

[0017] Landmarks in sine and cosine trajectories are set within the field of view during the trajectory.

[0018] Step 1.2: After completing the sine and cosine motion, the simulated carrier undergoes projectile-like motion, ensuring that the velocity of the simulated carrier at the end of the projectile-like motion is the same as the initial velocity of the actual carrier during its actual motion. The expressions for the carrier's position p2(t), velocity v2(t), and acceleration a2(t) during the projectile-like motion are as follows:

[0019]

[0020]

[0021]

[0022] Where t2 represents the preset time for projectile-like motion, V y V z These represent the velocity components along the y-axis and z-axis, respectively, and component V y V zThe following relationship must be satisfied:

[0023]

[0024] In projectile motion, landmarks are placed within the field of vision of the trajectory of the projectile.

[0025] Furthermore, in step 3, after running the simulation data, the initial values ​​of the simulated carrier's state variables are obtained. This process involves two state dockings: the first docking occurs at the end of the sine and cosine motion with the initial moment of the projectile-like motion; the second docking occurs at the end of the projectile-like motion with the state of the actual carrier during actual motion at the end of the virtual initialization. A sliding window is set for each docking process, with k keyframes in a single sliding window. During a particular docking, the position, attitude, and velocity of the i-th keyframe in the sliding window are denoted as ps. i Rs i vs i Where i = 1, 2, ..., k; Angle compensation is performed on the position, attitude, and velocity in the i-th keyframe to obtain the compensated position ps. i' Posture Rs i' Speed ​​vs i' for:

[0026]

[0027] Rs comp The expression for attitude angle compensation is shown below:

[0028]

[0029] Where, δ k This represents the yaw angle value in the velocity direction of the k-th frame in the sliding window.

[0030] Beneficial effects: This invention provides an important method for monocular vision-based inertial odometry calculation under degenerate motion states where initialization is difficult. It only requires inputting the initial velocity parameters of the motion state to adaptively generate visual and inertial data for the virtual initialization process and complete the initialization solution. It is simple to use, highly accurate, and highly reliable. This invention extends monocular vision-based inertial odometry calculation to conditions that allow for startup in any state. Attached Figure Description

[0031] Figure 1 This is a flowchart of the overall algorithm of the present invention;

[0032] Figure 2 This is a schematic diagram of the virtual initialization trajectory designed for this invention;

[0033] Figure 3This is a schematic diagram of the landmark point setting during the sine and cosine motion process designed in this invention;

[0034] Figure 4 This is a schematic diagram of the landmark point setting during the projectile-like motion designed in this invention;

[0035] Figure 5 A schematic diagram of the urban environment simulation scene and environmental road sign settings designed for this invention;

[0036] Figure 6 Figure 1 shows the field of view of the UAV during motion simulation. Figure 2 shows the field of view of the UAV from 0s to 7s, Figure 3 shows the field of view of the UAV from 7s to 14s, and Figure 4 shows the field of view of the UAV from 14s to 20s.

[0037] Figure 7 Figure (a) shows the field of view of the vehicle during motion simulation, while Figure (b) shows the field of view of the vehicle during motion simulation, and Figure (c ... Detailed Implementation

[0038] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0039] like Figure 1 As shown, a virtual initialization method for monocular vision inertial odometry based on velocity input is presented. This method uses monocular vision inertial odometry calculation and specifically includes the following steps:

[0040] Step 1: In the computer, input the initial velocity parameters of the actual carrier during its actual motion, and adaptively set the motion trajectory equation and landmark positions of the simulated carrier during the virtual initialization process. Set the configuration parameter file for the virtual initialization process, setting the inertial device noise parameter to an appropriately small value, and keeping the remaining parameters the same as the actual configuration file.

[0041] Step 2: Use the simulation tool imusim to set up and generate virtual initialization simulation data as in Step 1. Then, in chronological order, match the visual keyframes in the virtual initialization with the visual keyframes in the actual motion process, and match the inertial sensor data in the virtual initialization with the inertial sensor data in the actual motion process.

[0042] Step 3: Activate the monocular vision-based inertial odometry calculation method. Within the set virtual initialization time period, first run the virtual initialization simulation data generated in Step 2 to obtain the initial values ​​of the simulated vehicle's state variables, completing the docking of the simulated vehicle's state variables with the actual vehicle's state variables at the end of the virtual initialization. Then, use a nonlinear optimization process to solve for the actual motion trajectory parameters. This step requires performing two state docking operations sequentially.

[0043] For the virtual initialization method of monocular visual inertial odometry based on velocity input described in step 1, the velocity parameter input to the actual vehicle during the actual motion process refers to the instantaneous velocity of the actual vehicle at the current moment after the virtual initialization calculation is completed by the monocular visual inertial odometry method. Ideally, if the virtual initialization can be completed in a short time, then velocity prediction is not required, and the actual motion velocity of the vehicle at the current moment can be directly input.

[0044] Steps 1, 2, and 3, which involve initialization simulation and calculation, are all virtualization processes and must be completed before using the monocular vision inertial odometry method to solve for the actual motion trajectory using nonlinear optimization.

[0045] The adaptively generated virtual initialization process trajectory in step 1 must satisfy the following two rules:

[0046] Rule 1: The initial velocity and attitude angle values ​​of the simulated vehicle must be continuous during the trajectory process;

[0047] Rule 2: The first derivative of the initial velocity of the simulation vehicle with respect to time and the first derivative of the attitude angle with respect to time must be continuous for the trajectory process.

[0048] The virtual initialization trajectory and landmark points adaptively set in step 1 must satisfy sufficient triaxial acceleration excitation and visual parallax excitation during the initialization process. The specific steps are as follows:

[0049] Step 1.1: To satisfy the excitation conditions for triaxial acceleration variation, the excitation is first initialized using sine and cosine motion along the three axes. The three axes are the north axis (y) pointing due north, the east axis (x) pointing due east, and the sky axis (z). The expressions for the simulated carrier's position p1(t), velocity v1(t), and acceleration a1(t) are as follows: Figure 2 The first half of the trajectory shown:

[0050]

[0051]

[0052]

[0053] In the formulas, t in p1(t), v1(t), and a1(t) represents time as the independent variable of the function. A in the formula... x A y A z These represent the amplitudes of the sine and cosine function trajectories along the three axes, respectively. K is the multiple of the angular velocity of the celestial axis, t1 represents the preset time for initialization step 1.1, and π represents the constant pi. The adaptive function π(·) represents a direct proportional mapping relationship: Let the magnitude of the input carrier velocity in step 1 be V0, and let the ratio of the amplitude values ​​of this velocity along the x, y, and z axes be A. x0 :A y0 :A z0 Therefore, the triaxial amplitude setting is directly proportional to the magnitude of the input velocity, which can be expressed as follows:

[0054] A x =A x0 ·π(V0),A y =A y0 ·π(V0),A z =A z0 ·π(V0)

[0055] A x0 A y0 A z0 All of them are dimensionless values.

[0056] In the formula, K and t1 can be set to appropriate positive integers, independent of the adaptive function π(·). The landmark points corresponding to the trajectory described in step 1.1 are set within the field of view during the trajectory's movement and must be set according to the trajectory's shape. For example... Figure 3 The diagram shown is a schematic diagram of the road sign setting in step 1.1.

[0057] Step 1.2: After completing initialization step 1.1, execute a projectile-like motion. The purpose is to ensure that the simulated carrier's velocity at the end of step 1.2 is the same as the actual initial velocity (the carrier velocity was input in step 1). The expressions for the position p2(t), velocity v2(t), and acceleration a2(t) during the projectile-like motion process are as follows: Figure 2 The latter half of the trajectory:

[0058]

[0059]

[0060]

[0061] In the formula, t2 represents the preset time for step 1.2, and V y V zThese represent the velocity components along the y-axis and z-axis, respectively. They also represent the velocity components obtained through the first state docking (the first docking refers to the virtual point transitioning from sine / cosine motion to projectile-like motion) after the virtual initialization process of setting the trajectory in step 1.1, to smoothly transition to the next stage. After docking, the input velocity will be fixed on the plane with a heading angle of 0, and the component V... y V z The following relationship must be satisfied:

[0062]

[0063] In the formula, t2 can be set to an appropriate positive integer, and is independent of the adaptive function π(·). The landmark points corresponding to this trajectory are set within the field of view during the trajectory's movement, and their settings need to be based on the trajectory's shape. Figure 4 A schematic diagram showing the setting of path markers in projectile-like motion.

[0064] Step 1.3: Configure the virtual initialization process parameter file used in Step 1. The inertial data used for virtual initialization is noise-free data, so only a small inertial noise parameter needs to be set to complete the initialization calculation.

[0065] In step 3, after running the simulation data, the initial values ​​of the state variables of the simulated carrier are obtained. This process involves two state dockings. The first docking is between the state of the simulated carrier at the end of the sine and cosine motion and the initial moment of the projectile-like motion. The second docking is between the state of the virtual carrier at the end of the projectile-like motion and the state of the actual carrier at the end of the virtual initialization.

[0066] Step 3.1: First State Docking Process. At the end of the adaptive virtual initialization process in Step 1.1, due to the uncertainty of the initialization completion node and the 90-degree difference between the definition of the plane with a heading angle of 0 in the tool imusim and the monocular visual inertial odometry code (the two x-axis and two y-axis differ by 90 degrees in their coordinate systems), angle compensation is required during the state docking process. This ensures that the input velocity in Step 1 can be successfully fixed in the plane with a heading angle of 0 for two-dimensional decomposition. Using this method, the velocity is fixed in the plane with a heading angle of 0, and the velocity is decomposed in two dimensions in this plane. Since this scheme assumes that the carrier velocity vector after virtual initialization is located in the plane with a heading angle of 0, this velocity has exactly two components in the plane with a heading angle of 0. The attitude angle compensation formula is as follows:

[0067]

[0068] Let the keyframe number i of the sliding window be counted from 0 to k, where Rs in the formula comp Denotes the attitude angle compensation matrix, δk The yaw angle (δ) representing the odometer velocity direction at the moment of docking. k This represents the yaw angle value in the velocity direction of the k-th frame within the sliding window. Step 1.1: Set the position, attitude, and velocity of each keyframe in the sliding window at the end of the trajectory, denoted as ps. i Rs i vs i Where i = 1, 2, ..., k; after the motion state variables are docked, the state of each frame is ps. i' Rs i' vs i' The formula for the state docking process is as follows:

[0069]

[0070] Step 3.2: At the end time of the projectile motion velocity docking phase set in Step 1.2, execute the second state docking process. This process is the same as in Step 3.1, and the attitude angle compensation matrix and the states for each frame are also the same as in Step 3.1. The value of k in the sliding window can be different. After Step 3.2, it can be ensured that at the start time of the actual trajectory calculation, the initial velocity of the carrier is in the plane with a heading angle of 0. Therefore, the estimated system state variables obtained from the virtual initialization are retained, allowing the system to continue executing the odometry nonlinear calculation process.

[0071] The method of this invention can restore the initial values ​​of a system in a degenerate motion state where the initialization solution cannot be completed by the monocular visual inertial odometry method itself. The degenerate motion mentioned here refers to the motion state that causes the system initialization process to fail, such as: the initial motion state is uniform linear motion, constant acceleration linear motion, etc.

[0072] To verify the effectiveness of this invention, an urban simulation experiment based on drones and unmanned vehicles is used to verify the algorithm based on actual motion conditions, such as... Figure 5 As shown. First, the configuration file needs to be modified based on the noise parameters calibrated using inertial devices in actual motion. This requires modifying the image resolution, camera intrinsic parameter matrix G, and joint calibration extrinsic parameter matrix. Accelerometer white noise variance σ 1n Accelerometer bias variance σ 1w Angular velocity meter white noise variance σ 2n Angular velocity meter bias variance σ 2w The present invention provides reference parameter settings as shown in Table 1.

[0073] Table 1

[0074]

[0075] Power spectral density is a unit used to describe the size of inertial devices.

[0076] Figure 5 The initial stage of the motion process is represented as follows: two buildings are grouped together and denoted as a single block, with a total block length of d1 meters. A forward movement of d2 meters constitutes the complete motion process. The drone begins its single-axis uniformly accelerated dive motion from an altitude of d3 meters, while the car starts d4 meters before the first block, initially moving at a constant speed, then decelerating uniformly until it stops. Both of these motion scenarios initially exhibit degenerate motion. The invention provides reference settings: d1 is set to 100, d2 to 300, d3 to 150, and d4 to 50.

[0077] The view of the road sign points during the drone's movement is as follows Figure 6 As shown, to simulate a realistic visual scene, overlapping points are set among the road signs in the field of view. The initial motion condition V for the drone... y For v fy m / s, V z For v fz The velocity is m / s, and it moves forward at a uniform linear velocity with a vertical acceleration of... The total motion time is t f Seconds. The present invention provides a reference setting, v fy Set to 15, v fz Set to -10, Set to +0.5, t f Set it to 20.

[0078] The first three parts of the vehicle's movement process are as follows: the visibility of road signs is as follows Figure 7 As shown, analogous to actual drone flight, a shared field of view is also assumed. Initial motion condition V y For v cy m / s, V z For v cz m / s, first undergoing uniform linear motion t c1 After 1 second, it undergoes uniformly decelerated linear motion for t seconds. c2 seconds, acceleration a c m / s 2 The total motion time is t c Seconds. The present invention provides a reference setting, v cy Set to 20, v cz Set to 0, a c Set to -2, t c Set to 20, t c1 Set to 10, t c2 Set it to 10.

[0079] The experiment was repeated ten times under the same noise conditions. The average value of the results in the east, north, and sky directions was calculated and compared with the true value for evaluation. The formula for calculating the probability error (SEP) is shown below, where p... E p N p U p represents the east, north, and celestial position values ​​obtained from the odometer calculation. Egt p Ngt p Ugt This represents the true values ​​of the current east, north, and celestial positions. In the formula, n represents the number of samples, and the image data frequency is f Hz. In this invention, the image data frequency f is set to 30, therefore the maximum value of n within 20 seconds is 600.

[0080]

[0081] Using the reference values ​​of this invention, the statistical results of SEP values ​​obtained from the UAV motion experiment are shown in Table 2:

[0082] Table 2

[0083]

[0084] The statistical results of SEP values ​​obtained from the vehicle motion experiment are shown in Table 3:

[0085] Table 3

[0086]

[0087]

[0088] Based on the statistical data in the table, the following conclusions can be drawn: Using this invention, the initialization process of the odometer can be completed correctly after the speed parameters are input, and the actual motion trajectory of the odometer can be solved with good accuracy.

[0089] The embodiments of the present invention have been described in detail above with reference to the accompanying drawings; however, the present invention is not limited to the above embodiments. Various changes can be made within the scope of knowledge possessed by those skilled in the art without departing from the spirit of the present invention.

Claims

1. A virtual initialization method for monocular vision inertial odometry based on velocity input, characterized in that, Specifically, the steps include the following: Step 1: Set up the simulated carrier. By inputting the initial velocity parameters of the actual carrier during its actual movement, adaptively set the trajectory and landmark positions of the monocular vision inertial odometry virtual initialization process, and set the configuration parameter file for the virtual initialization process and the noise parameters of the inertial sensor. Step 2: Generate the virtual initialization simulation data from Step 1 in the simulation software; match the visual keyframes in the virtual initialization with the visual keyframes in the actual motion process in chronological order; match the inertial sensor data in the virtual initialization with the inertial sensor data in the actual motion process. Step 3: Start the monocular visual inertial odometry calculation method, run the virtual initialization simulation data generated in step 2 within the set virtual initialization time period, obtain the initial values ​​of the state variables of the simulated carrier, and complete the docking of the state variables of the simulated carrier with the state variables of the actual carrier at the end of the virtual initialization, thereby completing the virtual initialization of the monocular visual inertial odometry.

2. The virtual initialization method for monocular vision inertial odometry based on velocity input according to claim 1, characterized in that, The virtual initialization process trajectory generated in step 1 satisfies the following two rules: Rule 1: The initial velocity and attitude angle values ​​of the simulated carrier in the process trajectory are continuous values; Rule 2: The first derivative of the initial velocity of the simulated carrier with respect to time and the first derivative of the attitude angle of the simulated carrier with respect to time are both continuous values.

3. The virtual initialization method for monocular vision inertial odometry based on velocity input according to claim 1, characterized in that, The adaptive setting of the trajectory and landmark position for the monocular vision inertial odometry virtual initialization process in step 1 is specifically as follows: Step 1.1: Initialize the excitation using sine and cosine motion on three axes to form a sine and cosine trajectory. The three axes are the north axis (y) pointing due north, the east axis (x) pointing due east, and the sky axis (z). The expressions for the simulated carrier's position p1(t), velocity v1(t), and acceleration a1(t) during sine and cosine motion are as follows: Where t represents the time variable, A x A y A z Let A and B represent the amplitudes of the sine and cosine motion trajectories along the three axes, respectively. K is the multiple of the angular velocity of the celestial axis motion, t1 represents the preset time for the sine motion, and π(·) is the direct proportional mapping relationship. x =A x0 ·π(V0),A y =A y0 ·π(V0),A z =A z0 ·π(V0), A x0 A y0 and A z0 All represent dimensionless values; where V0 represents the magnitude of the velocity at the initial moment during the actual motion of the carrier, that is, the instantaneous velocity of the actual carrier at the current moment after the virtual initialization calculation is completed; Landmarks in sine and cosine trajectories are set within the field of view during the trajectory. Step 1.2: After completing the sine and cosine motion, the simulated carrier undergoes projectile-like motion, ensuring that the velocity of the simulated carrier at the end of the projectile-like motion is the same as the initial velocity of the actual carrier during its actual motion. The expressions for the carrier's position p2(t), velocity v2(t), and acceleration a2(t) during the projectile-like motion are as follows: Where t2 represents the preset time for projectile-like motion, V y V z These represent the velocity components along the y-axis and z-axis, respectively, and component V y V z The following relationship must be satisfied: In projectile motion, landmarks are placed within the field of vision of the trajectory of the projectile.

4. The virtual initialization method for monocular vision inertial odometry based on velocity input according to claim 3, characterized in that, In step 3, after running the simulation data, the initial values ​​of the state variables of the simulated carrier are obtained. This process involves two state dockings: the first docking is between the state of the simulated carrier at the end of the sine and cosine motion and the initial state of the projectile-like motion; the second docking is between the state of the simulated carrier at the end of the projectile-like motion and the state of the actual carrier in actual motion at the end of virtual initialization. A sliding window is set for each docking process, with k keyframes in a sliding window. During a certain docking, the position, attitude, and velocity of the i-th keyframe in the sliding window are recorded as ps. i Rs i vs i Where i = 1, 2, ..., k; Angle compensation is performed on the position, attitude, and velocity in the i-th keyframe to obtain the compensated position ps. i' Posture Rs i' Speed ​​vs i' for: Rs comp The expression for attitude angle compensation is shown below: Where, δ k This represents the yaw angle value in the velocity direction of the k-th frame in the sliding window.

Images