Visual fusion positioning navigation method and device

Abstract

The invention discloses a visual fusion positioning navigation method and device. The method comprises the following steps: extracting feature points and descriptors thereof from a preprocessed image; feature point matching is carried out between the left image and the right image of the binocular camera by using the descriptor, and three-dimensional point cloud data of the environment is recovered according to a matching result; reading IMU data acquired by an IMU sensor carried on the mobile equipment in real time; predicting the pose of the binocular camera by using the IMU data, and updating the feature point position of the current frame according to the prediction result; fusing the updated feature point positions with the three-dimensional point cloud data to construct a global map; and determining the current position of the mobile equipment, planning a path according to the target position and the current position, and carrying out positioning navigation on the mobile equipment according to the planned path. The problem of how to provide more reliable data support for positioning and navigation of mobile equipment to improve the precision of positioning and navigation on the premise of not increasing the complexity and cost of the system is solved.

Description

Technical Field

[0001] The present application relates to the field of positioning and navigation technology, and in particular to a visual fusion positioning and navigation method and device. Background Art

[0002] In the field of positioning and navigation, binocular cameras and IMU sensors are two important data sources. Binocular cameras can collect three-dimensional information about the environment, providing a rich data source for positioning and navigation. IMU sensors can obtain the posture information of the device in real time, which helps maintain the stability of the system when image information is missing or unreliable.

[0003] However, binocular cameras are easily affected by factors such as lighting and occlusion when collecting images, resulting in inaccurate feature point extraction and matching. IMU sensors will produce cumulative errors after long-term use, affecting the accuracy of positioning and navigation.

[0004] To solve these problems, existing technologies correct the accumulated errors of IMU by fusing other sensor data. However, existing technologies require additional hardware support, which increases the complexity and cost of the system.

[0005] Therefore, how to provide more reliable data support for the positioning and navigation of mobile devices without increasing system complexity and cost, so as to improve the accuracy of positioning and navigation, has become a problem that needs to be solved urgently. Summary of the invention

[0006] In the embodiments of the present application, a visual fusion positioning and navigation method and device are provided to solve the problem of how to provide more reliable data support for the positioning and navigation of mobile devices to improve the accuracy of positioning and navigation without increasing the complexity and cost of the system.

[0007] In a first aspect, an embodiment of the present application provides a visual fusion positioning and navigation method, the method comprising: using a binocular camera mounted on a mobile device to capture environmental images, and preprocessing the captured images; extracting feature points and their descriptors from the preprocessed images; using the descriptors to match feature points between the left and right images of the binocular camera, and restoring three-dimensional point cloud data of the environment based on the matching results; reading IMU data acquired by an IMU sensor mounted on the mobile device in real time, including acceleration and angular velocity information; using the IMU data to predict the position and posture of the binocular camera, and updating the feature point position of the current frame based on the prediction results; fusing the updated feature point position with the three-dimensional point cloud data to construct a global map; determining the current position of the mobile device based on the global map and the current position and posture of the binocular camera, and planning a path based on the target position and the current position, and positioning and navigating the mobile device according to the planned path.

[0008] In a possible implementation, the preprocessing of the captured image includes: obtaining a grayscale histogram of the captured image; calculating the mean of the grayscale histogram, and if the mean is less than a first preset threshold, judging that the image is too dark; calculating the standard deviation of the grayscale histogram, and if the standard deviation is less than a second preset threshold, judging that the image has insufficient contrast; if the image is judged to be too dark or the contrast is insufficient, adjusting the brightness or contrast of the image.

[0009] In a possible implementation, the method of extracting feature points and descriptors from a preprocessed image includes: calculating the gradient amplitude and direction of the preprocessed image to obtain gradient information; calculating the corner point response value of each pixel of the image according to the gradient information; considering the pixel points whose corner point response values ​​are greater than a third preset threshold as potential feature points; retaining only the feature points with the largest corner point response values ​​in a local area as target feature points; for each target feature point, selecting an image block of a fixed size around it as a neighborhood; randomly selecting a first preset number of pixel points in the neighborhood; sampling each pixel point to obtain a sampling point; recording the index of the direction interval to which the gradient direction of each sampling point belongs and the relative size of the gradient amplitude; constructing a fixed-length descriptor vector to extract the descriptor; wherein each element of the descriptor vector corresponds to the accumulation of the gradient amplitude of the sampling point in a direction interval.

