Robust multi-layer parking lot mapping positioning method based on visual inertia information

Abstract

The invention provides a robust multi-layer parking lot mapping positioning method based on visual inertia information, and the method comprises the steps: firstly obtaining an original image gray-scale map and IMU original information, inputting a feature detection and tracking module, and outputting an image frame pose and an IMU pre-integration result; then the local vision inertial odometer module receives the data, VIO optimization is carried out through a sliding window, and the poses of the key frame and the loopback frame are judged and output; then, the back-end graph optimization module receives the pose construction factor graph for global optimization, and relocation thread acceleration is started; and finally, the floor control and constraint module receives the high-frequency elevation information of the local module, processes the high-frequency elevation information, inputs the processed high-frequency elevation information into a rear-end module, receives a rear-end feedback pose to eliminate a height accumulated error, and finally outputs a vehicle pose. Compared with the prior art, the method has the advantages that the same-layer mapping availability is improved, the multi-layer mapping robustness is enhanced, and light weight and precision are both considered.

Description

Technical Field

[0001] This invention relates to the field of intelligent driving technology, and in particular to a robust multi-level parking lot mapping and positioning method based on visual inertial information. Background Technology

[0002] Mapping and localization technologies are core algorithms for automated parking systems, playing a crucial role in both rule-driven and data-driven system architectures. In rule-driven systems, they receive raw input from the perception module and sensors, outputting pose and map information to the planning and control module. In data-driven systems, they provide ground-value map and pose data for end-to-end model training. As advanced autonomous driving technology becomes more prevalent in the low-cost automotive market, reducing the hardware costs of sensors and computing platforms while improving software algorithm performance has become a mainstream approach to lowering the overall cost of automated parking systems. Therefore, lightweight mapping and localization algorithms are of great significance for cost reduction and efficiency improvement in autonomous driving systems.

