Vehicle pose determination method, device, equipment, medium and product

Abstract

This application discloses a method, apparatus, device, medium, and product for determining vehicle pose. The method includes: acquiring the vehicle's position data, velocity data, and environmental spatial data at the current moment; predicting the vehicle's first pose data using the velocity data; matching point cloud information with a preset point cloud map to determine the vehicle's second pose data; adjusting the parameters of a preset error state filter based on the initialization data of the inertial measurement unit to obtain a target error state filter; and using the target error state filter to filter and fuse the position data, the first pose data, and the second pose data to determine the target pose data of the vehicle at the current moment. This improves the accuracy of vehicle pose data acquisition and, with the assistance of multiple sensors, enhances the adaptability and flexibility of the vehicle in performing pose acquisition in different environments, thereby improving the accuracy of autonomous driving.

Description

Technical Field

[0001] This application relates to the field of autonomous driving, and in particular to a method, apparatus, device, medium, and product for determining vehicle pose. Background Technology

[0002] With the development of new energy vehicles, autonomous driving technology has become an important component of intelligent transportation. In autonomous driving technology, vehicles need to perform operations such as steering and braking based on environmental perception, decision-making and planning, and automatic control capabilities.

[0003] In related technologies, high-precision positioning of vehicles in autonomous driving technology is the foundation for achieving safe driving, path planning and environmental perception of autonomous vehicles. Real-time dynamic differential positioning (RTK) is usually used to achieve the positioning of autonomous vehicles.

[0004] However, the aforementioned positioning methods have poor accuracy and robustness in complex environments, which has a significant impact on the safe operation of autonomous vehicles. Summary of the Invention

[0005] This application provides a method, apparatus, device, medium, and product for determining vehicle position and orientation. The technical solution provided by this application includes the following aspects.

[0006] According to one aspect of the embodiments of this application, a method for determining a vehicle's pose is provided, the method comprising: The vehicle's current position data, speed data, and environmental space data are acquired. The speed data is obtained through inertial measurement unit measurement, and the environmental space data includes point cloud information corresponding to multiple objects around the vehicle. The first pose data of the vehicle is predicted using the speed data; The point cloud information is matched with a preset point cloud map to determine the second pose data corresponding to the vehicle. The parameters corresponding to the preset error state filter are adjusted based on the initialization data of the inertial measurement unit to obtain the target error state filter; The target error state filter is used to filter and fuse the position data, the first pose data, and the second pose data to determine the target pose data of the vehicle at the current moment.

[0007] In an optional embodiment, the step of using the target error state filter to filter and fuse the position data, the first pose data, and the second pose data to determine the target pose data corresponding to the vehicle at the current moment includes: The target error state filter uses the first pose data and the second pose data to correct the position data, thereby obtaining the target pose data of the vehicle at the current moment.

[0008] In an optional embodiment, the step of correcting the position data using the first pose data and the second pose data through the target error state filter to obtain the target pose data of the vehicle at the current moment includes: The average value corresponding to the position data, the first pose data, and the second pose data is determined as the target pose data; or... Based on the position data, a first weight corresponding to the first pose data and a second weight corresponding to the second pose data are determined; the position data is then corrected based on the first weight and the second weight to obtain the target pose data; or, A first error between the first pose data and the position data is determined, and a second error between the second pose data and the position data is determined; the average error between the first error and the second error is determined, and the position data is adjusted using the average error to obtain the target pose data.

[0009] In an optional embodiment, adjusting the parameters corresponding to the preset error state filter based on the initialization data of the inertial measurement unit to obtain the target error state filter includes: Obtain the stationary speed data corresponding to the vehicle when it is stationary, and obtain the stationary duration of the vehicle in the stationary state; In response to the static duration meeting a preset duration requirement, the variance result corresponding to the static speed data is determined; If the variance result meets the preset value requirements, it is determined that the inertial measurement unit is successfully initialized, and the initial zero bias, noise variance and gravity direction corresponding to the inertial measurement unit are obtained. The initial zero bias is used to characterize the noise data generated by the vehicle in the velocity dimension in the stationary state. The target error state filter is obtained by adjusting the parameters of the preset error state filter based on the initial zero bias, the noise variance, and the gravity direction.

[0010] In an optional embodiment, the method further includes: Obtain the wheel speed data of the vehicle at the current moment, where the wheel speed data refers to the speed data corresponding to the vehicle's tires; The third pose data of the vehicle is predicted using the wheel speed data; The step of filtering and fusing the position data, the first pose data, and the second pose data using the target error state filter to determine the target pose data of the vehicle at the current moment includes: The target error state filter is used to filter and fuse the position data, the first pose data, the second pose data, and the third pose data to determine the target pose data corresponding to the vehicle at the current time.

[0011] In an optional embodiment, the step of matching the plurality of point cloud information with a preset point cloud map using a preset matching algorithm to determine the second pose data corresponding to the vehicle includes: The multiple point cloud information is filtered to obtain multiple filtered point cloud information. The filtered point cloud information is subjected to distortion correction processing to obtain the distorted point cloud information. The preset matching algorithm is used to match the multiple point cloud information after distortion removal with the preset point cloud map to determine the second pose data corresponding to the vehicle.

[0012] According to one aspect of the embodiments of this application, a vehicle pose determination device is provided, the device comprising: The acquisition module is used to acquire the vehicle's position data, speed data, and environmental space data at the current moment. The speed data is obtained by measuring the inertial measurement unit, and the environmental space data includes point cloud information corresponding to multiple objects around the vehicle. The prediction module is used to predict the first pose data of the vehicle using the speed data; and to match the point cloud information with a preset point cloud map to determine the second pose data corresponding to the vehicle. The adjustment module is used to adjust the parameters corresponding to the preset error state filter based on the initialization data of the inertial measurement unit to obtain the target error state filter; The determination module is used to filter and fuse the position data, the first pose data and the second pose data using the target error state filter to determine the target pose data of the vehicle at the current moment.

[0013] According to one aspect of the embodiments of this application, a computer device is provided, the computer device including a processor and a memory, the memory storing a computer program, the computer program being loaded and executed by the processor to implement the above-described method for determining vehicle pose.

[0014] According to one aspect of the embodiments of this application, a computer-readable storage medium is provided, wherein a computer program is stored in the computer-readable storage medium, the computer program being loaded and executed by a processor to implement the above-described method for determining vehicle pose.

[0015] According to one aspect of the embodiments of this application, a computer program product is provided, the computer program product including a computer program stored in a computer-readable storage medium, and a processor reading from the computer-readable storage medium and executing the computer program to implement the above-described method for determining vehicle pose.

[0016] The technical solutions provided in this application can bring at least the following beneficial effects: By using speed data to predict the vehicle's first pose data and using environmental spatial data to predict the vehicle's second pose data, the vehicle's position data acquired by the RTK module can be corrected based on the first and second pose data. This allows the first and second pose data to assist in the calibration of position data even when the RTK module's accuracy is poor or the signal is blocked, thus improving the accuracy of vehicle pose data acquisition. Furthermore, with the assistance of multiple sensors, the adaptability and flexibility of the vehicle in performing pose acquisition in different environments are improved, thereby enhancing the accuracy of autonomous driving. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of a vehicle pose determination system provided in one embodiment of this application; Figure 2 This is a flowchart of a method for determining vehicle pose provided in an exemplary embodiment of this application; Figure 3 This is a flowchart of a method for determining vehicle pose provided in another exemplary embodiment of this application; Figure 4 This is a flowchart illustrating the initialization process of an inertial measurement unit provided in an exemplary embodiment of this application; Figure 5 This is a flowchart of the overall process provided in an exemplary embodiment of this application; Figure 6 This is a structural block diagram of a vehicle pose determination device provided in an exemplary embodiment of this application; Figure 7 This is a structural block diagram of a computer device provided in one embodiment of this application. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0019] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numerals in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application as detailed in the appended claims.

[0020] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used in this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any and all possible combinations of one or more of the associated listed items.

[0021] It should be understood that although the terms first, second, etc., may be used in this application to describe various information, this information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, without departing from the scope of this application, a first parameter may also be referred to as a second parameter, and similarly, a second parameter may also be referred to as a first parameter. Depending on the context, the word "if" as used herein may be interpreted as "when," "when," or "in response to determination."

[0022] It should be noted that this application may display prompt interfaces, pop-ups, or output voice prompts before and during the collection of user data. These prompt interfaces, pop-ups, or voice prompts are used to inform the user that their data is being collected. This ensures that the application only begins the steps for collecting user data after receiving confirmation from the user regarding the prompt interface or pop-up; otherwise (i.e., without user confirmation), the steps for collecting user data end, meaning no user data is collected. In other words, all user data collected in this application is collected with the user's consent and authorization, and the collection, use, and processing of related user data must comply with the relevant laws, regulations, and standards of the relevant countries and regions.