[0010] In a possible implementation, the method uses IMU data to predict the posture of the binocular camera and updates the feature point position of the current frame according to the prediction result, including: defining the posture of the binocular camera and constructing a dynamic model to describe the change of the posture of the binocular camera over time; discretizing the dynamic model using the fourth-order Runge-Kutta method, predicting the posture of the binocular camera at the next moment, and obtaining the prediction result; using three-dimensional point back projection to convert the feature points of the current frame back to points in the three-dimensional space; using the prediction result, using coordinate system transformation, transforming the points in the three-dimensional space from the camera coordinate system at the current moment to the camera coordinate system at the next moment; using the reprojection equation, projecting the transformed points in the three-dimensional space back to the two-dimensional image plane to obtain updated feature point coordinates to update the feature point position of the current frame.

[0011] In a possible implementation, the position and posture of the binocular camera is defined as: ;in, is the position of the binocular camera at the current moment, which is used to describe the rigid body transformation in three-dimensional space. is the set of all rigid body transformations, for The three-dimensional rotation orthogonal matrix of for The column vector represents the position of the origin of the camera coordinate system in the world coordinate system. for The row vector of is the homogeneous term of homogeneous coordinates; the dynamic model is: ;in, is the position of the binocular camera at the current moment The time derivative of represents the rate of change of the position of the binocular camera over time. For expansion element, , is the angular velocity The skew-symmetric matrix is , , is the three-dimensional acceleration vector, is the angular velocity exist The weight on the axis, is the angular velocity exist The components on the axis; the expression for discretizing the dynamic model using the fourth-order Runge-Kutta method is: ;in, To predict the position of the binocular camera at the next moment, take it as the prediction result. is the time interval from the current moment to the next moment, The function is the matrix exponential, , , and is the slope term, , , , , An extension of the current binocular camera element, Extension of the binocular camera in the middle element, Extension of the binocular camera before the next moment Elements; the expression for the back projection of a three-dimensional point is: ;in, is a point in three-dimensional space, is the back projection function, is the feature point coordinate of the current frame, is the depth value corresponding to the feature point, is the inverse matrix of the internal parameters of the binocular camera, To expand the two-dimensional coordinates into homogeneous coordinate form; the expression of coordinate system transformation is: ;in, is the point in the transformed three-dimensional space, is the predicted position of the binocular camera at the next moment, is the inverse matrix of the binocular camera's position at the current moment; the reprojection equation is: ;in, is the updated feature point coordinates of the current frame, is the projection function, are the first three components of the point in the transformed three-dimensional space, is the homogeneous coordinate component of the point in the transformed three-dimensional space, is the intrinsic parameter matrix of the binocular camera.

[0012] In a possible implementation, the updated feature point positions are merged with the three-dimensional point cloud data to construct a global map, including: extracting descriptors from the feature points of each frame after the update; generating a global point cloud map using the three-dimensional point cloud data; in the global point cloud map, for each extracted descriptor, searching for a second preset number of candidate points closest to it; calculating the similarity between each extracted descriptor and the descriptor of the candidate point, and retaining matching pairs whose similarity is less than a preset similarity threshold; the similarity calculation formula is: ;in, is the similarity between the extracted descriptor and the descriptor of the candidate point, is the extracted descriptor, is the descriptor of the candidate point, is the Euclidean distance; using the predicted position of the binocular camera at the next moment, the expression for projecting the candidate point from the three-dimensional space to the two-dimensional image plane of the current frame is: ;in, is the candidate point, The candidate point is projected to the current frame through the predicted position of the binocular camera at the next moment; the reprojection error is calculated, and abnormal matching pairs whose reprojection error is greater than the pixel error threshold are eliminated; the calculation formula of the reprojection error is: ;in, is the reprojection error, are the updated feature point coordinates of the current frame; the retained matching pairs are fused with the candidate points in the corresponding 3D point cloud data to construct a global map.

[0013] In a possible implementation, the current position of the mobile device is determined based on the global map and the current position of the binocular camera, and a path is planned based on the target position and the current position, and the positioning and navigation of the mobile device is performed according to the planned path, including: dividing the global map into multiple grid cells, each grid cell represents a spatial position that can be occupied by the mobile device; marking inaccessible grid cells according to the location of obstacles; the goal of path planning is to find a path that can bypass all inaccessible grid cells and make the sum of the Euclidean distances between adjacent grid cells passed through reach the shortest path.

