Pose optimization method, readable storage medium and navigation equipment

Abstract

The invention discloses a pose optimization method, a readable storage medium and navigation equipment, and belongs to the technical field of intelligent driving, and the method comprises the steps: converting a current frame of vehicle surrounding ground point cloud into a world coordinate system, dividing a global ground into a local region, forming a plurality of space voxels, and extracting common-view voxels of historical frames; calculating the importance scores of the time scale and the space scale, and carrying out normalized weighted fusion to obtain a space-time importance score; constructing a local plane parameterized model, establishing a region-level invariant ground residual error, and performing weighted fusion on a point-surface residual error of a long-term ground by using a time-space importance score to form a comprehensive ground residual error; and combining point-line / plane residual errors to construct a cost function, and optimizing the pose. The computer program of the method is stored in the readable storage medium. The navigation equipment applies the method. According to the invention, under the condition of no global information, the motion constraint can be enhanced, the error accumulation of the pose is further inhibited, and the positioning precision is further improved.

Description

Technical Field

[0001] This application belongs to the field of intelligent driving technology, and in particular relates to a pose optimization method, a readable storage medium, and a navigation device. Background Technology

[0002] In recent years, autonomous driving technology has developed rapidly and is increasingly being used in ground vehicles operating in urban environments. As a key component of autonomous driving, accurate and reliable autonomous positioning is an important prerequisite for ensuring the safe operation of ground vehicles in complex urban scenarios.

[0003] Existing technologies construct motion constraints by performing data association between two LiDAR scans and utilize filtering-based or optimization-based methods to construct constraint estimates of relative pose. However, when LiDAR odometry lacks external global information, it is essentially a dead reckoning system. Sensor errors and inaccurate data association can lead to pose accumulation errors, and erroneous data association will further exacerbate the error accumulation.

[0004] To address the aforementioned issues, the first approach involves fusing measurements from other sensors to construct complementary heterogeneous constraints. Wheel encoders, inertial measurement units (IMUs), and cameras can all provide complementary measurement information, and heterogeneous constraints can be built using various measurement models to assist LiDAR in achieving autonomous localization. However, this approach not only increases system complexity and cost but also its performance is affected by the measurement accuracy of the sensors and the spatiotemporal calibration between different sensors. The second approach introduces prior maps to establish constraints between the pose and the prior map. Utilizing global information introduced by high-precision prior maps or topological maps can mitigate the cumulative error of laser odometry. However, this approach requires pre-constructing a scene map, which is both expensive and lacks flexibility. The third approach uses loop closure detection to establish constraints between the current pose and historical poses, suppressing cumulative errors through loop closure detection and relocalization mechanisms. However, loop closures are not always triggered in large-scale urban scenes, limiting the adaptability of this approach. In summary, existing methods are prone to pose error accumulation in the absence of global information, lacking cost-effective means that can ensure pose accuracy. Summary of the Invention

[0005] This application aims to at least partially solve the technical problem of how to reduce pose error accumulation when there is no global information. To this end, this application provides a pose optimization method, a readable storage medium, and a navigation device. In the absence of global information, this method comprehensively considers the temporal and spatial scale information of the regional ground, uses ground observation data to establish long-term regional ground constraints, enhances motion constraints, and further suppresses pose error accumulation and improves positioning accuracy.

[0006] In a first aspect, embodiments of this application provide a pose optimization method, which includes: Transform the ground point set around the vehicle in the current frame to the world coordinate system, divide the global ground into several local regions, form several spatial volume elements, and obtain the same spatial volume element that is observed jointly between the current LiDAR frame and historical frames, denoted as the common-view voxel. Calculate the temporal scale importance score of co-visual voxels Importance score of spatial scale ; Will and Normalized weighted fusion is performed to obtain the spatiotemporal scale importance scores of co-visible voxels. ; A parametric model of the local ground is constructed using the nearest point representation method; By associating plane parameters of different coordinate systems through homogeneous transformation matrices, different ground surfaces can be unified to a global reference frame; Based on the continuity of adjacent local ground in the parametric model, a region-level invariant ground residual is constructed. The regional invariant ground residuals and long-term ground point-to-surface residuals are obtained by... Weighted summaries are applied to construct a comprehensive ground residual. A cost function is constructed, including integrated ground residuals, point-to-line residuals and point-to-area residuals commonly used in LiDAR odometry, and the cost function is optimized for pose adjustment.