[0023] With the development of new energy vehicles, autonomous driving technology has also made significant breakthroughs. As an important component of intelligent transportation, autonomous driving, with its powerful environmental perception, decision-making, planning, and automatic control capabilities, has become a research hotspot in the automotive industry. Compared with human driving, intelligent driving technology demonstrates numerous advantages in safety, comfort, and traffic efficiency. Through high-precision sensors and intelligent algorithms, it achieves real-time perception and accurate judgment of complex traffic environments, effectively reducing the risk of traffic accidents, improving the passenger experience, alleviating road congestion, and enhancing overall transportation efficiency. The application prospects of autonomous driving technology are broad, and it has become an important driving force for promoting the construction of intelligent transportation and smart cities.

[0024] The importance of positioning technology in autonomous driving is self-evident. High-precision positioning is fundamental to the safe driving, path planning, and environmental perception of autonomous vehicles. Autonomous driving systems need to obtain the vehicle's accurate position in the global coordinate system in real time, and then combine this with maps, sensors, and decision-making algorithms to make safe and reliable driving decisions. Positioning technology is one of the cornerstones of autonomous driving systems. Currently, single sensors are prone to failure, real-time dynamic differential positioning (RTK) is susceptible to signal interference, and cameras rely on lighting information. Furthermore, positioning systems often lack sufficient accuracy and robustness in complex environments (such as tall buildings obstructing view, tunnels, and inclement weather), significantly impacting the safe operation of autonomous vehicles.

[0025] To address the shortcomings of existing autonomous driving positioning technologies, this application proposes a multi-sensor fusion positioning method that integrates RTK, inertial measurement unit (IMU), wheel speed sensors, and lidar. This method effectively improves the positioning accuracy and robustness of vehicles in complex scenarios, reduces the impact of single sensor failure on the overall vehicle positioning, and ensures the safe operation of autonomous vehicles.

[0026] The vehicle pose determination method provided in this application embodiment is implemented by a computer device. The vehicle pose determination method can be implemented by the vehicle terminal alone, or it can be implemented by the vehicle terminal and the server working together.

[0027] When the method for determining the vehicle pose is implemented by the vehicle terminal alone, the vehicle's position data and speed data are obtained on the vehicle terminal side, and environmental space data is also obtained. The vehicle's first pose data is predicted based on the speed data, the vehicle's second pose data is predicted based on the environmental space data, and the vehicle's target pose data is generated based on the position data, the first pose data, and the second pose data.

[0028] When the method for determining the vehicle pose is jointly implemented by the vehicle terminal and the server, for example, the vehicle terminal collects the vehicle's position data, speed data, and environmental space data, and sends the position data, speed data, and environmental space data to the server. The server predicts the vehicle's first pose data based on the speed data, predicts the vehicle's second pose data based on the environmental space data, and generates the vehicle's target pose data based on the position data, the first pose data, and the second pose data, and feeds the vehicle's target pose data back to the vehicle terminal.

[0029] Please refer to Figure 1 This illustration shows a schematic diagram of a vehicle pose determination system provided in one embodiment of this application. The computer system 100 includes a terminal 120, or a terminal 120 and a server 140. Specifically, when the vehicle pose determination method is implemented by a configuration of a terminal and a server, the computer system 100 includes a terminal 120 and a server 140.

[0030] The device type of terminal 120 includes at least one of the following: in-vehicle terminal, smartphone, laptop, smartwatch, desktop computer, tablet, intelligent robot, augmented reality (AR) device, virtual reality (VR) device, etc. The aforementioned terminal 120 is used within the vehicle's cabin environment.

[0031] In this embodiment, terminal 120 is implemented as a vehicle-mounted terminal, which is connected to server 140 via a wireless network or wired network. Terminal 120 collects vehicle location data, speed data, and environmental spatial data. The location data is data obtained by terminal 120 through an RTK module to express vehicle location information, the speed data is data obtained by inertial measurement unit (IMU) to express vehicle speed information, and the environmental spatial data is data obtained by lidar to express point cloud information of objects around the vehicle.

[0032] In one example, the method for determining the vehicle pose is implemented collaboratively by terminal 120 and server 140. Illustratively, after terminal 120 sends position data, velocity data, and ambient space data to server 140, server 140 generates target pose data for the vehicle based on the position data, velocity data, and ambient space data, and provides this target pose data to terminal 120. Thus, terminal 120 implements autonomous driving control of the vehicle based on the target pose data.

[0033] Server 140 is also used to provide terminal 120 with basic data required for positioning, such as map data.

[0034] Those skilled in the art will understand that the number of the aforementioned devices can be more or less. For example, there may be only one device, or there may be dozens or hundreds of devices, or even more. This application does not limit the number or type of devices.

[0035] Server 140 includes at least one of one server or multiple servers. In some embodiments, server 140 is implemented as a private server connected to terminal 120, and the model provided by server 140 is a private model deployed on the private server. Optionally, server 140 undertakes the main computing work, and terminal 120 undertakes the secondary computing work; or, server 140 undertakes the secondary computing work, and terminal 120 undertakes the main computing work; or, server 140 and terminal 120 adopt a distributed computing architecture for collaborative computing.

[0036] It is worth noting that the server 140 described above can be implemented as a physical server or as a cloud server in the cloud. In some embodiments, the server 140 can also be implemented as a node in a blockchain system.

[0037] Please refer to Figure 2 The diagram illustrates a flowchart of a method for determining a vehicle pose according to an exemplary embodiment of this application. This method can be implemented by a computer device (which may be configured as follows). Figure 1 The method is executed by the terminal 120 or server 140 shown, or it is executed jointly by the terminal and the server. In this embodiment, the method is executed by the terminal / vehicle terminal as an example. Figure 2 As shown, the method includes at least one of the following steps 210 to 250.

[0038] Step 210: Obtain the vehicle's location data, speed data, and environmental spatial data at the current moment.

[0039] Velocity data was obtained through inertial measurement unit (IMU) measurements. Environmental spatial data included point cloud information corresponding to multiple objects around the vehicle.

[0040] The vehicle's operational status data at the current moment is collected through various sensors on the vehicle. This operational status data includes location data, speed data, and environmental spatial data. The process of acquiring this operational status data is explained below.

[0041] Optionally, before running the algorithm, the user first calibrates the rotation and translation data of the lidar sensor and the RTL module receiver.

[0042] Location data: The initial location information of the vehicle is obtained through the RTK module, that is, the vehicle's current location data is obtained through the RTK module. RTK is a high-precision satellite positioning technology based on carrier phase observations, capable of achieving centimeter-level positioning accuracy in real time in the field. RTK technology has revolutionized the accuracy level of satellite positioning, improving from ordinary meter-level accuracy to centimeter-level accuracy, and is a core technology in the field of high-precision positioning.

[0043] The RTK module is optionally installed on the top of the vehicle to obtain unobstructed satellite signals and improve the reliability of location data acquisition; the RTK module is used to obtain the vehicle's latitude, longitude and elevation information.

[0044] The core principle of RTK technology lies in the shift from "single-point positioning" to "differential elimination." In conventional positioning technologies, the receiver receives satellite signals independently and calculates its own position. However, due to factors such as satellite clock errors, atmospheric delays (ionosphere, troposphere), and orbital errors, its accuracy is typically around 1-10 meters. RTK technology, on the other hand, uses a differential mode of "base station + rover" for positioning. By comparing the observation data of the two stations in real time, it can significantly eliminate common errors, achieving a qualitative leap.

[0045] The base station is erected at a point with precisely known coordinates. It receives satellite signals and calculates a differential correction value in real time. This differential correction value includes the sum of all common errors at the current location. The base station transmits the differential correction data to the rover in real time via radio, cellular network, or network RTK service. The rover receives the correction data from the base station while simultaneously receiving the raw satellite signals. It uses this correction data to correct its own observations.

[0046] Optionally, the vehicle's location data includes the vehicle's latitude and longitude coordinates, elevation data, horizontal accuracy, vertical accuracy, reference station, and other information.

[0047] Latitude and longitude coordinates are used to express the vehicle's position in the latitude and longitude dimensions. Elevation data is used to express the vehicle's height relative to a reference surface (such as the reference sea level). Horizontal accuracy is used to express the estimated position error in the horizontal dimension. Vertical accuracy is used to express the estimated elevation data error. The reference station is used to express the reference station data used to obtain the position data.

[0048] Speed ​​data: Vehicle speed data is obtained by measuring an inertial measurement unit (IMU). The IMU sensor collects acceleration and angular velocity along multiple axes of the vehicle at a preset frequency. Schematic, the IMU sensor collects the acceleration and angular velocity along the x, y, and z axes of the vehicle at a frequency of 100 Hz. In some embodiments, the current vehicle speed data is inferred based on previously collected vehicle speed data.

[0049] In some embodiments, the inertial measurement unit (IMU) includes an accelerometer and a gyroscope. The accelerometer is used to measure specific force, which is the inference per unit mass not caused by gravity. When at rest, the accelerometer measures the vector in the opposite direction of gravitational acceleration. The gyroscope is used to measure the three-axis angular velocity of the carrier coordinate system relative to inertial space.

[0050] Optionally, the current attitude angles of the vehicle, including roll angle, pitch angle and yaw angle, are obtained by integrating the angular velocity measured by the gyroscope. An attitude matrix is ​​obtained based on the attitude angles. The attitude matrix is ​​used to describe the transformation relationship from the carrier coordinate system to the navigation coordinate system. Based on the attitude matrix, the specific force measured by the accelerometer is transformed from the carrier coordinate system to the navigation coordinate system. The actual motion acceleration of the vehicle is obtained in the navigation coordinate system based on the gravity vector.

