Vehicle-mounted real-time positioning mapping method and system based on domain controller and laser radar

Abstract

The application discloses a vehicle-mounted real-time positioning mapping method and system based on a domain controller and a laser radar, and belongs to the technical field of vehicle-mounted positioning and mapping. The method comprises the following steps: receiving and preprocessing three-dimensional point cloud data collected by the laser radar; performing scan-to-scan matching on a current frame and a previous frame, outputting a relative pose transformation matrix and determining an initial pose; calculating the displacement change and the angle change of the current frame and the previous key frame, and storing in a key frame database when the final translation threshold or the preset rotation threshold is reached; retrieving the first group and the second group of key frame point clouds, and splicing to generate a multi-scale local subgraph; performing scan-to-map matching, outputting a global optimization pose transformation matrix, transforming the point cloud to a world coordinate system, superimposing the global map, and outputting a six-degree-of-freedom pose. The system comprises a laser radar and a domain controller, and the two are connected through a high-speed Ethernet line and a synchronization signal line. The application realizes high-precision, high-real-time and power-adaptive vehicle-mounted positioning and mapping.

Description

Technical Field

[0001] This invention belongs to the field of vehicle positioning and mapping technology, and specifically relates to a method and system for real-time vehicle positioning and mapping based on a domain controller and lidar. Background Technology

[0002] With the development of autonomous driving technology, high-precision and high-reliability real-time positioning and mapping have become core requirements. Simultaneous localization and mapping (SLAM) technology based on LiDAR is one of the mainstream solutions.

[0003] Currently, publicly available technical solutions include the following: First, integrated navigation systems. These systems integrate sensors such as LiDAR and IMU with a specific computing board. However, their internal architecture and interfaces are usually closed, and they are primarily geared towards specific application scenarios, lacking flexible support and targeted optimization for efficient open-source algorithms. Second, dedicated SLAM processor solutions. This solution uses embedded processors or FPGA acceleration boards specifically designed for SLAM, embedding specific simultaneous localization and mapping (SLAM) algorithms into hardware logic to achieve low-power, high-speed positioning calculations. Third, solutions combining general-purpose computing platforms with LiDAR. General-purpose automotive computing platforms, such as the NVIDIA DRIVE series and Qualcomm Snapdragon Ride, connect to LiDAR sensors via Ethernet interfaces, running deep learning-based perception algorithms and SLAM algorithms. These platforms possess strong GPU computing power and can process high-resolution point cloud data. Fourth, industrial control computers combined with LiDAR. This solution uses an industrial control computer as the core computing unit, connecting to multi-line LiDAR via a gigabit Ethernet interface. The industrial control computer runs an open-source or commercial LiDAR odometry algorithm. The algorithm processes the point cloud data transmitted from the radar and outputs the vehicle's pose information and an environmental map. The system typically relies on GPS PPS signals or NTP for coarse synchronization.

[0004] Current technical solutions have the following drawbacks: First, they rely on NTP or PPS for synchronization, with accuracy typically in the millisecond or sub-millisecond range. This cannot meet the microsecond-level time synchronization requirements of centimeter-level positioning accuracy for high-speed moving vehicles. This leads to a time deviation between point cloud data and vehicle pose, directly affecting the accuracy of positioning and mapping, as well as map consistency. Second, while traditional synchronous positioning and mapping algorithms offer high accuracy, they also have high computational complexity. When processing radars like the RoboSense RS-Helios-16p that generate dense point clouds, they can easily cause CPU overload and frame loss on industrial control computers, making real-time processing impossible. Third, the industrial control computer and the LiDAR use multiple interface connections, resulting in numerous cables with weak resistance to vibration and electromagnetic interference. In the vehicle operating environment, this can lead to loose connections and signal interference, affecting system stability. Summary of the Invention

[0005] To address the aforementioned technical challenges, this invention proposes a vehicle-mounted real-time localization and mapping method and system based on a domain controller and a lidar. By achieving nanosecond-level time synchronization between the domain controller and the lidar, and by using an improved DLO algorithm that dynamically adjusts keyframe selection and subgraph construction scale based on hardware load and environmental space, high-precision, high-real-time performance, and computationally adaptive vehicle localization and mapping are achieved.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: In a first aspect, this invention proposes a vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar, wherein the vehicle-mounted domain controller performs the following steps: Receive and preprocess 3D point cloud data collected by lidar to obtain preprocessed point cloud data; The current frame in the preprocessed point cloud data is scanned to match the previous frame, and the relative pose transformation matrix between adjacent frames is output. The initial pose for scan-to-map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame. Calculate the displacement and angle changes between the current frame and the previous keyframe; when the displacement changes reach the final translation threshold or the angle changes reach the preset rotation threshold, store the point cloud data of the current frame, the global pose transformation matrix of the current frame, and the corresponding covariance matrix into the keyframe database. Retrieve the first set of keyframe point clouds closest to the current pose and the second set of keyframe point clouds that form the convex hull boundary from the keyframe database; stitch the two sets of keyframe point clouds together to generate a multi-scale local sub-image. The point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output the global optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the global optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as the six-degree-of-freedom pose output.