[0007] In some implementations... for: in, Indicates the first n Time-scale importance score of each co-visible voxel; Indicates by the first n Among the co-visual voxels, the front The ground point set consisting of the oldest LiDAR points; The minimum number of points within different co-visual voxels; For LiDAR points The frame index interval, i.e., the current frame. The interval between the first observed historical frame and the first observed frame.

[0008] In some implementations... for: in, Indicates the first n Spatial scale importance score of each co-visible voxel; yes The smallest eigenvalue of the covariance matrix of the spatial distribution of points on the inland surface. It is the [number]th ... n A common visual element.

[0009] In some implementations... for: in, Represents co-visual voxels The spatiotemporal scale importance score; and These represent the maximum and minimum temporal scale importance scores for all co-visual voxels in a single LiDAR scan, respectively. and These represent the maximum and minimum spatial scale importance scores for all co-visual voxels, respectively.

[0010] In some implementations, the nearest point representation method constructs a parametric model of the local ground as follows: in, represents the plane parameters in the nearest point representation; n is the normalized normal vector of the plane; d This represents the distance between the origin of the reference frame and the plane.

[0011] In some implementations, the region-level invariant ground residual is: in, For regional invariant ground residuals; Indicates the first i Pseudo-observation values ​​of regional ground at a historical moment in the world coordinate system; Indicates the current time (i.e., the time of the current moment). j (Time) Estimated regional ground level in the world coordinate system; Indicates the current time (i.e., the time of the current moment). j The estimated distance between the origin of the reference frame and the plane at any given time, in the world coordinate system. Indicates the current time (i.e., the time of the current moment). j The estimated value of the normalized normal vector of the plane in the world coordinate system at time (time).

[0012] In some implementations, the integrated ground residual is: in, To integrate ground residuals; ω n As weight; Indicates the first nThe total number of current LiDAR points within a common voxel; Indicates by Ground points and LiDAR points within The first i A long-term ground residual; It is the [number]th ... n A common visual element.

[0013] In some implementations, the cost function is: in, and These are the point-to-line residuals and point-to-area residuals commonly used in LiDAR odometry. To integrate ground residuals.

[0014] Secondly, embodiments of this application provide a readable storage medium storing a computer program, which, when executed, performs the pose optimization method as described above.

[0015] Thirdly, embodiments of this application provide a navigation device, including a memory, a processor, and a terminal program stored in the memory and executable in the processor, wherein the terminal program includes steps corresponding to the pose optimization method described above.

[0016] As can be seen from the above technical solution, the beneficial effects of this application are as follows: 1. The method of this application differs from directly constructing hard constraints using non-holonomic constraints (NHC). Sensor errors and inaccurate data associations can lead to pose accumulation errors. However, this application does not require any additional sensors and does not make any idealized assumptions or depend on the scene in the absence of external global information. By dividing local ground regions in the world coordinate system, common-view voxels between the current LiDAR frame and historical frames are obtained. The temporal and spatial scales of LiDAR points within the common-view voxels are considered and weighted and fused into a spatiotemporal scale importance score. This comprehensively considers the distribution characteristics of spatiotemporal scale information of the regional ground. Different grounds are unified to the global reference system through a homogeneous transformation matrix. Based on the planar continuity of adjacent local grounds, the constraint relationship between the current ground and historical grounds is determined, and a regional invariant ground residual is constructed. This constraint relationship is a long-term constraint rather than a short-term constraint. The ground constraints are then adaptively weighted into a comprehensive ground residual to achieve adaptive optimization. Finally, long-term point-area residuals, point-line residuals, and comprehensive ground residuals are established from historical ground points and current frame ground points within the common-view voxels, and adaptive weights are assigned. This hierarchical enhancement of motion constraints using local ground features significantly suppresses accumulated errors, thereby improving the autonomous positioning performance of ground vehicles in urban scenarios. This application comprehensively considers the temporal and spatial scale information of the regional ground, and uses ground observation data to establish long-term regional planar constraints, thereby enhancing motion constraints, further suppressing the accumulation of pose errors, and thus improving positioning accuracy.