[0014] In the second aspect, an embodiment of the present application provides a visual fusion positioning and navigation device, including: a preprocessing module, which is used to use a binocular camera mounted on a mobile device to collect environmental images and preprocess the collected images; an extraction module, which is used to extract feature points and their descriptors from the preprocessed images; a matching module, which is used to use descriptors to match feature points between the left and right images of the binocular camera, and restore the three-dimensional point cloud data of the environment according to the matching results; a reading module, which is used to read the IMU data acquired by the IMU sensor mounted on the mobile device in real time, including acceleration and angular velocity information; a prediction module, which is used to use the IMU data to predict the posture of the binocular camera, and update the feature point position of the current frame according to the prediction result; a construction module, which is used to fuse the updated feature point position with the three-dimensional point cloud data to construct a global map; a positioning and navigation module, which is used to determine the current position of the mobile device according to the global map and the current posture of the binocular camera, and plan a path according to the target position and the current position, and perform positioning and navigation of the mobile device according to the planned path.

[0015] In a third aspect, an embodiment of the present application provides a visual fusion positioning and navigation server, comprising a memory and a processor; the memory is used to store computer executable instructions; the processor is used to execute the computer executable instructions to implement the method described in the first aspect or any possible implementation method of the first aspect.

[0016] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores executable instructions, and when a computer executes the executable instructions, the method described in the first aspect or any possible implementation method of the first aspect can be implemented.

[0017] One or more technical solutions provided in the embodiments of the present application have at least the following technical effects: The embodiment of the present application provides a visual fusion positioning and navigation method, which can realize the positioning of a mobile device by collecting environmental images and restoring three-dimensional point cloud data through a binocular camera, and combining the real-time acceleration and angular velocity information provided by an IMU sensor. Binocular vision provides rich environmental information, and IMU data helps to maintain the positioning accuracy and stability of the system when image information is missing or unstable, thereby improving the overall navigation performance. The IMU data is used to predict the position of the binocular camera, and the feature point position of the current frame is updated according to the prediction result, thereby reducing the cumulative error caused by the image matching error. The present application makes full use of the existing binocular camera and IMU sensor resources on the mobile device, without the need for additional high-precision sensors or complex equipment, thereby reducing the system cost and complexity. Whether in indoor or outdoor environments, the present application can fuse the updated feature point position with the three-dimensional point cloud data to build a global map, determine the current position of the mobile device, and perform path planning based on the target position and the current position. It solves the problem of how to provide more reliable data support for the positioning and navigation of mobile devices without increasing the complexity and cost of the system to improve the accuracy of positioning and navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments of the present application or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0019] Figure 1 A flowchart of a visual fusion positioning and navigation method provided in an embodiment of the present application; Figure 2 A schematic diagram of a visual fusion positioning and navigation device provided in an embodiment of the present application; Figure 3 A schematic diagram of a visual fusion positioning and navigation server provided in an embodiment of the present application. DETAILED DESCRIPTION

[0020] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0021] The following describes some of the techniques involved in the embodiments of the present application to facilitate understanding, and they should be considered as merely exemplary. Therefore, it should be appreciated by those of ordinary skill in the art that various changes and modifications may be made to the embodiments described herein without departing from the scope and spirit of the present application. Similarly, for the sake of clarity and conciseness, some descriptions of well-known functions and structures are omitted in the following description.

[0022] The present application embodiment provides a visual fusion positioning and navigation method, such as Figure 1 As shown, the method includes steps S101 to S107. Among them, Figure 1 This is only an execution order shown in the embodiment of the present application, and does not represent the only execution order of a visual fusion positioning and navigation method. If the final result can be achieved, Figure 1 The steps shown may be performed in parallel or reversed.

[0023] S101: Use a binocular camera mounted on a mobile device to collect environmental images, and pre-process the collected images.

[0024] It should be noted that the mobile device of the present application may be an unmanned robot and a navigation unmanned vehicle.

[0025] The collected image is preprocessed, including: obtaining a grayscale histogram of the collected image. Calculating the mean of the grayscale histogram, if the mean is less than a first preset threshold, judging that the image is too dark. Calculating the standard deviation of the grayscale histogram, if the standard deviation is less than a second preset threshold, judging that the image has insufficient contrast. If the image is judged to be too dark or the contrast is insufficient, adjusting the brightness or contrast of the image.

[0026] Specifically, the grayscale histogram is an array of size 256, and the subscripts of the array range from 0 to 255, corresponding to each grayscale level from 0 to 255 in the grayscale image. By traversing each pixel of the image and counting the number of times each grayscale level appears in the image, a grayscale histogram is obtained. The mean of the grayscale histogram reflects the overall brightness level of the image. The mean is calculated by dividing the sum of the number of times all grayscale levels appear and the product of their grayscale values ​​by the total number of pixels. The standard deviation of the grayscale histogram reflects the degree of distribution dispersion of the grayscale levels in the image, that is, the contrast. The standard deviation is calculated by dividing the square root of the sum of the product of the number of times each grayscale level appears and the square of the difference between its grayscale value and the mean by the square root of the total number of pixels. It should be noted that the first preset threshold can be 128, and the second preset threshold can be 30. In practical applications, the first preset threshold and the second preset threshold can also be flexibly adjusted according to specific circumstances.