[0051] Environmental spatial data refers to point cloud data obtained by scanning and collecting the environment around a vehicle using lidar. Scanning the vehicle's surroundings and generating point cloud data using lidar is a sophisticated process integrating optical, mechanical, electronic, and computational technologies. Its core principle is to actively emit a laser beam, measure its round-trip time, and thus calculate the precise three-dimensional position of the target point.

[0052] First, the laser generates a highly concentrated, directional infrared or near-infrared laser pulse. This beam is collimated and shaped by a transmitting optics system (such as a lens), forming a very thin "light ruler." A scanning system directs this laser beam in a specific direction. The weak laser signal reflected from the object is collected by a receiving optics system (lens, filters). Filters filter out ambient light interference, allowing only specific wavelengths of laser light to pass through. The received light signal is converted into a weak electrical signal by a photodetector. After amplification and filtering, the precise time of flight is calculated by a timing circuit or processor. The distance to a single point is calculated based on the speed of light *c* and the measured time of flight. Combined with the precise pointing angle of the laser beam, the polar coordinates are converted to three-dimensional coordinates in the vehicle coordinate system. All points (tens of thousands to millions) obtained in a single scan (e.g., within 100 milliseconds) are aggregated to form a frame of point cloud data.

[0053] Each frame of point cloud data is a collection of massive 3D points. Each point contains at least the following information: 1. 3D coordinates: (x, y, z), accurate to the centimeter level; 2. Reflection intensity: a numerical value representing the reflectivity of the object's surface; 3. Timestamp: the precise acquisition time; 4. Possible additional information: the laser beam ID to which the point belongs, tag, etc.

[0054] The current moment refers to the current period in which the vehicle collects the above data. That is, the vehicle periodically collects location data, speed data, and environmental space data. For example, if the vehicle collects location data, speed data, and environmental space data at a frequency of 100Hz, then the current moment refers to the current collection period in which the vehicle is currently in.

[0055] The methods and results of collecting location data, velocity data, and environmental spatial data described above are merely illustrative examples, and the embodiments of this application do not limit them.

[0056] Step 220: Use the speed data to predict the vehicle's first pose data.

[0057] In some embodiments, vehicle speed data is dynamically and periodically collected, thereby predicting the vehicle's first pose data based on historical pose data acquired within a historical time period and continuously acquired speed data.

[0058] Among them, historical pose data is the target pose data acquired by the vehicle at a historical moment, that is, the pose data after comprehensive correction of data collected by multiple sensors.

[0059] Pose data is used to express the position and attitude of a vehicle. Position refers to the vehicle's location, and attitude refers to the vehicle's orientation, tilt angle, and other postures.

[0060] Optionally, the vehicle's historical pose data at a historical moment can be obtained, and the vehicle's first pose data at the current moment can be predicted based on the time difference between the historical moment and the current moment, as well as the vehicle's speed data within the time difference.

[0061] Optionally, an IMU sensor is used to collect the acceleration and angular velocity of the vehicle along the x, y, and z axes at a frequency of 100Hz. The vehicle's pose data is predicted for each velocity data collected in a recursive manner, and the first pose data of the vehicle at the current moment is obtained by continuously recursively collecting the pose data.

[0062] The first pose data includes the vehicle's first latitude and longitude, first elevation data, and first orientation. Among them, the first latitude and longitude are the vehicle's latitude and longitude information obtained through continuous recursive prediction, the first elevation data are the vehicle's elevation information obtained through continuous recursive prediction, and the first orientation is the vehicle's orientation information obtained through continuous recursive prediction.

[0063] Step 230: Match the point cloud information with the preset point cloud map to determine the second pose data corresponding to the vehicle.

[0064] After acquiring point cloud information of the vehicle's surrounding environment using the vehicle's LiDAR, a preset point cloud map is obtained. This can be either a high-definition map or a priori point cloud map. Taking the acquisition of a high-definition map as an example, the high-definition map includes point cloud data corresponding to each object in the road environment. The point cloud data is used to express the position information of the objects.

[0065] The acquisition of a pre-defined point cloud map is a complete industrial process, from professional data collection to refined post-processing. During the data collection process, a professional surveying and acquisition vehicle equipped with sensors is used. For example, this vehicle is equipped with a high-line-count LiDAR, capable of acquiring dense and accurate point cloud data. It is also equipped with a high-precision integrated navigation system, capable of outputting high-precision, high-frequency continuous pose data. Furthermore, it is equipped with a panoramic camera to acquire panoramic images, assisting in subsequent semantic annotation of the point cloud data.

[0066] A specialized mapping and data acquisition vehicle travels along a planned route on roads requiring mapping (such as highways and urban roads), simultaneously recording raw data from all sensors. This includes recording each frame of laser point cloud data and its corresponding high-precision pose. Using this acquired high-precision pose data, thousands of discrete point cloud frames are pieced together... Figure 1 In this way, they can be accurately pieced together to form a complete, continuous, and large-scale three-dimensional point cloud scene.

[0067] The core of a preset point cloud map is to remove moving objects (vehicles, pedestrians), flying points, and other temporary, non-static interference points from the point cloud data, retaining only static structures such as roads, guardrails, traffic signs, and buildings. This map describes the unchanging physical structure.

[0068] Based on the acquisition of 3D point cloud scenes, semantic information of different elements is labeled manually or with the help of AI algorithms and managed in layers. For example, the positioning layer includes the high-precision point cloud itself, which is used for vehicle positioning and matching; the road layer includes lane lines, lane center lines, lane types, slopes, curvatures, etc.; and the traffic facility layer includes the precise location and type of traffic signs, traffic lights, and road poles.

[0069] The massive amounts of raw point cloud data are compressed and converted into a format suitable for efficient reading and querying by the vehicle engine.

[0070] After the vehicle acquires the currently collected point cloud information, it matches the point cloud information with a preset point cloud map, finds the position in the preset point cloud map that matches the currently collected point cloud information, and then obtains the vehicle's current second pose data based on the matching result.

[0071] In some embodiments, when matching point cloud information with a preset point cloud map, the matching degree, such as similarity, between each point cloud region and the point cloud information is traversed in the preset point cloud map to obtain the location on the preset point cloud map that matches the point cloud information. Optionally, since the preset point cloud map is usually implemented as a large map area, a pre-selected area is first divided from the preset point cloud map based on the location data, and the point cloud information is matched with the point cloud data in the pre-selected area to obtain the location in the pre-selected area that matches the point cloud information.

[0072] In a schematic way, the center point is determined in the preset point cloud map using location data. Using the center point as the center, the preset distance is the block side length or block radius to divide the area into pre-selected regions. The matching degree between each point cloud region in the pre-selected region and the point cloud information is traversed to obtain the position in the pre-selected region that matches the point cloud information.

[0073] The second pose data includes the second latitude and longitude, the second elevation data, and the vehicle's second orientation.

[0074] In some embodiments, multiple point cloud information is filtered to obtain multiple filtered point cloud information, and the filtered point cloud information is then deformed to obtain multiple deformed point cloud information. A preset matching algorithm is used to match the deformed point cloud information with a preset point cloud map to determine the second pose data corresponding to the vehicle.

[0075] Filtering point cloud information is a crucial step in the LiDAR data processing workflow. Its core purpose is to remove noise, reduce data volume, and highlight effective information while preserving the key geometric structure of the scene, providing "clean" data for subsequent tasks such as registration, segmentation, and recognition. In some embodiments, in the field of autonomous driving, filtering point cloud information is used to achieve at least one of the following effects: ① Retaining points in front of the vehicle, on both sides, and within a certain height range, while removing point clouds from irrelevant areas such as the sky; ② Removing outlier noise, i.e., isolated noise points caused by dust, raindrops, sensing errors, etc., which are often present in point cloud information, and filtering removes outlier noise from the point cloud information; ③ Simplifying downsampling, as point cloud information is usually very large in volume, filtering reduces the data volume of point cloud information without destroying its geometry.

[0076] Distortion removal of point cloud information is essentially a "spatiotemporal alignment" process, which involves using known continuous motion trajectories to project each point from its actual acquisition time to a unified reference time. Optionally, point cloud information with timestamps, i.e., continuous high-frequency motion trajectories, can be input to perform distortion removal on the point cloud information based on these motion trajectories.

[0077] Optionally, step 220 can be executed first and then step 230, or step 230 can be executed first and then step 220, or steps 220 and 230 can be executed simultaneously.

[0078] Step 240: Adjust the parameters corresponding to the preset error state filter based on the initialization data of the inertial measurement unit to obtain the target error state filter.

[0079] Optionally, the initial zero bias, noise variance, and gravity direction of the IMU are estimated through static initialization. Based on the initial zero bias, noise variance, and gravity direction of the IMU, the parameters corresponding to the preset error state filter are adjusted to obtain the target error state filter.

[0080] Step 250: Use the target error state filter to filter and fuse the position data, the first pose data and the second pose data to determine the target pose data of the vehicle at the current moment.