[0017] 2. The readable storage medium of this application stores the above pose optimization method in the form of a computer program, providing a storage medium that can be applied to various electronic devices. This allows the pose optimization method to be applied in different usage scenarios, expanding the application scope of the pose optimization method and facilitating its application.

[0018] 3. The navigation device of this application stores the steps corresponding to the above pose optimization method in its memory. When the processor is working, the navigation device can apply the pose optimization method to quickly output the vehicle position and attitude, help autonomously locate the vehicle, provide hardware support for positioning of various intelligent agents or transportation equipment, and is suitable for navigation and positioning in multiple fields. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced one by one below. Obviously, the accompanying drawings described below are some embodiments of this application. For those skilled in the art, other embodiments and drawings can be obtained based on these drawings without creative effort. The flowcharts shown in the accompanying drawings are merely illustrative and do not necessarily include all content and operations / steps, nor do they necessarily have to be performed in the described order. For example, some operations / steps can be decomposed, while some operations / steps can be combined or partially combined. Therefore, the actual execution order may change according to the actual situation.

[0020] Figure 1 A schematic diagram illustrating an embodiment of the pose optimization method of the present invention is shown; Figure 2 This diagram illustrates the distribution of ground points in different frames within a shared voxel according to an embodiment of the present invention. Figure 3 A schematic diagram illustrating the nearest point representation of the present invention is shown; Figure 4 A schematic diagram comparing driving trajectories according to an embodiment of the present invention is shown. Detailed Implementation

[0021] The technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. The detailed description of the embodiments of this application provided in the drawings is not intended to limit the scope of the claimed application. The described embodiments are only a part of the embodiments of this application, not all of them. Based on the embodiments in this application, they can be arranged and designed in various different configurations. All other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0022] This application is described below with reference to the accompanying drawings and specific embodiments: Please refer to Figure 1 According to a first aspect of this application, a pose optimization method is provided, which includes: S1. Set the ground points around the vehicle in the current frame. Transforming to the world coordinate system, the ground area around the vehicle is divided into several local regions, forming multiple spatial volume elements, i.e., spatial voxels. The same spatial voxel observed jointly between the current LiDAR frame and historical frames is obtained and denoted as the common-view voxel; the LiDAR / carrier coordinate system is represented as { L}, the world coordinate system is represented as { w}, which is determined as the initial LiDAR coordinate system and remains unchanged. The LiDAR transformation task is to determine { L}and{w The relative poses between}. When obtaining co-viewed voxels, the nearest neighbor search method is used. The nearest neighbor search uses a hash table data structure to manage spatial voxels, uses a pseudo-Hilbert space-filling curve to build a spatial voxel index, and queries the nearest spatial voxels based on a given search range and search center.

[0023] S2. Calculate the temporal scale importance score of co-visible voxels. Importance score of spatial scale The temporal continuity of pose estimation is optimized by using temporal scale importance scores, and the spatial consistency of pose estimation is optimized by using spatial scale importance scores.