[0027] Furthermore, if the image is judged to be too dark, it can be adjusted by increasing the brightness value of the image. The specific method is to add a brightness increment to the gray value of each pixel in the image. If the image contrast is judged to be insufficient, it can be improved by adjusting the image contrast. The specific method is to multiply the gray value of each pixel in the image by a contrast coefficient, and the contrast can be enhanced when the contrast coefficient is greater than 1. It should be noted that the brightness increment can be 30 and the contrast coefficient can be 1.2. In practical applications, the brightness increment and the contrast coefficient can also be flexibly adjusted according to specific circumstances.

[0028] S102: Extract feature points and their descriptors from the preprocessed image.

[0029] Extracting feature points and their descriptors from a preprocessed image includes: calculating the gradient amplitude and direction of the preprocessed image to obtain gradient information. Calculating the corner point response value of each pixel of the image based on the gradient information. Pixels whose corner point response values ​​are greater than a third preset threshold are regarded as potential feature points. Only feature points with the largest corner point response values ​​are retained in a local area as target feature points. For each target feature point, an image block of a fixed size is selected around it as a neighborhood. A first preset number of pixels are randomly selected in the neighborhood. Sampling is performed for each pixel to obtain a sampling point. The index of the direction interval to which the gradient direction of each sampling point belongs and the relative size of the gradient amplitude are recorded. Constructing a fixed-length descriptor vector to extract the descriptor. Each element of the descriptor vector corresponds to the accumulation of the gradient amplitude of the sampling point in a direction interval.

[0030] Specifically, a differential operator can be used to perform a convolution operation on the preprocessed image to calculate the gradient magnitude and direction of the preprocessed image and obtain gradient information. The gradient magnitude reflects the speed of the change of the pixel value in the image, while the gradient direction indicates the direction of the change. The differential operator can be a Sobel operator. The corner point response value reflects the degree of change in the brightness around the pixel point and is an important basis for identifying the corner point. The pixel points whose corner point response value is greater than the third preset threshold are regarded as potential feature points. This step helps to exclude those pixels whose degree of change is not enough to be regarded as feature points. In the local area, only the feature point with the largest corner point response value is retained as the target feature point. This step can ensure that each local area contains only one most representative feature point, thereby reducing the number of feature points and improving the accuracy of matching. Randomly selecting a first preset number of pixel points for sampling in the neighborhood helps to reduce the influence of noise and outliers and improve the robustness of the descriptor. Random sampling also introduces a certain degree of randomness, making the descriptor less sensitive to slight image changes to a certain extent. Since the descriptor is based on the accumulation of the gradient amplitude of the sampling points, even if the local brightness of the image changes, as long as the relative change between the grayscale values ​​remains unchanged, the gradient amplitude will not be affected, and it can resist the local brightness change in the image to a certain extent. The relative size of the gradient amplitude can be achieved by normalizing the gradient amplitude. In practical applications, it is necessary to select appropriate parameters according to the characteristics of the image and processing requirements, such as the third preset threshold, the neighborhood size, and the first preset number.

[0031] S103: Use the descriptor to match feature points between the left and right images of the binocular camera, and restore the three-dimensional point cloud data of the environment according to the matching results.

[0032] Specifically, the matching process aims to find the best corresponding points in the right image for each feature point in the left image. These corresponding points should reflect the projection of the same physical point under two different perspectives. The 3D coordinates of all matching feature points are combined to form the 3D point cloud data of the restored environment.

[0033] S104: Reading in real time the IMU data acquired by the IMU sensor mounted on the mobile device, including acceleration and angular velocity information.

[0034] Specifically, the IMU sensor is an inertial measurement unit sensor, and the IMU data is inertial measurement unit data.

[0035] S105: Use the IMU data to predict the position and posture of the binocular camera, and update the feature point position of the current frame according to the prediction result.

[0036] The IMU data is used to predict the position of the binocular camera, and the position of the feature points of the current frame is updated according to the prediction results, including: defining the position of the binocular camera and constructing a dynamic model to describe the change of the position of the binocular camera over time. The fourth-order Runge-Kutta method is used to discretize the dynamic model, predict the position of the binocular camera at the next moment, and obtain the prediction results. The feature points of the current frame are converted back to points in the three-dimensional space using the three-dimensional point back projection. Using the prediction results, the coordinate system transformation is used to transform the points in the three-dimensional space from the camera coordinate system at the current moment to the camera coordinate system at the next moment. Using the reprojection equation, the points in the transformed three-dimensional space are projected back to the two-dimensional image plane to obtain the updated feature point coordinates to update the feature point positions of the current frame.