[0007] Furthermore, before receiving the 3D point cloud data collected by the lidar, the vehicle domain controller also performs the following: sending a synchronization signal to the lidar as the master clock via a synchronization signal line to achieve nanosecond-level time synchronization with the lidar.

[0008] Furthermore, the current frame in the preprocessed point cloud data is scan-to-scan matching with its previous frame, and the relative pose transformation matrix between adjacent frames is output, specifically: ; in, Indicates the first Frame relative to the first The relative pose transformation matrix, ; Represents the transformation matrix variable to be solved; Indicates the first The residual vector corresponding to each point; This represents the transpose of the residual vector; Indicates the first point in the current frame source point cloud One point; Indicates the target point cloud in the previous frame and The corresponding nearest neighbor; express The covariance matrix of the local neighborhood; express The covariance matrix of the local neighborhood.

[0009] Furthermore, the initial pose for map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame, specifically as follows: ; in, Indicates the initial pose; This represents the global pose transformation matrix of the previous frame; .

[0010] Furthermore, the displacement and angle changes between the current frame and the previous keyframe are calculated, specifically as follows: ; ; in, Indicates the amount of displacement change; Represents the global translation vector for the current frame; Represents the global translation vector of the previous keyframe; Indicates the change in angle; This represents the quaternion corresponding to the global rotation in the current frame; This represents the quaternion corresponding to the global rotation in the previous keyframe.

[0011] Furthermore, the final translation threshold The method for determining this is as follows: ; in, This represents the basic translation threshold determined by the environmental spaciousness index; This represents the hardware adjustment factor determined based on the real-time hardware load.

[0012] Furthermore, the basic translation threshold is expressed as: ; in, This represents the filtered median sequence value; Indicates the long-distance threshold; Indicates the mid-distance threshold; Indicates the near-distance threshold; Indicates a high translation threshold; Indicates the translation threshold; Indicates a low translation threshold; Indicates the minimum translation threshold; The hardware adjustment coefficient is expressed as: ; in, Indicates real-time hardware load; Indicates a low load threshold; Indicates the load threshold; Indicates a high load threshold; Indicates a low load regulation coefficient; This represents the normal adjustment coefficient; Indicates a high load adjustment coefficient; This represents the maximum value of the load regulation coefficient; The real-time hardware load is represented as follows: ; in, Indicates the CPU utilization of the domain controller; This indicates the GPU utilization of the domain controller; Indicates the first weighting coefficient; This represents the second weighting coefficient; .

[0013] Furthermore, the number of point clouds in the first set of keyframes and the number of point clouds in the second set of keyframes The load is dynamically determined by the real-time hardware load, specifically: ; in, These represent the number of point clouds in the first group of keyframes under high, medium, and low load conditions, respectively, and satisfy the following conditions: ; These represent the number of point clouds in the second set of keyframes under high, medium, and low load conditions, respectively, and satisfy the following conditions: ; The two sets of keyframe point clouds are stitched together to generate multi-scale local sub-images, specifically: ; in, Represents a multi-scale local subgraph; This represents the point cloud of the first set of keyframes; This represents the point cloud of the second set of keyframes.

[0014] Furthermore, the point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output a globally optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the globally optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as a six-degree-of-freedom pose output, specifically: The GICP algorithm is used, with the initial pose as the initial value for iteration. The point cloud data of the current frame is registered with the multi-scale local sub-image, and a globally optimized pose transformation matrix is ​​output. ; Transform the point cloud data of the current frame to the world coordinate system according to the global optimized pose transformation matrix. ;in, Point cloud for the current frame any point in it; Represents the coordinates transformed to the world coordinate system; Transform all points Overlay on the global map In, and from The vehicle's x-coordinate, y-coordinate, z-coordinate, roll angle, pitch angle, and yaw angle are extracted as the six-degree-of-freedom pose output.

[0015] Secondly, the present invention also proposes a vehicle-mounted real-time positioning and mapping system based on a domain controller and lidar, characterized in that it includes: LiDAR: Used to collect three-dimensional point cloud data of the environment surrounding a vehicle; Domain controller: Used to execute the vehicle real-time positioning and mapping method based on domain controller and LiDAR; The lidar is connected to the domain controller via a high-speed Ethernet cable and a synchronization signal line.

