Map optimization method, device and equipment

Abstract

This application provides a map optimization method, apparatus, and device. The method includes: acquiring sensor data obtained from the driving of a target vehicle on a specific floor; determining the floor height of the current floor based on the sensor data, determining the associated floor corresponding to the current floor, and correcting the floor height of the current floor based on the reference floor height of the associated floor to obtain a target pseudo floor height for the current floor; determining the relative pose constraint residual and height increment residual of two trajectory points in the target vehicle's trajectory based on the sensor data of the target vehicle driving on the current floor and / or associated floor; performing pose smoothing optimization on the target vehicle based on the relative pose constraint residual and height increment residual to obtain a pose optimization result; and establishing an optimized map based on the pose optimization result. The technical solution of this application can solve the problems of map floor aliasing and layering, and the method consumes relatively few computational resources.

Description

Technical Field

[0001] This application relates to the field of data processing technology, and in particular to a map optimization method, apparatus and device. Background Technology

[0002] In recent years, with the development of artificial intelligence technology, the demand for maps has been increasing. Mapping technology can obtain accurate spatial locations, attributes, and topological connections of static objects in a scene. Maps built based on mapping technology can be used as input for unmanned systems to achieve high-precision positioning, lane-level decision planning, and static obstacle avoidance. They can also serve as reference base maps for modeling smart cities or smart transportation systems, possessing enormous commercial and social value.

[0003] Traditional mapping techniques employ laser SLAM or high-precision GNSS combined with high-frequency single-line mapping lidar to acquire scene maps. Mapping based on this method requires high hardware costs and significant computational resources. In some consumer-level fields, limitations in sensor costs, such as autonomous driving systems which often only utilize consumer-grade IMUs, wheel speed sensors, GPS, cameras, and low-power computing platforms, make it difficult to utilize traditional mapping techniques. Particularly during indoor map building, the height measured by these sensors can diverge over time, leading to map layer overlap and delamination.

[0004] Therefore, there is an urgent need to propose a map optimization method that can be implemented on low computing power platforms to solve the problems of map layer overlap and layering. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the purpose of this application is to provide a map optimization method, apparatus, and device that consumes less computing resources and achieves higher map accuracy.

[0006] To achieve the above objectives, this application adopts the following technical solution:

[0007] This application provides a map optimization method, the method comprising:

[0008] During the process of the target vehicle traveling across layers, sensor data of the target vehicle is acquired;

[0009] If the target vehicle travels to the boundary between the ramp and the current level, the floor height of the current level is determined based on sensor data; the associated level corresponding to the current level is determined, and the floor height of the current level is corrected based on the reference floor height of the associated level that has been stored, so as to obtain the target pseudo floor height of the current level.

[0010] Based on sensor data of the target vehicle traveling on the current level and / or associated level, determine the relative pose constraint residual and height increment residual of two trajectory points in the target vehicle's running trajectory;

[0011] The pose smoothing optimization of the target vehicle is performed based on the relative pose constraint residual and the height increment residual to obtain the pose optimization result;

[0012] Based on the pose optimization results, an optimized map is built.

[0013] Furthermore, before determining the relative pose constraint residuals and height increment residuals of two trajectory points in the target vehicle's trajectory, the method also includes:

[0014] The target vehicle's trajectory is determined based on sensor data and odometer observation data. The pose of the target vehicle corresponding to each trajectory point in the trajectory is determined based on the odometer observation data.

[0015] Based on the pose of the target vehicle corresponding to each trajectory point, determine the pose increment observation between the poses of the target vehicle corresponding to two trajectory points.

[0016] Furthermore, pose includes position and attitude, and pose increment observations include position increment observations and attitude increment observations; the process of determining the relative pose constraint residuals of two trajectory points in the target vehicle's trajectory includes:

[0017] Based on the target vehicle's attitude at the first trajectory point, the target vehicle's attitude at the second trajectory point, and the attitude increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, the relative attitude constraint residuals are determined; wherein, the relative attitude constraint residuals are determined using the following formula:

[0018]

[0019] In formula 1, err odmo_rota q represents the relative attitude constraint residual. ij_dr q represents the attitude increment observation. i q represents the attitude of the target vehicle corresponding to the first trajectory point. j This indicates the attitude of the target vehicle corresponding to the second trajectory point;

[0020] Based on the target vehicle's position at the first trajectory point, the target vehicle's position at the second trajectory point, and the position increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, the relative position constraint residual is determined; wherein, the relative position constraint residual is determined using the following formula:

[0021] err odom_posi =R(q) l )*t Tj_dr -(t i -t i ) (Formula 2);

[0022] In formula 2, err odmo_posi t represents the relative position constraint residual. ij_dr t represents the position increment observation. i t represents the position of the target vehicle corresponding to the first trajectory point. j Let q represent the position of the target vehicle corresponding to the second trajectory point. i R(q) represents the attitude of the target vehicle corresponding to the first trajectory point. i ) represents a matrix established based on the attitude of the target vehicle corresponding to the first trajectory point;

[0023] Based on the obtained relative attitude constraint residuals and relative position constraint residuals, the relative pose constraint residuals are determined.

[0024] Furthermore, the process of determining the height increment residual between two trajectory points in the target vehicle's trajectory includes:

[0025] Based on the position of the target vehicle at the first trajectory point and the position of the target vehicle at the second trajectory point, determine the height components of the target vehicle at the two trajectory points respectively;

[0026] The height increment residual is determined based on the difference between the height component of the target vehicle at the first trajectory point and the height component of the target vehicle at the second trajectory point; the height increment residual is determined using the following formula:

[0027] err heigh_posi =(t i [2]-t j [2]) (Formula 3);

[0028] In formula 3, err heigh_posi The height increment residual, t i [2] represents the height component of the target vehicle at the first trajectory point, t j [2] represents the height component of the target vehicle at the second trajectory point.

[0029] Furthermore, pose smoothing optimization is performed on the target vehicle based on the relative pose constraint residual and the height increment residual to obtain the pose optimization results, including:

[0030] Based on the relative pose constraint residual and the height increment residual, a residual function is established;

[0031] Based on the residual function, a loss function with global pose as the state variable is established, where global pose includes the pose of the target vehicle at all trajectory points in the current level and / or associated levels.

[0032] The loss function is solved using a nonlinear least squares optimization algorithm, which allows the loss function to reach a local minimum. The loss function is determined using the following formula:

[0033]

[0034] In formula (4), F(x) represents the loss function, f i (x) represents the residual function, and m represents the number of trajectory points;