[0037] Define the position and posture of the binocular camera as: .in, is the position of the binocular camera at the current moment, which is used to describe the rigid body transformation in three-dimensional space. is the set of all rigid body transformations, for The three-dimensional rotation orthogonal matrix of for The column vector represents the position of the origin of the camera coordinate system in the world coordinate system. for The row vector of is the homogeneous term of homogeneous coordinates.

[0038] The kinetic model is: .in, is the position of the binocular camera at the current moment The time derivative of represents the rate of change of the position of the binocular camera over time. For expansion element, , is the angular velocity The skew-symmetric matrix is , , is the three-dimensional acceleration vector, is the angular velocity exist The weight on the axis, is the angular velocity exist Components on the axis.

[0039] The expression for discretizing the dynamic model using the fourth-order Runge-Kutta method is: .in, To predict the position of the binocular camera at the next moment, take it as the prediction result. is the time interval from the current moment to the next moment, The function is the matrix exponential, , , and is the slope term, , , , , An extension of the current binocular camera element, Extension of the binocular camera in the middle element, Extension of the binocular camera before the next moment element.

[0040] The expression for the back projection of a three-dimensional point is: .in, is a point in three-dimensional space, is the back projection function, is the feature point coordinate of the current frame, is the depth value corresponding to the feature point, is the inverse matrix of the intrinsic parameter matrix of the binocular camera, To expand the two-dimensional coordinates into homogeneous coordinate form.

[0041] The expression of coordinate system transformation is: .in, is the point in the transformed three-dimensional space, is the predicted position of the binocular camera at the next moment, It is the inverse matrix of the binocular camera's position at the current moment.

[0042] The reprojection equation is: .in, is the updated feature point coordinates of the current frame, is the projection function, are the first three components of the point in the transformed three-dimensional space, is the homogeneous coordinate component of the point in the transformed three-dimensional space, is the intrinsic parameter matrix of the binocular camera.

[0043] Specifically, this application can more accurately predict the motion trajectory of the camera by combining IMU data and visual information from a binocular camera, thereby improving the positioning accuracy of the mobile device. In a complex or dynamic environment, a single sensor may be affected by factors such as noise, occlusion, or changes in illumination, resulting in positioning failure. The combination of IMU data and binocular cameras can complement each other and enhance the robustness of the system. By using numerical methods such as the fourth-order Runge-Kutta method, the dynamic model equations can be efficiently solved, thereby optimizing computational efficiency while ensuring accuracy.

[0044] S106: Fusing the updated feature point positions with the three-dimensional point cloud data to construct a global map.

[0045] The updated feature point positions are merged with the three-dimensional point cloud data to construct a global map, including: extracting descriptors for the feature points of each frame after the update. Generate a global point cloud map using the three-dimensional point cloud data. In the global point cloud map, for each extracted descriptor, search for the second preset number of candidate points closest to it. Calculate the similarity between each extracted descriptor and the descriptor of the candidate point, and retain the matching pairs whose similarity is less than the preset similarity threshold. The similarity calculation formula is: .in, is the similarity between the extracted descriptor and the descriptor of the candidate point, is the extracted descriptor, is the descriptor of the candidate point, is the Euclidean distance. Using the predicted position of the binocular camera at the next moment, the expression for projecting the candidate point from the three-dimensional space to the two-dimensional image plane of the current frame is: .in, is the candidate point, The position of the candidate point after being projected to the current frame by the predicted position of the binocular camera at the next moment. Calculate the reprojection error and remove abnormal matching pairs whose reprojection error is greater than the pixel error threshold. The calculation formula of the reprojection error is: .in, is the reprojection error, is the updated feature point coordinate of the current frame. The retained matching pairs are fused with the candidate points in the corresponding 3D point cloud data to construct a global map.

[0046] Specifically, in the global point cloud map, for each extracted descriptor, searching for the second preset number of candidate points closest to it can be achieved by a nearest neighbor search algorithm (such as a KD tree) to efficiently find the point most similar to the descriptor. In order to retain reliable matching pairs, a preset similarity threshold is set. Only when the similarity between the descriptors is less than the threshold, they are considered to be a valid matching pair. This step helps to eliminate unreliable matches caused by noise, occlusion or mismatch. In order to further verify the effectiveness of the matching, the predicted position of the binocular camera at the next moment is used to project the candidate points from the three-dimensional space to the two-dimensional image plane of the current frame, and then the reprojection error is calculated. If the reprojection error is greater than the pixel error threshold, the matching pair is considered to be abnormal and eliminated. The retained matching pairs are fused with the candidate points in the corresponding three-dimensional point cloud data. This step involves adding new points in the three-dimensional space to the global point cloud map, or updating the positions of existing feature points. By repeating this process, a complete, accurate and real-time global map can be gradually constructed.