[0016] The effects described in the invention are merely those of the embodiments, and not all the effects of the invention. One of the above technical solutions has the following advantages or beneficial effects: This invention proposes a vehicle-mounted real-time localization and mapping method and system based on a domain controller and LiDAR, belonging to the field of vehicle-mounted localization and mapping technology. The method includes the following steps: receiving and preprocessing 3D point cloud data acquired by LiDAR to obtain preprocessed point cloud data; performing scan-to-scan matching between the current frame and its previous frame in the preprocessed point cloud data, outputting the relative pose transformation matrix between adjacent frames; determining the initial pose for scan-to-map matching based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame; calculating the displacement change and angle change between the current frame and the previous keyframe; when the displacement change reaches the final translation threshold or the angle change reaches the preset rotation threshold, the point cloud data of the current frame and the global pose transformation matrix of the current frame are... The transformation matrix and its corresponding covariance matrix are stored in the keyframe database. The first set of keyframe point clouds closest to the current pose and the second set of keyframe point clouds forming the convex hull boundary are retrieved from the keyframe database. The two sets of keyframe point clouds are stitched together to generate a multi-scale local sub-map. The point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output a globally optimized pose transformation matrix. The point cloud data of the current frame is transformed to the world coordinate system according to the globally optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as a six-degree-of-freedom pose output. Based on this method, the present invention also proposes a corresponding system. The present invention achieves high-precision, high-real-time, and computationally adaptive vehicle localization and mapping by using nanosecond-level time synchronization between the domain controller and the lidar, and an improved DLO algorithm that dynamically adjusts the keyframe selection and sub-map construction scale based on hardware load and environmental spaciousness.

[0017] This invention employs PTP / gPTP hardware synchronization technology to improve the clock synchronization accuracy between the lidar and the computing unit to the nanosecond level, fundamentally solving the point cloud distortion and pose estimation errors caused by time misalignment, resulting in higher positioning accuracy and better modeling. Figure 1 It has better consistency.

[0018] This invention leverages the powerful computing capabilities of a high-performance domain controller, combined with a lightweight and improved DLO algorithm, enabling the system to handle the massive amounts of data generated by the RoboSense RS-Helios-16p LiDAR with ease. The dynamic keyframe selection strategy further ensures that the system maintains a stable processing frame rate even in complex scenarios or when computing power fluctuates, avoiding data loss and positioning interruptions, and meeting the stringent real-time requirements of automotive applications.

[0019] The improved DLO algorithm in this invention can proactively sense hardware load and adaptively adjust algorithm behavior according to load status, thus achieving optimal matching between the algorithm and computing resources. Attached Figure Description

[0020] Figure 1This is a flowchart of the vehicle real-time positioning and mapping method based on domain controller and lidar proposed in Embodiment 1 of the present invention; Figure 2 This is a schematic diagram of the three-dimensional point cloud map proposed in Embodiment 1 of the present invention; Figure 3 This is a schematic diagram of the vehicle-mounted real-time positioning and mapping system based on a domain controller and lidar proposed in Embodiment 2 of the present invention. Figure 4 The present invention provides a schematic diagram of a vehicle for embodiment 3 of the present invention. Detailed Implementation

[0021] To clearly illustrate the technical features of this solution, the invention will be described in detail below through specific embodiments and in conjunction with the accompanying drawings. The following disclosure provides many different embodiments or examples for implementing different structures of the invention. To simplify the disclosure of the invention, components and arrangements of specific examples are described below. Furthermore, reference numerals and / or letters may be repeated in different examples. This repetition is for simplification and clarity and does not in itself indicate a relationship between the various embodiments and / or arrangements discussed. It should be noted that the components illustrated in the drawings are not necessarily drawn to scale. Descriptions of well-known components, processing techniques, and processes are omitted in this invention to avoid unnecessarily limiting the invention.

[0022] I. Definitions 1. IMU (Inertial Measurement Unit); 2. SLAM (Simultaneous Localization and Mapping): Simultaneous localization and mapping.

[0023] 3. DLO Algorithm (Direct LiDAR Odometry): The DLO algorithm is a lightweight and computationally efficient LiDAR odometry calculation method that can directly process dense point clouds and estimate the robot's pose in real time. 4. FPGA (Field Programmable Gate Array): An FPGA is a programmable hardware chip that allows users to configure and program its internal logic circuits in the field (i.e., in the application scenario) to achieve specific hardware functions. 5. PPS (Pulse Per Second): The PPS signal is a pulse-per-second signal, a precise time reference signal that outputs one pulse per second, usually generated by GPS (Global Positioning System) or BeiDou satellite navigation modules; 6. NTP (Network Time Protocol): Network Time Protocol; 7. LiDAR (Light Detection and Ranging): A type of sensor that measures the distance to a target and generates three-dimensional point cloud data by emitting a laser beam and receiving the reflected echo. 8. PTP (Precision Time Protocol): Precision Time Protocol; gPTP (Generalized Precision Time Protocol): A generalized precision time protocol.

[0024] Example 1 Embodiment 1 of this invention proposes a vehicle-mounted real-time positioning and mapping method based on a domain controller and LiDAR, which is used to solve the technical problems of existing vehicle-mounted LiDAR positioning and mapping systems, such as low time synchronization accuracy (millisecond level), processing delay caused by computing power bottleneck, and the inability to dynamically adapt the algorithm and hardware resources, making it difficult to meet the requirements of high-precision real-time positioning.

[0025] This invention employs a domain controller, which adds hardware load awareness to the original DLO algorithm. Based on this, it dynamically adjusts the keyframe selection threshold and subgraph construction scale, achieving deep collaboration between the algorithm and hardware resources. This solves the problems of computing power bottleneck and insufficient real-time performance in existing solutions.