[0035] If a local minimum of the loss function is obtained, the height difference between any two trajectory points in the current level and the associated level approaches 0, and the pose change of the target vehicle at the two trajectory points remains unchanged, so as to obtain the pose of the target vehicle at the two trajectory points after pose smoothing optimization, and the pose after pose smoothing optimization is used as the pose optimization result.

[0036] Furthermore, determine the location where the target vehicle has traveled to the boundary between the ramp and the current level, including:

[0037] Determine the first pitch angle corresponding to the sensor data of the target vehicle at the first sampling time;

[0038] Starting from the first sampling time, search backward to the second sampling time to determine the second pitch angle corresponding to the sensor data of the target vehicle at the second sampling time;

[0039] If the absolute value of the difference between the first pitch angle and the second pitch angle is greater than the first threshold, then the position of the target vehicle at the second sampling time is determined as the boundary position between the ramp and the current level.

[0040] Further, determine the associated floor level corresponding to the current floor level, including:

[0041] Based on the current floor height and the reference floor height of each stored floor, determine whether there is a related floor for the current floor, and the difference between the reference floor height of the related floor and the floor height of the current floor is less than the second threshold.

[0042] If so, the height of the current floor is corrected based on the reference height of the associated floor that has been stored, so as to obtain the target pseudo height of the current floor.

[0043] If not, the current floor height will be stored as the reference floor height for the current floor.

[0044] This application also provides a map optimization apparatus, which includes:

[0045] The determination module is used to acquire sensor data of the target vehicle during the cross-level driving process; if the target vehicle drives to the boundary between the ramp and the current level, the level height of the current level is determined based on the sensor data.

[0046] The correction module is used to determine the associated floor corresponding to the current floor and correct the floor height of the current floor based on the reference floor height of the stored associated floor to obtain the target pseudo floor height of the current floor.

[0047] The optimization module is used to determine the relative pose constraint residual of the target vehicle at two sampling times and the height increment residual of the target vehicle at the two sampling times based on sensor data of the target vehicle driving on the current level and / or associated level; and to perform pose smoothing optimization on the target vehicle based on the relative pose constraint residual and the height increment residual to obtain the pose optimization result.

[0048] The mapping module is used to create an optimized map based on the pose optimization results.

[0049] This application also provides an in-vehicle terminal device, which includes: a processor and a machine-readable storage medium, the machine-readable storage medium storing machine-executable instructions that can be executed by the processor; the processor is used to execute the machine-executable instructions to implement the above-described map optimization method.

[0050] This application also provides a vehicle comprising:

[0051] IMU sensors are used to acquire acceleration and angular velocity information;

[0052] Wheel speed sensor, used to obtain wheel speed;

[0053] The processor receives acceleration and angular velocity information from the IMU sensor and wheel speed information from the wheel speed sensor, and implements the map optimization method described above based on the acceleration, angular velocity, and wheel speed information.

[0054] As can be seen from the above technical solutions, in this embodiment, during the cross-floor travel of the target vehicle, if the target vehicle reaches the current floor, the floor height of the current floor is corrected based on the reference floor height of the stored associated floors to obtain the target pseudo floor height of the current floor. That is, multiple floors are first clustered to facilitate the subsequent elimination of height divergence between the current floor and associated floors. For the current floor and associated floors, the relative pose constraint residual of the target vehicle at two sampling times and the height increment residual at the position at the two sampling times are determined. The pose optimization result of the target vehicle when both the relative pose constraint residual and the height increment residual are 0 are calculated to build an optimized map, thereby constructing a high-precision map with less computational resources.

[0055] For example, as a target vehicle travels along the current level, the accumulated error of the sensor data increases (sensor data is obtained recursively, and the error gradually increases with the recursion distance). Therefore, during the mapping process, floor overlap or layering issues may occur between the previous level and related levels. Based on this, by jointly optimizing the relative pose constraint residual and height increment residual using the pose at two sampling moments in the current level and related levels, and building a map based on the optimized pose result, the accumulated error of the sensor data can be eliminated, avoiding map floor overlap and layering issues, and improving map accuracy. Attached Figure Description

[0056] Figure 1 This is a flowchart of the map optimization method in the embodiments of this application;

[0057] Figure 2 This is a schematic diagram of the sensors of the target vehicle in an embodiment of this application;

[0058] Figure 3 This is a flowchart illustrating the mapping process in an embodiment of this application;

[0059] Figure 4 This is a schematic diagram illustrating the principle of the pseudo-layer height algorithm in the embodiments of this application;

[0060] Figure 5 This is a flowchart illustrating the pose constraints of the running trajectory in the embodiments of this application;

[0061] Figure 6 This is a schematic diagram showing the height divergence at different positions of the current floor in the embodiments of this application;

[0062] Figure 7 This is a schematic diagram of the map optimization device in the embodiments of this application;

[0063] Figure 8 This is a schematic diagram of the structure of the vehicle-mounted terminal device in the embodiments of this application. Detailed Implementation

[0064] To enable those skilled in the art to better understand the present application, the technical solutions in specific embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings.

[0065] This application proposes a map optimization method that can be applied to in-vehicle terminal devices deployed on the target vehicle. The in-vehicle terminal device supports an intelligent driving system to enable intelligent driving for the target vehicle. For example, the target vehicle can be a vehicle equipped with an intelligent driving system (such as a regular vehicle equipped with an intelligent driving system or an automatic parking system, where the intelligent driving system is used to build the scene map). Alternatively, the target vehicle can also be a vehicle equipped with an autonomous driving system (i.e., a vehicle that does not require a driver; the autonomous driving system independently completes scene map building and automatic parking). For example, it can be a robot, a logistics vehicle, or an autonomous passenger vehicle; the type of target vehicle is not limited.

[0066] like Figure 1 The diagram shown illustrates the flowchart of a map optimization method, which includes the following steps:

[0067] Step S101: Acquire sensor data of the target vehicle during the cross-level driving process.

[0068] Step S102: If the target vehicle travels to the boundary between the ramp and the current level, the floor height of the current level is determined based on sensor data; the associated level corresponding to the current level is determined, and the floor height of the current level is corrected based on the reference floor height of the stored associated level to obtain the target pseudo floor height of the current level.

[0069] Step S103: Based on the sensor data of the target vehicle traveling on the current level and / or associated level, determine the relative pose constraint residual and height increment residual of two trajectory points in the target vehicle's running trajectory.

