Vehicle driving track construction method and device and automatic driving vehicle

Abstract

The invention discloses a vehicle driving track construction method and device and an automatic driving vehicle. The method comprises the following steps: determining local pose information corresponding to track points in a track point set of a vehicle, and initial global pose information corresponding to the track points; adjusting the initial global pose information by using the local pose information to obtain first target global pose information corresponding to the track point; in response to the situation that the deformation degree of an initial track skeleton corresponding to the track point set is greater than a deformation degree threshold value, adjusting the initial track skeleton to obtain a target track skeleton, and adjusting first target global pose information of track points in the target track skeleton into second target global pose information; and determining a target track point from the track point set based on the second target global pose information, and constructing a target driving track of the vehicle according to the target track point. According to the invention, the technical problem of low accuracy of vehicle driving track construction is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and in particular, to a method and device for constructing a driving trajectory of a vehicle and an autonomous driving vehicle. Background Art

[0002] Currently, in the process of map construction and autonomous driving, vehicle positioning and driving trajectory recording are very important. Especially for memory driving mapping, the core lies in real-time recording of the position information of the vehicle during driving and generating a fine map model based on this information.

[0003] In the related art, memory driving mapping mainly relies on the fusion of various sensor data, including but not limited to lidar, vision sensors, inertial measurement units (IMUs for short), global positioning systems (GPSs for short), etc. Although the above sensors can provide the real-time position and attitude information of the vehicle, they each have inherent limitations and noise problems. For example, for GPS positioning, in urban canyons and areas with dense vegetation, GPS signals are easily affected by occlusion and multipath effects, resulting in positioning errors. For IMU measurement, although it can provide high-frequency pose updates, due to the cumulative error in the integration process, the accuracy of pose estimation will decrease after a long time. Vision and lidar data may be unstable in low visibility or fast-moving scenarios and are greatly affected by environmental factors.

[0004] The data recorded at the vehicle end in the above related art is usually the perception data in the local coordinate system, that is, the local pose information (local_pose). The above local pose information can be converted into the global coordinate system to obtain the initial global pose information. However, the trajectory constructed directly using the preliminary global pose information may be deformed due to error accumulation, affecting subsequent map construction and positioning accuracy. Therefore, there is still a technical problem of low accuracy in constructing the driving trajectory.

[0005] In view of the above problems, no effective solution has been proposed yet. Summary of the Invention

[0006] Embodiments of the present invention provide a method and device for constructing a driving trajectory of a vehicle and an autonomous driving vehicle, so as to at least solve the technical problem of low accuracy in constructing the driving trajectory of the vehicle.

[0007] According to one aspect of an embodiment of the present invention, a method for constructing a driving trajectory of a vehicle is provided. The method includes: determining local pose information corresponding to a trajectory point in a trajectory point set of the vehicle and initial global pose information corresponding to the trajectory point, where the trajectory point set is used to represent an initial driving trajectory of the vehicle on a road, the local pose information is used to represent the position and orientation of the vehicle at the trajectory point in a local coordinate system, and the initial global pose information is used to represent the position and orientation of the vehicle at the trajectory point in a global coordinate system; using the local pose information to adjust the initial global pose information to obtain first target global pose information corresponding to the trajectory point, where the error degree of the first target global pose information is less than the error degree of the initial global pose information; in response to the deformation degree of an initial trajectory skeleton corresponding to the trajectory point set being greater than a deformation degree threshold, adjusting the initial trajectory skeleton to obtain a target trajectory skeleton, and adjusting the first target global pose information of the trajectory point in the target trajectory skeleton to second target global pose information, where the initial trajectory skeleton is constructed from multiple trajectory points, the deformation degree of the target trajectory skeleton is less than or equal to the deformation degree threshold, and the error degree of the second target global pose information is less than the error degree of the first target global pose information; based on the second target global pose information, determining target trajectory points from the trajectory point set, and constructing a target driving trajectory of the vehicle according to the target trajectory points.

[0008] Optionally, using the local pose information to adjust the initial global pose information to obtain first target global pose information corresponding to the trajectory point includes: according to an adjustment strategy in an adjustment strategy set, using the local pose information to adjust the initial global pose information to obtain the first target global pose information, where different adjustment strategies in the adjustment strategy set correspond to different error types of the global pose information, and the error degree of the first target global pose information under the error type is less than the error degree of the initial global pose information under the error type.

[0009] Optionally, the adjustment strategy is a first adjustment strategy, and the first adjustment strategy is used to represent a rule for adjusting the initial global pose information. According to the adjustment strategy in the adjustment strategy set, using the local pose information to adjust the initial global pose information to obtain the first target global pose information includes: according to the first adjustment strategy, based on the local pose information of adjacent trajectory points in the trajectory point set, determining the relative pose information between the adjacent trajectory points; based on the relative pose information, determining a constraint condition for constraining the initial global pose information; and according to the constraint condition, adjusting the initial global pose information to obtain the first target global pose information.

[0010] Optionally, the adjustment strategy is a second adjustment strategy, which is used to represent the rule for adjusting the initial global pose information. According to the adjustment strategy in the adjustment strategy set, using the local pose information, the initial global pose information is adjusted to obtain the first target global pose information, including: according to the second adjustment strategy, based on the local pose information of adjacent trajectory points in the trajectory point set, determining the relative pose information between adjacent trajectory points; based on the relative pose information and the initial global pose information, determining the constraint conditions for constraining the initial global pose information; based on the local pose information and the initial global pose information, determining the rigid body transformation information, and adjusting the product of the rigid body transformation information and the initial global pose information according to the constraint conditions to obtain the first target global pose information, where the rigid body transformation information is used to represent the conversion relationship of the trajectory points between the local coordinate system and the global coordinate system.

[0011] Optionally, the adjustment strategy is a third adjustment strategy, which is used to represent the rule for adjusting the initial global pose information. According to the adjustment strategy, using the local pose information, the initial global pose information is adjusted to the first target global pose information corresponding to the adjustment strategy, including: according to the third adjustment strategy, performing smoothing processing on the local pose information to obtain smoothed pose information, where the accuracy of the smoothed pose information is greater than the accuracy of the local pose information; fusing the smoothed pose information and the initial global pose information to obtain a fusion result; adjusting the initial global pose information according to the fusion result to obtain the first target global pose information.

[0012] Optionally, the method further includes: on the road, at intervals of a target length, determining an initial trajectory point set from the trajectory point set, and constructing an initial trajectory skeleton according to the initial trajectory points in the initial trajectory point set; in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, adjusting the initial trajectory skeleton to obtain a target trajectory skeleton, including: in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, based on the local pose information of adjacent initial trajectory points in the initial trajectory point set, determining the relative pose information between adjacent initial trajectory points; based on the relative pose information and the initial global pose information of the initial trajectory points, adjusting the initial trajectory skeleton to obtain a target trajectory skeleton.

[0013] Optionally, adjusting the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information includes: performing interpolation processing on the first target global pose information of the trajectory points between the initial trajectory points under different adjustment strategies to obtain the third target global pose information corresponding to the adjustment strategy; determining the second target global pose information of the trajectory points from the third target global pose information of different adjustment strategies corresponding to the same trajectory point.

[0014] Optionally, determining the second target global pose information of the trajectory point from the third target global pose information of different adjustment strategies corresponding to the same trajectory point includes: an obtaining step of obtaining a subset of trajectory points including the current trajectory point from the set of trajectory points and determining a relationship matrix between the local pose information of the current trajectory point and the corresponding third target global pose information, where the third target global pose information corresponds to the adjustment strategy; during the process of traversing the trajectory points in the subset of trajectory points starting from the current trajectory point, determining the error information of the traversed trajectory points based on the relationship matrix; in response to traversing all the trajectory points in the subset of trajectory points, comparing the error information of different adjustment strategies corresponding to the same trajectory point to obtain a comparison result; and determining the third target global pose information corresponding to the adjustment strategy with the smallest error information in the comparison result as the second target global pose information.

[0015] Optionally, determining the target trajectory point from the set of trajectory points based on the second target global pose information includes: determining the trajectory points in the subset of trajectory points whose error information corresponding to the second target global pose information is less than or equal to the error information threshold as the target trajectory points; the method further includes: deleting, in the subset of trajectory points, the trajectory points whose error information corresponding to the second target global pose information is greater than the error information threshold.

[0016] Optionally, constructing the target driving trajectory of the vehicle according to the target trajectory points includes: constructing a driving sub-trajectory corresponding to the subset of trajectory points according to the target trajectory points in the subset of trajectory points; on the road, determining the next subset of trajectory points according to the target direction of the subset of trajectory points and returning to execute from the obtaining step to determine the target trajectory points in the next subset of trajectory points to construct the driving sub-trajectory corresponding to the next subset of trajectory points until there is no next subset of trajectory points in the set of trajectory points, and determining the target driving trajectory based on the driving sub-trajectory.

[0017] According to another aspect of the embodiments of the present invention, there is also provided a device for constructing a driving trajectory of a vehicle, including: a determining unit, configured to determine the local pose information corresponding to the trajectory points in the trajectory point set of the vehicle, and the initial global pose information corresponding to the trajectory points, where the trajectory point set is used to represent the initial driving trajectory of the vehicle on the road, the local pose information is used to represent the position and attitude of the vehicle at the trajectory point in the local coordinate system, and the initial global pose information is used to represent the position and attitude of the vehicle at the trajectory point in the global coordinate system; a first adjustment unit, configured to use the local pose information to adjust the initial global pose information to obtain the first target global pose information corresponding to the trajectory points, where the error degree of the first target global pose information is less than the error degree of the initial global pose information; a second adjustment unit, configured to, in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, adjust the initial trajectory skeleton to obtain a target trajectory skeleton, and adjust the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information, where the initial trajectory skeleton is constructed by multiple trajectory points, the deformation degree of the target trajectory skeleton is less than or equal to the deformation degree threshold, and the error degree of the second target global pose information is less than the error degree of the first target global pose information; a constructing unit, configured to determine target trajectory points from the trajectory point set based on the second target global pose information, and construct the target driving trajectory of the vehicle according to the target trajectory points.

[0018] According to another aspect of the embodiments of the present invention, there is also provided an electronic device, including: a memory storing an executable program; a processor configured to run the program, where when the program runs, it executes the methods in the various embodiments of the present invention.

[0019] According to another aspect of the embodiments of the present invention, there is also provided a computer-readable storage medium, the computer-readable storage medium including a stored executable program, where when the executable program runs, it controls the device where the computer-readable storage medium is located to execute the methods in the various embodiments of the present invention.

[0020] According to another aspect of the embodiments of the present invention, there is also provided a computer program product, including a computer program, where the computer program, when executed by a processor, implements the methods in the various embodiments of the present invention.

[0021] According to another aspect of the embodiments of the present invention, there is also provided a computer program product, including a non-volatile computer-readable storage medium storing a computer program, where the computer program, when executed by a processor, implements the methods in the various embodiments of the present invention.

[0022] According to another aspect of the embodiments of the present invention, there is also provided a computer program, where the computer program, when executed by a processor, implements the methods in the various embodiments of the present invention.

[0023] According to another aspect of the embodiments of the present invention, an autonomous vehicle is further provided, including a memory and a processor. The memory stores an executable program, and the processor is used to run the program. When the program runs, it executes the methods in the various embodiments of the present invention.