[0081] In some embodiments, the position data is corrected using the first pose data and the second pose data by a target error state filter to obtain the target pose data of the vehicle at the current moment.

[0082] Error state filters are an efficient and robust algorithmic framework for state estimation in the field of autonomous driving. Their core function is not to directly estimate the complete state of the system, but rather to estimate and correct errors in the state in real time, thereby obtaining more accurate and stable navigation results. Error state refers to the deviation between the position data and the true position. By filtering the position data, the first pose data, and the second pose data using a target error state filter, error data is obtained. This error data is then used to correct the position data, resulting in the target pose data.

[0083] Optionally, the target error state filter is used to filter out data points in the first pose data and the second pose data that do not meet the data requirements, and to correct the position data based on the filtered first pose data and the filtered second pose data, thereby obtaining the target pose data of the vehicle at the current moment.

[0084] The methods for correcting position data using the first pose data and the second pose data include one or more of the following: 1. Determine the average value of the position data, the first pose data, and the second pose data as the target pose data; Optionally, the location data includes the vehicle's elevation data and latitude and longitude; the first pose data includes first latitude and longitude, first elevation data, and / or the vehicle's first orientation; the second pose data includes second latitude and longitude, second elevation data, and / or the vehicle's second orientation. When determining the target pose data, the elevation data, first elevation data, and second elevation data in the location data are averaged to obtain the elevation data in the target pose data; the latitude and longitude data, first latitude and longitude, and second latitude and longitude data in the location data are averaged to obtain the latitude and longitude in the target pose data. Optionally, the average of the first orientation and the second orientation is used to obtain the orientation in the target material data.

[0085] 2. Based on the position data, determine the first weight corresponding to the first pose data and the second weight corresponding to the second pose data; correct the position data based on the first weight and the second weight to obtain the target pose data; Using a target error state filter, a first weight for the first pose data and a second weight for the second pose data are determined based on the position data. The first and second weights are parameters used to correct the position data. In some embodiments, the first and second weights are multiplied by the position data to obtain the target pose data.

[0086] In some embodiments, a first weight is determined based on the difference between the first pose data and the position data. Schematic, the first latitude and longitude in the first pose data are divided by the latitude and longitude in the position data, and then divided by a preset first parameter to obtain a first sub-weight. Also, the first elevation data in the first pose data are divided by the elevation data in the position data, and then divided by a second parameter to obtain a second sub-weight. The first sub-weight and the second sub-weight are combined to obtain the first weight.

[0087] In some embodiments, a second weight is determined based on the difference between the second pose data and the position data. Schematic, the result of dividing the second latitude and longitude in the second pose data by the latitude and longitude in the position data is calculated and divided by a preset third parameter to obtain a third sub-weight. Also, the result of dividing the second elevation data in the second pose data by the elevation data in the position data is calculated and divided by a fourth parameter to obtain a fourth sub-weight. The third sub-weight and the fourth sub-weight are then combined to obtain the second weight.

[0088] The first and second weights are multiplied sequentially or together with the position data to obtain the target pose data.

[0089] It is worth noting that the above-described methods for determining the first weight / second weight and for correcting the position data based on the first weight / second weight are merely illustrative examples, and the embodiments of this application do not limit them.

[0090] 3. Determine the first error between the first pose data and the position data, and determine the second error between the second pose data and the position data; determine the average error of the first error and the second error, and adjust the position data using the average error to obtain the target pose data.

[0091] Optionally, a first difference is calculated between the first latitude and longitude in the first pose data and the latitude and longitude in the location data, and a second difference is calculated between the first elevation data in the first pose data and the elevation data in the location data. The first difference and the second difference are then fused to obtain a first error. For example, the average / weighted average of the first difference and the second difference is calculated to obtain the first error.

[0092] Similarly, calculate the third difference between the second latitude and longitude in the second pose data and the latitude and longitude in the location data, and calculate the fourth difference between the second elevation data in the second pose data and the elevation data in the location data. Then, fuse the third difference and the fourth difference to obtain the second error. For example, calculate the average / weighted average between the third difference and the fourth difference to obtain the second error.

[0093] After obtaining the first error and the second error, calculate the average / weighted average of the first error and the second error to obtain the average error. Use the average error to adjust the position data to obtain the target pose data. For example, calculate the difference between the position data and the average error to obtain the target pose data.

[0094] It is worth noting that the above-described methods for determining the first error / second error and correcting the position data based on the first error / second error are merely illustrative examples, and the embodiments of this application do not limit them.

[0095] In some embodiments, wheel speed data corresponding to the vehicle at the current moment is obtained. Wheel speed data refers to the speed data corresponding to the vehicle tires. The third pose data of the vehicle is predicted using the wheel speed data. The position data, the first pose data, the second pose data and the third pose data are filtered and fused using a target error state filter to determine the target pose data corresponding to the vehicle at the current moment.

[0096] In this system, wheel speed sensor data is incorporated to assist in short-term vehicle localization when RTK signals are poor or lost, improving system robustness. In some embodiments, the RTK signal is corrected based on continuously acquired wheel speed data. For example, the amplitude of RTK signal changes is acquired, and the RTK signal is corrected based on the matching between wheel speed data and amplitude changes to obtain third pose data. Wheel speed data expresses the vehicle's wheel speed at different times. Based on the travel time and wheel speed data, the distance traveled by the vehicle within the travel time can be inferred. Therefore, the RTK signal can be corrected based on the amplitude of RTK signal changes and the travel distance to obtain third pose data.

[0097] It is worth noting that the method of correcting the RTK signal using wheel speed data is merely an illustrative example, and the embodiments of this application do not limit it.

[0098] In summary, the method provided in this application uses speed data to predict the vehicle's first pose data and environmental spatial data to predict the vehicle's second pose data. Based on the first and second pose data, it corrects and adjusts the vehicle's position data acquired by the RTK module. This allows for the use of the first and second pose data to assist in position data calibration even when the RTK module's accuracy is poor or the signal is obstructed, thus improving the accuracy of vehicle pose data acquisition. Furthermore, the use of multiple sensors enhances the adaptability and flexibility of the vehicle in acquiring pose data in different environments, thereby improving the accuracy of autonomous driving.

[0099] In an optional embodiment, the target error state filter is adjusted based on the initialization data of the inertial measurement unit (IMU). Figure 3 This is a flowchart illustrating a method for determining a vehicle pose according to another exemplary embodiment of this application. The method can be implemented by a computer device (which may be configured as follows). Figure 1 The method is executed by the terminal 120 or server 140 shown, or it is executed jointly by the terminal and the server. In this embodiment, the method is executed by the terminal / vehicle terminal as an example. Figure 3 As shown, the method further includes at least one of the following steps 310 to 340.

[0100] Step 310: Obtain the stationary speed data corresponding to the vehicle when it is stationary, and obtain the stationary duration of the vehicle.

[0101] In other words, the initialization of the inertial measurement unit (IMU) begins when the vehicle is stationary. First, it checks if the IMU has been initialized. If not, it checks if the vehicle is stationary. If the vehicle is not stationary, it waits for the vehicle to come to a stop, clears the data queue, and returns. If the vehicle is stationary, it collects measurement data from the IMU. After continuously collecting IMU measurement data, it acquires initial zero bias, noise variance, and gravity direction data based on the data. Initial zero bias and noise variance refer to the high-frequency random noise and slowly changing zero bias present in the IMU measurement data. These parameters accumulate over time during the continuous collection of measurement data.

[0102] Determine whether the data collection time of the inertial measurement unit has reached the preset time requirement. If not, maintain the queue and continue collecting measurement data.

[0103] That is, on the one hand, the measurement data obtained by the IMU when the vehicle is stationary is acquired, and on the other hand, the collection time of the IMU measurement data is acquired to ensure that sufficient time of IMU measurement data is acquired to support the subsequent adjustment requirements of the target error state filter.

[0104] Step 320: In response to the static duration meeting the preset duration requirement, determine the variance result corresponding to the static speed data.

[0105] After the vehicle has been stationary for a preset period, the IMU is initialized. The variance of the measurement data collected by the IMU, i.e., the stationary speed data, is calculated, and the variance is checked to see if it passes. If the variance is less than a preset threshold, the variance check is considered passed, meaning the data collected by the inertial measurement unit during the vehicle's stationary period has stabilized. If the variance check fails, the inertial measurement unit initialization is considered to have failed.

[0106] Step 330: If the variance result meets the preset value requirements, determine that the inertial measurement unit has been successfully initialized, and obtain the initial zero bias, noise variance and gravity direction corresponding to the inertial measurement unit.

[0107] Initial zero bias is used to characterize the noise data generated by the vehicle in the velocity dimension when it is stationary.

[0108] Once the variance check passes, the inertial measurement unit is considered to have been successfully initialized, and data such as the initial zero bias, noise variance, and gravity direction measured by the inertial measurement unit are acquired.

[0109] Step 340: Adjust the parameters of the preset error state filter based on the initial zero bias, noise variance, and gravity direction to obtain the target error state filter.