[0026] Figure 1 This is a flowchart of the vehicle real-time positioning and mapping method based on domain controller and lidar proposed in Embodiment 1 of the present invention; In step S1, the domain controller sends a synchronization signal to the lidar as the master clock via the synchronization signal line, thereby achieving nanosecond-level time synchronization with the lidar.

[0027] The domain controller unit connects to the lidar unit via a synchronization signal line (which can be integrated with the Ethernet cable or a separate high-precision coaxial cable) to transmit PTP / gPTP synchronization signals. After the system powers on, the domain controller continuously sends PTP / gPTP synchronization signals to the lidar via the synchronization signal line to ensure that their clocks are strictly aligned. The domain controller acts as the master clock, and the lidar acts as the slave clock, achieving time synchronization.

[0028] In addition to the PTP / gPTP protocol, this application can also adopt other hardware synchronization schemes with nanosecond-level synchronization accuracy, such as directly connecting the PPS signal of the GPS / BeiDou module to the hardware I / O interface of the domain controller and the lidar.

[0029] In step S2, the three-dimensional point cloud data collected by the lidar is received and preprocessed to obtain preprocessed point cloud data; The LiDAR, specifically the RoboSense RS-Helios-16p LiDAR, is mounted on the roof of the target vehicle to acquire 3D point cloud data of the surrounding environment. The LiDAR unit rotates and scans at a fixed frequency, generating dense point cloud data. Each point cloud data packet is tagged with a precise timestamp provided by a synchronization module. The timestamped point cloud data is then transmitted in real-time to the domain controller via a high-speed Ethernet cable.

[0030] The lidar is physically connected to the domain controller via a gigabit / 10-gigabit Ethernet cable. This connection is used to transmit the point cloud data acquired by the lidar to the domain controller at high speed and without loss.

[0031] The domain controller serves as the central computing platform, integrating a high-performance multi-core CPU, GPU, large-capacity memory, and high-speed solid-state drive. This domain controller is specifically designed to receive and preprocess 3D point cloud data acquired by LiDAR, run algorithms, process data, and implement system control.

[0032] The preprocessing of 3D point cloud data includes: First, invalid point cloud data generated by the LiDAR scanning of vehicle components (such as the front, roof, and bumper) is filtered out. Specifically, based on the LiDAR's installation location and calibration parameters on the vehicle, a near-field distance threshold (e.g., 0.5 meters) is set, and all point clouds with a distance less than this threshold from the LiDAR origin are deleted to avoid interference from vehicle body reflections in subsequent localization and mapping calculations.

[0033] Then, voxel mesh downsampling is performed to reduce the amount of point cloud data and lower the computational burden while preserving the point cloud geometry. Specifically, the 3D space is divided into a fixed-size voxel mesh (e.g., 0.1m × 0.1m × 0.1m), and only one representative point is retained in each voxel mesh (usually the centroid or the point closest to the centroid). This is used to control the number of point clouds per frame within a reasonable range (e.g., reducing it from hundreds of thousands of points to thousands of points), which facilitates efficient subsequent processing.

[0034] In step S3, the current frame in the preprocessed point cloud data is scanned to the previous frame for scanning matching, and the relative pose transformation matrix between adjacent frames is output; and the initial pose for scanning to map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame. The scan-to-scan process in this step includes: The registration process begins with an initial transformation matrix. This serves as the starting point for the iteration. If the IMU is available, its angular velocity integral is used to obtain the rotation estimate between the two frames, while the translation component is set to zero. ; in, This represents the rotation matrix obtained by integrating the angular velocity of the IMU.

[0035] If there is no IMU, initialize to the identity matrix. .

[0036] In this invention, the inertial measurement unit (IMU) is used to collect triaxial acceleration and triaxial angular velocity.

[0037] Establish point cloud correspondence for each point in the source point cloud. In the target point cloud Find its nearest neighbor. This process reuses the KD-Tree data structure for fast nearest neighbor search: 1. KD-Tree for target point cloud: The tree has already been built in the Scan-to-Scan of the previous frame and can be reused directly.

[0038] 2. Source Point Cloud's KD-Tree: For The tree is built once, and Scan-to-Map matching reuses the tree.

[0039] For each pair of corresponding points Its residual is defined as: ; covariance matrix and Let represent the planar distribution of the local neighborhoods of the source and target points, respectively, typically calculated using principal component analysis (PCA) of the neighborhood point set. The objective function minimized by GICP is: ; in, Indicates the first Frame relative to the first The relative pose transformation matrix, ; Denotes the transformation matrix variables to be solved; denotes the first... The residual vector corresponding to each point; This represents the transpose of the residual vector; Indicates the first point in the current frame source point cloud One point; Indicates the target point cloud in the previous frame and The corresponding nearest neighbor; express The covariance matrix of the local neighborhood; express The covariance matrix of the local neighborhood.