[0070] Step S104: Perform position smoothing optimization on the target vehicle based on the relative pose constraint residual and the height increment residual to obtain the pose optimization result.

[0071] Step S105: Based on the pose optimization results, build the optimized map.

[0072] The process of determining the boundary position between the ramp and the current leveling layer of the target vehicle includes: determining the first pitch angle corresponding to the sensor data of the target vehicle at the first sampling time; searching backward from the first sampling time to the second sampling time to determine the second pitch angle corresponding to the sensor data of the target vehicle at the second sampling time; if the absolute value of the difference between the first pitch angle and the second pitch angle is greater than a first threshold, then the position of the target vehicle at the second sampling time is determined as the boundary position between the ramp and the current leveling layer.

[0073] For example, the process of determining the associated floor corresponding to the current floor includes: determining whether there is an associated floor for the current floor based on the floor height of the current floor and the reference floor height of each stored floor, wherein the difference between the reference floor height of the associated floor and the floor height of the current floor is less than a second threshold; if so, then the floor height of the current floor is corrected based on the reference floor height of the stored associated floor to obtain the target pseudo floor height of the current floor; if not, then the floor height of the current floor is stored as the reference floor height of the current floor.

[0074] Before determining the relative pose constraint residual and height increment residual of two trajectory points in the target vehicle's running trajectory, the method further includes: determining the target vehicle's running trajectory and odometer observation data based on sensor data; determining the pose of the target vehicle corresponding to each trajectory point in the running trajectory based on the odometer observation data; and determining the pose increment observation value between the poses of the target vehicle corresponding to two trajectory points based on the pose of the target vehicle corresponding to each trajectory point.

[0075] For example, pose includes position and attitude, and pose increment observations include position increment observations and attitude increment observations; the process of determining the relative pose constraint residuals of two trajectory points in the target vehicle's trajectory includes:

[0076] Based on the target vehicle's attitude at the first trajectory point, the target vehicle's attitude at the second trajectory point, and the attitude increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, the relative attitude constraint residuals are determined; wherein, the relative attitude constraint residuals are determined using the following formula:

[0077]

[0078] In formula (1), err odmo_rota q represents the relative attitude constraint residual. ij_dr q represents the attitude increment observation. i q represents the attitude of the target vehicle corresponding to the first trajectory point. j This indicates the attitude of the target vehicle corresponding to the second trajectory point;

[0079] Based on the target vehicle's position at the first trajectory point, the target vehicle's position at the second trajectory point, and the position increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, the relative position constraint residual is determined; wherein, the relative position constraint residual is determined using the following formula:

[0080] err odom_posi -R(q i )*t ij_dr -(t i -t i ) (Formula 2);

[0081] In formula 2, err odmo_posi t represents the relative position constraint residual. ij_dr t represents the position increment observation. i t represents the position of the target vehicle corresponding to the first trajectory point. j R(q) represents the position of the target vehicle corresponding to the second trajectory point. i ) represents a matrix established based on the attitude of the target vehicle corresponding to the first trajectory point;

[0082] Based on the obtained relative attitude constraint residuals and relative position constraint residuals, the relative pose constraint residuals are determined.

[0083] For example, in determining the height increment residual between two trajectory points in the target vehicle's trajectory, the process includes: determining the height components of the target vehicle at the two trajectory points based on the target vehicle's position at the first trajectory point and the target vehicle's position at the second trajectory point; and determining the height increment residual based on the difference between the height component of the target vehicle at the first trajectory point and the height component of the target vehicle at the second trajectory point. The height increment residual is determined using the following formula:

[0084] err heigh_posi =(t i [2]-t j [2]) (Formula 3);

[0085] In formula (3), err heigh_posi The height increment residual, t i [2] represents the height component of the target vehicle at the first trajectory point, t j [2] represents the height component of the target vehicle at the second trajectory point.

[0086] For example, the process of performing pose smoothing optimization on the target vehicle based on the relative pose constraint residual and the height increment residual includes: establishing a residual function based on the relative pose constraint residual and the height increment residual; establishing a loss function with the global pose as the state variable based on the residual function, wherein the global pose includes the pose of the target vehicle at all trajectory points in the current level and / or associated levels; solving the loss function using a nonlinear least squares optimization algorithm to achieve a local minimum; wherein the loss function is determined using the following formula:

[0087]

[0088] In Equation 4, F(x) is the loss function, f i (x) represents the residual function, and m represents the number of trajectory points.

[0089] As can be seen from the above technical solutions, in the embodiments of this application, during the cross-level driving of the target vehicle, due to the increasing error of the accumulated sensor data (the sensor data is obtained recursively, and the error will gradually increase as the recursion distance increases), even when the target vehicle is driving on the current level's running trajectory, the heights corresponding to different trajectory points are deviated, resulting in a high degree of divergence in the running trajectory of the current level.

[0090] Alternatively, if the target vehicle is traveling on the trajectory of the current floor and related floors, and there is a height difference between the current floor's height and the reference height of the related floors, although the current floor's height can be corrected based on the reference height of the related floors to obtain the target pseudo-height of the current floor, and floor clustering can be performed on the current floor and related floors, building a map based on the target pseudo-height of the current floor would result in poor map smoothness, insufficient pose accuracy for different trajectory points in the trajectory, and abrupt changes in pose height.

[0091] Therefore, given the high degree of divergence in the operating trajectories of the current floor and associated floors, the floor height of the current floor is corrected by using the reference floor height of the associated floors to obtain the pseudo floor height of the current floor. This allows for floor clustering of the current floor and associated floors. Furthermore, based on the relative pose constraint residuals and height increment residuals of any two trajectory points in the operating trajectory, pose smoothing optimization is performed on the target vehicle to obtain the pose optimization result. An optimized map is then built based on the pose optimization result, which can eliminate the high degree of divergence caused by the increase of sensor data with the recursive distance, thereby improving the accuracy of the final map.

[0092] Due to limitations in sensor costs, target vehicles are mostly equipped with only consumer-grade IMUs, wheel speed sensors, GPS, cameras, and low-power computing platforms. The above technical solutions do not require LiDAR sensors, do not rely on road priors for the scene, and do not require the target vehicle's trajectory to have loops. Furthermore, they have lower computational resource requirements. For at least two related planes, the pose optimization results at each sampling time are highly consistent, resulting in a smoother trajectory and higher accuracy in the final map.

[0093] The technical solutions described above in the embodiments of this application will be explained below in conjunction with specific application scenarios.