[0110] Optionally, the acquired initial zero bias, noise variance, and gravity direction parameters are set into the error state filter to obtain the target error state filter.

[0111] Optionally, when adjusting the preset error state filter based on parameters such as initial zero bias, noise variance, and gravity direction, the initial state of the IMU is introduced into the preset error state filter. The resulting target error state filter can pre-fuse position data, first pose data, and second pose data based on the initial state of the IMU.

[0112] Indicative, Figure 4 This is a flowchart illustrating the initialization process of an inertial measurement unit provided in an exemplary embodiment of this application, as shown below. Figure 4 As shown, the process includes the following steps.

[0113] Step 401: Start IMU initialization.

[0114] This means triggering IMU initialization. Specifically, IMU initialization can be initiated after the vehicle is powered on, or in response to receiving an initialization control operation; or, the IMU initialization process can begin when the vehicle's RTK-acquired location data has not changed for a preset duration.

[0115] This is illustrative; the initialization process for the IMU is triggered immediately after the vehicle is powered on.

[0116] Step 402: Determine whether the IMU has been initialized.

[0117] That is, it determines whether the IMU has started initialization, or whether the IMU has completed initialization within a historical time period. For example, it determines whether the IMU has completed initialization within the past week. If it has, there is no need to repeat the IMU initialization. If it has not completed initialization, then the IMU initialization is performed.

[0118] Step 403: If the IMU is not initialized, determine whether the vehicle is stationary.

[0119] That is, it determines whether the vehicle's speed data is zero, or whether the vehicle's position data has not changed; or it determines the vehicle's gear position, such as if the vehicle is in park, which means the vehicle is stationary.

[0120] Optionally, the location data collected by the vehicle's RTK module can be continuously acquired to determine whether the vehicle is stationary based on the location data.

[0121] Specifically, the IMU is initialized only when the vehicle is stationary, thereby capturing the noise in the data collected by the IMU while the vehicle is stationary.

[0122] Step 404: If the vehicle is not stationary, wait for the vehicle to come to a stop and clear the queue.

[0123] This means clearing the storage queue used for IMU initialization. Since the vehicle is not stationary, the collected IMU measurement data is invalid, and IMU initialization cannot be performed. Therefore, the queue is cleared, and the vehicle is waited to come to a standstill.

[0124] Step 405: If the vehicle is stationary, collect IMU data.

[0125] In other words, IMU measurement data is continuously acquired. This data includes direct measurement data and indirect derived data. Direct measurement data refers to the data natively output by the IMU, including angular velocity from the gyroscope and specific force data from the accelerometer. Indirect derived data refers to data obtained by processing the native data, including three-dimensional attitude angle data, three-dimensional velocity data, and three-dimensional position data.

[0126] Based on the data continuously acquired by the IMU, parameters such as the IMU's initial zero bias, noise variance, and gravity direction can be obtained.

[0127] Step 406: Determine whether the data collection duration meets the preset duration requirement.

[0128] Optionally, a duration threshold is obtained, and it is determined whether the duration of collecting measurement data from the IMU reaches the duration threshold. If the duration threshold is reached, the data collection duration meets the preset duration requirement; if the duration threshold is not reached, the data collection duration does not meet the preset duration requirement.

[0129] The data collection duration refers to the distribution duration of the IMU measurement data stored in the data queue. For example, if the duration threshold is 10 seconds, and the distribution duration of the IMU measurement data stored in the data queue reaches 10 seconds, then the data collection duration is considered to meet the preset duration requirement.

[0130] Step 407: If the preset time requirement is not met, maintain the queue and return.

[0131] That is, continue to collect IMU measurement data into the data queue and return a data acquisition request to continue collecting IMU measurement data.

[0132] Step 408: If the preset duration requirement is met, attempt initialization.

[0133] That is, to attempt to initialize the IMU.

[0134] The initialization of the IMU is used to estimate the initial state and key error parameters of the IMU. In other words, the initialization of the IMU is used to obtain parameters such as the initial zero bias, noise variance and gravity direction of the IMU.

[0135] Optionally, the initialization of the IMU may include at least the following procedures: 1. Zero-bias calibration: This calibrates the sensor's "zero point." Both the accelerometer and gyroscope in an IMU have a zero-bias, outputting a non-zero constant value even without input. Place the IMU absolutely still and horizontally for a period of time (typically 1-30 seconds). In this static state, the resultant vector measured by the accelerometer should theoretically be equal to the local gravitational acceleration. Therefore, average the acceleration data during this period to obtain the average acceleration data. Theoretically, the acceleration in the x and y axes should be 0, and the acceleration in the z-axis should be g (approximately 9.81 m / s²). Any deviation is the estimate of the zero-bias, also known as the initial zero-bias of the acceleration. In the static state, the theoretical value of the angular velocity should be 0. Averaging the gyroscope data during this period yields the estimate of the gyroscope's zero-bias.

[0136] 2. Gravity alignment to determine the vehicle's initial attitude. In a stationary state, the direction of the constant gravity vector measured by the accelerometer uniquely determines the IMU's pitch and roll angles.

[0137] 3. Initial State Settings: This section assigns initial values ​​to the filter. Optionally, the initial state includes: position data: the first frame of data provided by a Global Navigation Satellite System (GNSS) or other absolute sensor; velocity data: typically initialized to [0, 0, 0] (assuming a rest start); attitude data: pitch and roll angles obtained from the gravity alignment process described above, plus the heading angle provided by an external sensor; initial error state value: typically set to a zero vector, assuming the initial nominal state is accurate; initial covariance matrix value: this reflects the certainty of our initial state estimation; position uncertainty: set according to the accuracy of the RTK module (e.g., meter-level); velocity uncertainty: set relatively small (e.g., 0.01 m / s); attitude uncertainty: pitch / roll is determined by the gravity alignment accuracy (very small, e.g., 0.1 degrees), heading is determined by the accuracy of the sensor providing it (potentially large, e.g., 5 degrees); IMU bias uncertainty: set to an empirical value reflecting the uncertainty of the bias estimate.

[0138] Step 409: Determine if the variance check has passed.

[0139] The variance of the measurement data collected by the IMU, i.e., the stationary speed data, is calculated, and the variance is checked to see if it passes the test. If the variance is less than a preset threshold, the variance check is considered passed, meaning the data collected by the inertial measurement unit during vehicle stationary operation is stable. If the variance check fails, the inertial measurement unit initialization is considered to have failed.

[0140] Step 410: If the variance check fails, the initialization fails.

[0141] Step 411: If the variance check passes, estimate zero bias and mark as passed.

[0142] If the variance check passes, the initial zero bias, noise variance, and gravity direction parameters are estimated based on the IMU measurement data, and the variance check is marked as passed, thus completing the IMU initialization.

[0143] Step 412: Log the information.

[0144] Optionally, the event indicating that the IMU initialization is complete can be logged in a log file.

[0145] In summary, the method provided in this application uses speed data to predict the vehicle's first pose data and environmental spatial data to predict the vehicle's second pose data. Based on the first and second pose data, it corrects and adjusts the vehicle's position data acquired by the RTK module. This allows for the use of the first and second pose data to assist in position data calibration even when the RTK module's accuracy is poor or the signal is obstructed, thus improving the accuracy of vehicle pose data acquisition. Furthermore, the use of multiple sensors enhances the adaptability and flexibility of the vehicle in acquiring pose data in different environments, thereby improving the accuracy of autonomous driving.

[0146] The method provided in this embodiment initializes the IMU and obtains parameters such as initial zero bias, noise variance, and gravity direction based on the IMU initialization, thereby adjusting the parameters of the preset error state filter and improving the fusion accuracy of position data, first pose data and second pose data.

[0147] In recent years, with the development of computer technology and the new energy vehicle industry, autonomous driving technology has also made significant breakthroughs. As an important component of intelligent transportation, autonomous driving, with its powerful environmental perception, decision-making, planning, and automatic control capabilities, has become a research hotspot in the automotive industry. Compared with human driving, intelligent driving technology demonstrates numerous advantages in safety, comfort, and traffic efficiency. Through high-precision sensors and intelligent algorithms, it achieves real-time perception and accurate judgment of complex traffic environments, effectively reducing the risk of traffic accidents, improving the passenger experience, alleviating road congestion, and enhancing overall transportation efficiency. The application prospects of autonomous driving technology are broad, and it has become an important driving force for promoting the construction of intelligent transportation and smart cities.

[0148] The importance of positioning technology in autonomous driving is self-evident. High-precision positioning is fundamental to the safe driving, path planning, and environmental perception of autonomous vehicles. Autonomous driving systems need to obtain the vehicle's accurate position in the global coordinate system in real time, and then combine this with maps, sensors, and decision-making algorithms to make safe and reliable driving decisions. Positioning technology is one of the cornerstones of autonomous driving systems. Currently, single sensors are prone to failure, RTK is susceptible to signal interference, and cameras rely on lighting information. Furthermore, positioning systems often lack sufficient accuracy and robustness in complex environments (such as tall buildings obstructing view, tunnels, and inclement weather), significantly impacting the safe operation of autonomous vehicles.