[0024] In the embodiments of the present invention, if it is necessary to construct the driving trajectory of the vehicle, a set of trajectory points of the vehicle on the road can be obtained, and the local pose information and the initial global pose information of each trajectory point in the set of trajectory points can be determined. To ensure the accuracy of the driving trajectory of the vehicle, the initial global pose information in the global coordinate system can be adjusted twice. The first adjustment is to use the local pose information to adjust the initial global pose information to obtain the first target global pose information with a smaller error degree. The deformation degree of the initial trajectory skeleton formed by the trajectory points can be detected. If it is detected that the deformation degree is greater than the deformation degree threshold, the initial trajectory skeleton can be adjusted to obtain the target trajectory skeleton. During the second adjustment process, the first target global pose information of each trajectory point in the adjusted target trajectory skeleton can be adjusted to obtain the second target global pose information. The target trajectory points can be determined from the set of trajectory points according to the second target global pose information to construct the target driving trajectory.

[0025] In the embodiments of the present invention, multi-stage pose optimization is realized, that is, the set of trajectory points of the vehicle on the road obtained is subjected to two global pose adjustments. The first adjustment uses the local pose information to optimize the initial global pose information to obtain the first target global pose information. The above process effectively reduces the pose uncertainty caused by sensor noise and cumulative error by integrating local dynamic information and global static information, achieves the technical effect of effectively avoiding errors and instability in the construction of the driving trajectory, and thus realizes the technical effect of improving the accuracy of the driving trajectory construction of the vehicle, and solves the technical problem of low accuracy in the construction of the driving trajectory of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] The drawings described herein are used to provide a further understanding of the present invention, and constitute a part of this application. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention. In the drawings:

[0027] Figure 1 is a flowchart of a method for constructing a driving trajectory of a vehicle according to an embodiment of the present invention;

[0028] Figure 2 is a flowchart of a process for determining a final second global pose according to an embodiment of the present invention;

[0029] Figure 3It is a flowchart of a method for post-processing a memory driving route according to an embodiment of the present invention;

[0030] Figure 4 It is a structural block diagram of a driving route construction device of a vehicle according to an embodiment of the present invention;

[0031] Figure 5 It is a structural block diagram of an autonomous vehicle according to an embodiment of the present invention. Detailed implementation manners

[0032] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0033] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and do not necessarily need to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described here can be implemented in an order different from those illustrated or described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device including a series of steps or units does not necessarily need to be limited to those clearly listed steps or units, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0034] According to an embodiment of the present invention, an embodiment of a method for constructing a driving route of a vehicle is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although the logical order is shown in the flowchart, in some cases, the steps shown or described here can be executed in a different order from here.

[0035] The embodiment of the present application provides a method for constructing a driving trajectory of a vehicle. This method can be used to provide memory driving mapping for vehicles in a preset application scenario, that is, to pre-construct a target driving trajectory for vehicle driving to guide the vehicle to drive in a future period. The above-mentioned preset application scenario may include the following scenarios in the vehicle field: commuting autonomous driving scenario, artificial intelligence (AI) driving scenario for household cars, automatic parking assist (APA) scenario (such as memory parking for self-owned parking spaces in a garage, intelligent parking for designated parking spaces in a parking lot, etc.), navigation guided pilot (NGP) scenario in urban areas or highway areas. In addition, the above-mentioned preset application scenario may also include, but is not limited to: the autonomous driving scenario that requires the use of augmented reality navigation function for intelligent driving trucks or driverless trucks in the logistics transportation field, the autonomous driving scenario that requires the use of augmented reality navigation function for autonomous agricultural vehicles in the agricultural machinery field, the autonomous driving scenario that requires the use of augmented reality navigation function for unmanned aerial vehicles, and the autonomous driving scenario that requires the use of augmented reality navigation function for intelligent robots (such as cleaning robots, service robots, delivery robots, etc.).

[0036] When the above-mentioned preset application scenario is a scenario in other fields except the vehicle field, those skilled in the art should be able to understand that the vehicle in the above-mentioned method for constructing a driving trajectory of a vehicle can be replaced with other objects (such as agricultural machinery, unmanned aerial vehicles, robots, etc.). Correspondingly, guiding the above-mentioned vehicle to drive by using the augmented reality navigation function is replaced with navigating other objects and guiding other objects to move, fly or drive. On this basis, in the embodiment of the present application, taking the vehicle field as an example, the specific implementation manner of the above-mentioned method for constructing a driving trajectory of a vehicle is described by way of example.

[0037] Figure 1 is a flowchart of a method for constructing a driving trajectory of a vehicle shown according to an embodiment of the present invention, as Figure 1 shown, the method includes the following steps:

[0038] Step S102, determine the local pose information corresponding to the trajectory points in the trajectory point set of the vehicle, and the initial global pose information corresponding to the trajectory points.

[0039] In the technical solution provided in step S102 of the present invention above, the trajectory point set can be used to represent the initial driving trajectory of the vehicle on the road. The trajectory point set is a set of a series of position information recorded at different time points during the vehicle's driving process. The trajectory points can be periodically collected by sensors carried on the vehicle, such as GPS, lidar, etc. during driving, and each trajectory point contains time and space information. When constructing the memory driving mapping, the trajectory point set is used to depict the path and attitude changes of the vehicle's driving.

[0040] Optionally, the local pose information can be used to represent the position and attitude of the vehicle at the trajectory point in the local coordinate system, and can be represented by local_pose. The local coordinate system can be established with the trajectory starting point as the origin. Therefore, local_pose reflects the change of the vehicle relative to the trajectory starting point and can be directly obtained by vehicle-mounted sensors, and may include the three-dimensional position coordinates (x, y, z) of the vehicle and the direction attitude (such as pitch angle, yaw angle, and roll angle). Due to the high-frequency update and relatively high accuracy of the sensors in the local coordinate system, local_pose can provide the dynamic change details of the vehicle during driving, but its absolute position information is limited by the cumulative error of the sensors and the uncertainty of the local environment.

[0041] Optionally, the initial global pose information can be used to represent the position and attitude of the vehicle at the trajectory point in the global coordinate system, and can be represented by raw_global_pose. The global coordinate system can be the Earth-Centered, Earth-Fixed (ECEF) coordinate system, that is, a coordinate system with the Earth's mass center as the origin. Raw_global_pose provides the absolute position information of the vehicle on the Earth, which helps to match and fuse the vehicle's driving trajectory with the geodetic system or map data. However, the raw_global_pose data may be affected by factors such as GPS signal occlusion and multipath effects, resulting in jumps and discontinuities in the position information. Directly using these data to construct the map will cause the offset and misalignment of map elements, reducing the accuracy and reliability of the map.

[0042] In this embodiment, if it is necessary to construct the driving trajectory of the vehicle, the trajectory point set of the vehicle can be obtained, and the local pose information and the initial global pose information corresponding to each trajectory point in the trajectory point set can be determined.

[0043] Optionally, data preparation is performed on the set of trajectory points of vehicle travel, and the local pose information local_pose and the initial global pose information raw_global_pose of each trajectory point can be determined. The above process lays a foundation for subsequent pose optimization and trajectory adjustment. Since local_pose and raw_global_pose respectively provide high-precision details of the dynamic changes of the vehicle and a macroscopic view of the absolute position, the combination of the above two pose information can overcome the deficiencies of their respective single data sources and provide the possibility for realizing smooth and accurate global pose information. The above embodiments can be a data preparation stage, ensuring the integrity and accuracy of the data in subsequent processing steps and providing key input information for the post-processing and optimization of the vehicle travel trajectory.

[0044] Optionally, local_pose can be provided by on-vehicle sensors, which can measure the dynamic information of the vehicle in the local coordinate system, including small changes in position and attitude. For example, through the IMU, information such as acceleration and angular velocity can be measured. Through integral operations, the speed and position of the vehicle in the local coordinate system (usually a coordinate system with the vehicle starting point as the origin) can be obtained. Further, the attitude angles of the vehicle (including pitch angle, yaw angle, and roll angle) can be obtained through the direction information. By emitting laser light and receiving the reflected signal by lidar, the distances and angles of the surrounding environment are measured, thereby constructing a three-dimensional point cloud map of the local environment. Combining with the motion information of the vehicle, the accurate pose of the vehicle in the local coordinate system can be updated. The measurement values of the IMU and lidar are fused, usually using a Kalman filter or other state estimation algorithms to solve the cumulative error of the IMU and the problems of short-term occlusion or measurement noise of the lidar, so as to obtain a more accurate local_pose.

[0045] It should be noted that the above process and method for determining the local pose information are only for illustrative purposes and are not specifically limited here.

[0046] Optionally, raw_global_pose can be provided by the vehicle-mounted GPS to describe the absolute position and orientation of the vehicle in the ECEF coordinate system. For example, by receiving signals from multiple satellites through a GPS receiver, the longitude and latitude coordinates of the vehicle on the earth's surface are calculated through the time difference and positioning algorithm, and further the three-dimensional coordinates of the vehicle in the ECEF coordinate system are obtained through elevation data, etc. The attitude information (pitch, yaw, roll) of the vehicle is usually directly measured by the IMU, but in the ECEF coordinate system, the attitude angles measured by the IMU can be converted to the ECEF coordinate system and calculated through the transformation matrix between coordinate systems. Although GPS can provide the absolute position of the vehicle, its attitude information may not be accurate enough, and in the case of multipath effects, signal occlusion, etc., the position information may also have errors. Therefore, the raw_global_pose needs to be fused with the data of local_pose, and its accuracy is improved through constraint conditions or algorithm correction. However, due to the limitations of GPS signals, raw_global_pose usually contains relatively large noise, and direct use may affect the quality of map construction.

[0047] It should be noted that the above processes and methods for determining the initial global pose information are only for illustrative purposes and are not specifically limited here.

[0048] Step S104: Use the local pose information to adjust the initial global pose information to obtain the first target global pose information corresponding to the trajectory point.

[0049] In the technical solution provided in step S104 of the present invention above, the error degree of the first target global pose information is less than that of the initial global pose information. The first target global pose information can be represented by first_global_pose and can be the ECEF coordinates of the trajectory point calculated for the first time.

[0050] In this embodiment, after determining the local pose information and the initial global pose information of each trajectory point in the trajectory point set, the local pose information can be used to adjust the initial global pose information to obtain the first target global pose information with a smaller error degree.

[0051] Optionally, this embodiment can be a key step for improving the accuracy of the global pose information. In this embodiment, the initial global pose information raw_global_pose is optimized by using the local pose information local_pose, so as to obtain the first target global pose information first_global_pose, and its error degree is significantly less than that of raw_global_pose.

[0052] Optionally, due to the instability and error accumulation of GPS signals, large jumps and discontinuities may occur. The local_pose information, based on measurements from high-frequency sensors such as IMUs and lidar, can provide high-precision relative positions and pose changes of the vehicle in the local coordinate system. By leveraging the high-precision characteristics of local_pose, the raw_global_pose can be corrected to reduce errors in the global pose and improve its continuity and reliability.