[0003] Low-cost automated parking systems typically rely on visual-inertial information for mapping and localization. Inputs include 2D / 3D semantic point clouds output from the perception module (2D semantic point clouds are often derived from surround-view camera visual information, stitched together and transformed by neural networks to form a bird's-eye view of the vehicle from a BEV perspective; 3D semantic point clouds require visual-inertial alignment, epipolar geometry, and bundle adjustment (BA) to restore pixel scale and assign semantic information) and vehicle motion information output from the inertial measurement unit (IMU). However, existing visual-inertial mapping and localization methods have significant limitations when applied to multi-story parking garages:

[0004] Limitations of 2D semantic point clouds: They cannot represent floor height information, require the creation of independent 2D point cloud maps for different floors, and are not conducive to unified map management;

[0005] Limitations of 3D semantic point clouds: Although they can represent floor heights, point clouds on different floors are prone to overlap due to errors in point cloud location and IMU observation, making it difficult to guarantee the quality of map construction.

[0006] Designing lightweight mapping and localization algorithms for automated parking systems in multi-story parking lots requires addressing the issues inherent in visual-inertial mapping and localization techniques. This involves ensuring the accuracy of each layer of mapping while maintaining the overall coherence of multi-layer mapping. This is crucial for improving map quality and the performance of automated parking systems. Summary of the Invention

[0007] This invention aims to solve at least one of the technical problems existing in the prior art, and proposes a robust multi-level parking lot mapping and positioning method based on visual inertial information.

[0008] In a first aspect, embodiments of the present invention provide a robust multi-level parking lot mapping and positioning method based on visual inertial information, comprising:

[0009] The original image grayscale image and IMU raw information are obtained, and the original image grayscale image and IMU raw information are input into the feature detection and tracking module for processing, and the frame pose and IMU pre-integration results at the time of receiving the image are output.

[0010] The local visual inertial odometry module receives the frame pose and IMU pre-integration results, adds a sliding window for VIO optimization, determines key frames and detects loop closure frames, and outputs the poses of key frames and loop closure frames.

[0011] The backend graph optimization module receives the poses of the keyframes and loop closure frames, constructs a factor graph for global pose optimization, and starts a relocation thread to accelerate the optimization process.

[0012] The floor control and constraint module receives high-frequency elevation information output by the local visual inertial odometry module. After processing by the floor judgment and same-floor constraint construction algorithm, the height constraint is input into the back-end graph optimization module. At the same time, it receives the pose feedback from the back-end graph optimization module to eliminate the cumulative error in the height direction, and finally outputs the vehicle pose.

[0013] Further, the original image grayscale image and the original IMU information are input into the feature detection and tracking module for processing, and the frame pose and IMU pre-integration results at the time of receiving the image are output. The specific steps include:

[0014] Key points are detected using Harris corner or FAST algorithms, and scale invariance is improved by combining image pyramids; based on the KLT sparse optical flow algorithm, feature matching is achieved by minimizing the grayscale error between adjacent frames and maintaining the feature point trajectory chain.

[0015] The random sampling consensus algorithm is used to estimate inter-frame motion and remove outliers; the average disparity of features between the current frame and the previous keyframe is calculated, and if the disparity exceeds the threshold or the feature loss rate is too high, a keyframe is inserted.

[0016] Integrate high-frequency IMU data into image frames, calculate the relative rotation ΔR, velocity Δv, and position Δp between adjacent frames, and if the deviation changes exceed the threshold, re-integrate based on the new deviation.

[0017] 3. The positioning method according to claim 1, characterized in that the local visual inertial odometry module receives the frame pose and IMU pre-integration results, adds a sliding window for VIO optimization, and determines key frames and detects loop closure frames; the specific steps include:

[0018] Define state vector

[0019]

[0020]

[0021] in, This refers to the state of frame k, including the position in the world coordinate system. ,speed and direction And acceleration deviation in the vehicle coordinate system and gyroscope deviation Where n is the number of keyframes and m is the number of features in the sliding window. It is the inverse distance between the l-th feature and the first observation;

[0022] Construct the objective function:

[0023]

[0024] in, This represents prior information indicating marginalization. IMU measurement residuals Represents visual reprojection error;

[0025] By employing a sliding window optimization, when the number of variables reaches a set dimension, information about discarding variables is passed through marginal probability to avoid information loss.

[0026] Furthermore, the back-end graph optimization module adopts 4DoF pose optimization, which only optimizes the x, y, z coordinates and yaw angle; when a keyframe is marginalized in the sliding window, the keyframe is treated as a vertex in the pose graph and connected to other vertices through two types of edges: sequential edges and loop edges.

[0027] Furthermore, when connected to other vertices via sequential edges, each keyframe establishes several connections with previous keyframes. A sequential edge represents the relative transition between two keyframes in a local sliding window, obtained from the results of visual inertial odometry. Sequential edges only contain relative position and yaw angle. When keyframes are on the same floor, their height value is fixed as a floor constraint.

[0028] Furthermore, when an edge connected to other vertices via a loopback edge, if a loopback frame is detected in the latest marginalized keyframe, the relative pose transformation between the current frame and the loopback frame is checked. The relative pose transformation is given by the relocalization result. The entire pose graph is optimized by minimizing the following cost function:

[0029]

[0030] Where γ i,j Let ρ be the residual term between the two vertices, and let ρ() be the robust kernel function.

[0031] Furthermore, the floor control and constraint module calculates the slope value of the road surface where the vehicle is currently located based on the pose information, and determines whether the vehicle is on a slope. The specific method includes: calculating the road slope value, setting a slope detection queue, determining that the vehicle has entered the slope when all slope values ​​in the queue are greater than a threshold, and determining that the vehicle has entered the plane when all slope values ​​in the queue are less than the threshold.

[0032] Furthermore, when it is determined that a vehicle has entered a ramp, the floor number needs to be changed and the same-floor constraint needs to be calculated and added. The calculation method includes: when the vehicle goes upstairs, the current elevation information is stored as the floor height, and a height constraint between two pose nodes is added during the graph optimization process; when the vehicle goes downstairs, the floor height is searched and a height constraint is added again; the graph optimization module also feeds back the optimized node pose for adjusting the floor height at any time; finally, the road surface state of the vehicle at the current moment and the previous moment are updated for the next round of loop.

[0033] Secondly, the present invention also discloses an electronic device, characterized in that it comprises:

[0034] One or more processors;

[0035] Memory, used to store one or more programs;

[0036] When the one or more programs are executed by the one or more processors, the one or more processors implement the positioning method.

[0037] Thirdly, the present invention also discloses a computer-readable medium storing a computer program, characterized in that the computer program, when executed by a processor, implements the steps in the positioning method.

[0038] This invention provides a robust multi-level parking lot mapping and localization method based on visual inertial information. First, the method acquires the original image grayscale and IMU raw information, inputs them into a feature detection and tracking module, and outputs the image frame pose and IMU pre-integration results. Next, a local visual inertial odometry module receives this data, performs VIO optimization through a sliding window, determines and outputs the poses of keyframes and loopback frames. Then, a back-end graph optimization module receives the pose construction factor map for global optimization and activates a relocalization thread for acceleration. Finally, a floor control and constraint module receives high-frequency elevation information from the local module, processes it, inputs the height constraint into the back-end module, and simultaneously receives back-end feedback pose to eliminate accumulated height errors, ultimately outputting the vehicle pose.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] 1. Improve the usability of same-floor mapping: By using the same-floor constraint mechanism of the backend map optimization module, the height of feature points on the same floor is controlled, effectively suppressing the cumulative error of positioning pose, ensuring that the ground markings on the same floor map remain on the same plane, and significantly improving the usability of mapping;