[0149] To address the shortcomings of existing autonomous driving positioning technologies, this application proposes a multi-sensor fusion positioning method that integrates RTK, IMU, wheel speed sensors, and LiDAR. This method effectively improves the positioning accuracy and robustness of vehicles in complex scenarios, reduces the impact of single sensor failure on the overall vehicle positioning, and ensures the safe operation of autonomous vehicles.

[0150] To achieve the above objectives, the specific technical solutions and implementation steps are as follows: (1) First, the initial location information of the vehicle is obtained through the RTK module, namely the location data mentioned above, to provide a global reference for subsequent positioning.

[0151] (2) Using an IMU sensor, the XYZ axis acceleration and angular velocity of the vehicle are collected at a frequency of 100Hz. The vehicle motion state is predicted once for each collection of motion state, and the first pose data of the vehicle at the current moment is obtained by recursion.

[0152] (3) Combine wheel speed sensor data to assist in short-term vehicle positioning when RTK signal is poor or lost, thereby improving system robustness.

[0153] (4) Spatial information of the vehicle’s surrounding environment is obtained by using lidar to achieve high-precision registration between the vehicle and the high-definition map, thereby obtaining the vehicle’s second pose data at the current moment.

[0154] (5) The system adopts a multi-sensor data fusion algorithm to deeply fuse RTK, IMU, wheel speed and lidar information to obtain high-precision and robust positioning results of the vehicle under the global map.

[0155] Optionally, in step (1), the RTK module can be installed on the top of the vehicle to obtain unobstructed satellite signals and improve the reliability of the initial positioning value; the RTK module can obtain the vehicle's latitude, longitude and elevation information.

[0156] Optionally, in step (1), before running the algorithm, the rotation and translation data of the lidar sensor and the RTK module receiver are calibrated.

[0157] Optionally, in step (2), the IMU unit estimates the initial zero bias, noise variance, and gravity direction of the IMU through a static initialization process to ensure the accuracy of subsequent state estimation.

[0158] Optionally, in step (3), an error state Kalman filter is used to fuse wheel speed data with the linear acceleration a and angular velocity theta of the IMU to improve the positioning accuracy of the vehicle in scenarios where RTK is unavailable, such as tunnels and urban canyons.

[0159] Optionally, in step (4), the lidar point cloud is filtered and distorted, and then matched with a pre-built high-precision map to achieve high-precision local positioning.

[0160] Optionally, in step (5), the multi-sensor fusion algorithm adopts the error state Kalman filter algorithm to automatically adjust the data of each sensor and achieve adaptive optimization of the positioning results.

[0161] The embodiments of this application, through the above technical solutions, take into account the high accuracy, real-time performance and robustness of the positioning system, and meet the high reliability positioning requirements of autonomous vehicles in complex traffic environments.

[0162] Figure 5 This is a flowchart of the overall process provided in an exemplary embodiment of this application, such as... Figure 5 As shown, the overall process includes at least the following steps.

[0163] Optionally, the overall process includes: Step 501: The main function starts and initializes the log configuration file.

[0164] Optionally, the program starts execution from the main() function entry point, first loading the log configuration file to configure information such as log output method, log level, and format.

[0165] Once the main function is started, the process of calculating the vehicle's location begins.

[0166] Step 502: Initialize the fusion main function.

[0167] Optionally, the initialization fusion main function refers to the key step of creating and configuring the core instance of the entire fusion algorithm at the program entry point of the multi-sensor fusion system, preparing all the software and data for its formal operation.

[0168] The initialization of the fusion main function includes at least one of the following processes: 1. Create a core instance of the fusion algorithm, such as instantiating a fusion filter object in memory, for example, an error state Kalman filter; 2. Configure algorithm parameters, optionally, read and set key parameters of the filter from the configuration file; 3. Initialize the initial state of the fusion engine; 4. Establish a data flow interface; 5. Start the background processing thread.

[0169] The initialization fusion main function essentially constructs the runtime environment for data fusion. This process doesn't handle specific sensor data, but rather sets up the stage and establishes the rules for processing that data.

[0170] Step 503: Read the map origin and directory.

[0171] Reading the map origin and directory is a crucial process for autonomous driving programs when they start up, involving loading and parsing metadata and index information from high-precision maps or prior point cloud maps. Its purpose is to inform the system of the map's absolute geographic location benchmark and internal data structure, preparing it for subsequent real-time positioning (point cloud matching).

[0172] First, reading the map origin is a step in establishing the map's absolute geographic reference. This involves reading a specific configuration file to establish the conversion relationship between local map coordinates and global geographic coordinates (or the navigation coordinates used by the vehicle). Once the positioning algorithm calculates the vehicle's position (x, y, z) in the local map coordinates, its latitude, longitude, and altitude can be immediately obtained through this origin conversion.

[0173] Reading the map catalog is a step in establishing an efficient map access mechanism. It reads the map's index file to build a lightweight "map query engine" in memory. When a vehicle travels to a general area, the system does not need to load the entire map. Instead, it dynamically loads the point cloud data blocks of the required area based on the current location, greatly saving memory and improving access speed.

[0174] Step 504: Load the map index.

[0175] Loading the map index is a specific execution step that follows reading the map catalog. Its core task is to parse and instantiate the spatial index structure described in the map catalog file into an efficient, queryable data object in memory, thereby establishing a fast mapping relationship from "physical location" to "map data file".

[0176] Step 505: Initialize the lidar and IMU message synchronizer.

[0177] The initialization of the LiDAR and IMU message synchronizer is used to establish a software mechanism to ensure that LiDAR point cloud data and IMU data from different sensors with different timestamps can be precisely aligned in time, providing paired, synchronous data packets for subsequent point cloud distortion correction and fusion localization.

[0178] Optionally, a message synchronizer, typically a class, is instantiated in memory; a time-ordered cache queue is created for both the LiDAR and IMU to determine the synchronization logic for the LiDAR and IMU.

[0179] Step 506: Read the external parameters of the lidar and IMU.

[0180] The external parameters of the lidar and IMU are read to load and parse an accurate rigid body transformation matrix from the configuration file. This matrix describes the fixed spatial relative relationship between the IMU sensor coordinate system and the lidar sensor coordinate system.

[0181] The process of reading external parameters involves loading a fixed spatial transformation relationship obtained through precise calibration (usually done offline using a calibration board) from the configuration file into runtime memory, thus becoming a spatial bridge connecting IMU motion data and LiDAR geometric data.

[0182] Step 507, Robot Operating System (ROS) message main loop.

[0183] The ROS message main loop is an event-driven asynchronous message processing loop managed by the ROS client library. In this embodiment, the ROS message main loop is used to process RTK messages, IMU messages, or point cloud messages.

[0184] Step 508: Determine the message type.

[0185] That is, it determines whether the received message is an RTK message sent by the RTK module, an IMU message sent by the IMU, or a point cloud message sent by the LiDAR.

[0186] Step 509: If it is an RTK message, then perform RTK message parsing and processing.

[0187] That is, parsing RTK messages to obtain the current location data of the vehicle collected by the RTK module.

[0188] Step 510: Save / correct GNSS messages.

[0189] That is, to save the location data collected by the RTK module, or to correct the stored GNSS messages based on the location data collected by the RTK module, thereby updating the vehicle's location data.

[0190] Step 511: If it is an IMU message, then perform IMU message parsing and processing.

[0191] That is, parsing IMU messages to obtain IMU measurement data, including angular velocity data, acceleration data, etc.

[0192] Step 512: If it is a point cloud message, then perform point cloud message parsing and processing.

[0193] That is, parsing point cloud messages to obtain point cloud data obtained from LiDAR scanning.

[0194] Step 513: The message synchronizer synchronizes IMU messages and point cloud messages.

[0195] Based on the message synchronizer already created, IMU messages and point cloud messages are synchronized, so that the measurement data collected by the IMU is aligned with the time axis of the point cloud data.

[0196] Step 514, Post-processing function for IMU and LiDAR synchronization.

[0197] Optionally, the IMU and LiDAR synchronization post-processing function is a module that performs the first-level core fusion processing after receiving time-aligned sensor data packets. Optionally, it first performs IMU pre-integration, then point cloud motion compensation, then point cloud preprocessing, and finally outputs two parallel results, which are encapsulated and passed to the downstream localization optimizer.

[0198] The two parallel results include: IMU pre-integration constraints, which describe the motion model of the vehicle's trajectory from the previous frame to the current frame; and distortion-free feature point clouds, which describe the observation model of the vehicle's surrounding environment at the end of the current frame.

[0199] Step 515: Determine if the IMU is initialized.

[0200] Determine if the IMU has already undergone initialization. Optionally, search the log file for IMU initialization records. If a record exists, it indicates that the IMU has been initialized; otherwise, the IMU has not undergone initialization.

[0201] Step 516: If initialization is required, use the IMU to remove distortion from the point cloud data.

[0202] That is, if the IMU has been initialized, the initial zero bias, noise variance, gravity direction and other data obtained by the IMU after initialization are obtained, and distortion removal processing is performed on the point cloud data based on the measurement data of the IMU.

[0203] Optionally, the continuous high-frequency motion information provided by the IMU is projected in reverse onto each point of a frame of lidar scanning to eliminate geometric distortion caused by carrier motion, and all points collected at different times in a frame are corrected to the same reference time coordinate system.