[0053] Optionally, the relative pose information in local_pose is corresponded to the global pose information of raw_global_pose to establish position and pose constraint conditions. The above constraints reflect the motion relationship between adjacent trajectory points of the vehicle and the absolute position information of raw_global_pose in the global coordinate system. Taking raw_global_pose as the initial value for optimization, the above process fully utilizes the absolute reference role of the initial global pose information. Based on the above constraint conditions and initial value, joint optimization processing is performed to adjust raw_global_pose, reducing errors and discontinuities in the global pose information, and finally obtaining first_global_pose. In the above optimization process, an attempt can be made to minimize the inconsistency between local_pose and raw_global_pose to improve the accuracy of the global pose.

[0054] In the embodiment of the present invention, by fusing local_pose and raw_global_pose information and using various optimization algorithms to adjust raw_global_pose, errors and discontinuities in the global pose information are reduced. The above process improves the accuracy and stability of the global pose information, providing more reliable basic data for subsequent trajectory skeleton construction and secondary global pose adjustment. In this way, the present invention effectively solves the problems of offset and misalignment that may occur when directly using raw_global_pose to construct a map, improving the quality of map building for memory driving.

[0055] Step S106, in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, adjusting the initial trajectory skeleton to obtain a target trajectory skeleton, and adjusting the first target global pose information of the trajectory points in the target trajectory skeleton to second target global pose information.

[0056] In the technical solution provided in step S106 of the present invention above, the initial trajectory skeleton is constructed from multiple trajectory points. The degree of deformation of the target trajectory skeleton is less than or equal to the deformation degree threshold. The error degree of the second target global pose information is less than that of the first target global pose information. The second target global pose information can be represented by second_global_pose and can be the ECEF coordinates obtained through the second calculation.

[0057] In this embodiment, after using the local pose information to adjust the initial global pose information to obtain the first target global pose information, the initial trajectory skeleton corresponding to the trajectory point set can be constructed, and the degree of deformation of the initial trajectory skeleton can be detected. If the degree of deformation is greater than the deformation degree threshold, the initial trajectory skeleton can be adjusted to obtain a target trajectory skeleton with a degree of deformation less than or equal to the deformation degree threshold. The first target global pose information of the trajectory points in the target trajectory skeleton can be adjusted to the second target global pose information.

[0058] Optionally, this embodiment is an optimization step that can generate more accurate and continuous global pose information. In this embodiment, attention is paid to how to further improve the initial trajectory skeleton constructed from multiple trajectory points and reduce the error of the global pose information by reducing the degree of deformation of the initial trajectory skeleton.

[0059] Optionally, after obtaining first_global_pose, the initial trajectory skeleton can be extracted from the trajectory points. For example, the trajectory points at a fixed distance interval can be selected as the skeleton points. The purpose of the above process is to reduce the number of points participating in the optimization, improve the calculation efficiency, and at the same time retain the main features of the driving trajectory.

[0060] Optionally, an evaluation of the degree of deformation of the constructed initial trajectory skeleton is performed. The above evaluation is based on the relative pose change of the trajectory points and the continuity of the global pose information to check whether there are significant inconsistencies or jumps. The degree of deformation can be quantified by calculating the relative error of the first_global_pose between the trajectory points, the change in trajectory curvature, or the degree of mismatch with other positioning sources (such as high-precision maps, visual features).

[0061] Optionally, if the detected degree of deformation exceeds the preset deformation degree threshold, it indicates that the initial trajectory skeleton constructed by first_global_pose may be incoherent or inaccurate in some parts. At this time, the initial trajectory skeleton needs to be further adjusted.

[0062] Optionally, after optimizing to obtain the target trajectory skeleton, adjust the first_global_pose based on the optimization result of the skeleton points (the target trajectory skeleton) to obtain the second_global_pose. The above adjustment ensures that the global pose information of the trajectory points is optimized, improving the accuracy and coherence of the entire driving trajectory.

[0063] Optionally, compared with the first_global_pose, the error degree of the second_global_pose in the ECEF coordinate system is significantly reduced, indicating that the accuracy and stability of the global pose information are further improved. The optimized second_global_pose can be used to construct a more accurate and stable memory driving map, reducing the possibility of map element offset and improving the usability and reliability of the map.

[0064] In the embodiment of the present invention, the accuracy of the global pose information is further improved by detecting and optimizing the deformation degree of the trajectory skeleton. The above process starts from constructing the initial trajectory skeleton, determines whether optimization is required by detecting the deformation degree, adjusts the trajectory skeleton using more stringent constraint conditions and optimization algorithms, and finally based on the optimized skeleton points. Through the secondary global pose adjustment, a second_global_pose with a smaller error degree is obtained. The above steps are crucial for eliminating the jitter of trajectory points caused by sensor errors or processing algorithms, and for improving the coherence and accuracy of the driving trajectory in the global coordinate system.

[0065] Step S108: Based on the second target global pose information, determine the target trajectory points from the trajectory point set, and construct the target driving trajectory of the vehicle according to the target trajectory points.

[0066] In the technical solution provided in step S108 of the present invention above, the target driving trajectory can also be referred to as the memory driving trajectory.

[0067] In this embodiment, after adjusting the first target global pose information to the second target global pose information, the target trajectory points can be determined from the trajectory point set based on the second target global pose information, and the target driving trajectory can be constructed according to the target trajectory points.

[0068] Optionally, this embodiment is a key step for constructing the final target driving trajectory based on the second_global_pose. The goal of the above process is to use the optimized global pose information to generate a smooth, accurate and highly globally consistent vehicle driving trajectory, providing support for subsequent memory driving map construction.

[0069] Optionally, after obtaining a more accurate second_global_pose, a series of trajectory points that meet the requirements can be filtered out from the original trajectory point set, that is, target trajectory points, as the basis for constructing the final driving trajectory. The selection of target trajectory points may be based on multiple factors, including the error evaluation of the second_global_pose, the distribution density of the trajectory points, and the requirements for the coherence and smoothness of the driving trajectory.

[0070] Optionally, further error evaluation can be performed on the second_global_pose. For example, by comparing the optimized global pose information with known high-precision maps or point cloud data to ensure the accuracy of the positions of the target trajectory points. The above steps help to identify and exclude points that may not be suitable for constructing the final trajectory due to excessive local errors.

[0071] Optionally, once the target trajectory points are determined, according to the order of the target trajectory points, the global pose information in the second_global_pose can be used to connect the individual target trajectory points to form a continuous trajectory line.

[0072] Optionally, based on the target trajectory points, additional trajectory optimization processing can be performed to further improve the coherence and accuracy of the driving trajectory. This can include fine-tuning the trajectory line, removing redundant points, and reducing noise through a smoothing algorithm to ensure that the finally constructed memory driving trajectory can more accurately reflect the actual driving path of the vehicle.

[0073] Optionally, after constructing the target driving trajectory, map elements associated with the driving trajectory can be updated, including road models, obstacle positions, landmark points, etc. By using the optimized global pose information, the positioning of the map elements can be ensured to be more accurate, thereby improving the quality of the entire memory driving mapping. The finally constructed target driving trajectory (memory driving trajectory) will be stored as the basic data for subsequent vehicle navigation, autonomous driving positioning, and map updating. The above target driving trajectory not only contains the path information of the vehicle's movement, but also contains the attitude changes of the vehicle during driving, providing key support for realizing the advanced functions of memory driving.

[0074] In an embodiment of the present invention, a series of target trajectory points meeting the requirements are determined from the original trajectory point set by using the optimized second_global_pose information. Then, based on the target trajectory points, a smooth, continuous and highly accurate target driving trajectory is constructed. The above process not only improves the accuracy of the driving trajectory, but also ensures the matching between the driving trajectory and map elements, as well as the global consistency of the driving trajectory, providing high-quality driving path data for memory driving mapping. Through the above steps, high-precision driving trajectory information that can be used for vehicle navigation, positioning and map construction can be generated, significantly improving the accuracy and reliability of memory driving mapping.

[0075] In steps S102 to S108 of the present invention, if it is necessary to construct the driving trajectory of the vehicle, the trajectory point set of the vehicle on the road can be obtained, and the local pose information and the initial global pose information of each trajectory point in the trajectory point set can be determined. To ensure the accuracy of the driving trajectory of the vehicle, the initial global pose information in the global coordinate system can be adjusted twice. The first adjustment is to use the local pose information to adjust the initial global pose information to obtain the first target global pose information with a smaller error degree. The deformation degree of the initial trajectory skeleton formed by the trajectory points can be detected. If it is detected that the deformation degree is greater than the deformation degree threshold, the initial trajectory skeleton can be adjusted to obtain the target trajectory skeleton. During the second adjustment, the first target global pose information of each trajectory point in the adjusted target trajectory skeleton can be adjusted to obtain the second target global pose information. The target trajectory points can be determined from the trajectory point set according to the second target global pose information to construct the target driving trajectory. Thus, the technical effect of improving the accuracy of the driving trajectory construction of the vehicle is achieved, and the technical problem of low accuracy of the driving trajectory construction of the vehicle is solved.

[0076] Next, the process of using the local pose information to adjust the initial global pose information to obtain the first target global pose information in this embodiment will be further described.

[0077] As an optional implementation manner, step S104, using the local pose information to adjust the initial global pose information to obtain the first target global pose information corresponding to the trajectory point, includes: adjusting the initial global pose information by using the local pose information according to the adjustment strategy in the adjustment strategy set to obtain the first target global pose information, where different adjustment strategies in the adjustment strategy set correspond to different error types of the global pose information, and the error degree of the first target global pose information under the error type is less than the error degree of the initial global pose information under the error type.

[0078] In this embodiment, in the process of using local pose information to adjust the initial global pose information to obtain the first target global pose information, the initial global pose information can be adjusted according to the adjustment strategies in the adjustment strategy set to obtain the first target global pose information. Among them, different adjustment strategies in the adjustment strategy set correspond to different error categories. The error degree of the first target global pose information under the error type is less than that of the initial global pose information under the error type. The adjustment strategy set can include different ways to read and adjust the initial global pose information.

[0079] Optionally, the process of adjusting the initial global pose information can be refined into a more customized process, aiming to optimize different error types in the initial global pose information by applying a specific set of adjustment strategies. The core of the above method is that different error types may stem from different reasons, so different strategies are needed to effectively reduce the error degree under that error type.

[0080] Optionally, identify and classify various error types that may exist in the initial global pose information (raw_global_pose). The above errors may include but are not limited to: position drift error, attitude angle error, time synchronization error, error caused by multipath effect, signal loss error caused by occlusion, etc.

[0081] Optionally, design corresponding adjustment strategies for each error type. For example, for position drift error, an optimization strategy based on relative pose constraint may be adopted; for attitude angle error, IMU data and visual feature matching may be used for correction; for time synchronization error, the timestamp of sensor data may be adjusted to ensure data alignment.

[0082] Optionally, according to the specific error type in the current trajectory point set, select a more appropriate adjustment strategy from the adjustment strategy set for application.

[0083] Optionally, use the high-precision relative pose information in local_pose, combined with the selected adjustment strategy, to adjust raw_global_pose. For example, techniques such as Kalman filtering, least squares optimization, and nonlinear optimization can be used to fuse the local pose information with the global pose information, thereby generating the optimized first_global_pose.

[0084] Optionally, after applying the adjustment strategy, evaluate the error degree of the first_global_pose under a specific error type and compare it with the corresponding error of the raw_global_pose. If the error degree of the first_global_pose is significantly less than that of the raw_global_pose, it indicates that the adjustment strategy is effective; otherwise, the strategy needs to be reselected or adjusted until the optimization goal is achieved. The entire optimization process can be iterated multiple times. Each iteration dynamically adjusts the application of the strategies in the strategy set based on the analysis results of the error type and error degree to ensure that the performance of the first_global_pose under various error types meets the requirements.