[0024] S3, will and Normalized weighted fusion is performed to obtain the spatiotemporal scale importance scores of co-visible voxels. Reasonable weighting of co-visible voxels is crucial; that is, it is necessary to quantitatively assess and rank the importance of different co-visible voxels so that different weights can be assigned to voxels of different importance levels when constructing residuals. First, we consider the basic idea of ​​temporal scale assessment of co-visible voxels, and then further combine this with the spatial distribution characteristics of ground points to establish a new co-visible voxel weighting mechanism. Temporal scale importance scores can assess the reliability of pose in a time series, while spatial scale importance scores can assess the consistency of pose in spatial structure. By weighting and fusing these two factors, both temporal robustness and spatial accuracy are achieved, forming the core of high-precision pose estimation.

[0025] S4. A parameterized model of the local ground plane is constructed using the nearest point representation method. By establishing long-term plane-to-plane constraints, the constraint effect on the carrier motion and the solvability of optimization are significantly enhanced, thereby suppressing cumulative errors and improving the accuracy of pose estimation.

[0026] S5. By associating plane parameters of different coordinate systems through homogeneous transformation matrices, different ground planes can be unified into a global reference system. This can maintain geometric consistency, ensure the uniformity of global parameters, and improve the reliability of pose estimation by performing weighted fusion under the same global reference system.

[0027] S6. Based on the continuity of adjacent local ground planes in the parametric model, a regional invariant ground residual is constructed. Through geometric constraints and data fusion, the accuracy, consistency and practicality of terrain representation are significantly improved. The regional invariant ground residual can smoothly transition the ground planes of different regions.

[0028] S7. LiDAR points within a common-view voxel with a time interval exceeding 10 frames are designated as long-term ground points, and the point-to-surface residual established using these long-term ground points is designated as the long-term ground point-to-surface residual. The region-level invariant ground residual and the long-term ground point-to-surface residual are then... Weighted summaries are used to construct a comprehensive ground residual. The weights for adaptive optimization are allocated on each common-view voxel. Subsequently, the comprehensive ground residual, together with other residuals, constitutes the cost function for performing the optimization process, achieving geometric consistency and error suppression, and improving positioning accuracy and robustness.

[0029] S8. Construct a cost function, including integrated ground residuals, point-to-line residuals and point-to-area residuals commonly used in LiDAR odometry, and optimize the cost function for pose adjustment. This allows for the comprehensive determination of the cost function. The principle is the coordination of global reference system coordinate consistency and local ground planar details, achieving optimization through multi-scale error coordination. Using the cost function value for position adjustment is existing technology. Based on the principle that residuals can be used as a benchmark for pose optimization, the pose can be optimized by comprehensively determining the cost function.

[0030] In some implementations, the pose optimization method further includes preprocessing, front-end data association, ground constraint processing between the front-end and back-end, and back-end optimization. Steps S1-S8 above pertain to the ground constraint processing and back-end optimization. Before this, ground points need to be acquired using LiDAR to form LiDAR points. Then, a preprocessing method is used to preprocess the point set. Preprocessing is an existing technology, and any ground segmentation algorithm can be embedded into the data preprocessing module. After preprocessing, feature extraction and feature association are performed on the processed LiDAR points. Edge and planar features extracted are used to perform feature association and establish residuals. The cost function formed by the residuals is optimized to obtain a coarse pose. On the other hand, points that conform to the set planar features are extracted and used as a ground point set for steps S1-S8. This allows association of local ground planar regions within the shared voxel. Then, the output pose from steps S1-S8 is transformed and fused with the coarse pose to obtain the fused pose, thus achieving pose optimization.

[0031] In some implementations, the importance score of the time scale in step S2 above is set as follows: , representing the common voxel set of the current LiDAR scan, where yes The first in n A common visual element, yes The number of voxels observed by the CCP, then Time Scale Importance Score for: in, Indicates the first n Time-scale importance score of each co-visible voxel; Indicates by the first n Among the co-visual voxels, the front The ground point set consisting of the oldest LiDAR points; The minimum number of points within different co-visual voxels; For LiDAR points The frame index interval, i.e., the current frame and... The interval between the first observed historical frames.

