Vision and inertia fused positioning method, system, equipment and medium

Abstract

The invention discloses a vision and inertia fused positioning method, system and device and a medium, and the method comprises the steps: extracting a first feature point and a line feature from an obtained image frame, and processing the obtained I MU data, so as to obtain the inter-frame pose information between a front frame image and a rear frame image; initializing a visual inertial navigation odometer according to the first feature point and the I MU data to obtain initial pose data; tracking each first feature point to construct a scene flow corresponding to each first feature point according to the inter-frame pose information; filtering dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points; and according to the static feature points and the line features, combining preset point-line feature constraints with the inter-frame pose information to perform tight coupling so as to optimize the initial pose data, and taking the pose data obtained after optimization as a positioning result so as to improve the positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the field of machine vision and positioning navigation technology, and in particular to a positioning method, system, device and medium integrating vision and inertia. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology refers to the process in which a robot equipped with sensors estimates its own motion state during movement and constructs a map of the environment perceived by the sensor in an unknown environment that does not require prior information. Compared with lidar sensors, visual sensors have the advantages of easy integration, low cost, low energy consumption, and rich image information. After decades of development, visual SLAM has become a key technology in many fields such as indoor navigation, robotics, virtual reality, augmented reality, drones, and autonomous driving. As an important component of visual SLAM, visual odometry (VO) or visual inertial odometry (VIO) has been widely studied in academia and industry due to its good balance between cost and accuracy.

[0003] Most existing visual odometry or visual inertial odometry such as MSCKF, OKVI S, VI NS-MONO, and ORB-SLAM3 are based on the assumption of stable lighting conditions and static environments, which means that all landmarks observed by the camera should remain unchanged. However, these assumptions are usually not true in practical applications because dynamic lighting and moving objects are inevitable and ubiquitous in the real world. In complex environments with dynamic lighting and moving objects, visual feature tracking becomes more challenging, which seriously affects the accuracy of positioning.

[0004] Furthermore, sufficiently reliable features are a prerequisite for high-precision positioning of the VIO system. However, the existing visual inertial odometry only extracts point features and does not effectively process dynamic feature points. This causes the VIO system to perform poorly in complex environments such as low texture, drastic lighting changes, and high dynamics, and positioning is easily lost. Summary of the invention

[0005] In order to solve the above technical problems, the present invention discloses a positioning method, system, device and medium integrating vision and inertia, which are used to improve the accuracy of positioning.

[0006] In order to achieve the above-mentioned object, in a first aspect, the present invention discloses a positioning method integrating vision and inertia, comprising:

[0007] Extracting a plurality of first feature points and a plurality of line features from the acquired image frames, and processing the acquired IMU data to obtain inter-frame pose information between the previous and next frame images;

[0008] Initializing a visual inertial navigation odometer according to the first feature point and the IMU data to obtain initial pose data output by the visual inertial navigation odometer;

[0009] Tracking each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information;

[0010] Filtering the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points;

[0011] According to the static feature points and the line features, the preset point and line feature constraints are tightly coupled with the inter-frame pose information to optimize the initial pose data, and the pose data obtained after optimization is used as the positioning result.

[0012] The present invention discloses a positioning method integrating vision and inertia. First, in order to solve the problem of insufficient feature extraction under poor lighting conditions, feature points and line features are extracted simultaneously in the captured image frame to ensure that sufficient features are extracted for positioning and improve the accuracy of positioning. At the same time, when positioning, the acquired IMU data is processed to extract the inter-frame pose information between camera frames, so that the subsequent fusion of IMU data and visual information is performed for positioning, thereby ensuring the accuracy of positioning. In the process of positioning, in order to avoid the influence of dynamic feature points on positioning, a scene flow can be constructed in the binocular image, and the dynamic feature points are detected and removed after being loosely coupled with the IMU, so that the feature points for positioning are only static feature points, thereby ensuring the accuracy of positioning. Finally, when positioning after removing the dynamic feature points, combined with the preset point-line feature constraints and tightly coupled with the inter-frame pose information, it is realized that the visual features and IMU measurements are tightly coupled for nonlinear optimization, and high-precision positioning result output is achieved.

[0013] As a preferred example, extracting a plurality of first feature points and a plurality of line features from the acquired image frame includes:

[0014] Extracting a plurality of Fast feature points and a plurality of LSD line features from the image frame according to a pre-trained feature extractor;

[0015] The preset LBD algorithm is used to perform descriptor matching on the line features between the previous and next frame images.

[0016] The present invention simultaneously extracts point features and line features of an image through a feature extractor to solve the problem of insufficient visual features under weak light conditions, thereby ensuring the accuracy of positioning.

[0017] As a preferred example, the acquired IMU data is processed to obtain inter-frame pose information between previous and next frame images, including:

[0018] Extracting raw linear acceleration data and raw angular acceleration data from the acquired IMU data;

[0019] Performing median method discrete integration processing on the original linear acceleration data and the original angular acceleration data respectively to obtain processed linear acceleration data and processed angular acceleration data;

[0020] Pre-integration processing is performed on the linear acceleration data and the angular acceleration data to obtain relative translation data, velocity data and rotation data between two consecutive camera frames.

[0021] After extracting the linear velocity and angular velocity data from the acquired IMU data, the present invention processes the linear velocity and angular velocity data using median method discrete integration based on the discrete sampling of linear acceleration and angular acceleration, so as to improve the accuracy of obtaining the posture information between camera frames, thereby ensuring the accuracy of positioning.

[0022] As a preferred example, the initializing the visual inertial navigation odometer according to the first feature point and the IMU data includes:

[0023] Estimate the initial camera pose according to the 2D pixel coordinates of the Fast feature points of each image frame and the corresponding matched 3D map points;

[0024] Obtaining motion measurement information and gravity vector information of the IMU from the IMU data;

[0025] The initial camera pose, the 2D pixel coordinates, the motion measurement information and the gravity vector information are integrated to achieve alignment of vision and IMU.

[0026] The present invention initializes the visual inertial navigation odometer to obtain initial pose information. During the initialization process, in order to achieve alignment between vision and IMU, it is first necessary to obtain the initial pose of the camera, and then fuse the initial pose with the motion measurement and gravity vector information of IMU to achieve the spatiotemporal consistency of the two sensor data, and then achieve the coupling of vision and IMU for positioning.

[0027] As a preferred example, the tracking of each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information includes:

[0028] Tracking each of the first feature points using the LK optical flow method to obtain the optical flow of each of the first feature points in the image frame;

[0029] According to the optical flow and the inter-frame pose information between the previous and next frame images, each of the first feature points in the camera frame coordinate system is transformed into the world coordinate system to construct a scene flow corresponding to each of the first feature points.

[0030] The present invention uses the optical flow method to track feature points to obtain the movement of the feature points between different images, and then determines whether they are dynamic feature points by performing motion analysis. Further, when obtaining the attributes of the feature points, just as the optical flow is the two-dimensional motion field of the points in the image, any optical flow is the projection of the scene flow on the camera imaging plane. Therefore, in order to further identify whether the feature points are dynamic, it is necessary to construct the scene flow for identification.

[0031] As a preferred example, the filtering of the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points includes:

[0032] Obtaining a three-dimensional motion vector of the scene flow, and calculating a vector modulus of the three-dimensional motion vector;

[0033] When the vector modulus is less than or equal to a preset empirical threshold, the first feature point corresponding to the current scene flow is retained as the static feature point.

[0034] The present invention is based on the characteristics that the vector modulus corresponding to the three-dimensional motion vector generated by the scene flow of dynamic feature points will be much greater than zero, while the vector modulus corresponding to the three-dimensional motion vector generated by the scene flow of static feature points will be infinitely close to zero. By calculating the vector modulus corresponding to each feature point, the dynamic feature points are deleted from the feature points, thereby retaining only the static feature points for subsequent positioning, thereby improving the accuracy of positioning.

[0035] As a preferred example, the combining of the preset point-line feature constraints and the inter-frame pose information for tight coupling to optimize the initial pose data includes:

[0036] According to the static feature points and the line features, constructing point feature reprojection residuals and line feature reprojection residuals respectively;

[0037] Inputting the image frame into a pre-constructed sliding window, optimizing a preset objective function in the sliding window with the goal of minimizing the sum of the point feature reprojection residual, the line feature reprojection residual, the pre-constructed marginalization residual, and the pre-constructed IMU measurement residual;