[0085] In the embodiment of the present invention, by defining an adjustment strategy set and selecting a suitable strategy according to the specific error type, the effective adjustment of the raw_global_pose is achieved, and the first_global_pose with a lower error degree is generated. The above method improves the pertinence and efficiency of global pose optimization and helps to construct a more accurate memory driving trajectory in a complex environment. By adopting the above method, various errors commonly found in the raw_global_pose can be processed and reduced more effectively, thereby ensuring the high precision and reliability of the final memory driving mapping.

[0086] Next, a further description will be given of the process of adjusting the initial global pose information when the adjustment strategy is the first adjustment strategy in this embodiment.

[0087] As an optional implementation manner, the adjustment strategy is the first adjustment strategy. The first adjustment strategy is used to represent the rule for adjusting the initial global pose information. According to the adjustment strategy in the adjustment strategy set, the initial global pose information is adjusted using the local pose information to obtain the first target global pose information, including: according to the first adjustment strategy, based on the local pose information of adjacent trajectory points in the trajectory point set, determine the relative pose information between adjacent trajectory points; based on the relative pose information, determine the constraint conditions for constraining the initial global pose information; according to the constraint conditions, adjust the initial global pose information to obtain the first target global pose information.

[0088] In this embodiment, if the adjustment strategy is the first adjustment strategy, during the process of adjusting the initial global pose information, the relative pose information between adjacent trajectory points can be determined according to the first adjustment strategy based on the local pose information of adjacent trajectory points in the trajectory point set. The constraint conditions for constraining the initial global pose information can be determined based on the relative pose information. The initial global pose information can be adjusted according to the constraint conditions to obtain the first target global pose information. Among them, the relative pose information can be the relative pose of the local_pose of adjacent trajectory points.

[0089] Optionally, when the adjustment strategy is specified as the first adjustment strategy, the specific rules corresponding to the first adjustment strategy will be followed, and the pose information in the local area will be used to adjust and optimize the global pose information to obtain the first target global pose information (first_global_pose).

[0090] Optionally, the local pose information local_pose of adjacent trajectory points is extracted from the trajectory point set. local_pose includes the displacement and attitude of the vehicle in the local coordinate system and usually comes from high-frequency sensors such as lidar, IMU (Inertial Measurement Unit), etc., so it has high accuracy and reliability. Based on the local_pose of adjacent trajectory points, the relative pose information between these two points is determined. The relative pose information describes the position change and attitude rotation between two adjacent trajectory points and is the key bridge connecting the local coordinate system and the global coordinate system.

[0091] Optionally, using the calculated relative pose information, constraint conditions are constructed. The above constraint conditions reflect the actual motion of the vehicle in the local coordinate system. The constraint conditions can be translational constraints, rotational constraints, or a combination of the above constraints to ensure that the adjusted global pose information can truly reflect the driving path of the vehicle.

[0092] Optionally, the constraint conditions not only consider the relative pose of adjacent trajectory points but also combine the initial global pose information raw_global_pose, enabling the adjustment process to take into account both the absolute positioning in the global coordinate system and the relative motion in the local coordinate system, thereby improving the accuracy of the global pose information.

[0093] Optionally, based on the established constraint conditions, the system uses an appropriate optimization algorithm to adjust raw_global_pose. The goal of the optimization algorithm is to minimize the inconsistency between the global pose information and the local relative pose information. For example, optimization algorithms such as the least squares method and nonlinear optimization can be used. After the optimization algorithm runs, a set of optimized global pose information, that is, first_global_pose, is obtained. Under the guidance of the constraint conditions, compared with raw_global_pose, the error degree of this set of information in terms of error types (such as position drift error, attitude angle error) is significantly reduced.

[0094] Optionally, error verification can also be performed on the generated first_global_pose to evaluate its performance under different error types. If there is still room for optimization, it may be necessary to readjust the optimization parameters or select a more suitable optimization algorithm for iterative optimization until the expected accuracy requirements are met.

[0095] In an embodiment of the present invention, during the process of adjusting the global pose information by adopting the first adjustment strategy, the relative pose information of adjacent trajectory points is calculated to establish a constraint condition, and then based on the above conditions, the initial global pose information is adjusted by using an optimization algorithm to generate the first target global pose information with a lower error degree. This strategy is particularly suitable for dealing with position drift and attitude angle errors caused by unstable GPS signals or cumulative sensor errors. By fusing local high-precision information with global pose information, the accurate correction of global pose information is achieved, providing more accurate and stable pose data for subsequent memory driving mapping.

[0096] The following details the process of how to adjust the initial global pose information when the adjustment strategy in this embodiment is the second adjustment strategy.

[0097] As an alternative embodiment, the adjustment strategy is the second adjustment strategy, and the second adjustment strategy is used to represent the rule for adjusting the initial global pose information. According to the adjustment strategy in the adjustment strategy set, the initial global pose information is adjusted by using local pose information to obtain the first target global pose information, including: according to the second adjustment strategy, based on the local pose information of adjacent trajectory points in the trajectory point set, determining the relative pose information between adjacent trajectory points; based on the relative pose information and the initial global pose information, determining the constraint condition for constraining the initial global pose information; based on the local pose information and the initial global pose information, determining the rigid body transformation information, and adjusting the product of the rigid body transformation information and the initial global pose information according to the constraint condition to obtain the first target global pose information, where the rigid body transformation information is used to represent the conversion relationship between the trajectory point in the local coordinate system and the global coordinate system.

[0098] In this embodiment, if the adjustment strategy is the second adjustment strategy, during the process of adjusting the initial global pose information, the relative pose information of adjacent trajectory points can be determined according to the second adjustment strategy based on the local pose information of adjacent trajectory points in the trajectory point set. The corresponding constraint condition can be determined according to the relative pose information and the initial global pose information. The rigid body transformation information is determined based on the local pose information and the initial global pose information, and the product of the rigid body transformation information and the initial global pose information can be adjusted according to the constraint condition to determine the rigid body transformation information. The product of the rigid body transformation information and the initial global pose information can be adjusted according to the agreed condition to obtain the first target global pose information. Among them, the rigid body transformation information can be used to represent the conversion relationship between the trajectory point in the local coordinate system and the global coordinate system, can be a rigid body transformation matrix, and can be represented by T. The rigid body transformation information can be used to describe the translation and rotation relationship when converting from the local coordinate system to the global coordinate system. T reflects the overall conversion rule, rather than just the change between local adjacent points.

[0099] Optionally, when the adjustment strategy is set to the second adjustment strategy, a more complex method will be adopted to adjust and optimize the initial global pose information to obtain the first target global pose information (first_global_pose). The above process involves key steps such as the determination of relative pose information, the establishment of constraint conditions, and the adjustment of rigid body transformation information.

[0100] Optionally, extract the local pose information local_pose of adjacent trajectory points from the trajectory point set. The above information includes the displacement and attitude change of the vehicle in the local coordinate system. Based on local_pose, calculate the relative pose information between adjacent trajectory points. The relative pose information describes the position and attitude change from one trajectory point to another, which is crucial for connecting the local and global coordinate systems.

[0101] Optionally, use the relative pose information and the initial global pose information raw_global_pose to construct constraint conditions. The above constraint conditions include: translation constraint, ensuring that the displacement of the adjusted global pose in the vehicle traveling direction is consistent with the local relative pose information; rotation constraint, keeping the vehicle attitude change synchronized with the local pose information; and time continuity constraint, ensuring the continuity and time consistency of the global pose information. The establishment of constraint conditions aims to guide the optimization of the global pose information, so that the adjusted first_global_pose can more accurately reflect the actual driving trajectory of the vehicle, and reduce the global pose error caused by unstable GPS signals, sensor error accumulation, etc.

[0102] Optionally, the determination of the rigid body transformation information (T) can be based on the local_pose and raw_global_pose of the trajectory points. The calculation of T may involve global optimization algorithms, such as the least squares method, nonlinear optimization, etc., to ensure that the obtained rigid body transformation information can minimize the inconsistency between the global pose information and the local relative pose information.

[0103] Optionally, adjust the product of T and raw_global_pose according to the established constraint conditions. This process may include iterative optimization, by modifying T, to minimize the difference between the global pose information and the local relative pose information. After the above adjustment, the optimized T is obtained, and then it is multiplied by raw_global_pose to obtain the first target global pose information first_global_pose. First_global_pose is significantly lower than raw_global_pose in terms of error degree, and is smoother and more continuous, and is more suitable for constructing a high-precision memory driving trajectory.

[0104] Optionally, the accuracy of the first_global_pose can also be verified to evaluate its performance under different error types. If there is still room for optimization, it may be necessary to readjust T or optimize the parameters of the algorithm and perform iterative optimization until a satisfactory accuracy level is achieved.

[0105] In the embodiment of the present invention, during the process of adjusting the global pose information using the second adjustment strategy, not only the relative pose information between adjacent trajectory points is calculated, but also a rigid body transformation information T covering the entire trajectory is determined. By combining T with the raw_global_pose and making adjustments under the guidance of the constraint conditions, the system generates the first_global_pose with improved smoothness and accuracy. The above adjustment strategy is particularly suitable for dealing with the systematic errors of the pose information in the global coordinate system. By introducing the rigid body transformation information and performing iterative optimization, the effective correction of the global pose information is realized, providing more stable and accurate global pose data for the memory driving mapping.

[0106] Next, a further explanation will be given on how to adjust the initial global pose information when the adjustment strategy in this embodiment is the third adjustment strategy.

[0107] As an optional implementation manner, the adjustment strategy is the third adjustment strategy, and the third adjustment strategy is used to represent the rule for adjusting the initial global pose information. According to the adjustment strategy, using the local pose information, the initial global pose information is adjusted to the first target global pose information corresponding to the adjustment strategy, including: smoothing the local pose information according to the third adjustment strategy to obtain smoothed pose information, where the accuracy of the smoothed pose information is greater than that of the local pose information; fusing the smoothed pose information and the initial global pose information to obtain a fusion result; and adjusting the initial global pose information according to the fusion result to obtain the first target global pose information.

[0108] In this embodiment, if the adjustment strategy is the third adjustment strategy, during the process of adjusting the initial global pose information, the local pose information can be smoothed according to the third adjustment strategy to obtain smoothed pose information with higher accuracy. The smoothed pose information and the initial global pose information can be fused to obtain a fusion result. The initial global pose information can be adjusted according to the fusion result to obtain the first target global pose information. Among them, the smoothed pose information can be represented by the smooth_pose of the trajectory point, and the accuracy of the smoothed pose information is greater than that of the local pose information.

[0109] Optionally, when the initial global pose information is optimized using the third adjustment strategy, the process is designed to include three key steps: smoothing, information fusion, and global pose adjustment, aiming to generate first target global pose information (first_global_pose) with a lower error level and higher smoothness.

[0110] Optionally, during the smoothing process, local pose information local_pose can be preprocessed to identify and reduce noise, as well as eliminate or mitigate the effects caused by sensor instability, multipath effects, occlusion, etc. A smoothing algorithm is used to process local_pose. The smoothed smooth_pose reflects the actual movement of the vehicle while significantly reducing the inaccuracies caused by random and systematic errors in sensor measurements, and thus is more accurate than the unprocessed local_pose.