[0094] During the mapping process, a driver navigates the target vehicle, and the intelligent driving system records the vehicle's trajectory and scans the surrounding environment to create a map. However, in multi-story or underground parking garages, when a vehicle travels across levels, the intelligent driving system cannot effectively identify the exact floor where the target vehicle is located, thus failing to provide precise location information.

[0095] For example, in situations like underground parking lots where positioning sensors malfunction, the autonomous driving system cannot obtain satellite positioning signals, thus failing to determine the absolute pose of the target vehicle. If there is a height difference between the current floor where the target vehicle is located and related floors, the autonomous driving system cannot determine the related floors. Therefore, pseudo-floor height can be used to estimate the floor height of the target vehicle's location. Combining this with map and visual observations can then determine the target vehicle's pose, effectively preventing false matching.

[0096] For example, the target vehicle may include an on-board terminal device, which enables vehicle positioning. In addition to the on-board terminal device, see [link to other documentation]. Figure 2 As shown, the target vehicle also includes the following sensors.

[0097] Surround-view cameras: Taking four cameras as an example, these four cameras are the left-side camera, right-side camera, front-side camera, and rear-side camera. Of course, the number of cameras can be more or less. For example, two cameras at the front and rear. Or a camera with eight lenses mounted on the roof of the vehicle. Surround-view cameras are used to provide visual observation of the environment around a target vehicle. For example, the cameras can collect surround-view image data around the target vehicle and send the surround-view image data to the on-board terminal equipment.

[0098] Positioning sensor: The positioning sensor can be a GPS (Global Positioning System) sensor or a BeiDou sensor; there are no restrictions. Taking a GPS sensor as an example, the positioning sensor provides satellite positioning signals and can output the absolute position and orientation of the target vehicle. In other words, it can send the absolute position and orientation of the target vehicle to the onboard terminal equipment.

[0099] IMU sensor / wheel speed sensor. The IMU sensor can be a 3-axis IMU sensor or a 6-axis IMU sensor, used to provide motion information of the target vehicle, such as acceleration information and angular velocity information. That is, it can send the acceleration information and angular velocity information of the target vehicle to the vehicle terminal equipment.

[0100] Wheel speed sensors are used to provide motion information of the target vehicle. For example, wheel speed sensors are used to detect wheel speed and can send the wheel speed of the target vehicle to the on-board terminal equipment.

[0101] For example, the data output by the surround-view camera (surround-view image data), the data output by the positioning sensor (absolute pose of the target vehicle), the data output by the IMU sensor (such as acceleration information and angular velocity information), and the data output by the wheel speed sensor (such as wheel speed) can be referred to as sensor data; and the data output by the IMU sensor (such as acceleration information and angular velocity information) and the data output by the wheel speed sensor (such as wheel speed) can be referred to as odometer observation data.

[0102] For example, see Figure 3 The image shown is a schematic diagram of the mapping process.

[0103] For the mapping process, the input data includes data provided by the camera (such as surround view image data around the target vehicle), data provided by the positioning sensor (such as the absolute pose of the target vehicle), data provided by the IMU sensor (such as the acceleration and angular velocity information of the target vehicle), and data provided by the wheel speed sensor (such as the wheel speed of the target vehicle).

[0104] After receiving input data, the vehicle-mounted terminal equipment can perform data preprocessing. During preprocessing, operations such as time synchronization and invalid data filtering can be performed. Time synchronization refers to associating data from the same moment in time, such as associating surround view image data, absolute pose, acceleration information, angular velocity information, and wheel speed from the same moment. Invalid data filtering refers to removing invalid or redundant data.

[0105] During the mapping process, the vehicle-mounted terminal equipment can build a map based on the input data, thus obtaining map data. The vehicle-mounted terminal equipment comprehensively utilizes the observation information from various sensors to extract and generate the necessary elements in the scene, and completes the map construction after fusion processing.

[0106] When the driver clicks the Start Mapping button, the target vehicle starts the mapping function. The mapping process includes at least four steps: odometer calculation, slope detection, pseudo-layer height estimation, and pose constraint of the running trajectory. These steps are explained below.

[0107] 1. Odometer Calculation. For the odometer calculation process, the input data can be sensor data (such as acceleration and angular velocity information output by IMU sensors, and wheel speed output by wheel speed sensors, etc.), and the output data can be odometer observation data. The odometer observation data can include the target vehicle's pose, i.e., position and attitude, and the attitude includes at least the pitch angle, i.e., the pitch angle corresponding to the target vehicle.

[0108] During the odometer calculation process, based on sensor data (such as acceleration and angular velocity information output by IMU sensors, and wheel speed output by wheel speed sensors), the relative pose of the target vehicle at different trajectory points in the running trajectory (i.e., odometer observation data) can be calculated recursively. This relative pose may include pitch angle.

[0109] For a 3-axis IMU sensor, the lateral acceleration, longitudinal acceleration, and yaw rate of the target vehicle can be output, and the pitch and roll rates can be obtained based on the lateral acceleration, longitudinal acceleration, and yaw rate. For a 6-axis IMU sensor, the output data also includes yaw acceleration, as well as the directly obtainable pitch and roll rates.

[0110] 2. Slope Detection. For the slope detection process, the input data can be the pitch angle corresponding to the target vehicle, and the output data can be the slope detection result, which can be a slope or a level.

[0111] For example, during ramp detection, if the pitch angle of the target vehicle is greater than a threshold, the target vehicle can be considered to be on a ramp.

[0112] 3. Pseudo-story height estimation. For the pseudo-story height estimation process, the input data can be sensor data, slope detection results, and GPS data, and the output data can be the pseudo-story height, denoted as the target pseudo-story height.

[0113] For example, in an outdoor setting, the positioning sensor can output the absolute altitude of the target vehicle. However, once inside an indoor environment (such as an underground parking lot), the positioning signal will be blocked by buildings and disappear, and the positioning sensor will no longer be able to output the absolute altitude of the target vehicle. After this, the absolute altitude of the target vehicle can be determined based on the sensor data. However, the absolute altitude of the target vehicle is obtained recursively from the sensor data, and the altitude error will gradually increase with the increase of the recursion distance.

[0114] For example, see Figure 4 The diagram shown illustrates the principle of the pseudo-floor height algorithm. For ease of description, the principle of the pseudo-floor height algorithm is explained by demonstrating the driving process of a teaching vehicle going from the outside, passing a ramp to floor -1, then from floor -1, passing a ramp to floor -2, then from floor -2, passing a ramp to floor -1, and finally from floor -1, passing a ramp to the ground.