[0041] 2. Enhance the robustness of multi-level mapping: The floor control and constraint module adopts a slope detection queue mechanism, which has strong robustness to road bumps and abnormal IMU observations. It can accurately identify floors and add height constraints, avoid the overlap of point clouds on different floors, and improve the integrity and coherence of multi-level parking lot mapping.

[0042] 3. Balancing lightweight design and accuracy: Through 4DoF pose optimization and IMU pre-integration, the computational complexity is reduced to adapt to low-cost hardware platforms while ensuring mapping and positioning accuracy, thus meeting the application requirements of low-cost automatic parking systems. Attached Figure Description

[0043] Figure 1 This is a flowchart illustrating a robust multi-level parking lot mapping and positioning method based on visual inertial information, provided in an embodiment of the present invention.

[0044] Figure 2 A schematic diagram of the overall framework and data flow of a robust multi-level parking lot mapping and positioning method based on visual inertial information provided in an embodiment of the present invention;

[0045] Figure 3 A schematic diagram of the floor control and constraint module structure and data flow for a robust multi-story parking lot mapping and positioning method based on visual inertial information provided in an embodiment of the present invention;

[0046] Figure 4 This is a structural block diagram of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0047] To enable those skilled in the art to better understand the technical solutions of the present invention, exemplary embodiments of the present invention are described below in conjunction with the accompanying drawings, including various details of the embodiments of the present invention to aid understanding. These should be considered merely exemplary. Therefore, those skilled in the art should recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the present invention. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0048] Where there is no conflict, the various embodiments of the present invention and the features thereof may be combined with each other.

[0049] As used herein, the term “and / or” includes any and all combinations of one or more related enumerated entries.

[0050] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used herein, the singular forms “a” and “the” are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will also be understood that when the terms “comprising” and / or “made of” are used in this specification, the presence of the stated feature, integral, step, operation, element, and / or component is specified, but the presence or addition of one or more other features, integrals, steps, operations, elements, components, and / or groups thereof is not excluded. Terms such as “connected” or “linked” are not limited to physical or mechanical connections but can include electrical connections, whether direct or indirect.

[0051] Unless otherwise specified, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art. It will also be understood that terms such as those defined in commonly used dictionaries should be interpreted as having the meaning consistent with their meaning in the context of the relevant art and the invention, and will not be interpreted as having an idealized or overly formal meaning unless expressly so defined herein.

[0052] In the technical solution of this invention, the collection, storage, use, processing, transmission, provision, and disclosure of user personal information all comply with relevant laws and regulations and do not violate public order and good morals. The use of user data in this technical solution follows relevant national laws and regulations (e.g., the "Information Security Technology - Personal Information Security Specification"). For example: appropriate measures are taken for personal information access control; restrictions are imposed on the display of personal information; the purpose of using personal information does not exceed the scope of direct or reasonable association; and explicit identity targeting is eliminated when using personal information to avoid precisely locating a specific individual.