[0111] Optionally, before fusing smooth_pose and raw_global_pose, they can be time-synchronized to ensure that each smooth_pose matches the corresponding raw_global_pose in time for effective fusion. The choice of fusion algorithm is based on the characteristics of smooth_pose and raw_global_pose and the requirements of a specific application. For example, weighted average fusion can be used, where the weights of smooth_pose and raw_global_pose reflect their respective accuracies and reliabilities. Another method is to use fusion based on least squares optimization to optimize the global pose information by minimizing the difference between the prediction and the actual measurement.

[0112] Optionally, after the fusion algorithm runs, the generated fusion result is a combination containing smoothed local data and global positioning information, providing a more accurate vehicle position and attitude estimate in the global coordinate system.

[0113] Optionally, the raw_global_pose is further adjusted according to the fusion result. The above process may involve iterative optimization to correct the error in the global pose information by minimizing the difference between the fusion result and the raw_global_pose. After the above smoothing process, information fusion, and global pose adjustment, the optimized first target global pose information first_global_pose is generated. first_global_pose is significantly superior to raw_global_pose in terms of smoothness and continuity, and its accuracy is also improved. It is also possible to verify the accuracy and continuity of first_global_pose. Based on the verification results, it may be necessary to readjust the parameters of the smoothing algorithm, the weights of the fusion algorithm, or the constraints in the optimization process to further improve the accuracy and stability of the global pose information.

[0114] In the embodiment of the present invention, a third adjustment strategy is adopted. By first smoothing the local pose information, the continuity and stability of the data are improved. The smoothed local pose information is fused with the initial global pose information to generate a more accurate fusion result. The initial global pose information is adjusted according to the fusion result to obtain first_global_pose with higher accuracy and better smoothness. The above strategy effectively utilizes the high-precision characteristics of local data while maintaining the global consistency of global positioning data, providing a powerful method for optimizing pose information for memory driving mapping.

[0115] For example, the Rauch-Tung-Striebel smoother (abbreviated as RTS smoother) algorithm can be used to fuse smooth_pose and raw_global_pose, which can overcome the limitations of relying solely on the forward processing of Kalman filtering, that is, errors may accumulate when dealing with noise and uncertainty. The backward smoothing process of the RTS smoother algorithm utilizes the information of the entire trajectory to correct the forward estimation and generates the pose information in the ECEF coordinate system after smoothing, significantly improving the accuracy and continuity of the global pose information. This method is particularly suitable for processing pose data containing a large amount of instantaneous noise and discontinuous changes. By using the complete trajectory information for smoothing, it provides more continuous, stable, and accurate global pose data for memory driving mapping.

[0116] The following further describes the process of how to adjust the deformation degree of the initial trajectory skeleton in this embodiment.

[0117] As an alternative implementation, the method further includes: on a road, at intervals of a target length, determining an initial trajectory point set from the trajectory point set, and constructing an initial trajectory skeleton according to the initial trajectory points in the initial trajectory point set; in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than a deformation degree threshold, adjusting the initial trajectory skeleton to obtain a target trajectory skeleton, including: in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than a deformation degree threshold, determining the relative pose information between adjacent initial trajectory points based on the local pose information of the adjacent initial trajectory points in the initial trajectory point set; adjusting the initial trajectory skeleton based on the relative pose information and the initial global pose information of the initial trajectory points to obtain a target trajectory skeleton.

[0118] In this embodiment, on a road, at intervals of a target length, an initial trajectory point can be determined from the trajectory point set, and an initial trajectory skeleton (skeleton) can be constructed according to the initial trajectory points in the initial trajectory point set. If the deformation degree of the initial trajectory skeleton is greater than the deformation degree threshold, the relative pose information between adjacent initial trajectory points can be determined based on the local pose information of the adjacent initial trajectory points in the initial trajectory point set. Based on the relative pose information and the initial global pose information of the initial trajectory points, the initial trajectory skeleton can be adjusted to obtain a target trajectory skeleton. Among them, the target length can be a preset length. For example, it can be preset to 10 meters. This is only for illustrative purposes and is not specifically limited. The relative pose information can also be referred to as relative pose constraint.

[0119] Optionally, this embodiment optimizes the global pose information by constructing and adjusting the trajectory skeleton, especially for applications in long-distance road environments.

[0120] Optionally, on a road, specific trajectory points are selected from the original trajectory point set according to a preset target length (for example, 10 meters). The selection of the target length should consider road characteristics, vehicle driving speed, and the accuracy and sampling rate of the positioning sensor to ensure that the selected points can represent the characteristics of the entire trajectory while maintaining computational efficiency. The selected initial trajectory point set is used to construct an initial trajectory skeleton. The above skeleton contains trajectory points distributed at intervals of the target length, and the initial trajectory points (skeleton points) carry preliminary global pose information raw_global_pose, forming a rough framework of the entire driving path. However, since raw_global_pose may contain large errors or deformations, the skeleton itself may also exhibit obvious discontinuities or deviations.

[0121] Optionally, evaluate the degree of deformation of the initial trajectory skeleton, which can be measured by calculating the geometric inconsistencies between the poses of each point on the trajectory skeleton, such as angular errors, position offsets, etc. Evaluating the degree of deformation helps identify potential error or discontinuity problems in the skeleton. Set a threshold for the degree of deformation to determine whether the initial trajectory skeleton needs to be adjusted. If the degree of deformation of the initial trajectory skeleton exceeds the deformation degree threshold, it indicates that the skeleton has significant inaccuracies and needs to be optimized through subsequent steps.

[0122] Optionally, based on the local pose information local_pose of adjacent trajectory points in the initial trajectory point set, determine the relative pose information between each pair of adjacent trajectory points. The relative pose information describes the displacement and rotational changes between two points and is an important basis for skeleton adjustment. Use an optimization algorithm (such as least squares optimization, nonlinear optimization, etc.) to adjust the initial trajectory skeleton using the relative pose information and the initial global pose information raw_global_pose. The above adjustment process aims to minimize the inconsistency between the global pose information of each point on the skeleton and the relative pose information, thereby generating a smoother and more accurate target trajectory skeleton.

[0123] Optionally, through the above adjustment, a target trajectory skeleton is generated, which is significantly lower than the initial trajectory skeleton in terms of the degree of deformation, that is, smoother, more continuous and accurate. The target trajectory skeleton not only improves the accuracy of the road driving trajectory but also reduces the random errors and systematic errors in the global pose information. To construct a complete optimized trajectory, interpolation can be performed on the trajectory points between the skeletons. By analyzing the positions and postures of the target trajectory skeleton points, an appropriate interpolation method (such as spline interpolation, linear interpolation, etc.) is used to calculate the global pose information of the trajectory points between the skeletons, so that the entire trajectory is not only accurate at the skeleton points but also maintains continuity and smoothness between the skeletons.

[0124] Optionally, verify the accuracy of the generated target trajectory skeleton and evaluate its performance under different error types. If there is still room for improvement, the parameters of the optimization algorithm can be readjusted, a more suitable target length or optimization method can be selected, and iterative optimization can be performed until the expected accuracy requirements are met.

[0125] In the embodiments of the present invention, by constructing and optimizing the trajectory skeleton, the above embodiments effectively handle the discontinuity and error problems in the road driving trajectory, especially in the driving scenario of a long distance. From constructing the initial trajectory skeleton, evaluating the degree of deformation, to adjusting and optimizing based on the relative pose information, and then to generating the target trajectory skeleton and interpolating between the skeletons, the entire process comprehensively considers the characteristics of local and global pose information, providing more accurate, continuous and stable global pose data for memory driving mapping.

[0126] Next, the process of adjusting the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information in this embodiment will be further described.

[0127] As an alternative implementation, step S106 of adjusting the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information includes: performing interpolation processing on the first target global pose information of the trajectory points between the initial trajectory points under different adjustment strategies to obtain the third target global pose information corresponding to the adjustment strategy; determining the second target global pose information of the trajectory points from the third target global pose information of different adjustment strategies corresponding to the same trajectory point.

[0128] In this embodiment, in the process of adjusting the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information, interpolation processing can be performed on the first target global pose information of the trajectory points between the initial trajectory points under different adjustment strategies to obtain the third target global pose information corresponding to the adjustment strategy. The second target global pose information of the trajectory points can be determined from the third target global pose information of different adjustment strategies corresponding to the same trajectory point.

[0129] Optionally, the above implementation further optimizes the pose information of the trajectory points in the target trajectory skeleton. By adopting a variety of different adjustment strategies, interpolation processing is performed on the trajectory points between the skeletons, and then based on the results of the above adjustment strategies, more accurate second target global pose information is determined.

[0130] Optionally, different adjustment strategies are applied to each initial trajectory point in the target trajectory skeleton. Each adjustment strategy processes the first target global pose information of the trajectory points between the initial trajectory points to generate the corresponding third target global pose information.

[0131] Optionally, in order to generate a continuous trajectory between the initial trajectory points, the system adopts an interpolation algorithm, such as spline interpolation, linear interpolation, or Gaussian process regression, etc., to calculate the third target global pose information of the trajectory points between the skeletons based on the first target global pose information (first_global_pose) and the raw_global_pose of the initial trajectory points obtained under the adjustment strategy.

[0132] Optionally, through the above process, for different adjustment strategies, the system generates different third target global pose information for each trajectory point between the skeletons. The above information includes pose estimation based on a variety of correction and optimization methods, providing a rich data source for subsequent comprehensive analysis.

[0133] Optionally, compare the third target global pose information obtained for the same trajectory point under different adjustment strategies to evaluate the quality of the pose information under each strategy. The evaluation criteria may include the continuity of the pose information, the consistency with surrounding trajectory points, and the degree of agreement with the ground truth data. Based on the above evaluation, determine which adjustment strategy's third target global pose information is suitable for a specific trajectory point among the above adjustment strategies.

[0134] Optionally, once the third target global pose information under the adjustment strategy that meets the requirements is determined, designate it as the second_global_pose of this trajectory point. The second_global_pose represents the pose information that is considered the most accurate and stable after comprehensive evaluation among multiple pose optimization strategies. For the entire trajectory, a global optimization can also be performed to further improve the smoothness and accuracy of the entire trajectory by minimizing the inconsistency between the second_global_poses of all trajectory points. The above optimization process may involve iterative algorithms until the preset convergence condition is met.

[0135] In the embodiment of the present invention, by combining multiple adjustment strategies and interpolation processing, third target global pose information is generated for the trajectory points between the skeletons, and then through comparison and selection, the second target global pose information of each trajectory point is determined. The above method makes full use of the advantages of different strategies, takes into account both local smoothness and global consistency, and provides strong support for constructing a high-quality memory driving mapping trajectory. By comprehensively evaluating and selecting the third target global pose information under the adjustment strategy that meets the requirements, the accuracy and reliability of the second target global pose information are further ensured, providing a more stable and accurate pose data basis for subsequent applications such as road feature recognition and path planning.

[0136] The following further describes the process of determining the second target global pose information of a trajectory point from the third target global pose information of different adjustment strategies corresponding to the same trajectory point in this embodiment.