[0038] The initial posture data is optimized according to the optimization result of the objective function.

[0039] The present invention introduces point feature reprojection residual terms and line feature reprojection residual terms on the basis of pre-constructed marginalization residuals and IMU measurement residuals, and achieves high-precision and robust positioning result output by tightly coupling visual features and IMU measurements for nonlinear optimization.

[0040] In a second aspect, the present invention discloses a positioning system integrating vision and inertia, including a data processing module, a posture initialization module, a point tracking module, a point filtering module and a positioning optimization module;

[0041] The data processing module is used to extract a plurality of first feature points and a plurality of line features from the acquired image frames, and process the acquired IMU data to obtain inter-frame pose information between the previous and next frame images;

[0042] The posture initialization module is used to initialize the visual inertial navigation odometer according to the first feature point and the IMU data to obtain initial posture data output by the visual inertial navigation odometer;

[0043] The point tracking module is used to track each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information;

[0044] The point filtering module is used to filter the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points;

[0045] The positioning optimization module is used to optimize the initial pose data according to the static feature points and the line features, in combination with preset point and line feature constraints and tightly coupled with the inter-frame pose information, and use the optimized pose data as the positioning result.

[0046] The present invention discloses a positioning system integrating vision and inertia. First, in order to solve the problem of insufficient feature extraction under poor lighting conditions, feature points and line features are extracted simultaneously in the captured image frame to ensure that sufficient features are extracted for positioning and improve the accuracy of positioning. At the same time, when positioning, the acquired IMU data is processed to extract the inter-frame pose information between camera frames, so that the subsequent fusion of IMU data and visual information is performed for positioning, thereby ensuring the accuracy of positioning. In the process of positioning, in order to avoid the influence of dynamic feature points on positioning, a scene flow can be constructed in the binocular image, and the dynamic feature points are detected and removed after being loosely coupled with the IMU, so that the feature points for positioning are only static feature points, thereby ensuring the accuracy of positioning. Finally, when positioning after removing the dynamic feature points, combined with the preset point-line feature constraints and tightly coupled with the inter-frame pose information, it is realized that the visual features and IMU measurements are tightly coupled for nonlinear optimization, and high-precision positioning result output is achieved.

[0047] In a third aspect, the present invention discloses an electronic device, comprising:

[0048] At least one processor; and a memory communicatively connected to at least one of the processors; wherein the memory stores instructions executable by at least one of the processors, and the instructions are executed by at least one of the processors so that at least one of the processors can execute a positioning method that integrates vision and inertia as described in the first aspect.

[0049] In a fourth aspect, the present invention discloses a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute a positioning method that integrates vision and inertia as described in the first aspect. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 : A flowchart of a positioning method integrating vision and inertia disclosed in an embodiment of the present invention;

[0051] Figure 2 : A structural diagram of a positioning system integrating vision and inertia disclosed in an embodiment of the present invention;

[0052] Figure 3 : A flowchart of a positioning method integrating vision and inertia disclosed in another embodiment of the present invention;

[0053] Figure 4 : A schematic diagram of a vision and IMU alignment process disclosed in another embodiment of the present invention;

[0054] Figure 5 : A schematic diagram of a scene flow construction process disclosed in another embodiment of the present invention. DETAILED DESCRIPTION

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

[0056] Embodiment 1

[0057] This embodiment discloses a positioning method that integrates vision and inertia. For the specific implementation process of the positioning method, please refer to Figure 1 , mainly including steps 101 to 105, wherein the steps are mainly:

[0058] Step 101: extract a plurality of first feature points and a plurality of line features from the acquired image frame, and process the acquired IMU data to obtain inter-frame pose information between the previous and next frame images.

[0059] In this embodiment, the steps mainly include: extracting multiple Fast feature points and multiple LSD line features from the image frame according to a pre-trained feature extractor; and performing descriptor matching on the line features between the previous and next frame images through a preset LBD algorithm.

[0060] At the same time, the original linear acceleration data and the original angular acceleration data are extracted from the acquired IMU data; the original linear acceleration data and the original angular acceleration data are respectively subjected to median method discrete integration processing to obtain processed linear acceleration data and processed angular acceleration data; the linear acceleration data and the angular acceleration data are pre-integrated to obtain relative translation data, velocity data and rotation data between two consecutive camera frames.