[0047] It should be noted that the second preset number, the preset similarity threshold and the pixel error threshold in the present application can be flexibly set and adjusted according to actual conditions. In the setting of the second preset number, 50 can be selected as the number of candidate points. In the setting of the preset similarity threshold, 0.2 can be selected as the threshold. In the setting of the pixel error threshold, 2 pixels can be selected as the threshold.

[0048] S107: Determine the current position of the mobile device according to the global map and the current position of the binocular camera, plan a path according to the target position and the current position, and perform positioning and navigation of the mobile device according to the planned path.

[0049] According to the global map and the current position of the binocular camera, the current position of the mobile device is determined, and a path is planned according to the target position and the current position. The mobile device is positioned and navigated according to the planned path, including: dividing the global map into multiple grid cells, each grid cell represents a spatial position that the mobile device can occupy. According to the location of the obstacle, the inaccessible grid cells are marked. The goal of path planning is to find a path that can bypass all inaccessible grid cells and make the sum of the Euclidean distances between adjacent grid cells pass through reach the shortest path.

[0050] Specifically, the size of the grid unit can be set according to the size of the mobile device, and the division of the grid unit can convert complex map information into discrete data that is easy to process. According to the location of the obstacle, the inaccessible grid cells are marked. These grid cells will be regarded as obstacles in the path planning process and need to be bypassed. Each grid can be regarded as an area with a specific size and shape, and the midpoint coordinates of the grid are used to represent the position of the grid. During the path planning process, the Euclidean distances between the midpoints of adjacent grids are calculated, and a path is found that minimizes the sum of these distances. Such a path will be able to effectively avoid obstacles while ensuring that the mobile device moves from the starting point to the end point in the shortest distance.

[0051] The present application also provides a visual fusion positioning navigation device 200, such as Figure 2 As shown, the device includes: a preprocessing module 201, an extraction module 202, a matching module 203, a reading module 204, a prediction module 205, a construction module 206 and a positioning and navigation module 207.

[0052] The preprocessing module 201 is used to collect environmental images using a binocular camera mounted on a mobile device and to preprocess the collected images.

[0053] The extraction module 202 is used to extract feature points and their descriptors from the preprocessed image.

[0054] The matching module 203 is used to use the descriptors to perform feature point matching between the left and right images of the binocular camera, and restore the three-dimensional point cloud data of the environment according to the matching results.

[0055] The reading module 204 is used to read in real time the IMU data acquired by the IMU sensor carried on the mobile device, including acceleration and angular velocity information.

[0056] The prediction module 205 is used to predict the position and posture of the binocular camera using the IMU data, and update the feature point position of the current frame according to the prediction result.

[0057] The construction module 206 is used to fuse the updated feature point positions with the three-dimensional point cloud data to construct a global map.

[0058] The positioning and navigation module 207 is used to determine the current position of the mobile device according to the global map and the current position of the binocular camera, and plan a path according to the target position and the current position, and perform positioning and navigation of the mobile device according to the planned path.

[0059] Some modules in the apparatus described in the present application can be described in the general context of computer executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, classes, etc. that perform specific tasks or implement specific abstract data types. The present application can also be practiced in distributed computing environments, in which tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules can be located in local and remote computer storage media including storage devices.

[0060] The devices or modules described in the above application embodiments can be implemented by computer chips or entities, or by products with certain functions. For the convenience of description, the above devices are described in various modules according to their functions. When implementing the embodiments of the present application, the functions of each module can be implemented in the same or multiple software and / or hardware. Of course, the module that implements a certain function can also be implemented by combining multiple sub-modules or sub-units.

[0061] The methods, devices or modules described in this application can be implemented in the form of computer-readable program codes. The controller can be implemented in any appropriate manner. For example, the controller can take the form of a microprocessor or processor and a computer-readable medium storing computer-readable program codes (such as software or firmware) that can be executed by the (micro)processor, logic gates, switches, application-specific integrated circuits (English: Application Specific Integrated Circuit, referred to as: ASIC), programmable logic controllers and embedded microcontrollers. Examples of controllers include but are not limited to the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20 and Silicone Labs C8051F320. The memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art also know that in addition to implementing the controller in the form of pure computer-readable program codes, the controller can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, this controller can be considered as a hardware component, and the devices included in it for implementing various functions can also be regarded as structures within the hardware component. Or even, the means for realizing various functions may be regarded as both a software module for realizing the method and a structure within a hardware component.