[0137] As an alternative implementation, to determine the second target global pose information of a trajectory point from the third target global pose information corresponding to different adjustment strategies for the same trajectory point, it includes: an acquisition step of obtaining, from the set of trajectory points, a subset of trajectory points including the current trajectory point, and determining a relationship matrix between the local pose information of the current trajectory point and the corresponding third target global pose information, where the third target global pose information corresponds to the adjustment strategy; during the process of traversing the trajectory points in the subset of trajectory points starting from the current trajectory point, determining the error information of the traversed trajectory point based on the relationship matrix; in response to finishing traversing the trajectory points in the subset of trajectory points, comparing the error information corresponding to different adjustment strategies for the same trajectory point to obtain a comparison result; and determining the third target global pose information corresponding to the adjustment strategy with the smallest error information in the comparison result as the second target global pose information.

[0138] In this embodiment, during the process of determining the second target global pose information of a trajectory point from the third target global pose information corresponding to different adjustment strategies for the same trajectory point, the acquisition step can be executed. In the acquisition step, a subset of trajectory points including the current trajectory point can be obtained from the set of trajectory points, and a relationship matrix between the local pose information of the current estimated point and the corresponding third target global pose information can be determined. During the process of traversing the trajectory points in the subset of trajectory points starting from the current trajectory point, the error information of the traversed trajectory point can be determined based on the relationship matrix. After finishing traversing the trajectory points in the subset of trajectory points, the error information corresponding to different adjustment strategies for the same trajectory point can be compared to obtain a comparison result. The third target global pose information corresponding to the adjustment strategy with the smallest error information in the comparison result can be determined as the second target global pose information. Among them, the subset of trajectory points can be set M. The relationship matrix can be the transformation relationship Ti between the local_pose of the current trajectory point and the second_global_pose.

[0139] Optionally, by comparing and selecting the third target global pose information generated under different adjustment strategies, the most accurate and reliable second target global pose information for each trajectory point can be determined. The above process involves the comprehensive analysis and evaluation of local and global pose information, ensuring that the finally selected pose information can appropriately reflect the actual driving path of the vehicle.

[0140] Optionally, from the entire set of trajectory points, a subset of trajectory points including the current trajectory point is selected. This subset can be several points before and after the current trajectory point, usually a local area selected to ensure the accuracy of the evaluation. For each trajectory point in the subset, a relationship matrix between its local pose information local_pose and the corresponding third target global pose information is calculated. The above relationship matrix describes the transformation relationship between the local coordinate system and the global coordinate system, including translation and rotation, and is used for the determination of subsequent error information.

[0141] Optionally, starting from the current trajectory point, each point in the subset of trajectory points is traversed. The above traversal process is to evaluate the error information of all points in the subset, so as to provide a basis for subsequent strategy selection. Based on the calculated relationship matrix, the error information of each traversed trajectory point can be determined. The error information may include the position error and attitude error of the trajectory point in the global coordinate system. These error information reflect the degree of consistency between the global pose information and the local motion characteristics under a specific adjustment strategy.

[0142] Optionally, once the subset of trajectory points is traversed, the system will compare the error information under different adjustment strategies corresponding to the same trajectory point. The above steps help the system identify the strategy with the smallest error by quantifying the quality of the pose information under different strategies. The comparison process generates a comparison result, listing all the evaluated adjustment strategies and their corresponding error information. The comparison result is the key basis for selecting the appropriate adjustment strategy. Based on the comparison result, the adjustment strategy with the smallest error information is determined. The third target global pose information under the above adjustment strategy is considered to be the closest to the actual driving path, so the corresponding adjustment strategy is the most appropriate choice.

[0143] Optionally, the third target global pose information corresponding to the most appropriate adjustment strategy is specified as the second_global_pose of the current trajectory point. second_global_pose is the global pose information that is considered to be the most accurate and stable after multi-strategy comparison and is used to construct the final optimized trajectory.

[0144] In the embodiment of the present invention, through the above implementation manner, not only are multiple adjustment strategies applied to each trajectory point in the target trajectory skeleton, but also by comparing the error information of the trajectory points in the subset, it is ensured that the strategy result with the smallest error is selected as the second target global pose information. This process improves the accuracy of the global pose information, reduces the inaccuracy and bias that may be introduced by a single strategy, and provides more accurate and reliable pose data for subsequent applications such as road feature recognition and path planning. By combining local and global pose information, and through comparing and selecting strategies, this implementation effectively improves the accuracy and reliability of memory driving mapping.

[0145] For example, an evaluation function is established to evaluate the results of second_global_pose obtained in three different ways. For each trajectory point of the trajectory, the transformation relationship T_i between the current trajectory point's local_pose and second_global_pose is calculated. Then, the local_pose and second_global_pose within 100 meters before and after the current trajectory point are taken and denoted as set M. For each trajectory point in set M, T_i * local_pose - global_pose is calculated, and the maximum error local_max_error of the trajectory point is recorded. All trajectory points are traversed, and the maximum value of local_max_error of all trajectory points is taken as the maximum error max_error of the trajectory. Then, the maximum errors max_error of the second_global_pose obtained in the three ways are compared, and the second_global_pose with the smallest max_error is selected as the final result.

[0146] Next, in this embodiment, the process of determining the target trajectory point from the trajectory point set based on the second target global pose information will be further described.

[0147] As an optional implementation manner, step S108 of determining the target trajectory point from the trajectory point set based on the second target global pose information includes: determining, as the target trajectory point, the trajectory points in the trajectory point subset whose error information corresponding to the second target global pose information is less than or equal to the error information threshold; the method further includes: deleting, in the trajectory point subset, the trajectory points whose error information corresponding to the second target global pose information is greater than the error information threshold.

[0148] In this embodiment, in the process of determining the target trajectory point from the trajectory point set based on the second target global pose information, the trajectory points in the trajectory point subset whose error information corresponding to the second target global pose information is less than or equal to the error information threshold can be determined as the target trajectory points. It is also possible to delete the trajectory points in the trajectory point subset whose error information is greater than the error information threshold. Among them, the error information can be the maximum error of the trajectory, which can be represented by max_error.

[0149] Optionally, this implementation manner further optimizes the utilization of the second target global pose information, ensuring that the finally generated memory driving mapping trajectory has high precision and reliability.

[0150] Optionally, in the preprocessing stage, an error information threshold can be set, and this threshold serves as a criterion for judging the quality of trajectory points. The setting of the threshold should be based on the requirements of specific application scenarios, such as positioning accuracy requirements, map update frequency, etc., to ensure the accuracy and coherence of the final trajectory. Traverse the subset of trajectory points. For each trajectory point, it checks whether the error information corresponding to the second target global pose information of this point is less than or equal to the set error information threshold. If the error information meets the condition, that is, the pose estimation of this point is within the allowable error range, it is determined as a target trajectory point, and these points will be saved for subsequent trajectory construction and map update.

[0151] Optionally, through the above process, a set of target trajectory points are screened out from the original trajectory point set, and the second target global pose information of the above target trajectory points is considered to be the most accurate and reliable. The determination process of target trajectory points effectively removes those points with inaccurate pose information due to various reasons (such as poor GPS signal, sensor noise, etc.), thereby improving the overall quality of the trajectory.

[0152] Optionally, during the process of traversing and checking the subset of trajectory points, for those trajectory points whose error information corresponding to the second target global pose information is greater than the error information threshold, they are marked as problematic points. Perform a deletion operation to remove the trajectory points marked as problematic from the subset of trajectory points. Through the above operations, the negative impact of problematic points on subsequent trajectory construction and map update processes can be avoided, such as introducing unnecessary noise or deviation. After removing the problematic points, only the target trajectory points with error information within the threshold range are retained. Based on the target trajectory points, a more accurate and continuous driving trajectory can be constructed for memory-based driving mapping.

[0153] In the embodiment of the present invention, by setting the error information threshold and determining the target trajectory points and deleting the problematic points based on the error information of the second target global pose information, the high quality of the memory-based driving mapping trajectory is ensured. The above process helps to reduce the errors introduced by uncertain or inaccurate positioning data and improves the accuracy and reliability of the map. Target trajectory points are screened out from the original trajectory point set, and only the points with error information within the acceptable range are retained. Then, based on the above points, the final optimized driving trajectory is constructed, providing a solid positioning data foundation for applications such as vehicle positioning, path planning, and map construction. Through the above screening and optimization process, the complex and changeable driving environment can be effectively processed, and a mapping trajectory that meets the high-precision requirements can be generated.

[0154] For example, if the max_error deviation corresponding to the final second_global_pose is greater than the threshold, the problematic trajectory segments need to be removed, and the non-problematic trajectory segments need to be retained. Record the local_max_error of each trajectory point, and retain the continuous trajectory segments where the local_max_error is less than the threshold.

[0155] Next, the process of constructing the target driving trajectory according to the target trajectory points in this embodiment will be further described.

[0156] As an optional implementation manner, step S108 of constructing the target driving trajectory of the vehicle according to the target trajectory points includes: constructing a driving sub-trajectory corresponding to the trajectory point subset according to the target trajectory points in the trajectory point subset; on the road, determining the next trajectory point subset of the trajectory point subset according to the target direction of the trajectory point subset, and returning to execute from the acquisition step to determine the target trajectory points in the next trajectory point subset, so as to construct the driving sub-trajectory corresponding to the next trajectory point subset until there is no next trajectory point subset in the trajectory point set, and determining the target driving trajectory based on the driving sub-trajectory.

[0157] In this embodiment, during the process of constructing the target driving trajectory according to the target trajectory points, a driving sub-trajectory corresponding to the trajectory point subset can be constructed according to the target trajectory points in the trajectory point subset. On the road, the next trajectory point subset of the trajectory point subset can be determined according to the target direction of the trajectory point subset. And return to execute from the acquisition step to determine the target trajectory points in the next trajectory point subset, so as to construct the driving sub-trajectory corresponding to the next trajectory point subset until there is no next trajectory point subset in the trajectory point set. The final target driving trajectory can be determined based on the multiple driving sub-trajectories obtained above. Among them, the target direction can be in front of or behind the current trajectory point subset.

[0158] Optionally, the above implementation manner forms the final target driving trajectory by gradually constructing and connecting multiple driving sub-trajectories, and this process aims to ensure the continuity, accuracy, and integrity of the trajectory.

[0159] Optionally, select a trajectory point subset including the current trajectory point from the optimized and filtered target trajectory point set. This subset can be several points before and after the current trajectory point, and the specific range depends on the system design, aiming to construct a local driving sub-trajectory. Construct the driving sub-trajectory corresponding to this subset according to the target trajectory points in the trajectory point subset. The above process involves using the second target global pose information of the target trajectory points and may combine other sensor information (such as acceleration, steering angle, etc.) to ensure the accuracy and coherence of the sub-trajectory.

[0160] Optionally, while constructing the current driving sub-trajectory, determine the target direction of the subset of trajectory points. The target direction can be based on the relative position of the end point of the current sub-trajectory and the next point, and is used to guide the selection and construction of the next subset of trajectory points. According to the target direction, select the next subset of trajectory points from the set of trajectory points. The above subset should be closely related to the current subset and be able to smoothly connect the current sub-trajectory to form a continuous driving path. Return to the start of the "acquisition step" and repeat the above process for the next subset of trajectory points, that is, determine the target trajectory points in the subset and construct the corresponding driving sub-trajectory. The above iterative process continues until there is no next subset of trajectory points available for construction in the set of trajectory points.