[0061] In this embodiment, this step simultaneously extracts the point features and line features of the image through a feature extractor to solve the problem of insufficient visual features under weak light conditions, thereby ensuring the accuracy of positioning. At the same time, after extracting the linear velocity and angular velocity data from the acquired IMU data, based on the linear acceleration and angular acceleration being discrete sampling, the linear velocity and angular velocity data are processed using the median method discrete integration to improve the accuracy of obtaining the pose information between camera frames, thereby ensuring the accuracy of positioning.

[0062] Step 102: Initialize the visual inertial navigation odometer according to the first feature point and the IMU data to obtain initial pose data output by the visual inertial navigation odometer.

[0063] In this embodiment, the steps are mainly as follows: estimating the initial pose of the camera according to the 2D pixel coordinates of the Fast feature points of each image frame and the corresponding matching 3D map points; obtaining the motion measurement information and gravity vector information of the IMU from the IMU data; fusing the initial pose of the camera, the 2D pixel coordinates, the motion measurement information and the gravity vector information to achieve alignment of vision and IMU.

[0064] In this embodiment, this step initializes the visual inertial navigation odometer to obtain initial pose information. During the initialization process, in order to achieve alignment between vision and IMU, it is necessary to first obtain the initial pose of the camera, and then fuse the initial pose with the motion measurement and gravity vector information of IMU to achieve the spatiotemporal consistency of the two sensor data, and then achieve the coupling of vision and IMU for positioning.

[0065] Step 103: Track each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information.

[0066] In this embodiment, this step mainly includes: using the LK optical flow method to track each of the first feature points to obtain the optical flow of each of the first feature points in the image frame; according to the optical flow and the inter-frame pose information between the previous and next frame images, each of the first feature points in the camera frame coordinate system is transformed into the world coordinate system to construct the scene flow corresponding to each of the first feature points.

[0067] In this embodiment, this step uses the optical flow method to track feature points to obtain the movement of the feature points between different images, and then determines whether they are dynamic feature points by performing motion analysis. Further, when obtaining the attributes of the feature points, just as the optical flow is the two-dimensional motion field of the points in the image, any optical flow is the projection of the scene flow on the camera imaging plane. Therefore, in order to further identify whether the feature points are dynamic, it is necessary to construct the scene flow for identification.

[0068] Step 104: Filter the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points.

[0069] In this embodiment, this step mainly includes: obtaining the three-dimensional motion vector of the scene flow and calculating the vector modulus of the three-dimensional motion vector; when the vector modulus is less than or equal to a preset empirical threshold, retaining the first feature point corresponding to the current scene flow as the static feature point.

[0070] In this embodiment, this step is based on the characteristics that the vector modulus corresponding to the three-dimensional motion vector generated by the scene flow of the dynamic feature point will be much greater than zero, while the vector modulus corresponding to the three-dimensional motion vector generated by the scene flow of the static feature point will be infinitely close to zero. By calculating the vector modulus corresponding to each feature point, the dynamic feature points are deleted from the feature points, thereby retaining only the static feature points for subsequent positioning, thereby improving the accuracy of positioning.

[0071] Step 105: Based on the static feature points and the line features, the preset point and line feature constraints are tightly coupled with the inter-frame pose information to optimize the initial pose data, and the pose data obtained after optimization is used as the positioning result.

[0072] In this embodiment, the steps are mainly as follows: constructing point feature reprojection residuals and line feature reprojection residuals respectively according to the static feature points and the line features; inputting the image frame into a pre-constructed sliding window, optimizing the objective function preset in the sliding window with the goal of minimizing the sum of the point feature reprojection residuals, the line feature reprojection residuals, the pre-constructed marginalization residuals, and the pre-constructed IMU measurement residuals; optimizing the initial pose data according to the optimization result of the objective function.

[0073] In this embodiment, this step introduces point feature reprojection residual terms and line feature reprojection residual terms based on the pre-constructed marginalization residual and IMU measurement residual, and achieves high-precision and robust positioning result output by tightly coupling the visual features and IMU measurements for nonlinear optimization.

[0074] On the other hand, this embodiment also discloses a positioning system that integrates vision and inertia. For the specific structure of the positioning system, please refer to Figure 2 , including a data processing module 201, a posture initialization module 202, a point tracking module 203, a point filtering module 204 and a positioning optimization module 205.