[0053] In related technologies, multi-level parking lot mapping and localization schemes are mostly based on improvements to classic SLAM technologies such as VINS-Mono, ORB-SLAM, DSO, and Cartographer. Their core relies on visual inertial odometry (VIO) for real-time localization and mapping. VIOMA performs short-term state prediction by integrating IMU data, correlates visual feature points with the IMU predicted state, calculates the error, and uses a sliding window optimization to minimize the error function to update the current and historical states. Simultaneously, visual constraints, IMU constraints, and closed-loop constraints are incorporated into the objective function for nonlinear optimization.

[0054] However, existing multi-level parking lot mapping and positioning technologies based on visual inertial odometry still have two major drawbacks:

[0055] 1. Poor usability of same-floor mapping: Mapping and positioning quality depend on the quality of IMU observations. If the observation quality is poor, it can easily lead to cumulative positioning and pose errors, making it impossible for ground markings on the same floor map to be kept on the same plane, thus significantly reducing the usability of mapping.

[0056] 2. Poor robustness of multi-level mapping: The lack of a floor-based overall map optimization mechanism means that scale drift in visual observation can lead to local overlap of point cloud maps on different floors, resulting in insufficient robustness of mapping.

[0057] To address at least one of the technical problems existing in the aforementioned related technologies, the present invention provides a robust multi-level parking lot mapping and positioning method based on visual inertial information.

[0058] A robust multi-level parking lot mapping and localization method based on visual inertial information, such as Figure 1 and 2 ,include:

[0059] S100. Obtain the original image grayscale image and the original IMU information, input the original image grayscale image and the original IMU information into the feature detection and tracking module for processing, and output the frame pose and IMU pre-integration result at the time of receiving the image;

[0060] The original image grayscale is a single-channel image converted from a color image, containing only brightness and darkness information. The grayscale value of each pixel (usually ranging from 0 to 255, where 0 represents pure black and 255 represents pure white) reflects the brightness of that point, without color information. The most common method is to obtain it from an existing color image file, suitable for processing locally stored color images (such as JPG, PNG, and BMP formats), directly converting and saving them as grayscale images using an image processing library. The raw information from the IMU (Inertial Measurement Unit) refers to the core sensor data directly output by the IMU hardware, without filtering or fusion processing (such as attitude calculation and error compensation). It mainly includes the raw measurement values ​​from the accelerometer (3-axis) and gyroscope (3-axis).

[0061] In this embodiment, the original image grayscale image and the original IMU information are input into the feature detection and tracking module for processing, and the frame pose and IMU pre-integration result at the time of receiving the image are output. The specific steps include:

[0062] Key points are detected using Harris corner or FAST algorithms, and scale invariance is improved by combining image pyramids; based on the KLT sparse optical flow algorithm, feature matching is achieved by minimizing the grayscale error between adjacent frames and maintaining the feature point trajectory chain.

[0063] The random sampling consensus algorithm is used to estimate inter-frame motion and remove outliers; the average disparity of features between the current frame and the previous keyframe is calculated, and if the disparity exceeds the threshold or the feature loss rate is too high, a keyframe is inserted.

[0064] Integrate high-frequency IMU data into image frames, calculate the relative rotation ΔR, velocity Δv, and position Δp between adjacent frames, and if the deviation changes exceed the threshold, re-integrate based on the new deviation.

[0065] The raw visual and IMU data are normalized through step S100 disclosed in this embodiment, providing reliable input for backend optimization.

[0066] S200. The local visual inertial odometry module receives the frame pose and IMU pre-integration results, adds a sliding window for VIO optimization, determines key frames and detects loop closure frames, and outputs the poses of key frames and loop closure frames.

[0067] In this embodiment, the visual inertial odometry module receives the frame pose and IMU pre-integration results, adds a sliding window for VIO optimization, and determines key frames and detects loop closure frames; the specific steps include:

[0068] Define state vector

[0069]

[0070]

[0071] in, This refers to the state of frame k, including the position in the world coordinate system. ,speed and direction And acceleration deviation in the vehicle coordinate system and gyroscope deviation Where n is the number of keyframes and m is the number of features in the sliding window. It is the inverse distance between the l-th feature and the first observation;

[0072] Construct the objective function:

[0073]

[0074] in, This represents prior information indicating marginalization. IMU measurement residuals Represents visual reprojection error;

[0075] By employing a sliding window optimization, when the number of variables reaches a set dimension, information about discarding variables is passed through marginal probability to avoid information loss.