[0161] Optionally, during the iterative process, continuously connect each constructed driving sub-trajectory. The selection of the connection points should be based on the consistency and continuity of the global pose information to ensure the smoothness and uninterruptedness of the entire target driving trajectory. When all subsets of trajectory points are processed, that is, a complete chain of driving sub-trajectories is constructed, determine the final target driving trajectory based on the above driving sub-trajectories. The above trajectory should cover the entire driving path and have optimized pose information at each point. It is also possible to perform an overall evaluation and verification of the integrated target driving trajectory to ensure that it meets the preset accuracy and coherence standards. During the evaluation process, it is also possible to fuse with other sensor data and compare with known map data to further improve the reliability of the target driving trajectory.

[0162] In the embodiment of the present invention, the final target driving trajectory is gradually formed by constructing and connecting multiple driving sub-trajectories. This process not only ensures the accuracy and coherence of each sub-trajectory segment, but also realizes the optimized construction of the entire trajectory through iterative execution. The system constructs the driving sub-trajectory according to the target trajectory points in the target trajectory point set, and determines the construction of the next sub-trajectory based on the target direction until the entire set of trajectory points is processed. Through this segmented processing and iterative construction method, the system effectively manages the complex and large amount of trajectory point data, generates a continuous driving trajectory that meets the high-precision requirements, and provides a solid data foundation for applications such as vehicle positioning, path planning, and map construction.

[0163] For example, for the retained trajectory segment, extend it forward by 100 meters each time, and then perform the above steps on the extended trajectory segment; if the max_error of the extended trajectory segment is less than the threshold, extend it further forward until the trajectory extends to the first point of the trajectory or max_error is greater than the threshold; if the max_error of the extended trajectory is greater than the threshold, remove this extended segment. Then extend the trajectory backward in the same way as forward extension, extending 100 meters each time and calculating the max_error of the extended trajectory. Stop until the extended trajectory segment reaches the last point of the trajectory or max_error is greater than the threshold. And retain the trajectory segment with max_error less than the threshold for the last time.

[0164] The technical solution of the embodiment of the present invention will be illustrated by way of preferred embodiments below.

[0165] Currently, in memory driving mapping, the vehicle end records the perception data in the local coordinate system (referred to as local_pose) with the trajectory starting point as the origin, and it is necessary to convert the perception data into the ECEF coordinate system (a coordinate system with the earth's centroid as the origin). Therefore, it is necessary to obtain the pose of the ECEF coordinate system corresponding to each trajectory point. There are jumps in the pose of the trajectory points in the ECEF coordinate system (referred to as raw_global_pose) recorded at the vehicle end, and directly using raw_global_pose will cause map element offsets. Therefore, there is still the technical problem of low accuracy in constructing the driving trajectory.

[0166] However, the embodiment of the present invention proposes a method for post-processing the memory driving mapping trajectory, which recalculates the ECEF coordinate system corresponding to the trajectory points, solves the problem of trajectory point pose jumps, and improves the mapping quality. Thus, the technical effect of improving the accuracy of constructing the vehicle's driving trajectory is achieved, and the technical problem of low accuracy in constructing the vehicle's driving trajectory is solved.

[0167] The method of the embodiment of the present invention will be further illustrated below.

[0168] Figure 2 It is a flowchart showing the determination process of a final second global pose according to the embodiment of the present invention, as Figure 2 shown, and the method may include the following steps:

[0169] Step S201, obtain local_pose and raw_global_pose, and optimize raw_global_pose in three ways.

[0170] In this embodiment, according to the timestamps of the trajectory points in the local coordinate system, the corresponding raw_global_pose of the trajectory points is obtained by interpolation. The first calculation of the ECEF coordinates of the trajectory points is performed in three ways.

[0171] Optionally, in the first method, constraints are established based on the relative poses of adjacent trajectory points' local_pose and the raw_global_pose corresponding to the trajectory points, and joint optimization is performed with the raw_global_pose of the trajectory points as the initial value.

[0172] Optionally, in the second method, a rigid body transformation T is calculated from the local_pose and raw_global_pose of all trajectory points; then constraints are established based on the relative poses of adjacent trajectory points' local_pose and the raw_global_pose corresponding to the trajectory points, and joint optimization is performed with the product of the rigid body transformation T and the local_pose as the initial value.

[0173] Optionally, in the third method, the RTS smoother algorithm is used to fuse the smooth_pose and raw_global_pose of the trajectory points to obtain the smoothed ECEF coordinates.

[0174] Step S202: Extract the skeleton, perform joint optimization, and interpolate the trajectory between the skeletons based on the optimization results.

[0175] In this embodiment, there are large trajectory deformations in the ECEF coordinates (referred to as first_global_pose) of the trajectory points calculated in the first calculation, and a second calculation of the ECEF coordinates of the trajectory points is required. The second trajectory optimization only optimizes the skeleton of the trajectory: a trajectory point is selected every 10 meters to form the trajectory skeleton, and then for each point of the skeleton, relative pose constraints of the trajectory points are established with the adjacent 10 trajectory points, and the current raw_global_pose constraint is added for joint optimization; after obtaining the optimized skeleton, the trajectory points between the skeleton trajectory points are calculated by interpolation to obtain the ECEF coordinates (referred to as second_global_pose) of all trajectory points after the second calculation.

[0176] Optionally, the first_global_pose obtained by the three methods mentioned in the above step S201 all need to go through step S202 to calculate three different second_global_pose.

[0177] Step S203: Evaluation function, find the result with the minimum error.

[0178] In this embodiment, an evaluation function is established to evaluate the second_global_pose results obtained in three different ways. For each trajectory point of the trajectory, the transformation relationship T_i between the current trajectory point's local_pose and second_global_pose is calculated. Then, the local_pose and second_global_pose within 100 meters before and after the current trajectory point are taken and denoted as set M. For each trajectory point in set M, T_i * local_pose – global_pose is calculated, and the maximum error of the trajectory point, local_max_error, is recorded. All trajectory points are traversed, and the maximum value of all trajectory points' local_max_error is taken as the maximum error of the trajectory, max_error. Then, the maximum errors max_error of the second_global_poses obtained in the three ways are compared, and the second_global_pose with the minimum max_error is selected as the final result.

[0179] Step S204, obtain a suitable second_global_pose.

[0180] In this embodiment, after obtaining the above-mentioned suitable second_global_pose, the suitable second_global_pose can be output.

[0181] Figure 3 It is a flowchart of a memory driving route post-processing method shown according to an embodiment of the present invention. As Figure 3 shown, the method may include the following steps:

[0182] Step S301, obtain a suitable second_global_pose.

[0183] In this embodiment, the above-mentioned output suitable second_global_pose can be obtained.

[0184] Step S302, determine whether max_error is less than the threshold.

[0185] In this embodiment, it can be determined whether max_error is less than the threshold. If so, step S303 can be executed; otherwise, step S306 can be executed. If the deviation of max_error corresponding to the suitable second_global_pose is greater than the threshold, the problematic trajectory segments need to be removed, and the non-problematic trajectory segments are retained.

[0186] Step S303, extract consecutive trajectory segments with local_max_error less than the threshold.

[0187] In this embodiment, the local_max_error of each trajectory point is recorded, and continuous trajectory segments with local_max_error less than the threshold are retained.

[0188] Step S304: Traverse the trajectory segment, extend the trajectory segment forward and backward, and perform trajectory optimization and max_error calculation.

[0189] In this embodiment, for the retained trajectory segment, it is extended forward by 100 meters each time, and then the above steps are performed on the extended trajectory segment.

[0190] Step S305: Determine whether the max_error is less than the threshold.

[0191] In this embodiment, it can be determined whether the max_error corresponding to the trajectory points in the extended trajectory segment is less than the threshold. If so, step S304 can be returned for execution; otherwise, step S305 can be executed. If the max_error of the extended trajectory is greater than the threshold, this extension segment is removed.

[0192] Step S306: Output the trajectory segment.

[0193] In this embodiment, if the max_error of the extended trajectory is greater than the threshold, this extension segment is removed. Then the trajectory is extended backward in the same way as the forward extension, 100 meters each time, and the max_error of the extended trajectory is calculated. It continues until the extended trajectory segment reaches the last point of the trajectory or the max_error is greater than the threshold. And the trajectory segment with the last max_error less than the threshold is retained.

[0194] In the embodiment of the present invention, the fusion of local_pose and raw_global_pose can also be directly performed at the vehicle end without post-processing.

[0195] In the embodiment of the present invention, the trajectory is optimized by using the method of multiple optimizations. The first optimization provides an initial value for the second optimization, and the second optimization uses a more rigorous structure to obtain more accurate results; the trajectory is optimized by extracting the skeleton: when constructing the optimization result, not all trajectory points are added to the optimization structure, but the skeleton of the trajectory is extracted; the skeleton is optimized first, and then the trajectory points between the skeletons are interpolated based on the optimized result; multiple different methods are used for trajectory optimization, and an evaluation function is used to select the appropriate result; for the trajectory with max_error greater than the threshold, it is segmented. The trajectory growth optimization is performed on the front and back segments respectively, and the longest trajectory segment is retained. Thus, the technical effect of improving the accuracy of the vehicle's driving trajectory construction is achieved, and the technical problem of low accuracy in abnormal road object recognition is solved.

[0196] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in this application are all information and data that have been authorized by the user or fully authorized by all parties. Moreover, the collection, use, and processing of relevant data need to comply with the relevant laws, regulations, and standards of the relevant countries and regions, and corresponding operation entrances are provided for users to choose to authorize or reject.

[0197] According to another aspect of the embodiments of the present invention, corresponding to the embodiments of the method for constructing the driving trajectory of the vehicle described above, this specification also provides a device for constructing the driving trajectory of a vehicle. Figure 4 It is a structural block diagram of a device for constructing the driving trajectory of a vehicle shown according to the embodiments of the present invention, as Figure 4 shown. The device 400 for constructing the driving trajectory of the vehicle may include: a determination unit 402, a first adjustment unit 404, a second adjustment unit 406, and a construction unit 408.

[0198] The determination unit 402 is configured to determine the local pose information corresponding to the trajectory points in the trajectory point set of the vehicle, as well as the initial global pose information corresponding to the trajectory points.

[0199] The first adjustment unit 404 is configured to use the local pose information to adjust the initial global pose information to obtain the first target global pose information corresponding to the trajectory points.

[0200] The second adjustment unit 406 is configured to, in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, adjust the initial trajectory skeleton to obtain a target trajectory skeleton, and adjust the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information.

[0201] The construction unit 408 is configured to determine target trajectory points from the trajectory point set based on the second target global pose information, and construct the target driving trajectory of the vehicle according to the target trajectory points.

[0202] In this embodiment, the determination unit 402 determines the local pose information corresponding to the trajectory points in the trajectory point set of the vehicle, as well as the initial global pose information corresponding to the trajectory points. The first adjustment unit 404 uses the local pose information to adjust the initial global pose information to obtain the first target global pose information corresponding to the trajectory points. The second adjustment unit 406 responds to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, adjusts the initial trajectory skeleton to obtain the target trajectory skeleton, and adjusts the first target global pose information of the trajectory points in the target trajectory skeleton to the second target global pose information. The construction unit 408 determines the target trajectory points from the trajectory point set based on the second target global pose information, and constructs the target driving trajectory of the vehicle according to the target trajectory points. Thus, the technical effect of improving the accuracy of the vehicle driving trajectory construction is achieved, and the technical problem of low accuracy of the vehicle driving trajectory construction is solved.

[0203] According to another aspect of the embodiments of the present invention, an embodiment of the present application further provides an autonomous vehicle, including: a memory storing an executable program; a processor for running the program, wherein when the program runs, it executes the methods in the various embodiments of the present invention.

[0204] Figure 5 is a structural block diagram of an autonomous vehicle shown according to an embodiment of the present invention, as Figure 5 shown, the components of the autonomous vehicle 500 include but are not limited to a memory 510 and a processor 520. The processor 520 and the memory 510 are connected through a bus 530, and the database 560 is used to store data.

[0205] The autonomous vehicle 500 may also include an access device 540 that enables the autonomous vehicle 500 to communicate via one or more networks 550. Examples of such networks include the Public Switched Telephone Network (PSTN), Local Area Network (LAN), Wide Area Network (WAN), Personal Area Network (PAN), or a combination of communication networks such as the Internet. The access device 540 may include one or more of any type of wired or wireless network interface (e.g., a network interface controller (NIC)), such as an IEEE 802.11 Wireless Local Area Network (WLAN) wireless interface, a Worldwide Interoperability for Microwave Access (Wi-MAX) interface, an Ethernet interface, a Universal Serial Bus (USB) interface, a cellular network interface, a Bluetooth interface, a Near Field Communication (NFC) interface, and so on.

[0206] In one embodiment of the present disclosure, the above components of the autonomous vehicle 500, as well as Figure 5 other components not shown, may also be connected to each other, for example, via a bus. It should be understood that Figure 5 the block diagram of the autonomous vehicle shown is for illustrative purposes only and is not a limitation on the scope of the present disclosure. Those skilled in the art may add or replace other components as needed.

[0207] Embodiments of the present application also provide a computer-readable storage medium that includes a stored executable program. When the executable program runs, it controls the device where the computer-readable storage medium is located to execute the methods in various embodiments of the present invention.

[0208] Embodiments of the present application also provide a computer program product that includes a computer program which, when executed by a processor, implements the methods in various embodiments of the present invention.

[0209] Embodiments of the present application also provide a computer program product, including a non-volatile computer-readable storage medium for storing a computer program, which when executed by a processor implements the methods in various embodiments of the present invention.

[0210] Embodiments of the present application also provide a computer program, which when executed by a processor implements the methods in various embodiments of the present invention described above.

[0211] In the above embodiments of the present invention, the descriptions of the various embodiments have their own focuses. For parts not detailed in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

[0212] In several embodiments provided by the present application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are merely illustrative. For example, the division of the units can be a logical function division. In actual implementation, there can be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections between each other can be through some interfaces. The indirect couplings or communication connections of the units or modules can be in an electrical or other form.

[0213] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place or distributed to multiple units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0214] In addition, the functional units in various embodiments of the present invention can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.