[0075] The data processing module 201 is used to extract a plurality of first feature points and a plurality of line features from the acquired image frames, and process the acquired IMU data to obtain inter-frame pose information between the previous and next frame images;

[0076] The posture initialization module 202 is used to initialize the visual inertial navigation odometer according to the first feature point and the IMU data to obtain initial posture data output by the visual inertial navigation odometer;

[0077] The point tracking module 203 is used to track each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information;

[0078] The point filtering module 204 is used to filter the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points;

[0079] The positioning optimization module 205 is used to optimize the initial pose data according to the static feature points and the line features, in combination with the preset point and line feature constraints and the inter-frame pose information, and use the optimized pose data as the positioning result.

[0080] According to Figure 1The positioning method of integrating vision and inertia shown in the embodiment, accordingly, this embodiment provides an electronic device, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, and when the processor executes the computer program, the following is implemented Figure 1 The positioning method of integrating vision and inertia shown in the figure. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of the terminal, and various interfaces and lines are used to connect various parts of the entire terminal. The memory may be used to store the computer program, and the processor implements various functions of the terminal by running or executing the computer program stored in the memory and calling the data stored in the memory. The memory may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function (such as a sound playback function, an image playback function, etc.), etc.; the data storage area may store data created according to the use of the mobile phone (such as audio data, a phone book, etc.), etc. In addition, the memory may include a high-speed random access memory, and may also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Med ia Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0081] Furthermore, this embodiment also provides a computer-readable storage medium, wherein the computer-readable storage medium includes a stored computer program, wherein when the computer program is executed, the device where the computer-readable storage medium is located is controlled to execute the following Figure 1 The positioning method that integrates vision and inertia is shown.

[0082] Among them, Figure 2If the fusion vision and inertial positioning system shown is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above-mentioned embodiment method can also be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned various method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electric carrier signal, telecommunication signal and software distribution medium, etc.

[0083] Compared with the prior art, this embodiment provides the following beneficial effects:

[0084] The present embodiment discloses a method and system for positioning by integrating vision and inertia. First, in order to solve the problem of insufficient feature extraction under poor lighting conditions, feature points and line features are extracted simultaneously in the captured image frames to ensure that sufficient features are extracted for positioning and improve the accuracy of positioning. At the same time, when positioning, the acquired IMU data is processed to extract inter-frame pose information between camera frames, so that the subsequent fusion of IMU data and visual information is performed for positioning, thereby ensuring the accuracy of positioning. In the process of positioning, in order to avoid the influence of dynamic feature points on positioning, a scene flow can be constructed in the binocular image, and the dynamic feature points can be removed after being loosely coupled with the IMU, so that the feature points for positioning are only static feature points, thereby ensuring the accuracy of positioning. Finally, when positioning after removing the dynamic feature points, combined with the preset point-line feature constraints and tightly coupled with the inter-frame pose information, it is achieved by tightly coupling the visual features and IMU measurements for nonlinear optimization, and achieving high-precision positioning result output.

[0085] Embodiment 2

[0086] With the development of science and technology, extracting sufficiently reliable features is an important prerequisite for the VIO system to achieve high-precision positioning. However, the existing visual inertial odometer only extracts point features and does not effectively process dynamic feature points. This causes the VIO system to perform poorly in complex environments such as low texture, drastic lighting changes, and high dynamics, and positioning is easily lost.

[0087] In view of the above problems, this embodiment proposes a binocular vision IMU fusion positioning method based on loose coupling and tight coupling. The method extracts two features, point features and line features, from the binocular image, and makes full use of the IMU loosely coupled binocular scene flow to eliminate dynamic feature points. Finally, high-precision positioning is achieved by using the vision IMU tightly coupled nonlinear optimization method.

[0088] Specifically, please refer to the specific implementation process of the fusion positioning method. Figure 3 , mainly including steps 301 to 305, wherein the steps are mainly:

[0089] Step 301: Extract Fast feature points and LSD line features from the acquired binocular image data simultaneously, and perform pre-integration processing on the IMU data obtained by the IMU to obtain the inter-frame pose information between two consecutive camera frames.