[0062] like Figure 3As shown, an embodiment of the present application also provides a visual fusion positioning and navigation server, including a memory 301 and a processor 302; the memory 301 is used to store computer executable instructions; the processor 302 is used to execute computer executable instructions to implement a visual fusion positioning and navigation method described above in an embodiment of the present application.

[0063] An embodiment of the present application also provides a computer-readable storage medium, which stores executable instructions. When a computer executes the executable instructions, it can implement the visual fusion positioning and navigation method described above in the embodiment of the present application.

[0064] It can be seen from the description of the above implementation methods that those skilled in the art can clearly understand that the present application can be implemented by means of software plus necessary hardware. Based on this understanding, the technical solution of the present application can be essentially or partly contributed to the prior art in the form of a software product, or it can be reflected in the implementation process of data migration. The computer software product can be stored in a storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a mobile terminal, a server, or a network device, etc.) to execute the method described in the embodiment of the present application.

[0065] The various embodiments in this specification are described in a progressive manner, and the same or similar parts between the various embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. All or part of this application can be used in many general or special computer system environments or configurations.

[0066] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit the present application. Although the present application has been described in detail with reference to the aforementioned embodiments, a person of ordinary skill in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the present application.

Claims

1. A visual fusion positioning and navigation method, characterized in that: include: Use the binocular camera on the mobile device to collect environmental images and pre-process the collected images; Extract feature points and their descriptors from the preprocessed image; Use descriptors to match feature points between the left and right images of the binocular camera, and restore the three-dimensional point cloud data of the environment based on the matching results; Read the IMU data acquired by the IMU sensor on the mobile device in real time, including acceleration and angular velocity information; Use IMU data to predict the position of the binocular camera and update the feature point position of the current frame based on the prediction results; Fuse the updated feature point positions with the 3D point cloud data to construct a global map; According to the global map and the current position of the binocular camera, the current position of the mobile device is determined, and a path is planned according to the target position and the current position, and the mobile device is positioned and navigated according to the planned path.

2. The visual fusion positioning and navigation method according to claim 1, characterized in that: The preprocessing of the collected images includes: Get the grayscale histogram of the collected image; Calculate the mean of the grayscale histogram, and if the mean is less than a first preset threshold, determine that the image is too dark; Calculating the standard deviation of the grayscale histogram, and if the standard deviation is less than a second preset threshold, determining that the image contrast is insufficient; If the image is judged to be too dark or the contrast is insufficient, the brightness or contrast of the image is adjusted.

3. The visual fusion positioning and navigation method according to claim 1, characterized in that: The step of extracting feature points and their descriptors from the preprocessed image includes: Calculate the gradient magnitude and direction of the preprocessed image to obtain gradient information; Calculate the corner point response value of each pixel of the image based on the gradient information; Considering pixel points whose corner point response values ​​are greater than a third preset threshold as potential feature points; Only retain the feature points with the largest corner response value in the local area as the target feature points; For each target feature point, a fixed-size image block is selected around it as a neighborhood; Randomly selecting a first preset number of pixels in the neighborhood; Sample each pixel to obtain a sampling point; Record the index of the direction interval to which the gradient direction of each sampling point belongs and the relative size of the gradient amplitude; A fixed-length descriptor vector is constructed to extract the descriptor, wherein each element of the descriptor vector corresponds to the accumulation of the gradient magnitude of the sampling point in a direction interval.

4. The visual fusion positioning and navigation method according to claim 1, characterized in that: The method of using IMU data to predict the position of the binocular camera and updating the feature point position of the current frame according to the prediction result includes: Define the position and posture of the binocular camera, and build a dynamic model to describe the change of the position and posture of the binocular camera over time; The fourth-order Runge-Kutta method is used to discretize the dynamic model, predict the position and posture of the binocular camera at the next moment, and obtain the prediction result; Use 3D point back projection to convert the feature points of the current frame back to points in 3D space; Using the prediction results, coordinate system transformation is used to transform the points in the three-dimensional space from the camera coordinate system at the current moment to the camera coordinate system at the next moment; Using the reprojection equation, the points in the transformed three-dimensional space are projected back to the two-dimensional image plane to obtain the updated feature point coordinates to update the feature point positions of the current frame.