[0215] When the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, read-only memories (ROMs), random access memories (RAMs), mobile hard disks, magnetic disks, or optical discs that can store program codes.

[0216] The foregoing are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A method for constructing a vehicle driving trajectory, characterized in that: include: Determine local pose information corresponding to a trajectory point in a trajectory point set of the vehicle, and initial global pose information corresponding to the trajectory point, wherein the trajectory point set is used to represent the initial driving trajectory of the vehicle on the road, the local pose information is used to represent the position and pose of the vehicle at the trajectory point in a local coordinate system, and the initial global pose information is used to represent the position and pose of the vehicle at the trajectory point in a global coordinate system; Using the local pose information, adjusting the initial global pose information to obtain first target global pose information corresponding to the trajectory point, wherein the error degree of the first target global pose information is less than the error degree of the initial global pose information; In response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than a deformation degree threshold, the initial trajectory skeleton is adjusted to obtain a target trajectory skeleton, and the first target global pose information of the trajectory points in the target trajectory skeleton is adjusted to second target global pose information, wherein the initial trajectory skeleton is constructed by a plurality of the trajectory points, the deformation degree of the target trajectory skeleton is less than or equal to the deformation degree threshold, and the error degree of the second target global pose information is less than the error degree of the first target global pose information; Based on the second target global posture information, a target trajectory point is determined from the trajectory point set, and a target driving trajectory of the vehicle is constructed according to the target trajectory point.

2. The method according to claim 1, characterized in that: Using the local pose information, adjusting the initial global pose information to obtain the first target global pose information corresponding to the trajectory point, including: According to the adjustment strategy in the adjustment strategy set, the initial global pose information is adjusted using the local pose information to obtain the first target global pose information, wherein different adjustment strategies in the adjustment strategy set correspond to different error types of global pose information, and the error degree of the first target global pose information under the error type is less than the error degree of the initial global pose information under the error type.

3. The method according to claim 2, characterized in that The adjustment strategy is a first adjustment strategy, and the first adjustment strategy is used to represent a rule for adjusting the initial global pose information. According to an adjustment strategy in an adjustment strategy set, the initial global pose information is adjusted using the local pose information to obtain the first target global pose information, including: According to the first adjustment strategy, based on the local pose information of the adjacent track points in the track point set, relative pose information between the adjacent track points is determined; Based on the relative pose information, determining a constraint condition for constraining the initial global pose information; According to the constraint condition, the initial global pose information is adjusted to obtain the first target global pose information.

4. The method according to claim 2, characterized in that: The adjustment strategy is a second adjustment strategy, and the second adjustment strategy is used to represent a rule for adjusting the initial global pose information. According to the adjustment strategy in the adjustment strategy set, the initial global pose information is adjusted using the local pose information to obtain the first target global pose information, including: According to the second adjustment strategy, based on the local pose information of the adjacent track points in the track point set, relative pose information between the adjacent track points is determined; Based on the relative pose information and the initial global pose information, determining a constraint condition for constraining the initial global pose information; Based on the local pose information and the initial global pose information, the rigid body transformation information is determined, and according to the constraint condition, the product of the rigid body transformation information and the initial global pose information is adjusted to obtain the first target global pose information, wherein the rigid body transformation information is used to represent the conversion relationship between the trajectory point in the local coordinate system and the global coordinate system.

5. The method according to claim 2, characterized in that: The adjustment strategy is a third adjustment strategy, and the third adjustment strategy is used to represent a rule for adjusting the initial global pose information. According to the adjustment strategy, the initial global pose information is adjusted to the first target global pose information corresponding to the adjustment strategy by using the local pose information, including: According to the third adjustment strategy, the local posture information is smoothed to obtain smoothed posture information, wherein the accuracy of the smoothed posture information is greater than the accuracy of the local posture information; Fusing the smoothed pose information and the initial global pose information to obtain a fusion result; According to the fusion result, the initial global pose information is adjusted to obtain the first target global pose information.

6. The method according to claim 2, characterized in that The method further comprises: On the road, at each target length interval, an initial trajectory point set is determined from the trajectory point set, and the initial trajectory skeleton is constructed according to the initial trajectory points in the initial trajectory point set; In response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, adjusting the initial trajectory skeleton to obtain a target trajectory skeleton, including: In response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than the deformation degree threshold, determining relative pose information between adjacent initial trajectory points in the initial trajectory point set based on the local pose information of adjacent initial trajectory points; Based on the relative pose information and the initial global pose information of the initial trajectory point, the initial trajectory skeleton is adjusted to obtain the target trajectory skeleton.

7. The method according to claim 6, characterized in that Adjusting the first target global pose information of the trajectory point in the target trajectory skeleton to second target global pose information includes: interpolating the first target global pose information of the trajectory points between the initial trajectory points under different adjustment strategies to obtain third target global pose information corresponding to the adjustment strategy; The second target global pose information of the trajectory point is determined from the third target global pose information of different adjustment strategies corresponding to the same trajectory point.

8. The method according to claim 7, characterized in that Determining the second target global pose information of the trajectory point from the third target global pose information of different adjustment strategies corresponding to the same trajectory point includes: An acquisition step, acquiring a trajectory point subset including a current trajectory point from the trajectory point set, and determining a relationship matrix between the local pose information of the current trajectory point and corresponding third target global pose information, wherein the third target global pose information corresponds to the adjustment strategy; In a process of traversing the trajectory points in the trajectory point subset starting from the current trajectory point, determining error information of the traversed trajectory points based on the relationship matrix; In response to traversing the track points in the track point subset, comparing the error information of different adjustment strategies corresponding to the same track point to obtain a comparison result; The third target global pose information corresponding to the adjustment strategy with the smallest error information in the comparison result is determined as the second target global pose information.

9. The method according to claim 8, characterized in that Determining a target trajectory point from the trajectory point set based on the second target global pose information includes: Determine the trajectory point in the trajectory point subset whose error information corresponding to the second target global pose information is less than or equal to the error information threshold as the target trajectory point; The method further includes: in the trajectory point subset, deleting the trajectory points whose error information corresponding to the second target global pose information is greater than the error information threshold.

10. The method according to claim 9, characterized in that According to the target trajectory points, constructing the target driving trajectory of the vehicle includes: According to the target trajectory point in the trajectory point subset, construct a driving sub-trajectory corresponding to the trajectory point subset; On the road, according to the target direction of the trajectory point subset, the next trajectory point subset of the trajectory point subset is determined, and the execution is returned to start from the acquisition step to determine the target trajectory point in the next trajectory point subset to construct the driving sub-trajectory corresponding to the next trajectory point subset, until the next trajectory point subset does not exist in the trajectory point set, and the target driving trajectory is determined based on the driving sub-trajectory.

11. A vehicle driving trajectory construction device, characterized in that: include: A determination unit, used to determine local pose information corresponding to a trajectory point in a trajectory point set of a vehicle, and initial global pose information corresponding to the trajectory point, wherein the trajectory point set is used to represent the initial driving trajectory of the vehicle on the road, the local pose information is used to represent the position and pose of the vehicle at the trajectory point in a local coordinate system, and the initial global pose information is used to represent the position and pose of the vehicle at the trajectory point in a global coordinate system; A first adjustment unit is used to adjust the initial global pose information by using the local pose information to obtain first target global pose information corresponding to the trajectory point, wherein the error degree of the first target global pose information is less than the error degree of the initial global pose information; A second adjustment unit is configured to adjust the initial trajectory skeleton in response to the deformation degree of the initial trajectory skeleton corresponding to the trajectory point set being greater than a deformation degree threshold, to obtain a target trajectory skeleton, and to adjust the first target global pose information of the trajectory points in the target trajectory skeleton to second target global pose information, wherein the initial trajectory skeleton is constructed by a plurality of the trajectory points, the deformation degree of the target trajectory skeleton is less than or equal to the deformation degree threshold, and the error degree of the second target global pose information is less than the error degree of the first target global pose information; A construction unit is used to determine a target trajectory point from the trajectory point set based on the second target global posture information, and to construct a target driving trajectory of the vehicle according to the target trajectory point.

12. An autonomous driving vehicle, characterized in that: include: A memory storing an executable program; A processor, configured to run the program, wherein the program executes the method according to any one of claims 1 to 10 when running.

13. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored executable program, wherein when the executable program is executed, the device where the storage medium is located is controlled to execute the method according to any one of claims 1 to 10.

14. A computer program product, characterized in that It comprises a computer program which, when executed by a processor, implements the method according to any one of claims 1 to 10.

Images