[0090] In this embodiment, this step mainly includes: extracting multiple Fast feature points and multiple LSD line features from the image frame according to a pre-trained feature extractor, and extracting original linear acceleration data and original angular acceleration data from the acquired IMU data, so as to process the original linear acceleration data and the original angular acceleration data to obtain relative translation data, velocity data and rotation data between two consecutive camera frames.

[0091] Specifically, after completing image acquisition by the left camera and the right camera to obtain binocular image data and performing data acquisition through the IMU (inertial sensor) to obtain IMU data, first, for the binocular image data, the corresponding Fast feature points and LSD line features can be extracted from the binocular image data through the Fast feature point extraction algorithm and the LSD line feature extraction algorithm.

[0092] Preferably, in some implementations of this embodiment, the OpenCV library is called to construct a feature extractor to simultaneously extract Fast feature points and LSD line features, wherein the LK optical flow method can be used to track point features, and the LBD line feature matching can be used to perform descriptor matching on line features of previous and next frames.

[0093] At the same time, the linear acceleration and angular acceleration obtained by the IMU are pre-integrated to obtain the relative translation, velocity and rotation between two consecutive camera frames. The pre-integration formula is as follows:

[0094]

[0095] in, and Respectively represent the coordinates from the IMU coordinate system b i to b j The relative translation, velocity and rotation of k.i ,t j ] at a certain moment, δt represents b i to b j The time interval between. and They represent the linear acceleration and angular velocity obtained from the original measurement, and denote the Gaussian white noise terms of the accelerometer and gyroscope measurements, respectively, and Represents the bias of the accelerometer and gyroscope.

[0096] Preferably, in the process of obtaining the relative translation, velocity and rotation through the linear acceleration and angular acceleration, since the IMU data, i.e., the linear acceleration and angular acceleration, are discretely sampled, the posture from two adjacent moments k to k+1 of the IMU data acquisition can be processed by discrete integration using the median method, that is, the average values ​​of the gyroscope and the accelerometer at moments k and k+1 are used for calculation, specifically:

[0097]

[0098]

[0099] Where Δt represents the time interval between the I MU data collection times k and k+1. and White Gaussian noise term representing the gyroscope and accelerometer bias variations.

[0100] Step 302: Initialize the VIO system to obtain initial pose data output by the VIO system.

[0101] In this embodiment, this step mainly includes: estimating the initial camera pose according to the 2D pixel coordinates of the Fast feature points of each image frame and the corresponding matching 3D map points, and fusing the initial camera pose, the 2D pixel coordinates with the motion measurement information and gravity vector information in the IMU data to achieve alignment of vision and IMU.

[0102] Preferably, after obtaining the original data, i.e., the image data and the IMU data, using the binocular vision sensor and the IMU sensor and processing them, the VIO (visual inertial odometry) system is initialized first to perform initial positioning through the initialized VIO system.

[0103] Specifically, the initialization of the VIO system includes two parts: point feature PNP and visual I MU alignment. Figure 4As shown, the initial camera pose is estimated based on the matched 3D map points and the 2D pixel coordinates of the visual feature points of the current image frame, and then the visual IMU alignment is performed, that is, the initial pose and feature point information provided by the vision are integrated with the motion measurement and gravity vector information of the IMU to achieve the spatiotemporal consistency of the two sensor data. After completing the initialization, the VIO system outputs the current initial positioning result.

[0104] Step 303: Track each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information.

[0105] In this embodiment, this step mainly includes: using the LK optical flow method to track each of the first feature points to obtain the optical flow of each of the first feature points in the image frame; according to the optical flow and the inter-frame pose information between the previous and next frame images, each of the first feature points in the camera frame coordinate system is transformed into the world coordinate system to construct the scene flow corresponding to each of the first feature points.

[0106] Specifically, after the VIO system is initialized, a scene stream corresponding to each Fast feature point is constructed in the visual image, so that the dynamic Fast feature points can be detected and removed by loosely coupling the visual scene stream with the IMU information.

[0107] Preferably, the scene flow is a three-dimensional motion field of points in the world coordinate system, describing the motion of each three-dimensional point. Just as the optical flow is a two-dimensional motion field of points in the image, any optical flow is the projection of the scene flow on the camera imaging plane. Figure 5 As shown, to facilitate the calculation of scene flow, assuming that the origin of the world frame coordinate system coincides with the camera frame cj coordinate system (both use right-handed coordinate systems), the camera frame c i m three-dimensional points in the coordinate system Through the camera frame pose Transform to world coordinate system c j To construct the scene flow of each 3D point, the coordinate system changes as follows:

[0108]

[0109] in It is the camera IMU external parameter, which is obtained by joint calibration between the camera and IMU before the VIO system runs.

[0110] Step 304: Calculate the vector modulus corresponding to the scene flow to filter static feature points from the Fast feature points according to the vector modulus.

[0111] In this embodiment, this step mainly includes: when the IMU pre-integration between two camera frames, that is, the transformation matrix obtained by the inter-frame position information When accurate, the vector modulus of the scene flow of a static 3D point should be zero or close to zero.

[0112] The scene flow of dynamic feature points generates a three-dimensional motion vector, and the vector modulus is much greater than zero. The dynamic points are judged based on the empirical threshold and the dynamic points are eliminated, wherein the feature points with state s=0 are eliminated and the feature points with state s=1 are retained. Specifically, the feature point screening process is as follows:

[0113]

[0114]

[0115] Among them, the is the I MU pre-integration between two camera frames, is the camera frame c i Three-dimensional point in the coordinate system, is the camera frame pose.

[0116] Step 305: Input the static feature points, the line features and the IMU data into a pre-constructed sliding window, so as to tightly couple the vision and IMU data in the sliding window through a preset nonlinear optimization algorithm, thereby optimizing the initial pose data.

[0117] In this embodiment, the steps are mainly as follows: first, construct marginalization residuals, IMU measurement residuals, point feature reprojection residuals, and line feature reprojection residuals, wherein the point feature reprojection residuals are constructed using static feature points.

[0118] Secondly, after constructing the residual term, a nonlinear optimization algorithm is used to tightly couple the visual and IMU data in the sliding window to achieve the optimal estimation of each state variable in the sliding window. Among them, the sliding window method is used to maintain a certain number of state variables (generally set to 10) under a fixed window size, and the old state variables are continuously replaced with new ones, which better balances accuracy and real-time performance.

[0119] Preferably, the nonlinear optimization formula in the sliding window is as follows:

[0120]

[0121] in It is all the variables to be optimized in the current sliding window, including the position, velocity, rotation, zero bias, external parameters and time offset of IMU for each key frame in the window. is the relative pose measurement obtained from I MU, is the relative pose measurement obtained from the visual point features, is the relative pose measurement obtained from the visual line feature. p is the marginalized prior residual, J p represents the marginalized measurement estimation matrix, is the I MU measurement residual, is the point feature reprojection residual, is the line feature reprojection residual, and ρ is the loss function used to resist the influence of outliers.

[0122] When solving the problem, the Ceres Solver optimizer is called to calculate the optimal variable value that minimizes the sum of squared residuals, thereby outputting high-precision positioning results.

[0123] The present embodiment proposes a binocular vision IMU fusion positioning method based on loose coupling and tight coupling, which can output positioning results in complex scenes with high precision, robustness and real-time. This method solves the problem of insufficient visual features under weak lighting conditions by extracting point features and line features in binocular vision images at the same time. For dynamic feature points appearing in high dynamic environments, a scene flow is constructed in the binocular image, and the dynamic feature points are removed after being loosely coupled with IMU to detect them. When constructing the optimization problem at last, the line feature reprojection residual term is introduced on the basis of the prior residual term, the IMU measurement residual term and the point feature reprojection residual term. By tightly coupling the visual features and IMU measurements for nonlinear optimization, high-precision and robust positioning result output is achieved, which provides strong technical support for the autonomous navigation of intelligent devices such as self-driving cars, drones, and mobile robots.

[0124] The specific embodiments described above further illustrate the purpose, technical solutions and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. It is particularly pointed out that for those skilled in the art, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the scope of protection of the present invention.

Claims

1. A positioning method integrating vision and inertia, characterized in that: include: Extracting a plurality of first feature points and a plurality of line features from the acquired image frames, and processing the acquired IMU data to obtain inter-frame pose information between the previous and next frame images; Initializing a visual inertial navigation odometer according to the first feature point and the IMU data to obtain initial pose data output by the visual inertial navigation odometer; Tracking each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information; Filtering the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points; According to the static feature points and the line features, the preset point and line feature constraints are tightly coupled with the inter-frame pose information to optimize the initial pose data, and the pose data obtained after optimization is used as the positioning result.