[0032] Please refer to Figure 2 Due to the influence of accumulated errors, the spatial distribution of locations across different frames exhibits significant differences. Multiple historical ground points from the same voxel are collected and projected onto the world coordinate system. It is important to note that when projecting the ground point set onto the world coordinate system, due to accumulated errors, ground points that are spatially close but temporally distant will show significant differences in spatial distribution, especially in the vertical direction. This will greatly increase the disorder of ground points within the voxel and introduce accumulated errors into the scan matching process. It is generally believed that a well-managed and reliable set of ground points will be more ordered in the vertical direction, i.e., its spatial distribution covariance in the vertical direction will be smaller. Based on this, the distribution characteristics of ground points within the co-view voxel are incorporated into the importance score to better manage and utilize ground points.

[0033] In some embodiments, in step S2 above, for co-visual voxels... Its spatial scale importance score for: in, Indicates the first n The spatial scale importance score of each co-visible voxel; yes The smallest eigenvalue of the covariance matrix of inland points. It is the [number]th ... n There are 10 co-visible voxels. The larger the eigenvalue, the greater the variance in the vertical direction, which means that the variance distributed across the co-visible voxels is greater. The ground points within the interior are more dispersed and disordered in the vertical direction, or have a larger cumulative error, therefore They will be assigned a smaller spatial scale importance score.

[0034] In some implementations, the time scale importance score in step S3 above Importance score of spatial scale Each contains evaluation information at different levels of co-visual voxels, and weighting coefficients are introduced. γ After normalizing and weighting the two factors mentioned above, the spatiotemporal scale importance score is obtained. for: in, Represents co-visual voxels The spatiotemporal scale importance score; and These represent the maximum and minimum temporal scale importance scores for all co-visual voxels in a single LiDAR scan, respectively. and These represent the maximum and minimum spatial scale importance scores for all co-visual voxels, respectively; γ This represents the weighting coefficient.

[0035] The aforementioned region-level invariant ground residuals are long-term constraints, not short-term constraints. In a given LiDAR scan, both short-term and long-term co-view voxels will exist simultaneously. For short-term co-view voxels, their temporal importance scores are relatively small. If the variance in the vertical direction is small, the spatial importance score will become the primary indicator of the importance of each short-term co-view voxel. Conversely, for long-term co-view voxels, their temporal importance scores are relatively large, and the temporal importance score will become the primary indicator of importance. It is important to note that because the historical frame cumulative error of long-term co-view is smaller, its constraint effect on motion is more effective than that of short-term co-view. When measuring the importance between the two types of voxels, the temporal scale importance score is the primary consideration. γ The allocation is relatively large, for example, in this article γ =0.8 (0.5-0.8).

[0036] Please refer to Figure 3 Regarding the closest point representation method, it involves constructing a vector perpendicular to the plane, where the magnitude represents the distance from the origin of the reference frame to the plane. A region-level invariant ground constraint is designed to further enhance motion constraints using ground points. The basic idea is that the local ground model does not change significantly within the adjacent spatial region. The global ground is divided into several local ground regions using iVox. This allows the proposed method to avoid the strong assumption of a globally flat ground required by many other ground constraint enhancement methods. CP (closest point representation) is introduced, described using Hessian form. Achieve concise ground parameterization. Specifically, n is the normalized normal vector of the plane. d This represents the distance between the origin of the reference frame and the plane.

[0037] In some implementations, the nearest-point representation method constructs a parametric model of the local ground (CP-parametric ground) as follows: Θ= d n Where Θ is the plane parameter in the nearest point representation; n is the normalized normal vector of the plane; d This represents the distance between the origin of the reference frame and the plane.

[0038] To establish planar constraints between different coordinate systems, different ground parameters Θ must be determined. a and Θ b In different coordinate systems a and b The transformation under the following assumptions. Let be the homogeneous transformation matrix between coordinate systems a and b. and The correspondence between them is as follows: Within adjacent local ground spatial regions, the ground surface does not change significantly. If global coordinates are used as a reference, the projections of planar parameter vectors at different times within the same region will tend to coincide. Based on this idea, a region-level invariant ground constraint is introduced. Since global points with longer time scales are less affected by accumulated errors, the ground points with the longest temporal distance captured within the common voxel will be used as pseudo-observations for ground fitting within the region.