[0115] against Figure 4 The first image is an uncorrected diagram of the altitude variation. After the GPS signal disappears, the altitude of the target vehicle can be recursively calculated based on sensor data. As the recursion distance increases, the altitude gradually diverges, and after the teaching vehicle returns to the ground, a significant altitude error appears compared to the actual value.

[0116] It should be noted that, since the GPS signal is lost, the intelligent driving system cannot obtain the absolute altitude of the target vehicle. Therefore, the change in altitude of the target vehicle can be determined based on sensor data to determine the altitude difference between the current position of the target vehicle and its initial position when the GPS signal disappeared, and thus determine the altitude H2 of the current position of the target vehicle.

[0117] The system determines the pitch angle of the target vehicle using sensor data, and then determines whether the target vehicle is on a slope or level based on the pitch angles corresponding to the sensor data at sampling times of two adjacent data frames in the driving trajectory. For example, the intelligent driving system determines the pitch angle corresponding to the target vehicle as the first pitch angle based on the sensor data at the first sampling time t1. Starting from the first sampling time, it searches backward to the second sampling time t2 and determines the pitch angle corresponding to the target vehicle as the second pitch angle based on the sensor data at the second sampling time t2. If the intelligent driving system determines that the difference between the first pitch angle and the second pitch angle is greater than a first threshold, then at the first sampling time t1, the target vehicle is located on a slope, and at the second sampling time t2, the target vehicle is located at the boundary between the slope and level.

[0118] Understandably, if the intelligent driving system determines that the difference between the first pitch angle and the second pitch angle is less than a first threshold, the target vehicle is located on either a ramp or a level surface at both sampling times. If both the first pitch angle and the second pitch angle are less than the first angle threshold, then the target vehicle is located on a level surface at both the first sampling time t1 and the second sampling time t2; if both the first pitch angle and the second pitch angle are greater than the second angle threshold, then the target vehicle is located on a ramp at both the first sampling time t1 and the second sampling time t2; wherein, the first angle threshold is less than the second angle threshold.

[0119] against Figure 4 The second image is a schematic diagram illustrating the correction of the leveling height recursion error. The height of the leveling position is corrected using the ramp detection results; that is, the leveling height remains unchanged, ensuring that the height error is only caused by the ramp recursion error. Clearly, compared to the first image, this method can correct some of the height error.

[0120] For example, if the ramp detection results determine that locations A and B belong to the same level, and location A is the starting point of that level, then based on the height x of location A, the height of all locations from A to B is set to height x. In other words, the height of all locations on the level is height x. This way, the height of all locations on the level no longer changes, meaning the height is no longer altered based on sensor data. Clearly, after this processing, the height error caused by sensor data in the leveling process can be corrected.

[0121] against Figure 4The third image is a diagram illustrating the relationship between the -1 floor height. For example, you can search for related floors based on a height difference threshold; when passing through the -1 floor twice, the corresponding height of that floor will be associated.

[0122] For example, when the target vehicle passes through level -1 for the first time, the height of level -1 can be recorded as x1. When the target vehicle passes through level -1 for the second time, the height of level -1 can be recorded as x2. If the absolute value of the difference between height x1 and height x2 is less than the second threshold, it means that height x1 and height x2 correspond to the same level.

[0123] against Figure 4 The fourth image is a diagram illustrating the relationship between the -1 floor height correction and the ground height. The associated floor will be corrected based on the height of the first pass. After the -1 floor height is corrected, the height difference between the two passes will be less than the height difference threshold, therefore, the ground height will also be associated.

[0124] For example, the height x1 of the second passage through the -1 layer can be adjusted based on the height x1 of the first passage through the -1 layer. In other words, the height x2 is replaced with the height x1, thereby adjusting the height of the second passage through the -1 layer.

[0125] against Figure 4 The fifth image is a diagram illustrating ground height correction. After the ground height is linked, it will be corrected according to the initial height value, thus eliminating height errors between floors.

[0126] Of course, this application is not limited to two-story scenarios. For indoor scenarios involving floors across ramps, the above method can be used to calculate the pseudo-floor height to eliminate the cumulative error of sensor data between floors.

[0127] When using the above method for correction, the corrected floor height is not completely consistent with the actual height. Therefore, the corrected floor height can be called pseudo-floor height. As can be seen from the principle of the pseudo-floor height algorithm, when the corresponding floor ramp is traversed for the first time, the recursive error of the sensor data is not corrected. Therefore, in the subsequent mapping process, the problem of height divergence in the running trajectory still exists, thus affecting the accuracy of the final map.

[0128] Therefore, in addition to estimating the pseudo-layer height, it is also necessary to optimize the height divergence of the running trajectory.

[0129] 4. Pose constraints on the trajectory. The process of constraining the pose of the trajectory includes, for example: Figure 5 The following steps are shown:

[0130] S1031. Based on the attitude of the target vehicle at the first trajectory point, the attitude of the target vehicle at the second trajectory point, and the attitude increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, determine the relative attitude constraint residuals; the relative attitude constraint residuals are determined using the following formula:

[0131]

[0132] In formula (1), err odmo_rota q represents the relative attitude constraint residual. ij_dr q represents the attitude increment observation. i qj represents the attitude of the target vehicle corresponding to the first trajectory point, and qj represents the attitude of the target vehicle corresponding to the second trajectory point.

[0133] S1032. Based on the position of the target vehicle at the first trajectory point, the position of the target vehicle at the second trajectory point, and the position increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, determine the relative position constraint residual; wherein, the relative position constraint residual is determined using the following formula:

[0134] err odom_posi =R(q) i )*t ij_dr -(t i -t i ) (Formula 2);

[0135] In formula 2, err odmo_posi t represents the relative position constraint residual. ij_dr t represents the position increment observation. i t represents the position of the target vehicle corresponding to the first trajectory point. j R(q) represents the position of the target vehicle corresponding to the second trajectory point. i ) represents a matrix established based on the attitude of the target vehicle corresponding to the first trajectory point.

[0136] S1033. Determine the relative pose constraint residual based on the obtained relative attitude constraint residual and relative position constraint residual.

[0137] S1034. Based on the position of the target vehicle at the first trajectory point and the position of the target vehicle at the second trajectory point, determine the height components of the target vehicle at the two trajectory points respectively.