[0204] The time required for a lidar to scan one frame is ( T (e.g., 100ms). During this time, if the vehicle is moving, the sensor pose of points scanned earlier and later within a frame will be different. This causes the point cloud of a stationary object to be "stretched" or "distorted".

[0205] Step 517: Register the point cloud data and the global map.

[0206] Optionally, the point cloud data is matched with the global point cloud in the global map to find the geographical location that matches the point cloud data in the global map. That is, the stage of registering the point cloud data with the global map begins.

[0207] Step 518: Determine the status.

[0208] This status indicates the reception status of the RTK signal, such as currently receiving an RTK signal, or the working status, which refers to the normal operation of the error state filter.

[0209] Step 519: If the system is in a state of waiting for an RTK signal, determine whether an RTK signal exists.

[0210] Step 520: If present, perform an RTK grid search.

[0211] The initial pose of the vehicle, i.e., its position data, is determined. If the mesh search successfully determines the initial pose, the error state filter is reset, and the system enters the working state. If the mesh search fails to determine the initial pose, the failure event is recorded, and the system waits for a new RTK signal.

[0212] Step 521: If not, wait for the RTK signal.

[0213] Step 522: If the system is in working condition, then the point cloud localization processing function is executed.

[0214] The point cloud localization processing function receives the current frame point cloud after preprocessing (distortion removal, filtering, and feature extraction), matches it with prior information (such as the global map or the state of the previous frame), and finally outputs the vehicle's pose in space.

[0215] Step 523: If the IMU is not initialized, execute the IMU initialization function.

[0216] Step 524: Determine whether the IMU has been successfully initialized.

[0217] Step 525: If initialization fails, wait for IMU messages.

[0218] Step 526: If initialization is successful, configure the state error filter and enter the normal working process.

[0219] Overall, the process of this application embodiment includes: (1) Initialization: After the localization algorithm starts, the system first performs initialization operations. This step includes initializing the log module, loading internal parameters of the IMU, loading external parameters of the LiDAR, IMU, and RTK, and preparing related hardware and software modules. The system loads the map origin and map data directory according to the configuration file, and loads the map index in the storage system for efficient subsequent retrieval. Subsequently, the system initializes the external parameters of the LiDAR and IMU and starts the visualization interface. After completing the above preparations, the system enters the main loop and waits for the input of multi-sensor data.

[0220] (2) Data Acquisition and Synchronization: The system receives data streams from RTK, IMU, wheel speed sensors, and LiDAR via a multi-sensor interface. RTK data is used to provide initial position information in the global map, and IMU data is used to estimate continuous motion states. Wheel speed sensor data assists in the accuracy of short-term vehicle positioning, and LiDAR data is used for registration with the global point cloud map. After input, all sensor data is first time-aligned by the message synchronization module to ensure that all types of sensor data are processed in the same time reference frame before further processing.

[0221] (3) IMU Static Initialization: The system performs IMU static initialization at the beginning of startup. The overall process is as follows: Figure 2 By analyzing IMU data over a period of at least 10 seconds and at most 20 seconds, the initial zero bias, gravity direction, IMU noise, etc., are estimated to provide accurate initial values ​​for subsequent state estimation. If IMU initialization is successful, the obtained zero bias parameters, gravity direction, noise, etc., are set into the error state Kalman filter (ESKF). If initialization fails, the data is re-recorded and re-initialized.

[0222] (4) RTK-assisted localization and mesh search: After IMU initialization, the system enters the RTK-assisted localization phase. Since RTK only provides XYZ position information and lacks Roll, Pitch, and Yaw rotation attitude information, the system adopts a mesh search method to perform multi-resolution matching searches at different angles near the initial position provided by RTK. Matching scores for each attitude are calculated through registration of the LiDAR point cloud and the high-definition map. Finally, the attitude with the highest score is selected as the vehicle's initial pose, completing the system's global coarse localization and filter state reset.

[0223] (5) Multi-sensor fusion localization: After entering normal operation, the system receives and synchronizes IMU and LiDAR point cloud data according to the time of transmission from each sensor. The IMU data is used to recursively calculate the vehicle's XYZ and Roll, Pitch, and Yaw positions using short-term high-frequency calculations. Simultaneously, the IMU data can be used for the distortion correction process of the LiDAR data. After the algorithm corrects the distortion of the point cloud data, high-precision pose observation is obtained by registering the point cloud at this moment with the global point cloud map using the NDT algorithm. Subsequently, an error-state Kalman filter is used to fuse the IMU predictions with the LiDAR observations, achieving high-precision vehicle localization on the global map. Furthermore, if wheel speed information exists, the error-state Kalman filter incorporates wheel speed observation information to further improve localization performance.

[0224] (6) Map Management and Optimization: To ensure system efficiency and real-time performance, the map adopts a partitioned loading and dynamic management mechanism. The system loads the necessary point cloud map areas around the vehicle based on its current location and unloads point cloud map areas that are no longer needed, reducing memory usage and improving NDT point cloud registration efficiency. After each map update, the KD tree in the PCL reconstructs the map point cloud to ensure registration accuracy. To ensure system real-time performance, the map management and optimization module runs independently on a single thread.

[0225] (7) Status Output and Visualization: The system displays the current point cloud, vehicle status, and positioning results in real time through a visualization interface, and outputs the positioning results to the upper-level autonomous driving decision and control module. The entire algorithm executes repeatedly, providing high-precision attitude information including X, Y, Z, Roll, Pitch, and Yaw speeds.

[0226] In summary, the method provided in this application uses speed data to predict the vehicle's first pose data and environmental spatial data to predict the vehicle's second pose data. Based on the first and second pose data, it corrects and adjusts the vehicle's position data acquired by the RTK module. This allows for the use of the first and second pose data to assist in position data calibration even when the RTK module's accuracy is poor or the signal is obstructed, thus improving the accuracy of vehicle pose data acquisition. Furthermore, the use of multiple sensors enhances the adaptability and flexibility of the vehicle in acquiring pose data in different environments, thereby improving the accuracy of autonomous driving.

[0227] Figure 6 This is a structural block diagram of a vehicle pose determination device provided in an exemplary embodiment of this application, as shown below. Figure 6 As shown, the device includes: The acquisition module 610 is used to acquire the vehicle's position data, speed data, and environmental space data at the current moment. The speed data is obtained by measuring the inertial measurement unit, and the environmental space data includes point cloud information corresponding to multiple objects around the vehicle. The prediction module 620 is used to predict the first pose data of the vehicle using the speed data; and to match the point cloud information with a preset point cloud map to determine the second pose data corresponding to the vehicle. The adjustment module 630 is used to adjust the parameters corresponding to the preset error state filter based on the initialization data of the inertial measurement unit to obtain the target error state filter; The determination module 640 is used to filter and fuse the position data, the first pose data and the second pose data using the target error state filter to determine the target pose data of the vehicle at the current moment.

[0228] In an optional embodiment, the determining module 640 is further configured to correct the position data using the first pose data and the second pose data through the target error state filter to obtain the target pose data corresponding to the vehicle at the current moment.

[0229] In an optional embodiment, the determining module 640 is further configured to determine the average value corresponding to the position data, the first pose data, and the second pose data as the target pose data; or, The determining module 640 is further configured to, based on the position data, determine a first weight corresponding to the first pose data and a second weight corresponding to the second pose data; and correct the position data based on the first weight and the second weight to obtain the target pose data; or... The determining module 640 is further configured to determine a first error between the first pose data and the position data, and to determine a second error between the second pose data and the position data; determine the average error of the first error and the second error, and adjust the position data using the average error to obtain the target pose data.

[0230] In an optional embodiment, the acquisition module 610 is further configured to acquire stationary speed data corresponding to the vehicle when it is stationary, and to acquire the stationary duration of the vehicle in the stationary state. The adjustment module 630 is further configured to, in response to the stationary duration meeting a preset duration requirement, determine the variance result corresponding to the stationary speed data; if the variance result meets a preset value requirement, determine that the inertial measurement unit has been successfully initialized, and acquire the initial zero bias, noise variance, and gravity direction corresponding to the inertial measurement unit, wherein the initial zero bias is used to characterize the noise data generated by the vehicle in the velocity dimension in the stationary state; and adjust the parameters of the preset error state filter based on the initial zero bias, the noise variance, and the gravity direction to obtain the target error state filter.

[0231] In an optional embodiment, the acquisition module 610 is further configured to acquire wheel speed data of the vehicle at the current moment, wherein the wheel speed data refers to the speed data corresponding to the vehicle tires; The prediction module 620 is also used to predict the third pose data of the vehicle using the wheel speed data; The determining module 640 is further configured to use the target error state filter to filter and fuse the position data, the first pose data, the second pose data and the third pose data to determine the target pose data corresponding to the vehicle at the current time.

[0232] In an optional embodiment, the prediction module 620 is further configured to filter the plurality of point cloud information to obtain filtered point cloud information; perform distortion correction processing on the filtered point cloud information to obtain distorted point cloud information; and use the preset matching algorithm to match the distorted point cloud information with a preset point cloud map to determine the second pose data corresponding to the vehicle.

[0233] In summary, the apparatus provided in this application uses speed data to predict the vehicle's first pose data and uses environmental space data to predict the vehicle's second pose data. Based on the first and second pose data, it corrects and adjusts the vehicle's position data acquired by the RTK module. This allows for the use of the first and second pose data to assist in position data calibration even when the RTK module's accuracy is poor or the signal is obstructed, thus improving the accuracy of vehicle pose data acquisition. Furthermore, with the assistance of multiple sensors, it enhances the adaptability and flexibility of the vehicle in performing pose acquisition under different environments, thereby improving the accuracy of autonomous driving.

[0234] It should be noted that the apparatus provided in the above embodiments is only illustrated by the division of the above functional modules when implementing its functions. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the content structure of the device can be divided into different functional modules to complete all or part of the functions described above. In addition, the apparatus and method embodiments provided in the above embodiments belong to the same concept, and the specific implementation process can be found in the method embodiments, which will not be repeated here.

[0235] Please refer to Figure 7 This diagram illustrates a structural block diagram of a computer device 700 provided in one embodiment of this application. The computer device 700 may be... Figure 1 The terminal or server in the computer system shown, in this embodiment of the application, is implemented as a vehicle-mounted terminal, specifically for implementing the vehicle pose determination method provided in the above embodiments. Typically, computer device 700 includes a processor 701 and a memory 702.

[0236] Processor 701 may include one or more processing cores, such as a quad-core processor, an octa-core processor, etc. Processor 701 may be implemented using at least one hardware form selected from Digital Signal Processing (DSP), Field Programmable Gate Array (FPGA), and Programmable Logic Array (PLA). Processor 701 may also include a main processor and a coprocessor. The main processor, also known as the CPU, is used to process data in the wake-up state; the coprocessor is a low-power processor used to process data in the standby state. In some embodiments, processor 701 may integrate a Graphics Processing Unit (GPU), which is responsible for rendering and drawing the content to be displayed on the screen. In some embodiments, processor 701 may also include an Artificial Intelligence (AI) processor, which is used to handle computational operations related to machine learning.

[0237] The memory 702 may include one or more computer-readable storage media, which may be non-transitory. The memory 702 may also include high-speed random access memory and non-volatile memory, such as one or more disk storage devices or flash memory devices. In some embodiments, the non-transitory computer-readable storage media in the memory 702 is used to store a computer program configured to be executed by one or more processors to implement the above-described method for determining vehicle pose.

[0238] In some embodiments, the computer device 700 may also optionally include other components 703: a peripheral device interface and at least one peripheral device. The processor 701, memory 702, and peripheral device interface can be connected via a bus or signal lines. Each peripheral device can be connected to the peripheral device interface via a bus, signal lines, or a circuit board. Specifically, the peripheral device includes at least one of: radio frequency circuitry, a display screen, audio circuitry, and a power supply.

[0239] Those skilled in the art will understand that Figure 7 The structure shown does not constitute a limitation on the computer device 700, and may include more or fewer components than shown, or combine certain components, or use different component arrangements.

[0240] In an exemplary embodiment, a computer-readable storage medium is also provided, wherein a computer program is stored in the storage medium, and the computer program, when executed by a processor, implements the above-described method for determining the vehicle pose. Optionally, the computer-readable storage medium may include: read-only memory (ROM), random access memory (RAM), solid-state drives (SSDs), or optical discs, etc. The random access memory may include resistive random access memory (ReRAM) and dynamic random access memory (DRAM).

[0241] In an exemplary embodiment, a computer program product is also provided, the computer program product including a computer program stored in a computer-readable storage medium. A processor of a computer device reads the computer program from the computer-readable storage medium, and the processor executes the computer program, causing the computer device to perform the above-described method for determining vehicle pose.

[0242] It should be noted that the collection and processing of relevant data (such as account information, list data, etc.) in this application should strictly comply with the requirements of relevant national laws and regulations, obtain the informed consent or separate consent of the personal information subject, and carry out subsequent data use and processing within the scope of laws and regulations and the authorization of the personal information subject.

[0243] It should be understood that "multiple" as used in this article refers to two or more. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. The character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0244] Furthermore, the step numbers described herein are merely illustrative of one possible execution order between steps. In some other embodiments, the steps may not be executed in the order of their numbers, such as two steps with different numbers being executed simultaneously, or two steps with different numbers being executed in the reverse order of the illustration. This application does not limit this.