[0039] In some implementations, for the first n For each common voxel, the region-level invariant ground residual is: in, For regional invariant ground residuals; Indicates the first i Pseudo-observation values ​​of regional ground points at a historical moment in the world coordinate system; Indicates the current time (i.e., the time of the current moment). j (Time) Estimated values ​​of regional ground points in the world coordinate system; Indicates the current time (i.e., the time of the current moment). j The estimated distance between the origin of the reference frame and the plane at any given time, in the world coordinate system. Indicates the current time (i.e., the time of the current moment). j The estimated value of the normalized normal vector of the plane in the world coordinate system at time (time).

[0040] The above It was obtained by directly fitting historical ground points in the global coordinate system, and The planar parameter vector is at the th jBy projecting from the LiDAR coordinate system to the global coordinate system at any time, and substituting the parameterized model of the local ground plane into the region-level invariant ground residuals, we can obtain: in, and The image shows the global pose to be optimized. It can be seen that the translational components of the pose to be optimized are mainly constrained by the magnitude of the regional planar parameters, while the rotational components are mainly constrained by the normal vectors of the planar parameters.

[0041] The Jacobian matrix of the ground residuals for each region can be calculated as follows: Where × represents the cross product operator; The Jacobian matrix of the region-level invariant ground residuals with respect to rotation; It is the Jacobian matrix of the region-level invariant ground residuals with respect to translation; The projection of the ground plane normal vector onto the LiDAR frame; This is the distance from the origin of the LiDAR system to the ground plane; The rotation matrix to be estimated; Let be the translation vector to be estimated. The region-level invariant ground residuals will be incorporated into the cost function and optimized using the Levenberg-Marquardt (LM) algorithm.

[0042] In some implementations, to achieve adaptive optimization, the constructed ground residuals will be scored based on their spatiotemporal scale importance. Weighted, the first n The combined ground residual for each co-visible voxel is: in, To integrate ground residuals; ω n As weights, and relative importance scores based on spatiotemporal scales. related; Indicates the first n The total number of current LiDAR points within a common voxel; Indicates by Ground points and LiDAR points within The first i A long-term ground residual; It is the [number]th ... n A common visual element.

[0043] In some implementations, the above weights are calculated. for: in,{ k 1, k 2, k 3, k 4} is an empirical parameter ( k Take 1.0-1.5, k 2. Take 1.5-2.0. k 3. Take 3.0-5.0, k 4. Taking values ​​of 0.2-0.5), the empirical parameters mentioned above are set to {1.5, 2.0, 5.0, 0.2} in this application. It can be seen that the higher the importance score, the more... ω n The larger the value, the more likely it is to trust co-view voxels with long-term co-view potential and smaller variance in the vertical distribution of ground points. This is achieved through weighting calculations. Introduced into the construction of the residuals, the adaptive optimization weights are allocated on each co-visible voxel, thus achieving adaptive pose optimization on the co-visible voxel.

[0044] Given a set of ground points , No. n Ground parameters of a common voxel PCA can be used for calculation. Let Indicates the first i Given the coordinates of a LiDAR point in the current LiDAR frame, the long-term ground point-to-surface residual is constructed as follows: in, Indicates by Fitting ground points within the area to ground and LiDAR points The first i Long-term ground point-to-surface residuals; This indicates the current pose that needs to be optimized; The first in the LiDAR series i n long-term ground points; n For the first n Ground plane normal vector of a common voxel; d n From the origin of the reference frame to the th n The distance between the ground planes of the common voxels.

[0045] In some implementations, the cost function is: in, and These are the point-to-line residuals and point-to-area residuals commonly used in LiDAR odometry. The current pose to be optimized; To integrate ground residuals; To integrate ground residuals.