2. A positioning method integrating vision and inertia according to claim 1, characterized in that: Extracting a plurality of first feature points and a plurality of line features from the acquired image frame includes: Extracting a plurality of Fast feature points and a plurality of LSD line features from the image frame according to a pre-trained feature extractor; The preset LBD algorithm is used to perform descriptor matching on the line features between the previous and next frame images.

3. The positioning method integrating vision and inertia according to claim 1, characterized in that: The acquired IMU data is processed to obtain inter-frame pose information between the previous and next frame images, including: Extracting raw linear acceleration data and raw angular acceleration data from the acquired IMU data; Performing median method discrete integration processing on the original linear acceleration data and the original angular acceleration data respectively to obtain processed linear acceleration data and processed angular acceleration data; Pre-integration processing is performed on the linear acceleration data and the angular acceleration data to obtain relative translation data, velocity data and rotation data between two consecutive camera frames.

4. The method for integrating vision and inertia according to claim 2, characterized in that: Initializing the visual inertial navigation odometer according to the first feature point and the IMU data includes: Estimate the initial camera pose according to the 2D pixel coordinates of the Fast feature points of each image frame and the corresponding matched 3D map points; Acquire motion measurement information and gravity vector information of the IMU from the IMU data; The initial camera pose, the 2D pixel coordinates, the motion measurement information and the gravity vector information are integrated to achieve alignment of vision and IMU.

5. The positioning method integrating vision and inertia according to claim 4, characterized in that: The tracking of each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information includes: Tracking each of the first feature points using the LK optical flow method to obtain the optical flow of each of the first feature points in the image frame; According to the optical flow and the inter-frame pose information between the previous and next frame images, each of the first feature points in the camera frame coordinate system is transformed into the world coordinate system to construct a scene flow corresponding to each of the first feature points.

6. The method for integrating vision and inertia according to claim 5, characterized in that: The filtering of the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points includes: Obtaining a three-dimensional motion vector of the scene flow, and calculating a vector modulus of the three-dimensional motion vector; When the vector modulus is less than or equal to a preset empirical threshold, the first feature point corresponding to the current scene flow is retained as the static feature point.

7. The method for positioning by integrating vision and inertia according to claim 1, characterized in that: The step of tightly coupling the preset point-line feature constraint with the inter-frame pose information to optimize the initial pose data includes: According to the static feature points and the line features, constructing point feature reprojection residuals and line feature reprojection residuals respectively; Input the image frame into a pre-constructed sliding window, and optimize a preset objective function in the sliding window with the goal of minimizing the sum of the point feature reprojection residual, the line feature reprojection residual, the pre-constructed marginalization residual, and the pre-constructed IMU measurement residual; The initial posture data is optimized according to the optimization result of the objective function.

8. A positioning system integrating vision and inertia, characterized in that: It includes data processing module, posture initialization module, point tracking module, point filtering module and positioning optimization module; The data processing module is used to extract a plurality of first feature points and a plurality of line features from the acquired image frames, and process the acquired IMU data to obtain inter-frame pose information between the previous and next frame images; The posture initialization module is used to initialize the visual inertial navigation odometer according to the first feature point and the IMU data to obtain initial posture data output by the visual inertial navigation odometer; The point tracking module is used to track each of the first feature points to construct a scene flow corresponding to each of the first feature points according to the inter-frame pose information; The point filtering module is used to filter the dynamic feature points in the first feature points according to the scene flow to obtain a plurality of static feature points; The positioning optimization module is used to optimize the initial pose data according to the static feature points and the line features, in combination with preset point and line feature constraints and tightly coupled with the inter-frame pose information, and use the optimized pose data as the positioning result.

9. An electronic device, characterized in that: include: at least one processor; And a memory communicatively connected to at least one of the processors; wherein the memory stores instructions executable by at least one of the processors, and the instructions are executed by at least one of the processors so that at least one of the processors can execute a positioning method integrating vision and inertia as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute a positioning method integrating vision and inertia according to any one of claims 1 to 7.

Images