[0076] Specifically, visual inertial odometry (VIO) uses IMU data integration for short-term state prediction, correlates visual feature points with IMU predicted states, calculates the error between observed and predicted values, and uses a sliding window optimization method to update the current and historical states by minimizing the error function. Generally, visual constraints, IMU constraints, and closed-loop constraints are placed into a large objective function for nonlinear optimization to solve for PVQ, bias, etc., for all frames within the sliding window. The basic idea behind visual and IMU constraints is to find the optimal state vector, and then obtain the Jacobian and covariance matrices of the visual and IMU pre-integrated states by differentiating the state vector from the visual and IMU measurement residuals. This embodiment includes four parts: VIO residual function construction, visual constraints, IMU constraints, and sliding window and marginalization.

[0077] (1) Construction of VIO residual function

[0078] The state vector to be optimized is as follows: X, which includes the IMU state (PVQ, acceleration bias, gyroscope bias) within the sliding window, the extrinsic parameters from the IMU to the camera, and the inverse depth of m+1 3D landmark points.

[0079]

[0080]

[0081]

[0082] in, This refers to the state of frame k, including the position in the world coordinate system. ,speed and direction And acceleration deviation in the vehicle coordinate system and gyroscope deviation n is the number of keyframes, and m is the number of features in the sliding window. It is the inverse distance between the l-th feature and the first observation.

[0083] The algorithm uses the visual-inertial bundle adjustment formula to minimize the sum of the prior and Mahalanobis norms of all measurement residuals to obtain the maximum a posteriori estimate. The objective function is as follows, consisting of three parts: marginalized prior information, IMU measurement residuals, and visual reprojection error.

[0084]

[0085] (2) Visual constraints

[0086] The visual constraint solves for the error between the observed and estimated values ​​of the same landmark point on the normalized plane. Since the depth of the observed feature points can be very large and difficult to optimize, inverse depth is chosen as a parameter. Inverse depth follows a Gaussian distribution more closely and reduces the number of parameter variables in the actual optimization. The residuals are compared on the camera's normalized plane, and then the visual residuals are projected onto the tangent plane of a unit sphere. If the l-th landmark point is observed for the first time in frame i, the residual for observing that landmark point in frame j is:

[0087]

[0088]

[0089]

[0090] The first equation is the expression for the residual, and the second equation is the observation data converted from the observed pixel coordinates into unit vectors by the fisheye camera's backprojection function. and It is a basis set on the tangent plane of this unit vector. The third equation is the reprojection estimation model.

[0091] (3) IMU constraints

[0092] In the IMU constraints, the optimization variables are the MU's position (p), velocity (v), rotation (q) at times i and j (PVQ), and two biases (ba, bg). Considering that calculating the pose of the IMU relative to the first frame for each frame would be computationally expensive, the PVQ integral formula is modified to be relative to the pose at time i.

[0093] Consider two consecutive frames in the window and According to the IMU measurement function, the residual of the IMU measurement model is defined as:

[0094]

[0095] (4) Sliding window and edge detection

[0096] As the positioning system explores new environments, it encounters new camera poses and environmental features, leading to an increasing number of least-squares residuals, a larger information matrix, and a continuous increase in computational complexity. To keep the number of optimization variables within a certain range, a sliding window algorithm is needed to dynamically add or remove optimization variables.

[0097] The general process of the sliding window algorithm is as follows: 1) Add new variables to the least squares system for optimization; 2) If the number of variables reaches a certain dimension, remove the old variables; 3) The system continuously repeats the above two steps.

[0098] Simply discarding variables and their corresponding measurements results in information loss. The correct approach is to use marginal probabilities to pass the information carried by the discarded variables to the remaining variables.

[0099] S300. The backend graph optimization module receives the poses of the keyframes and loopback frames, constructs a factor graph for global pose optimization, and starts a relocation thread to accelerate the optimization process.

[0100] In this embodiment, the back-end graph optimization module uses 4DoF pose optimization, optimizing only the x, y, z coordinates and yaw angle; when a keyframe is edged out in the sliding window, the keyframe is treated as a vertex in the pose graph and connected to other vertices through two types of edges: sequential edges and loop edges.