[0046] Embodiments of this application test vehicle driving paths in urban outdoor environments, such as... Figure 4 As shown, this paper compares the proposed method with three other state-of-the-art methods. The black line represents the ground truth trajectory, the dark red line represents the position trajectory of the proposed method, and the other colors represent the position trajectories of the three state-of-the-art methods being compared. It can be seen that the proposed method has the smallest cumulative error compared to the other methods, meaning it has the highest positioning accuracy. The effectiveness and superiority of the proposed method are verified through comparisons with various state-of-the-art methods in public datasets and real-world scenarios. Experimental results show that in challenging large-scale urban scenarios, the pure LiDAR method proposed in this paper exhibits superior positioning performance compared to the current state-of-the-art LiDAR-inertial combination method. Compared to the tested baseline, this application can further improve positioning accuracy; the comparison of absolute trajectory errors is shown in the table below. Table 1 Comparison of absolute trajectory error between the test baseline and this application A second aspect of this application provides a readable storage medium storing a computer program. When executed, the computer program performs the pose optimization method as described in any of the above embodiments. If the integrated module / unit corresponding to this method is implemented as a software functional unit and sold or used as an independent product, it can be stored in a readable storage medium. Based on this understanding, all or part of the processes of the methods described in the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in the readable storage medium. When executed by a processor, the computer program can implement the steps of each method in the above embodiments. When the computer program is executed by the processor, the specific implementation of each step and the resulting technical effects are the same as in the aforementioned method embodiments. For the sake of brevity, any parts not mentioned in this embodiment can be referred to the corresponding content in the aforementioned method embodiments.

[0047] A third aspect of this application provides a navigation device, including a memory, a processor, and a terminal program stored in the memory and executable in the processor. The terminal program includes steps corresponding to the pose optimization method described above. When the processor executes the terminal program, it implements the steps in the method embodiments described above, or, when the processor executes the terminal program, it implements the functions of each module / unit in the embodiments described above. It should be understood that the apparatuses of various embodiments of this application can be implemented based on a memory and a processor. Each memory is used to store a terminal program for executing the methods described above, and the processor executes the terminal program, causing the navigation device to implement the methods of the various embodiments described above.

[0048] In some embodiments, the navigation device described above can be integrated into desktop computers, laptops, PDAs, tablets, or other mobile terminals, as well as cloud servers and other computer devices, and is not limited to running any particular operating system. If the navigation device uses a mobile terminal, it can be installed in a vehicle as hardware for the vehicle's intelligent driving system. Those skilled in the art will understand that the examples of navigation devices are not intended to limit its functionality; a navigation device may include more or fewer components than described above, or a combination of certain components, or different components. For example, a navigation device may also include input devices, output devices, network access devices, buses, displays, etc.

[0049] Regarding the specific implementation methods of this application, it should be noted that: In the description of this application, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, apparatus, or readable storage medium that comprises a list of elements includes not only those elements but also other elements not expressly listed that conform to the concept of this application, or elements inherent to such a process, method, apparatus, or readable storage medium. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of additional elements in the process, method, apparatus, or readable storage medium that includes said element.

[0050] In the description of this application, the use of terms such as "some embodiments," "optional embodiments," "example," "specific example," "optional example," or "optional embodiment," etc., indicates that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application, but does not imply that these embodiments illustrate and describe all possible forms of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, those skilled in the art can combine and integrate the different embodiments or examples described in this specification.

[0051] The present invention has been described in detail above with reference to specific embodiments and exemplary examples. The above description is exemplary and not exhaustive, and is not limited to the disclosed embodiments; the above description should not be construed as a limitation of the present invention. Technical solutions between various embodiments can be combined with each other, but must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application. Although embodiments of the present application have been shown and described, various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present application. Those skilled in the art will understand that various other specific changes and combinations of embodiments based on the technical teachings disclosed in this application, without departing from the essence of the present application, are still within the scope of protection defined by the claims of the present invention and their equivalent technical solutions.