[0040] This nonlinear least squares problem is solved iteratively using the Gauss-Newton method or the Levenberg-Marquardt method.

[0041] Before each iteration, the RANSAC algorithm is used to screen interior points: three pairs of corresponding points are randomly selected, the hypothetical transformation is calculated, and then the number of points in all pairs that satisfy the transformation is counted. This process is repeated multiple times, and the transformation with the most interior points is retained as the initial estimate for this iteration. Then, GICP optimization is performed using only interior points.

[0042] Each iteration will be based on the current transformation Recalculate the residuals ,in Construct a system of linear equations and solve for the update variables. and update the transformation Iterate until the convergence condition is met or the maximum number of iterations is reached, and finally output the relative pose. .

[0043] The initial pose for map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame, specifically as follows: ; in, Indicates the initial pose; This represents the global pose transformation matrix of the previous frame; .

[0044] In step S4, the displacement change and angle change between the current frame and the previous keyframe are calculated; when the displacement change reaches the final translation threshold or the angle change reaches the preset rotation threshold, the point cloud data of the current frame, the global pose transformation matrix of the current frame and the corresponding covariance matrix are stored in the keyframe database. ; ; in, Indicates the amount of displacement change; Represents the global translation vector for the current frame; Represents the global translation vector of the previous keyframe; Indicates the change in angle; This represents the quaternion corresponding to the global rotation in the current frame; This represents the quaternion corresponding to the global rotation in the previous keyframe.

[0045] For the lightweight point cloud of the current frame Each point in Its Euclidean distance to the origin of the lidar Specifically ; For sets Sort and take the median To smooth fluctuations in the median sequence Perform first-order low-pass filtering: ; in, The median sequence values ​​after filtering, initial value .

[0046] Based on the filtered median sequence value The base translation threshold is mapped according to the following rules, specifically: ; in, This represents the filtered median sequence value; Indicates the long-distance threshold; Indicates the mid-distance threshold; Indicates the near-distance threshold; Indicates a high translation threshold; Indicates the translation threshold; Indicates a low translation threshold; Indicates the minimum translation threshold; The value is 20m. The value is 10m. The value is 5m; The value is 10m. The value is 5m. The value is 1m. The value is 0.5m. At this time... Represented as: .

[0047] Real-time hardware load is represented as: ; in, Indicates the CPU utilization of the domain controller; This indicates the GPU utilization of the domain controller; Indicates the first weighting coefficient; This represents the second weighting coefficient; .

[0048] according to The values ​​are divided into four intervals, which are mapped to hardware adjustment coefficients. ; The hardware adjustment coefficient is expressed as: ; in, Indicates real-time hardware load; Indicates a low load threshold; Indicates the load threshold; Indicates a high load threshold; Indicates a low load regulation coefficient; This represents the normal adjustment coefficient; Indicates a high load adjustment coefficient; This represents the maximum value of the load regulation coefficient; The value is 0.8. The value is 1.0. The value is 1.2. The value is 1.5; The value is 40%. The value is 60%. The value is 80%. Represented as: ; Among them, the hardware adjustment coefficient The number and specific values ​​of the tier classifications can be configured based on the computing power of the domain controller, the complexity of the application scenario, and real-time requirements.

[0049] The scope of protection of this invention is not limited to the specific values ​​listed in Example 1, and those skilled in the art can make reasonable selections based on the actual situation.

[0050] Basic translation threshold Adjustment coefficients output by the hardware load sensing module Calculate the final translation threshold Simultaneously receive the pose change calculation module. and When the displacement change achieve or change in angle When the set rotation threshold is reached, the current frame is determined to be a new keyframe, and its lightweight point cloud is processed. Global pose and GICP matching covariance matrix Write to the keyframe database.

[0051] In step S5, the first set of keyframe point clouds closest to the current pose and the second set of keyframe point clouds constituting the convex hull boundary are retrieved from the keyframe database; the two sets of keyframe point clouds are stitched together to generate a multi-scale local sub-map. The keyframe database includes a lightweight point cloud for each frame. Global pose and the covariance matrix used for subsequent GICP matching It provides a nearest neighbor retrieval interface based on pose Euclidean distance (given the current pose). Return the nearest (1 keyframe), and a boundary keyframe retrieval interface based on the convex hull of all keyframe positions (extracting the global positions of all keyframes from the keyframe database, calculating their convex hulls, and selecting the keyframes that constitute the boundary of the convex hull). (Keyframes).

[0052] Number of point clouds in the first set of keyframes and the number of point clouds in the second set of keyframes The load is dynamically determined by the real-time hardware load, specifically: ; in, These represent the number of point clouds in the first group of keyframes under high, medium, and low load conditions, respectively, and satisfy the following conditions: ; These represent the number of point clouds in the second set of keyframes under high, medium, and low load conditions, respectively, and satisfy the following conditions: ; in, The number and specific values ​​of the tier classifications can be configured based on the computing power of the domain controller, the complexity of the application scenario, and real-time requirements. For example: ; The two sets of keyframe point clouds are stitched together to generate multi-scale local sub-images, specifically: ; in, Represents a multi-scale local subgraph; This represents the point cloud of the first set of keyframes; This represents the point cloud of the second set of keyframes.