5. The visual fusion positioning and navigation method according to claim 4, characterized in that: Define the position and posture of the binocular camera as: ;in, is the position of the binocular camera at the current moment, which is used to describe the rigid body transformation in three-dimensional space. is the set of all rigid body transformations, for The three-dimensional rotation orthogonal matrix of for The column vector represents the position of the origin of the camera coordinate system in the world coordinate system. for The row vector of is the homogeneous term of homogeneous coordinates; The kinetic model is: ;in, is the position of the binocular camera at the current moment The time derivative of represents the rate of change of the position of the binocular camera over time. For expansion element, , is the angular velocity The skew-symmetric matrix is , , is the three-dimensional acceleration vector, is the angular velocity exist The weight on the axis, is the angular velocity exist Components on the axis; The expression for discretizing the dynamic model using the fourth-order Runge-Kutta method is: ;in, To predict the position of the binocular camera at the next moment, take it as the prediction result. is the time interval from the current moment to the next moment, The function is the matrix exponential, , , and is the slope term, , , , , An extension of the current binocular camera element, Extension of the binocular camera in the middle element, Extension of the binocular camera before the next moment element; The expression for the back projection of a three-dimensional point is: ;in, is a point in three-dimensional space, is the back projection function, is the feature point coordinate of the current frame, is the depth value corresponding to the feature point, is the inverse matrix of the internal parameters of the binocular camera, To expand the two-dimensional coordinates into homogeneous coordinate form; The expression of coordinate system transformation is: ;in, is the point in the transformed three-dimensional space, is the predicted position of the binocular camera at the next moment, is the inverse matrix of the binocular camera's position at the current moment; The reprojection equation is: ;in, is the updated feature point coordinates of the current frame, is the projection function, are the first three components of the point in the transformed three-dimensional space, is the homogeneous coordinate component of the point in the transformed three-dimensional space, is the intrinsic parameter matrix of the binocular camera.

6. The visual fusion positioning and navigation method according to claim 5, characterized in that: The step of fusing the updated feature point positions with the three-dimensional point cloud data to construct a global map includes: Extract descriptors from the feature points of each updated frame; Generate a global point cloud map using 3D point cloud data; In the global point cloud map, for each extracted descriptor, searching for a second preset number of candidate points closest to it; Calculate the similarity between each extracted descriptor and the descriptor of the candidate point, and retain the matching pairs whose similarity is less than the preset similarity threshold; The similarity calculation formula is: ;in, is the similarity between the extracted descriptor and the descriptor of the candidate point, is the extracted descriptor, is the descriptor of the candidate point, is the Euclidean distance; Using the predicted position of the binocular camera at the next moment, the expression for projecting the candidate point from the three-dimensional space to the two-dimensional image plane of the current frame is: ;in, is the candidate point, The candidate point is projected to the current frame through the predicted position of the binocular camera at the next moment; Calculate the reprojection error and remove abnormal matching pairs whose reprojection error is greater than the pixel error threshold; The calculation formula of the reprojection error is: ;in, is the reprojection error, is the updated feature point coordinates of the current frame; The retained matching pairs are fused with the candidate points in their corresponding 3D point cloud data to construct a global map.

7. The visual fusion positioning and navigation method according to claim 6, characterized in that: The method of determining the current position of the mobile device according to the global map and the current position of the binocular camera, planning a path according to the target position and the current position, and performing positioning and navigation of the mobile device according to the planned path includes: Divide the global map into multiple grid cells, each grid cell represents a spatial position that a mobile device can occupy; Mark the grid cells that are impassable based on the location of the obstacles; The goal of path planning is to find a path that bypasses all inaccessible grid cells and minimizes the sum of the Euclidean distances between adjacent grid cells.

8. A visual fusion positioning and navigation device, characterized in that: include: A preprocessing module is used to collect environmental images using a binocular camera mounted on a mobile device and to preprocess the collected images; An extraction module, used to extract feature points and their descriptors from the preprocessed image; A matching module is used to match feature points between the left and right images of the binocular camera using descriptors, and restore the three-dimensional point cloud data of the environment based on the matching results; The reading module is used to read the IMU data acquired by the IMU sensor on the mobile device in real time, including acceleration and angular velocity information; The prediction module is used to predict the position and posture of the binocular camera using IMU data and update the feature point position of the current frame according to the prediction results; A construction module for fusing the updated feature point positions with the 3D point cloud data to construct a global map; The positioning and navigation module is used to determine the current position of the mobile device based on the global map and the current position of the binocular camera, and plan a path based on the target position and the current position, and perform positioning and navigation of the mobile device according to the planned path.

9. A visual fusion positioning navigation server, characterized in that: including memory and processor; The memory is used to store computer executable instructions; The processor is configured to execute the computer executable instructions to implement the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores executable instructions, and when a computer executes the executable instructions, the method according to any one of claims 1 to 7 can be implemented.

Images