Claims

1. A pose optimization method, characterized in that, include: Transform the ground point set around the vehicle in the current frame to the world coordinate system, divide the global ground into several local regions, form several spatial volume elements, and obtain the same spatial volume element that is observed jointly between the current LiDAR frame and historical frames, denoted as the common-view voxel. Calculate the temporal scale importance score of co-visual voxels Importance score of spatial scale ; The and stated Normalized weighted fusion is performed to obtain the spatiotemporal scale importance score of the co-viewed voxels. ; A parametric model of the local ground is constructed using the nearest point representation method; By associating the plane parameters of different coordinate systems through a homogeneous transformation matrix, different ground surfaces are unified into a global reference frame; Based on the continuity of adjacent local ground in the parametric model, a region-level invariant ground residual is constructed. The regional invariant ground residuals and long-term ground point-to-surface residuals are processed through the... Weighted summaries are applied to construct a comprehensive ground residual. A cost function is constructed, including the integrated ground residual, the point-to-line residual and the point-to-area residual commonly used in LiDAR odometry, and the cost function is optimized for pose adjustment.

2. The pose optimization method according to claim 1, characterized in that, The for: in, Indicates the first n Time-scale importance score of each co-visible voxel; Indicates by the first n Among the co-visual voxels, the front The ground point set consisting of the oldest LiDAR points; The minimum number of points within different co-visual voxels; For LiDAR points The frame index interval, i.e., the current frame and... The interval between the first observed historical frames.

3. The pose optimization method according to claim 2, characterized in that, The for: in, Indicates the first n Spatial scale importance score of each co-visible voxel; yes The smallest eigenvalue of the covariance matrix of the spatial distribution of points on the inland surface. It is the [number]th ... n A common visual element.

4. The pose optimization method according to claim 3, characterized in that, The for: in, Represents co-visual voxels The spatiotemporal scale importance score; and These represent the maximum and minimum temporal scale importance scores for all co-visual voxels in a single LiDAR scan, respectively. and These represent the maximum and minimum spatial scale importance scores for all co-visual voxels, respectively.

5. The pose optimization method according to claim 1, characterized in that, The nearest point representation method constructs a parameterized model of the local ground as follows: in, represents the plane parameters in the nearest point representation; n is the normalized normal vector of the plane; d This represents the distance between the origin of the reference frame and the plane.

6. The pose optimization method according to claim 5, characterized in that, The regional-level invariant ground residual is: in, For regional invariant ground residuals; Indicates the first i Pseudo-observations of regional ground levels at historical moments in the world coordinate system; Indicates the current time (i.e., the time of the current moment). j (Time) Estimated regional ground level in the world coordinate system; Indicates the current time (i.e., the time of the current moment). j The estimated distance between the origin of the reference frame and the plane at any given time, in the world coordinate system. Indicates the current time (i.e., the time of the current moment). j The estimated value of the normalized normal vector of the plane in the world coordinate system at time (time).

7. The pose optimization method according to claim 6, characterized in that, The integrated ground residual is: in, To integrate ground residuals; ω n As weight; Indicates the first n The total number of current LiDAR points within a common voxel; Indicates by Ground points and LiDAR points within The first i A long-term ground residual; It is the [number]th ... n A common visual element.

8. The pose optimization method according to claim 7, characterized in that, The cost function is: in, and These are the point-to-line residuals and point-to-area residuals commonly used in LiDAR odometry. The current pose to be optimized; To integrate ground residuals.

9. A readable storage medium storing a computer program, characterized in that, When the computer program is executed, it performs the pose optimization method as described in any one of claims 1-8.

10. A navigation device, comprising a memory, a processor, and a terminal program stored in the memory and executable in the processor, characterized in that, The terminal program includes the steps corresponding to the pose optimization method as described in any one of claims 1-8.

Images