[0101] When a keyframe is connected to other vertices via a sequential edge, several connections are established with previous keyframes. A sequential edge represents the relative transformation between two keyframes within a local sliding window, obtained from visual inertial odometry results. Sequential edges only contain relative position and yaw angle. When keyframes are on the same floor, their height is fixed as a floor constraint. When a keyframe is connected to other vertices via a loopback edge, if a loopback frame is detected in the latest marginalized keyframe, the relative pose transformation between the current frame and the loopback frame is checked. The relative pose transformation is given by the relocalization results. The entire pose graph is optimized by minimizing the following cost function:

[0102]

[0103] Where γ i,j Let ρ be the residual term between the two vertices, and let ρ() be the robust kernel function.

[0104] The S400 floor control and constraint module receives high-frequency elevation information output from the local visual inertial odometry module. After processing by the floor judgment and same-floor constraint construction algorithm, the height constraint is input into the back-end graph optimization module. At the same time, it receives the pose feedback from the back-end graph optimization module to eliminate the cumulative error in the height direction, and finally outputs the vehicle pose.

[0105] In this embodiment, the floor control and constraint module calculates the slope value of the road surface where the vehicle is currently located based on the pose information, and determines whether the vehicle is on a slope. Figure 3 The specific methods include: calculating the road surface slope value, setting up a slope detection queue, determining that a vehicle has entered the slope when all slope values ​​in the queue are greater than the threshold, and determining that a vehicle has entered the flat surface when all slope values ​​in the queue are less than the threshold.

[0106] When a vehicle is determined to have entered a ramp, the floor number needs to be changed, and same-floor constraints need to be calculated and added. The calculation method includes: when the vehicle goes upstairs, the current elevation information is stored as the floor height, and height constraints between two pose nodes are added during graph optimization; when the vehicle goes downstairs, the floor height is searched and height constraints are added again; the graph optimization module also provides feedback on the optimized node poses for adjusting the floor height at any time; finally, the road surface state of the vehicle at the current moment and the previous moment are updated for the next round of loop.

[0107] This embodiment provides a robust multi-level parking lot mapping and localization method based on visual inertial information. First, it acquires the original image grayscale and IMU raw information, inputs them into a feature detection and tracking module, and outputs the image frame pose and IMU pre-integration results. Next, a local visual inertial odometry module receives this data, performs VIO optimization through a sliding window, determines and outputs the poses of keyframes and loopback frames. Then, a back-end graph optimization module receives the pose construction factor map and performs global optimization, activating a relocalization thread for acceleration. Finally, a floor control and constraint module receives high-frequency elevation information from the local module, processes it, inputs height constraints into the back-end module, and simultaneously receives back-end feedback pose to eliminate accumulated height errors, ultimately outputting the vehicle pose. Compared with existing technologies, this invention improves the usability of same-level mapping, enhances the robustness of multi-level mapping, and balances lightweight design with accuracy.

[0108] Based on the same inventive concept, embodiments of the present invention also provide an electronic device. Figure 4 This is a structural block diagram of an electronic device provided in an embodiment of the present invention. Figure 4 As shown, an embodiment of the present invention provides an electronic device including: one or more processors 101, a memory 102, and one or more I / O interfaces 103. The memory 102 stores one or more programs, which, when executed by the one or more processors, cause the one or more processors to implement any of the positioning methods described in the above embodiments; the one or more I / O interfaces 103 are connected between the processor and the memory, configured to enable information interaction between the processor and the memory.

[0109] The processor 101 is a device with data processing capabilities, including but not limited to a central processing unit (CPU); the memory 102 is a device with data storage capabilities, including but not limited to random access memory (RAM, more specifically SDRAM, DDR, etc.), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), and flash memory (FLASH); the I / O interface (read / write interface) 103 is connected between the processor 101 and the memory 102, and can realize information interaction between the processor 101 and the memory 102, including but not limited to a data bus (Bus).

[0110] In some embodiments, the processor 101, memory 102, and I / O interface 103 are interconnected via bus 104, and thus connected to other components of the computing device.

[0111] In some embodiments, the one or more processors 101 include a field-programmable gate array.

[0112] This invention also provides a computer-readable medium. The computer-readable medium stores a computer program, which, when executed by a processor, implements the steps of any of the positioning methods described in the above embodiments. The computer-readable storage medium may be volatile or non-volatile.