[0138] For example, the position of the target vehicle corresponding to the first trajectory point can be determined based on odometer observation data. A two-dimensional plane is selected within the position of the target vehicle corresponding to the first trajectory point. This two-dimensional plane is parallel to the z-axis, and the value of this two-dimensional plane in the z-axis direction is the height component H1 of the first trajectory point. When the target vehicle travels to the second trajectory point, the position of the target vehicle corresponding to the second trajectory point is determined based on odometer observation data. A two-dimensional plane is selected within the position of the target vehicle corresponding to the second trajectory point. This two-dimensional plane is parallel to the z-axis, and the value of this two-dimensional plane in the z-axis direction is the height component H2 of the second trajectory point.

[0139] S1035. Based on the difference between the height component of the target vehicle at the first trajectory point and the height component of the target vehicle at the second trajectory point, determine the height increment residual; wherein, the height increment residual is determined using the following formula:

[0140] errr heigh_posi =(t i [2]-t i [2]) (Formula 3);

[0141] In formula (3), err heigh_posi Indicates the high-increment residual, t i [2] represents the height component of the target vehicle at the first trajectory point, t j [2] represents the height component of the target vehicle at the second trajectory point.

[0142] Since pseudo-floor height estimation is only used for floor clustering among multiple horizontal floors, the target vehicle's trajectory determined based on sensor data still exhibits significant divergence during the pose constraint process of the running trajectory. Therefore, the target vehicle's actual running trajectory is as follows: Figure 6 As shown. For example, in the trajectory of the target vehicle passing through the -1 level for the second time, the trajectory of the target vehicle has a first trajectory point A1 and a second trajectory point A2, and the first trajectory point A1 and the second trajectory point A2 have a certain height difference in the height direction.

[0143] like Figure 6 As shown, for the trajectory of the target vehicle in the current level, in order to make the expected value of the height difference between the first trajectory point A1 and the second trajectory point A2 approach 0, the intelligent driving system acquires odometer observation data (acceleration information, angular velocity information, and wheel speed) based on sensor data, and determines the pose of the target vehicle at the first trajectory point A1 and the second trajectory point A2 based on the odometer observation data. The pose includes both position and attitude. Based on this, the intelligent driving system can determine the position and attitude of the target vehicle at the first trajectory point A1, the position and attitude of the second trajectory point A2, and the attitude increment observation value q between the poses of the target vehicle corresponding to the first trajectory point A1 and the second trajectory point A2.ij_dr and position increment observation t ij_dr This allows us to determine the relative pose constraint residual between the first trajectory point A1 and the second trajectory point A2, as well as the height increment residual between the first trajectory point A1 and the second trajectory point A2.

[0144] S1036. Based on the relative pose constraint residual and the height increment residual, establish the residual function; wherein, the height increment residual is determined by the following formula:

[0145]

[0146] In formula 5, err odmo_rota Represents the relative attitude constraint residual, err odmo_posi Represents the relative position constraint residual, err heigh_posi q represents the height increment residual. ij_dr t represents the attitude increment observation. ij_dr q represents the position increment observation. i q represents the attitude of the target vehicle corresponding to the first trajectory point. j t represents the attitude of the target vehicle corresponding to the second trajectory point. i t represents the position of the target vehicle corresponding to the first trajectory point. j R(q) represents the position of the target vehicle corresponding to the second trajectory point. i ) represents the matrix established based on the attitude of the target vehicle corresponding to the first trajectory point, t i [2] represents the height component of the target vehicle at the first trajectory point, t j [2] represents the height component of the target vehicle at the second trajectory point.

[0147] In the process of constraining the trajectory of an intelligent driving system, the expected value of both the height increment residual and the relative pose constraint residual approaches zero. Specifically, the expected value of the height increment residual approaching zero indicates that after the intelligent driving system performs pose smoothing optimization on the target vehicle, the height difference between any two trajectory points in the current level approaches zero. The expected value of the relative pose constraint residual approaching zero indicates that after the intelligent driving system performs pose smoothing optimization on the target vehicle, the pose change of the target vehicle at the two trajectory points remains unchanged. In other words, the pose change of the target vehicle at the two trajectory points before pose smoothing optimization is defined as the initial change, and the pose change of the target vehicle at the two trajectory points after pose smoothing optimization is defined as the optimized change. The initial change and the optimized change are the same. The pose after pose smoothing optimization is taken as the pose optimization result. This is to avoid changes in the pose of any trajectory point due to changes in the height component, which could affect the actual position of necessary elements (lane markings, parking spaces, pillars, etc.) in the established map.

[0148] Based on the above technical solution, for the poses of any two trajectory points on the same floor, the expected values ​​of the height increment residual and the relative pose constraint residual also approach 0, thereby eliminating height divergence at different locations on the same floor. That is, when eliminating height divergence at different locations on the same floor, the two trajectory points can be located in the current floor and the associated floor, respectively.

[0149] S1037. Based on the residual function, establish a loss function with the global pose as the state variable. The global pose includes the pose of the target vehicle at all trajectory points in the current level and / or associated levels. Solve the loss function using a nonlinear least squares optimization algorithm to achieve a local minimum. The loss function is determined using the following formula:

[0150]

[0151] In Equation 4, F(x) represents the loss function, 1 / 2 represents a fixed coefficient, x represents the pose of different trajectory points in the trajectory and serves as the state variable of the loss function, f i (x) represents the residual function, and m represents the number of trajectory points.

[0152] For example, when the loss function reaches a local minimum, the height difference between any two trajectory points in the current level and the associated level approaches 0, and the pose change of the target vehicle at the two trajectory points remains unchanged, so as to obtain the pose of the target vehicle at the two trajectory points after pose smoothing optimization, and the pose after pose smoothing optimization is used as the pose optimization result.

[0153] For example, arbitrarily select two trajectory points in the current level and associated levels, and define them as the first trajectory point and the second trajectory point, respectively. The height component of the first trajectory point is H1, and the height component of the second trajectory point is H2. Then, the height difference between the first and second trajectory points is H1 - H2. When the loss function reaches a local minimum, the height difference between the first and second trajectory points approaches 0, and the pose changes of the target vehicle at the first and second trajectory points remain constant. That is, while the height difference changes, the calculated poses of the first and second trajectory points are determined based on a nonlinear least squares optimization algorithm, and the change between them approaches 0, thus obtaining the poses of the target vehicle at the two trajectory points after pose smoothing optimization. Based on this, the poses of all trajectory points in the current level and associated levels after pose smoothing optimization are obtained, and the poses after pose smoothing optimization are used as the optimization result.