[0053] In step S6, the point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output the global optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the global optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as the six-degree-of-freedom pose output.

[0054] This step employs the GICP algorithm from step S3, using the initial pose as the initial value for iteration. The point cloud data of the current frame is registered with the multi-scale local sub-image, outputting a globally optimized pose transformation matrix. ; Transform the point cloud data of the current frame to the world coordinate system according to the global optimized pose transformation matrix. ;in, Point cloud for the current frame any point in it; Represents the coordinates transformed to the world coordinate system; Transform all points Overlay on the global map In, and from The vehicle's x-coordinate, y-coordinate, z-coordinate, roll angle, pitch angle, and yaw angle are extracted as the six-degree-of-freedom pose output. Figure 2 This is a schematic diagram of a three-dimensional point cloud map proposed in Embodiment 1 of the present invention.

[0055] The scope of protection of this invention is not limited to the detailed values ​​listed in Example 1. Those skilled in the art can make reasonable selections and adjustments based on the actual situation.

[0056] The vehicle-mounted real-time positioning and mapping method based on domain controller and lidar provided in Embodiment 1 of this invention no longer adopts the loose combination, coarse synchronization, and algorithm-hardware separation implementation method of the existing scheme. Instead, it: (1) adopts a high-performance domain controller as the core computing unit and establishes a physical connection between lidar and domain controller through high-speed Ethernet data line and PTP / gPTP synchronization signal line. (2) uses hardware-level PTP / gPTP Ethernet time synchronization technology to establish a nanosecond-level precise synchronization link between lidar and domain controller, fundamentally eliminating point cloud distortion and pose estimation errors caused by time misalignment. (3) deploys a new algorithm architecture that can directly "sense" the hardware status, namely the improved DLO algorithm, which enables it to dynamically and adaptively adjust the key frame selection strategy and sub-graph construction scale according to the real-time computing load (CPU / GPU utilization) of the domain controller and environmental geometric features, thereby realizing deep collaboration between algorithm and hardware resources.

[0057] Example 2 Based on Embodiment 2 of the present invention, an on-board real-time positioning and mapping system based on a domain controller and lidar is also proposed. Figure 3 This is a schematic diagram of the vehicle-mounted real-time positioning and mapping system based on a domain controller and lidar proposed in Embodiment 2 of the present invention. The system includes: LiDAR: The RoboSense RS-Helios-16p LiDAR is installed on the roof of the target vehicle to collect 3D point cloud data of the surrounding environment. Domain controller: Used to execute the vehicle real-time positioning and mapping method based on domain controller and lidar disclosed in Example 1; The domain controller serves as a central computing platform, integrating a high-performance multi-core CPU, GPU, large-capacity memory, and high-speed solid-state drive. It is used to receive and preprocess 3D point cloud data acquired by LiDAR, run algorithms, process data, and implement system control.

[0058] The domain controller in this system employs an improved DLO algorithm, achieving high-precision, high-real-time performance, and computationally adaptive vehicle positioning and mapping. Specifically, this includes: In step S1, the domain controller sends a synchronization signal to the lidar as the master clock via the synchronization signal line, thereby achieving nanosecond-level time synchronization with the lidar.

[0059] In step S2, the three-dimensional point cloud data collected by the lidar is received and preprocessed to obtain preprocessed point cloud data; In step S3, the current frame in the preprocessed point cloud data is scanned to the previous frame for scanning matching, and the relative pose transformation matrix between adjacent frames is output; and the initial pose for scanning to map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame. In step S4, the displacement change and angle change between the current frame and the previous keyframe are calculated; when the displacement change reaches the final translation threshold or the angle change reaches the preset rotation threshold, the point cloud data of the current frame, the global pose transformation matrix of the current frame and the corresponding covariance matrix are stored in the keyframe database. In step S5, the first set of keyframe point clouds closest to the current pose and the second set of keyframe point clouds constituting the convex hull boundary are retrieved from the keyframe database; the two sets of keyframe point clouds are stitched together to generate a multi-scale local sub-map. In step S6, the point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output the global optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the global optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as the six-degree-of-freedom pose output.

[0060] The lidar and domain controller are physically connected via at least one high-speed Ethernet cable (for data transmission) and at least one synchronization signal line (for transmitting PTP / gPTP synchronization signals) to achieve high-speed transmission of massive point cloud data and nanosecond-level hardware time synchronization.

[0061] The domain controller used in the vehicle-mounted real-time positioning and mapping system based on domain controller and lidar proposed in Embodiment 2 of the present invention can also be replaced by other dedicated computing units with powerful parallel computing capabilities, such as a ruggedized vehicle-mounted computer equipped with a high-performance discrete graphics card.