[0113] This invention also provides a computer program product, including computer-readable code, or a non-volatile computer-readable storage medium carrying computer-readable code. When the computer-readable code is run in the processor of an electronic device, the processor in the electronic device executes the above-described positioning method.

[0114] Those skilled in the art will understand that all or some of the steps, systems, and apparatuses disclosed above, and their functional modules / units, can be implemented as software, firmware, hardware, or suitable combinations thereof. In hardware implementations, the division between functional modules / units mentioned above does not necessarily correspond to the division of physical components; for example, a physical component may have multiple functions, or a function or step may be performed collaboratively by several physical components. Some or all physical components may be implemented as software executed by a processor, such as a central processing unit, digital signal processor, or microprocessor, or as hardware, or as an integrated circuit, such as an application-specific integrated circuit (ASIC). Such software can be distributed on a computer-readable storage medium, which may include computer storage media (or non-transitory media) and communication media (or transient media).

[0115] As is known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information, such as computer-readable program instructions, data structures, program modules, or other data. Computer storage media includes, but is not limited to, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), static random access memory (SRAM), flash memory or other memory technologies, portable compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical disc storage, magnetic cartridges, magnetic tape, disk storage or other magnetic storage devices, or any other medium that can be used to store desired information and is accessible to a computer. Furthermore, it is known to those skilled in the art that communication media typically contain computer-readable program instructions, data structures, program modules, or other data in modulated data signals such as carrier waves or other transmission mechanisms, and may include any information delivery medium.

[0116] The computer-readable program instructions described herein can be downloaded from computer-readable storage media to various computing / processing devices, or downloaded via a network, such as the Internet, local area network, wide area network, and / or wireless network, to an external computer or external storage device. The network may include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and / or edge servers. A network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards them to the computer-readable storage media in the respective computing / processing device.

[0117] The computer program instructions used to perform the operations of this invention may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Smalltalk, C++, etc., and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The computer-readable program instructions may be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or may be connected to an external computer (e.g., via the Internet using an Internet service provider). In some embodiments, electronic circuitry, such as programmable logic circuitry, field-programmable gate arrays (FPGAs), or programmable logic arrays (PLAs), is personalized by utilizing state information from the computer-readable program instructions. This electronic circuitry can execute the computer-readable program instructions to implement various aspects of the invention.

[0118] The computer program product described herein can be implemented specifically through hardware, software, or a combination thereof. In one alternative embodiment, the computer program product is specifically embodied in a computer storage medium; in another alternative embodiment, the computer program product is specifically embodied in a software product, such as a software development kit (SDK), etc.

[0119] Various aspects of the present invention are described herein with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer-readable program instructions.

[0120] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that, when executed by the processor of the computer or other programmable data processing apparatus, they create means for implementing the functions / actions specified in one or more blocks of the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium that causes a computer, programmable data processing apparatus, and / or other device to operate in a particular manner; thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing aspects of the functions / actions specified in one or more blocks of the flowchart and / or block diagram.

[0121] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device to cause a series of operational steps to be performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to perform the functions / actions specified in one or more boxes of a flowchart and / or block diagram.

[0122] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of an instruction, which contains one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the blocks may occur in a different order than those shown in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, may be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0123] Example embodiments have been disclosed herein, and while specific terminology has been used, it is for illustrative purposes only and should be construed as such, and is not intended to be limiting. In some instances, it will be apparent to those skilled in the art that features, characteristics, and / or elements described in conjunction with particular embodiments may be used alone, or in combination with features, characteristics, and / or elements described in conjunction with other embodiments, unless otherwise expressly indicated. Therefore, those skilled in the art will understand that various changes in form and detail may be made without departing from the scope of the invention as set forth in the appended claims.

Claims

1. A robust multi-level parking lot mapping and positioning method based on visual inertial information, characterized in that, include: The original image grayscale image and IMU raw information are obtained, and the original image grayscale image and IMU raw information are input into the feature detection and tracking module for processing, and the frame pose and IMU pre-integration results at the time of receiving the image are output. The local visual inertial odometry module receives the frame pose and IMU pre-integration results, adds a sliding window for VIO optimization, determines key frames and detects loop closure frames, and outputs the poses of key frames and loop closure frames. The backend graph optimization module receives the poses of the keyframes and loop closure frames, constructs a factor graph for global pose optimization, and starts a relocation thread to accelerate the optimization process. The floor control and constraint module receives high-frequency elevation information output by the local visual inertial odometry module. After processing by the floor judgment and same-floor constraint construction algorithm, the height constraint is input into the back-end graph optimization module. At the same time, it receives the pose feedback from the back-end graph optimization module to eliminate the cumulative error in the height direction, and finally outputs the vehicle pose.