[0154] In other embodiments, when a local minimum of the loss function is achieved, the height difference between any two trajectory points in the current level and associated levels is 0, and the pose change of the target vehicle at the two trajectory points remains unchanged. This yields a pose optimization result where the expected values ​​of both the height increment residual and the relative pose constraint residual are 0. This results in a map where the height divergence between floors approaches 0, and the map accuracy is higher after pose smoothing optimization.

[0155] To simplify, the loss function in (Equation 4) is adjusted to the following formula:

[0156]

[0157] In Formula 6, f1(x) represents the residual function corresponding to the first trajectory point on the same floor. m f(x) represents the residual function corresponding to the m-th trajectory point on the same floor, and f(x) represents the matrix constructed based on the residual functions corresponding to all trajectory points on the same floor. T (x) represents the transpose of f(x).

[0158] make The following formula is obtained:

[0159]

[0160] In Formula 7, J i (x) represents taking the pose x of different trajectory points as state variables and applying it to the residual function f. i The partial derivative of the state variable x in (x) is based on J. i (x) determines the rate of change of the function with respect to the state variable x, while keeping other variables constant.

[0161] The first-order Taylor expansion of the residual function at x can be expressed by the following formula:

[0162] f(x+Δx)≈l(Δx)=f(x)+JΔx (Formula 8);

[0163] In Formula 8, Δx represents the change in pose corresponding to two trajectory points, and l represents a fixed constant value.

[0164] Based on Equation 8, the linear approximation of the loss function F(x) can be given by the following formula:

[0165]

[0166] In Formula 9, J T (x) denotes the transpose of J(x), f T (x) represents the transpose of f(x).

[0167] Formula 4 can be approximately linearized into a quadratic form using Formula 9, if the residual function f i If the Jacobian of (x) is full rank, then J T J(x) is positive definite. Therefore, the loss function F(x) has a minimum value F′(x+Δx)=0. This leads to the following linear equation:

[0168] J T (x)J(x)Δx=-J T (x)f(x) (Formula 10);

[0169] In Formula 10, J(x) represents the partial derivative of the residual function f(x) with respect to the state variable x, taking the pose x of different trajectory points as the state variable. T (x) represents the transpose of J(x), and Δx represents the result near the state variable x.

[0170] Since the linear equation is the result of a linear expansion around the initial state variable x, the change Δx obtained is the result around the initial state variable x.

[0171] Furthermore, for each trajectory point in the running trajectory, it is necessary to go through an iteration of the unfolded points to obtain the optimal solution when the loss function reaches a local minimum, that is, the pose optimization result.

[0172] Based on the above technical solution, the map optimization method provided in this application has the following beneficial effects:

[0173] There is no need to have prior knowledge of the scene to be mapped, i.e., to obtain the slope information, level information, or to create the initial map data in advance through machines and equipment.

[0174] The target vehicle can optimize its trajectory pose without being equipped with LiDAR to obtain a map with a height divergence between floors approaching 0.

[0175] The target vehicle does not need to complete the trajectory loop, which simplifies the mapping process and can be applied to low computing power platforms.

[0176] Based on the same concept as the methods described above, this application proposes a map optimization device, see [link to relevant documentation]. Figure 7 The diagram shown is a structural schematic of the map optimization device, which may include:

[0177] The determination module 11 is used to acquire sensor data of the target vehicle during the cross-level driving process; if the target vehicle drives to the boundary between the ramp and the current level, the floor height of the current level is determined based on the sensor data.

[0178] The correction module 12 is used to determine the associated floor corresponding to the current floor and correct the floor height of the current floor based on the reference floor height of the stored associated floor to obtain the target pseudo floor height of the current floor.

[0179] Optimization module 13 is used to determine the relative pose constraint residual of the target vehicle at two sampling times and the height increment residual of the target vehicle at the two sampling times based on sensor data of the target vehicle driving on the current level and / or associated level; and to perform pose smoothing optimization on the target vehicle according to the relative pose constraint residual and the height increment residual to obtain the pose optimization result.

[0180] Mapping module 14 is used to build a map based on the pose optimization results.

[0181] Based on the same application concept as the above method, this application proposes a vehicle, including:

[0182] IMU sensors are used to acquire acceleration and angular velocity information;

[0183] Wheel speed sensor, used to obtain wheel speed;

[0184] The processor is used to receive acceleration and angular velocity information sent by the IMU sensor and wheel speed sent by the wheel speed sensor, and to implement the vehicle positioning method of the above example of this application based on the acceleration information, angular velocity information and wheel speed.

[0185] Among them, vehicles include those equipped with intelligent driving systems; or,

[0186] The vehicles include those equipped with autonomous driving systems.

[0187] Based on the same application concept as the above method, this application proposes an in-vehicle terminal device 200, see [link to relevant documentation]. Figure 8 As shown, the vehicle-mounted terminal device 200 includes a processor 21 and a machine-readable storage medium 22, the machine-readable storage medium 22 storing machine-executable instructions that can be executed by the processor; the processor 21 is used to execute the machine-executable instructions to implement the map optimization method disclosed in the above example of this application.

[0188] Based on the same concept as the above method, this application also provides a machine-readable storage medium storing a plurality of computer instructions, which, when executed by a processor, can implement the map optimization method disclosed in the above examples of this application.

[0189] The aforementioned machine-readable storage medium can be any electronic, magnetic, optical, or other physical storage device that can contain or store information, such as executable instructions, data, etc. For example, machine-readable storage media can be: RAM (Random Access Memory), volatile memory, non-volatile memory, flash memory, storage drives (such as hard disk drives), solid-state drives, any type of storage disk (such as optical discs, DVDs, etc.), or similar storage media, or combinations thereof.

[0190] Based on the same application concept as the above method, this application embodiment also provides a computer program product, which may include a computer program that, when executed by a processor, implements the map optimization method disclosed in the above examples of this application.

[0191] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, embodiments of this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0192] The above description is merely an embodiment of this application and is not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

[0193] It should be understood that those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

Claims

1. A map optimization method, characterized in that, The method includes: During the process of the target vehicle traveling across layers, sensor data of the target vehicle is acquired; If the target vehicle travels to the boundary between the ramp and the current level, the floor height of the current level is determined based on the sensor data; the associated level corresponding to the current level is determined, and the floor height of the current level is corrected based on the stored reference floor height of the associated level to obtain the target pseudo floor height of the current level. Based on the sensor data of the target vehicle traveling on the current level and / or the associated level, determine the relative pose constraint residual and height increment residual of two trajectory points in the target vehicle's running trajectory; Based on the relative pose constraint residual and the height increment residual, the target vehicle is subjected to pose smoothing optimization to obtain the pose optimization result. Based on the pose optimization results, an optimized map is built.