[0245] The above description is merely an exemplary embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A method for determining the position and orientation of a vehicle, characterized in that, The method includes: The vehicle's current position data, speed data, and environmental space data are acquired. The speed data is obtained through inertial measurement unit measurement, and the environmental space data includes point cloud information corresponding to multiple objects around the vehicle. The first pose data of the vehicle is predicted using the speed data; The point cloud information is matched with a preset point cloud map to determine the second pose data corresponding to the vehicle. The parameters corresponding to the preset error state filter are adjusted based on the initialization data of the inertial measurement unit to obtain the target error state filter; The target error state filter is used to filter and fuse the position data, the first pose data, and the second pose data to determine the target pose data of the vehicle at the current moment.

2. The method according to claim 1, characterized in that, The step of filtering and fusing the position data, the first pose data, and the second pose data using the target error state filter to determine the target pose data of the vehicle at the current moment includes: The target error state filter uses the first pose data and the second pose data to correct the position data, thereby obtaining the target pose data of the vehicle at the current moment.

3. The method according to claim 2, characterized in that, The step of correcting the position data using the first pose data and the second pose data through the target error state filter to obtain the target pose data of the vehicle at the current moment includes: The average value corresponding to the position data, the first pose data, and the second pose data is determined as the target pose data; or... Based on the position data, a first weight corresponding to the first pose data and a second weight corresponding to the second pose data are determined; the position data is then corrected based on the first weight and the second weight to obtain the target pose data; or, A first error between the first pose data and the position data is determined, and a second error between the second pose data and the position data is determined; the average error between the first error and the second error is determined, and the position data is adjusted using the average error to obtain the target pose data.

4. The method according to any one of claims 1 to 3, characterized in that, The step of adjusting the parameters corresponding to the preset error state filter based on the initialization data of the inertial measurement unit to obtain the target error state filter includes: Obtain the stationary speed data corresponding to the vehicle when it is stationary, and obtain the stationary duration of the vehicle in the stationary state; In response to the static duration meeting a preset duration requirement, the variance result corresponding to the static speed data is determined; If the variance result meets the preset value requirements, it is determined that the inertial measurement unit is successfully initialized, and the initial zero bias, noise variance and gravity direction corresponding to the inertial measurement unit are obtained. The initial zero bias is used to characterize the noise data generated by the vehicle in the velocity dimension in the stationary state. The target error state filter is obtained by adjusting the parameters of the preset error state filter based on the initial zero bias, the noise variance, and the gravity direction.

5. The method according to any one of claims 1 to 3, characterized in that, The method further includes: Obtain the wheel speed data of the vehicle at the current moment, where the wheel speed data refers to the speed data corresponding to the vehicle's tires; The third pose data of the vehicle is predicted using the wheel speed data; The step of filtering and fusing the position data, the first pose data, and the second pose data using the target error state filter to determine the target pose data of the vehicle at the current moment includes: The target error state filter is used to filter and fuse the position data, the first pose data, the second pose data, and the third pose data to determine the target pose data corresponding to the vehicle at the current time.

6. The method according to any one of claims 1 to 3, characterized in that, The step of matching the multiple point cloud information with a preset point cloud map using a preset matching algorithm to determine the second pose data corresponding to the vehicle includes: The multiple point cloud information is filtered to obtain multiple filtered point cloud information. The filtered point cloud information is subjected to distortion correction processing to obtain the distorted point cloud information. The preset matching algorithm is used to match the multiple point cloud information after distortion removal with the preset point cloud map to determine the second pose data corresponding to the vehicle.

7. A device for determining the position and orientation of a vehicle, characterized in that, The device includes: The acquisition module is used to acquire the vehicle's position data, speed data, and environmental space data at the current moment. The speed data is obtained by measuring the inertial measurement unit, and the environmental space data includes point cloud information corresponding to multiple objects around the vehicle. The prediction module is used to predict the first pose data of the vehicle using the speed data; and to match the point cloud information with a preset point cloud map to determine the second pose data corresponding to the vehicle. The adjustment module is used to adjust the parameters corresponding to the preset error state filter based on the initialization data of the inertial measurement unit to obtain the target error state filter; The determination module is used to filter and fuse the position data, the first pose data and the second pose data using the target error state filter to determine the target pose data of the vehicle at the current moment.

8. A computer device, characterized in that, The computer device includes a processor and a memory, the memory storing a computer program that is loaded and executed by the processor to implement the method for determining the vehicle pose as described in any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which is loaded and executed by a processor to implement the method for determining the vehicle pose as described in any one of claims 1 to 6.

10. A computer program product, characterized in that, The computer program product includes a computer program stored in a computer-readable storage medium, and a processor reads from and executes the computer program to implement the vehicle pose determination method as described in any one of claims 1 to 6.

Images