2. The positioning method according to claim 1, characterized in that, The original image grayscale image and the original IMU information are input into the feature detection and tracking module for processing, and the frame pose and IMU pre-integration results at the time of receiving the image are output. The specific steps include: Key points are detected using Harris corner or FAST algorithms, and scale invariance is improved by combining image pyramids; based on the KLT sparse optical flow algorithm, feature matching is achieved by minimizing the grayscale error between adjacent frames and maintaining the feature point trajectory chain. The random sampling consensus algorithm is used to estimate inter-frame motion and remove outliers; the average disparity of features between the current frame and the previous keyframe is calculated, and if the disparity exceeds the threshold or the feature loss rate is too high, a keyframe is inserted. Integrate high-frequency IMU data into image frames, calculate the relative rotation ΔR, velocity Δv, and position Δp between adjacent frames, and if the deviation changes exceed the threshold, re-integrate based on the new deviation.

3. The positioning method according to claim 1, characterized in that, The local visual inertial odometry module receives the frame pose and IMU pre-integration results, adds a sliding window for VIO optimization, and determines key frames and detects loop closure frames. The specific steps include: Define state vector in, This refers to the state of frame k, including the position in the world coordinate system. ,speed and direction And acceleration deviation in the vehicle coordinate system and gyroscope deviation Where n is the number of keyframes and m is the number of features in the sliding window. It is the inverse distance between the l-th feature and the first observation; Construct the objective function: in, This represents prior information indicating marginalization. IMU measurement residuals Represents visual reprojection error; By employing a sliding window optimization, when the number of variables reaches a set dimension, information about discarding variables is passed through marginal probability to avoid information loss.

4. The positioning method according to claim 1, characterized in that, The backend graph optimization module uses 4DoF pose optimization, which only optimizes the x, y, z coordinates and yaw angle. When a keyframe is edged out in the sliding window, the keyframe is treated as a vertex in the pose graph and connected to other vertices through two types of edges: sequential edges and loop edges.

5. The positioning method according to claim 4, characterized in that, When connected to other vertices via sequential edges, each keyframe establishes several connections with previous keyframes. A sequential edge represents the relative transition between two keyframes in a local sliding window, obtained from the results of visual inertial odometry. Sequential edges only contain relative position and yaw angle. When keyframes are on the same floor, their height value is fixed as a floor constraint.

6. The positioning method according to claim 4, characterized in that, When an edge connected to other vertices via a loopback edge, if a loopback frame is detected in the latest marginalized keyframe, the relative pose transformation between the current frame and the loopback frame is checked. The relative pose transformation is given by the relocalization result. The entire pose graph is optimized by minimizing the following cost function: Where γ i,j Let ρ be the residual term between the two vertices, and let ρ() be the robust kernel function.

7. The positioning method according to claim 1, characterized in that, The floor control and constraint module calculates the slope value of the road surface where the vehicle is currently located based on the pose information, and determines whether the vehicle is on a slope. The specific method includes: calculating the road slope value, setting a slope detection queue, determining that the vehicle has entered the slope when all slope values ​​in the queue are greater than a threshold, and determining that the vehicle has entered the plane when all slope values ​​in the queue are less than the threshold.

8. The positioning method according to claim 7, characterized in that, When a vehicle is determined to have entered a ramp, the floor number needs to be changed, and same-floor constraints need to be calculated and added. The calculation method includes: when the vehicle goes upstairs, the current elevation information is stored as the floor height, and height constraints between two pose nodes are added during graph optimization; when the vehicle goes downstairs, the floor height is searched and height constraints are added again; the graph optimization module also provides feedback on the optimized node poses for adjusting the floor height at any time; finally, the road surface state of the vehicle at the current moment and the previous moment are updated for the next round of loop.

9. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the positioning method as described in any one of claims 1 to 8.

10. A computer-readable medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps in the positioning method as described in any one of claims 1 to 8.

Images