2. The method according to claim 1, characterized in that, Before determining the relative pose constraint residual and height increment residual of two trajectory points in the target vehicle's trajectory, the method further includes: The target vehicle's trajectory is determined based on the sensor data, along with odometer observation data. The pose of the target vehicle corresponding to each trajectory point in the trajectory is then determined based on the odometer observation data. Based on the pose of the target vehicle corresponding to each trajectory point, determine the pose increment observation value between the poses of the target vehicle corresponding to two trajectory points.

3. The method according to claim 2, characterized in that, The pose includes position and attitude, and the pose increment observations include position increment observations and attitude increment observations; the process of determining the relative pose constraint residuals of two trajectory points in the target vehicle's trajectory includes: Based on the attitude of the target vehicle at the first trajectory point, the attitude of the target vehicle at the second trajectory point, and the attitude increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, the relative attitude constraint residual is determined; wherein, the relative attitude constraint residual is determined using the following formula: In formula 1, err odmo_rota q represents the relative attitude constraint residual. ij_dr Let q represent the attitude increment observation. i q represents the attitude of the target vehicle corresponding to the first trajectory point. j This indicates the attitude of the target vehicle corresponding to the second trajectory point; Based on the position of the target vehicle at the first trajectory point, the position of the target vehicle at the second trajectory point, and the position increment observations between the poses of the target vehicle corresponding to the first and second trajectory points, the relative position constraint residual is determined; wherein, the relative position constraint residual is determined using the following formula: In formula 2, err odmo_posi The relative position constraint residual, t ij_dr t represents the incremental observation value of the location. i t represents the position of the target vehicle corresponding to the first trajectory point. j q represents the position of the target vehicle corresponding to the second trajectory point. i R(q) represents the attitude of the target vehicle corresponding to the first trajectory point. i ) represents a matrix established based on the attitude of the target vehicle corresponding to the first trajectory point; The relative pose constraint residual is determined based on the obtained relative attitude constraint residual and the relative position constraint residual.

4. The method according to claim 2, characterized in that, The process of determining the height increment residual between two trajectory points in the target vehicle's trajectory includes: Based on the position of the target vehicle at the first trajectory point and the position of the target vehicle at the second trajectory point, the height components of the target vehicle at the two trajectory points are determined respectively. The height increment residual is determined based on the difference between the height component of the target vehicle at the first trajectory point and the height component of the target vehicle at the second trajectory point; wherein, the height increment residual is determined using the following formula: err hei，gh_posi = (t i [2]-t j [2]) (Official 3); In formula 3, err heigh_posi The height increment residual, t i [2] represents the height component of the target vehicle at the first trajectory point, t j [2] represents the height component of the target vehicle at the second trajectory point.

5. The method according to claim 2, characterized in that, Based on the relative pose constraint residual and the height increment residual, pose smoothing optimization is performed on the target vehicle to obtain the pose optimization result, including: Based on the relative pose constraint residual and the height increment residual, a residual function is established; Based on the residual function, a loss function is established with the global pose as the state variable, wherein the global pose includes the pose of the target vehicle at all trajectory points in the current level and / or the associated level. The loss function is solved using a nonlinear least squares optimization algorithm, such that the loss function reaches a local minimum; wherein the loss function is determined using the following formula: In Formula 4, F(x) represents the loss function, 1 / 2 represents the fixed coefficient, fi(x) represents the residual function, and m represents the number of trajectory points; When the loss function reaches a local minimum, the height difference between any two trajectory points in the current level and the associated level approaches 0, and the pose change of the target vehicle at the two trajectory points remains unchanged, so as to obtain the pose of the target vehicle at the two trajectory points after position smoothing optimization, and the pose after pose smoothing optimization is used as the pose optimization result.

6. The method according to claim 1, characterized in that, Determining the location where the target vehicle has traveled to the boundary between the ramp and the current level includes: Determine the first pitch angle corresponding to the sensor data of the target vehicle at the first sampling time; Starting from the first sampling time, search backward to the second sampling time to determine the second pitch angle corresponding to the sensor data of the target vehicle at the second sampling time; If the absolute value of the difference between the first pitch angle and the second pitch angle is greater than the first threshold, then the position of the target vehicle at the second sampling time is determined as the boundary position between the ramp and the current level.

7. The method according to claim 1, characterized in that, Determining the associated floor corresponding to the current floor includes: Based on the current floor height and the reference floor height of each stored floor, determine whether there is an associated floor of the current floor, where the difference between the reference floor height of the associated floor and the floor height of the current floor is less than a second threshold. If so, the height of the current floor is corrected based on the stored reference height of the associated floor to obtain the target pseudo height of the current floor; If not, the current floor height is stored as the reference floor height for the current floor.

8. A map optimization device, characterized in that, The device includes: The determination module is used to acquire sensor data of the target vehicle during the cross-level driving process; if the target vehicle drives to the boundary between the ramp and the current level, the floor height of the current level is determined based on the sensor data. The correction module is used to determine the associated floor corresponding to the current floor, and correct the floor height of the current floor based on the stored reference floor height of the associated floor to obtain the target pseudo floor height of the current floor. The optimization module is used to determine the relative pose constraint residual of the target vehicle at two sampling times and the height increment residual of the target vehicle at the two sampling times based on the sensor data of the target vehicle driving on the current level and / or the associated level; and to perform pose smoothing optimization on the target vehicle according to the relative pose constraint residual and the height increment residual to obtain the pose optimization result. The mapping module is used to build an optimized map based on the pose optimization results.

9. A vehicle-mounted terminal device, characterized in that, include: A processor and a machine-readable storage medium, the machine-readable storage medium storing machine-executable instructions that can be executed by the processor; The processor is configured to execute machine-executable instructions to implement the method of any one of claims 1-7.

10. A vehicle, characterized in that, include: IMU sensors are used to acquire acceleration and angular velocity information; Wheel speed sensor, used to obtain wheel speed; A processor is configured to receive the acceleration information and angular velocity information sent by the IMU sensor, and the wheel speed sent by the wheel speed sensor, and to implement the method according to any one of claims 1-7 based on the acceleration information, the angular velocity information, and the wheel speed.

Images