[0062] The description of the relevant parts of the vehicle-mounted real-time positioning and mapping system based on domain controller and lidar provided in Embodiment 2 of this application can be found in the detailed description of the corresponding parts of the vehicle-mounted real-time positioning and mapping system based on domain controller and lidar provided in Embodiment 1 of this application, and will not be repeated here.

[0063] Example 3 Embodiment 3 of the present invention also proposes a vehicle, Figure 4A schematic diagram of a vehicle is provided for Embodiment 3 of the present invention. The vehicle includes: Vehicle body; And a vehicle-mounted real-time positioning and mapping system based on a domain controller and lidar, installed on the vehicle body.

[0064] The lidar unit is mounted on the top of the vehicle body (e.g., at the front or center of the roof) to collect 3D point cloud data of the 360-degree environment surrounding the vehicle. The lidar unit is connected to the domain controller unit via a high-speed Ethernet cable and a synchronization signal line.

[0065] The domain controller unit is installed in the center of the vehicle's cockpit (e.g., under the passenger seat, behind the glove box, or inside the dashboard), and integrates a multi-core CPU, GPU, memory, and solid-state drive. As the master clock, the domain controller unit sends PTP / gPTP synchronization signals to the LiDAR unit via a synchronization signal line to achieve nanosecond-level time synchronization; it receives point cloud data collected by the LiDAR unit via a high-speed Ethernet cable and executes the vehicle-mounted real-time positioning and mapping method based on the domain controller and LiDAR as described in claim 1. Specifically, it includes: In step S1, the domain controller sends a synchronization signal to the lidar as the master clock via the synchronization signal line, thereby achieving nanosecond-level time synchronization with the lidar.

[0066] In step S2, the three-dimensional point cloud data collected by the lidar is received and preprocessed to obtain preprocessed point cloud data; In step S3, the current frame in the preprocessed point cloud data is scanned to the previous frame for scanning matching, and the relative pose transformation matrix between adjacent frames is output; and the initial pose for scanning to map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame. In step S4, the displacement change and angle change between the current frame and the previous keyframe are calculated; when the displacement change reaches the final translation threshold or the angle change reaches the preset rotation threshold, the point cloud data of the current frame, the global pose transformation matrix of the current frame and the corresponding covariance matrix are stored in the keyframe database. In step S5, the first set of keyframe point clouds closest to the current pose and the second set of keyframe point clouds constituting the convex hull boundary are retrieved from the keyframe database; the two sets of keyframe point clouds are stitched together to generate a multi-scale local sub-map. In step S6, the point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output the global optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the global optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as the six-degree-of-freedom pose output.

[0067] The vehicle also includes an inertial measurement unit (IMU), which is installed in the center of the vehicle body near the center of gravity. It is connected to the domain controller unit via a CAN bus to collect the vehicle's three-axis acceleration and three-axis angular velocity data, providing an initial rotation estimate for scan-to-scan matching.

[0068] The vehicle also includes a GPS / BeiDou module, which is installed on the top of the vehicle body and connected to the domain controller unit via a CAN bus or a dedicated signal line to provide positioning information and PPS second pulse signals.

[0069] The vehicle provided in Embodiment 3 of this invention, equipped with an onboard real-time positioning and mapping system based on a domain controller and LiDAR, can determine its own six-degree-of-freedom pose (x, y, z coordinates and roll, pitch, and yaw angles) in real time and with high precision during driving, and simultaneously construct a three-dimensional point cloud map of the surrounding environment. This system employs PTP / gPTP nanosecond-level time synchronization technology to solve the problems of point cloud distortion and pose estimation errors caused by time misalignment; it adopts an improved DLO algorithm that dynamically adjusts keyframe selection and sub-map construction scale based on hardware load and environmental space, achieving deep collaboration between the algorithm and hardware resources. When computing power is limited, it proactively reduces accuracy to maintain real-time performance, and improves mapping accuracy when computing power is sufficient.

[0070] The vehicle provided in Embodiment 3 of this invention can be widely used in scenarios such as autonomous driving, advanced driver assistance systems (ADAS), automatic parking, and high-precision map collection.

[0071] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that the elements inherent in a process, method, article, or apparatus that includes a list of elements are included. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element. Additionally, portions of the technical solutions provided in the embodiments of this application that are consistent with the implementation principles of corresponding technical solutions in the prior art have not been described in detail to avoid excessive elaboration.

[0072] While specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art can make other modifications or variations based on the above description. It is neither necessary nor possible to exhaustively describe all embodiments here. Various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar, characterized in that, The following steps are performed by the vehicle domain controller: Receive and preprocess 3D point cloud data collected by lidar to obtain preprocessed point cloud data; The current frame in the preprocessed point cloud data is scanned to match the previous frame, and the relative pose transformation matrix between adjacent frames is output. The initial pose for scan-to-map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame. Calculate the displacement and angle changes between the current frame and the previous keyframe; when the displacement changes reach the final translation threshold or the angle changes reach the preset rotation threshold, store the point cloud data of the current frame, the global pose transformation matrix of the current frame, and the corresponding covariance matrix into the keyframe database. Retrieve the first set of keyframe point clouds closest to the current pose and the second set of keyframe point clouds that form the convex hull boundary from the keyframe database; stitch the two sets of keyframe point clouds together to generate a multi-scale local sub-image. The point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output the global optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the global optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as the six-degree-of-freedom pose output.

2. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 1, characterized in that, Before receiving the 3D point cloud data collected by the lidar, the vehicle domain controller also performs the following: sending a synchronization signal to the lidar as the master clock via a synchronization signal line to achieve nanosecond-level time synchronization with the lidar.

3. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 1, characterized in that, The current frame in the preprocessed point cloud data is scanned and matched with the previous frame to output the relative pose transformation matrix between adjacent frames, specifically: ； in, Indicates the first Frame relative to the first The relative pose transformation matrix, ; Represents the transformation matrix variable to be solved; Indicates the first The residual vector corresponding to each point; This represents the transpose of the residual vector; Indicates the first point in the current frame source point cloud One point; Indicates the target point cloud in the previous frame and The corresponding nearest neighbor; express The covariance matrix of the local neighborhood; express The covariance matrix of the local neighborhood.

4. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 3, characterized in that, The initial pose for map matching is determined based on the relative pose transformation matrix and the global pose transformation matrix of the previous frame, specifically as follows: ； in, Indicates the initial pose; This represents the global pose transformation matrix of the previous frame; .

5. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 4, characterized in that, Calculate the displacement and angle changes between the current frame and the previous keyframe, specifically as follows: ； ； in, Indicates the amount of displacement change; Represents the global translation vector for the current frame; Represents the global translation vector of the previous keyframe; Indicates the change in angle; This represents the quaternion corresponding to the global rotation in the current frame; This represents the quaternion corresponding to the global rotation in the previous keyframe.

6. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 1, characterized in that, The final translation threshold The method for determining this is as follows: ; in, This represents the basic translation threshold determined by the environmental spaciousness index; This represents the hardware adjustment factor determined based on the real-time hardware load.

7. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 6, characterized in that, The basic translation threshold is expressed as: ； in, This represents the filtered median sequence value; Indicates the long-distance threshold; Indicates the mid-distance threshold; Indicates the near-distance threshold; Indicates a high translation threshold; Indicates the translation threshold; Indicates a low translation threshold; Indicates the minimum translation threshold; The hardware adjustment coefficient is expressed as: ； in, Indicates real-time hardware load; Indicates a low load threshold; Indicates the load threshold; Indicates a high load threshold; Indicates a low load regulation coefficient; This represents the normal adjustment coefficient; Indicates a high load adjustment coefficient; This represents the maximum value of the load regulation coefficient; The real-time hardware load is represented as follows: ； in, Indicates the CPU utilization of the domain controller; This indicates the GPU utilization of the domain controller; Indicates the first weighting coefficient; This represents the second weighting coefficient; .

8. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 7, characterized in that, Number of point clouds in the first set of keyframes and the number of point clouds in the second set of keyframes The load is dynamically determined by the real-time hardware load, specifically: ； in, These represent the number of point clouds in the first group of keyframes under high, medium, and low load conditions, respectively, and satisfy the following conditions: ; These represent the number of point clouds in the second set of keyframes under high, medium, and low load conditions, respectively, and satisfy the following conditions: ; The two sets of keyframe point clouds are stitched together to generate multi-scale local sub-images, specifically: ； in, Represents a multi-scale local subgraph; This represents the point cloud of the first set of keyframes; This represents the point cloud of the second set of keyframes.

9. The vehicle-mounted real-time positioning and mapping method based on a domain controller and lidar according to claim 1, characterized in that, The point cloud data of the current frame is scanned and matched with the multi-scale local sub-map to the map, and the initial pose is used as the initial value for iteration to output a globally optimized pose transformation matrix; the point cloud data of the current frame is transformed to the world coordinate system according to the globally optimized pose transformation matrix, superimposed on the global map, and the vehicle's coordinates and attitude angles are extracted as a six-degree-of-freedom pose output, specifically: The GICP algorithm is used, with the initial pose as the initial value for iteration. The point cloud data of the current frame is registered with the multi-scale local sub-image, and a globally optimized pose transformation matrix is ​​output. ; Transform the point cloud data of the current frame to the world coordinate system according to the global optimized pose transformation matrix. ;in, Point cloud for the current frame any point in it; Represents the coordinates transformed to the world coordinate system; Transform all points Overlay on the global map In, and from The vehicle's x-coordinate, y-coordinate, z-coordinate, roll angle, pitch angle, and yaw angle are extracted as the six-degree-of-freedom pose output.

10. A vehicle-mounted real-time positioning and mapping system based on a domain controller and lidar, characterized in that, include: LiDAR: Used to collect three-dimensional point cloud data of the environment surrounding a vehicle; Domain controller: used to execute the vehicle real-time positioning and mapping method based on domain controller and lidar as described in any one of claims 1 to 9; The lidar is connected to the domain controller via a high-speed Ethernet cable and a synchronization signal line.

Images