Distance-based EKF positioning method, system, device, and medium

Abstract

A distance-based EKF positioning method, system, device and medium, comprising: collecting inertial measurement unit information, three-dimensional laser scanning points and three-dimensional point cloud map information; finding corresponding points of three-dimensional laser scanning points in three-dimensional point cloud map to realize data association; after data association, solving the distance between corresponding points to obtain geometric distance measurement and robust distance measurement; predicting the pose state at the current time according to the inertial measurement unit information; updating the pose state at the current time by using the geometric distance measurement and the robust distance measurement, and then outputting the result. The application discloses a distance-based EKF multi-sensor fusion positioning scheme, which takes the distance between three-dimensional laser scanning points and corresponding points of a three-dimensional point cloud map as the observation of a filter, realizes corresponding point finding through a k-NN fast nearest neighbor search algorithm, does not need to perform feature extraction, and does not need to perform an iterative calculation step, so that the method has great advantages in calculation efficiency compared with existing methods.

Description

Technical Field

[0001] This invention belongs to the field of precise positioning technology for unmanned systems, and specifically relates to a distance-based EKF positioning method, system, device, and medium. Background Technology

[0002] Autonomous unmanned systems rely on various onboard sensors to integrate environmental perception, enabling automatic localization, scene understanding, route planning, and control execution, thereby achieving autonomous operation. Autonomous localization technology is the foundation for these functional algorithms; it obtains the pose and attitude of the autonomous unmanned system and, by determining the boundaries of the operating space and the distances to other objects, ensures operational safety.

[0003] Currently, in the field of autonomous unmanned systems, most systems employ a fusion positioning system combining Global Navigation Satellite System (GNSS) and Inertial Measurement Unit (IMU), achieving centimeter-level positioning accuracy by deploying ground-based base stations. However, in scenarios where GNSS signals are unavailable, positioning is typically achieved by incorporating environmental information through sensing sensors.

[0004] Therefore, autonomous unmanned systems must perform autonomous positioning based on their onboard sensors, determining their current position and attitude by sensing their own state and incorporating environmental information. Among these methods, fusion positioning using 3D lasers is currently one of the preferred solutions for autonomous unmanned systems, primarily due to the stable physical characteristics of laser sensors, their independence from lighting conditions, and the decreasing cost of mass production. However, the large amount of laser point cloud data per frame and the reliance on iterative optimization algorithms for pose estimation result in low computational efficiency and insufficient robustness. Furthermore, the robustness and real-time performance of these algorithms often require repeated trade-offs. Summary of the Invention

[0005] The purpose of this invention is to provide a distance-based EKF positioning method, system, device, and medium to solve the problems of low computational efficiency and insufficient robustness caused by the large amount of single-frame laser point cloud data and the reliance on iterative optimization algorithms for pose estimation.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] In a first aspect, the present invention provides a distance-based EKF positioning method, comprising:

[0008] Collect inertial measurement unit information, 3D laser scanning points, and 3D point cloud map information;

[0009] Find the corresponding points of 3D laser scanning points in a 3D point cloud map to achieve data association;

[0010] After data association, the distance between corresponding points is calculated to obtain geometric distance measurement and robust distance measurement; based on the inertial measurement unit information, the pose state at the current moment is predicted;

[0011] The pose state at the current moment is updated using geometric distance measurement and robust distance measurement, and then the results are output.

[0012] Optionally, the input information of the inertial navigation unit is represented by pose transformation, which is the output information of a single IMU or wheel speed, or the result of the fusion of IMU and wheel speed, or the output of the pose estimation sensor; the 3D point cloud map is a point map, a point normal map, or a point covariance map; the 3D laser scanning points are directly acquired through multi-line laser.

[0013] Optionally, the data association specifically involves: using the pose state at the current moment, transforming the currently perceived 3D laser scanning point into the map coordinate system, and then finding the corresponding point through the k-NN fast nearest neighbor search algorithm.

[0014] Optionally, the distance between corresponding points can be calculated to obtain geometric distance measurements and robust distance measurements:

[0015] The distance between two points in three-dimensional space is divided into four types: point-to-point distance pt2pt, point-to-surface distance pt2pl, point-to-distribution distance pt2d, and distribution-to-distribution distance d2d.

[0016] s is a point in the laser observation, m is a point on the map, n is the normal vector of the map point, and Σ s and Σ m These are the covariances of points s and m, respectively; assume there is a transformation relationship between s and m.

[0017]

[0018] This is the pose prediction value; the distance is calculated separately in the vehicle coordinate system and the world coordinate system:

[0019]

[0020] in:

[0021]

[0022] Three map representations are used for the four types of distances: point map, point normal map, and point covariance map. The point map is composed of points in three-dimensional space and is used for the pt2pt distance method. The point normal map is composed of points in three-dimensional space and their normal vectors and is used for the pt2pl distance method. The point covariance map is composed of points in three-dimensional space and their covariances and is used for the pt2d and d2d distance methods.

[0023] By introducing a robust loss function commonly used in linear regression methods, the original geometric distance is redefined as the robust distance.

[0024] z=ρ(d) (3)

[0025] Where ρ(·) is the robust loss function, and the Jacobian of the robust distance measurement is:

[0026]

[0027] The influence function and weight function of the robust loss function are as follows:

[0028]

[0029] The Jacobian of robust distance measurement is obtained by the chain rule as follows:

[0030]

[0031] Optionally, the map representation is obtained offline first. In the map building stage, multiple frames of point clouds and their poses are obtained first, and then stitched together based on the poses. The point normal map is obtained by calculating the normal vector of each point after obtaining the point map. The point covariance map is obtained by calculating the covariance of each point based on the point map.

[0032] The methods for calculating covariance in point covariance maps are k-NN Search, Range Search, and Voxel Grid. The k-NN Search method finds the K nearest points and then calculates the covariance. The Range Search method finds all points whose distance from the query point does not exceed r and then calculates the covariance. The Voxel Grid method rasterizes the map and then calculates the mean and covariance of all points in each grid cell.

[0033] Optionally, the pose state at the current moment can be predicted based on the inertial measurement unit information:

[0034] Define the state as in, Represents the rotation matrix. Represents the translation vector; control is defined as uk =(ΔR) k ,Δp k Then, given the result of the previous frame; Predict the current state.

[0035]

[0036] in, These represent rotational and translational noise, respectively, which follows a Gaussian distribution.

[0037]

[0038] Optionally, the state can be updated using both geometric distance measurement and robust distance measurement:

[0039] z = d, or z = ρ(d) (9)

[0040] Jacobian solutions for observation equations include Jacobian for geometric distance measurements and Jacobian for robust distance measurements:

[0041]

[0042] in,

[0043]

[0044]

[0045] The formula for calculating residuals is:

[0046]

[0047] In the formula, s is a point in the laser observation, and m is a point on the map. It is the state prediction value, that is, the output of the state evolution module, h d (·) indicates distance calculation;

[0048] Detailed update calculation steps:

[0049] Kalman gain.

[0050] Status update.x←x+Kr

[0051] Covariance update. P←P-KHP (14)

[0052] Where Q is the measurement covariance, which in distance-based EKF refers to the variance of distance and is a scalar; P is the state covariance, a 6×6 matrix; K is the Kalman gain, which is 6×1; H is the Jacobian matrix of the measurement equation, which is 1×6; and r is the residual.

[0053] In a second aspect, the present invention provides a distance-based EKF positioning system, comprising:

[0054] The data acquisition module is used to collect information from the inertial measurement unit, 3D laser scanning points, and 3D point cloud map information.

[0055] The data association module is used to find the corresponding points of 3D laser scanning points in the 3D point cloud map to achieve data association;

[0056] The distance calculation module is used to calculate the distance between corresponding points after data association, and obtain geometric distance measurement and robust distance measurement.

[0057] The state evolution module is used to predict the pose state at the current moment based on the inertial measurement unit information; the state update module is used to update the pose state at the current moment using geometric distance measurement and robust distance measurement, and then output the results.

[0058] In a third aspect, the present invention provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor, when executing the computer program, implements the steps of a distance-based EKF positioning method.

[0059] In a fourth aspect, the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the distance-based EKF positioning method.

[0060] Compared with the prior art, the present invention has the following technical effects:

[0061] In existing technologies, single-frame laser point cloud data is large and pose estimation relies on iterative optimization algorithms, which typically involve more than 30 iterations, resulting in low computational efficiency. This invention discloses a distance-based EKF multi-sensor fusion localization scheme. This method uses the distance between the 3D laser scanning point and the corresponding point on the 3D point cloud map as the filter observation, and uses the k-NN fast nearest neighbor search algorithm to find the corresponding point. It does not require feature extraction or iterative calculation steps, thus having a significant advantage in computational efficiency compared to existing methods.

[0062] Existing methods require specially designed robustness mechanisms when dealing with complex scenarios, such as feature point extraction methods and robust optimization iterative algorithms. Improving robustness incurs additional computational overhead. Since distance is a scalar, and both the input and output of the robust loss function are scalars, we redefine the form of distance and introduce the robust loss function into the observation equation. This reduces errors generated during the data association stage by calculating robust distance, and further enhances the robustness of the method by weighting the Kalman gain using the Jacobian matrix of the observation equation. Attached Figure Description

[0063] Figure 1 Flowchart for distance-based EKF localization of 3D lasers.

[0064] Figure 2 A diagram showing the transformation relationship between two coordinate systems.

[0065] Figure 3 Four diagrams illustrating distance calculation.

[0066] Figure 4 Schematic diagram of three map representation forms.

[0067] Figure 5 Schematic diagrams for three types of covariance calculations.

[0068] Figure 6 Robust loss function and its impact function. Detailed Implementation

[0069] The present invention will be further described below with reference to the accompanying drawings:

[0070] Please see Figures 1 to 6 A distance-based EKF localization method. (See attached image.) Figure 1 As shown, the system structure includes a sensor input module, a data association module, a distance calculation module, a state evolution module, a state update module, and an output pose module. The following sections provide a detailed description of each module.

[0071] Step 1, Sensor Input:

[0072] The input information mainly includes: inertial navigation unit (IMU) information, 3D laser scanning points, and a 3D point cloud map. The IMU input information is represented by pose transformation, which in practical applications can be a separate IMU. The wheel speed output information can be the result of fusing the IMU and wheel speed data, or the output of other pose estimation sensors. The 3D point cloud map has three options depending on the distance format: a point map, a point normal map, and a point covariance map. The 3D laser scanning points are directly acquired through multi-line laser scanning. To accelerate computation, downsampling can be used to keep the number of laser scanning points below 5000.

[0073] The second step is data association.

[0074] The main function of this module is to find the corresponding point on the map for the current 3D laser scanning point. Our approach is to use the pose prediction results output by the state evolution module to transform the currently perceived 3D laser scanning point into the map coordinate system, and then find the corresponding point using the k-NN fast nearest neighbor search algorithm.

[0075] The third step is distance calculation.

[0076] After data association, it is necessary to calculate the distance between corresponding points. There are four main types of distances between two points in three-dimensional space: point-to-point distance (pt2pt), point-to-surface distance (pt2pl), point-to-distribution distance (pt2d), and distribution-to-distribution distance (d2d), as shown in the appendix. Figure 3 As shown.

[0077] Definition: s is a point in the laser observation, m is a point on the map, n is the normal vector of the map point, Σ s and Σ m Let S and M be the covariances of points s and m, respectively. Assume there is a transformation relationship between s and m.

[0078] Here In essence, it is the pose prediction value output by the state evolution module. (See attached image.) Figure 2 The diagram illustrates the transformation relationship between points s and m. Based on the diagram, we can calculate the distance in both the vehicle coordinate system and the world coordinate system (map system). Assumptions:

[0079]

[0080] The four types and eight kinds of distances between two points and their Jacobian matrices are shown in the table below:

[0081]

[0082] Corresponding to the four types of distances mentioned above, we designed three map representation formats: point map, point normal map, and point covariance map. Please refer to the appendix for details. Figure 4 Among them, the point map is composed of points in three-dimensional space and is mainly used for pt2pt distance mode; the point normal map is composed of points in three-dimensional space and their normal vectors and is mainly used for pt2pl distance mode; and the point covariance map is composed of points in three-dimensional space and their covariance and is mainly used for pt2d and d2d distance mode.

[0083] All of the above maps can be obtained offline. In the map building stage, multiple frames of point clouds and their poses are first obtained, and then the maps can be stitched together based on the poses. The point normal map is obtained by calculating the normal vector of each point after obtaining the point map. The point covariance map is obtained by calculating the covariance of each point based on the point map.

[0084] It should be noted that this invention supports three methods for calculating covariance in point covariance maps: k-NN Search, Range Search, and Voxel Grid, as shown in the appendix. Figure 5 As shown in the diagram. The k-NNSearch method finds the K nearest points and then calculates the covariance. The Range Search method finds all points within a distance r of the query point and then calculates the covariance. The Voxel Grid method rasterizes the map and then calculates the mean and covariance of all points within each grid cell. This method is the same as the covariance estimation method in NDT and is characterized by its fast computation speed.

[0085] Distance can be defined in various ways. To overcome the influence of outliers, we introduce a robust loss function commonly used in linear regression methods. Therefore, we redefine the original geometric distance as robust distance.

[0086] z=ρ(d) (3)

[0087] Where ρ(·) is the robust loss function. At this point, the Jacobian of the robust distance measurement is:

[0088]

[0089] Furthermore, since the influence function and weight function of the robust loss function are respectively:

[0090]

[0091] Therefore, the Jacobian of robust distance measurement, obtained by the chain rule, is:

[0092]

[0093] Appendix Figure 6 The waveforms are those of common robust loss functions and influence functions.

[0094] The fourth step is state evolution.

[0095] Define the state as in, Represents the rotation matrix. This represents the translation vector. The control is defined as u. k =(ΔR) k,Δp k Given the result of the previous frame. Predict the current state.

[0096]

[0097] in, These represent rotational and translational noise, respectively, which follows a Gaussian distribution.

[0098]

[0099] Step 5: Status update.

[0100] The state is updated using both geometric distance measurement and robust distance measurement.

[0101] z = d, or z = ρ(d) (9)

[0102] Jacobian solutions for observation equations include Jacobian for geometric distance measurements and Jacobian for robust distance measurements:

[0103]

[0104] in,

[0105]

[0106]

[0107] The selected robust loss function, its influence function, and weight function are shown in the table below:

[0108]

[0109] The formula for calculating residuals is:

[0110]

[0111] In the formula, s is a point in the laser observation, and m is a point on the map. It is the state prediction value, that is, the output of the state evolution module, h d (·) indicates distance calculation.

[0112] Detailed update calculation steps:

[0113] Kalman gain.

[0114] Status update.x←x+Kr

[0115] Covariance update. P←P-KHP (14)

[0116] Where Q is the measurement covariance, which is the variance of distance in distance-based EKF and is a scalar. P is the state covariance, which is a 6×6 matrix. K is the Kalman gain, which is 6×1. H is the Jacobian matrix of the measurement equation, which is 1×6, as shown in formula (10). r is the residual, as shown in formula (13).

[0117] Step 6: Output the pose module.

[0118] The output of the state update module is represented by pose transformation, and this result is simultaneously fed back to the state evolution module for the prediction calculation of the state at the next moment.

[0119] In another embodiment of the present invention, a distance-based EKF positioning system is provided, which can be used to implement the above-described distance-based EKF positioning method. Specifically, the system includes:

[0120] The data acquisition module is used to collect information from the inertial measurement unit, 3D laser scanning points, and 3D point cloud map information.

[0121] The data association module is used to find the corresponding points of 3D laser scanning points in the 3D point cloud map to achieve data association;

[0122] The distance calculation module is used to calculate the distance between corresponding points after data association, and obtain geometric distance measurement and robust distance measurement.

[0123] The state evolution module is used to predict the pose state at the current moment based on the inertial measurement unit information; the state update module is used to update the pose state at the current moment using geometric distance measurement and robust distance measurement, and then output the results.

[0124] The module division in this embodiment of the invention is illustrative and represents only one logical functional division. In actual implementation, other division methods may be used. Furthermore, the functional modules in the various embodiments of the invention can be integrated into a single processor, exist as separate physical entities, or be integrated into a single module. The integrated modules described above can be implemented in hardware or as software functional modules.

[0125] In another embodiment of the present invention, a computer device is provided, comprising a processor and a memory. The memory stores a computer program, which includes program instructions. The processor executes the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing and control core of the terminal, suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions from the computer storage medium to achieve a corresponding method flow or corresponding function. The processor described in this embodiment of the present invention can be used for the operation of a distance-based EKF positioning method.

[0126] In another embodiment of the present invention, a storage medium is provided, specifically a computer-readable storage medium (Memory), which is a memory device in a computer device used to store programs and data. It is understood that the computer-readable storage medium here can include both the built-in storage medium in the computer device and extended storage media supported by the computer device. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for loading and execution by a processor. These instructions can be one or more computer programs (including program code). It should be noted that the computer-readable storage medium here can be high-speed RAM or non-volatile memory, such as at least one disk storage device. The processor can load and execute one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the distance-based EKF positioning method in the above embodiments.

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

[0128] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0129] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0130] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0131] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A distance-based EKF positioning method, characterized in that, include: Collect inertial measurement unit information, 3D laser scanning points, and 3D point cloud map information; Find the corresponding points of 3D laser scanning points in a 3D point cloud map to achieve data association; After data association, the distance between corresponding points is calculated to obtain geometric distance measurement and robust distance measurement; Based on the information from the inertial measurement unit, predict the pose state at the current moment; The pose state at the current moment is updated using geometric distance measurement and robust distance measurement, and then the results are output. Solving for the distance between corresponding points yields geometric distance measurements and robust distance measurements: The distance between two points in three-dimensional space is divided into four types: point-to-point distance pt2pt, point-to-surface distance pt2pl, point-to-distribution distance pt2d, and distribution-to-distribution distance d2d. It is a point in laser observation. It is a point on the map. It is the normal vector of the map point. and They are points and points The covariance; assuming and There is a transformation relationship between them. （1） This is the pose prediction value; the distance is calculated separately in the vehicle coordinate system and the world coordinate system: in: （2） Three map representations are used for the four types of distances: point map, point normal map, and point covariance map. The point map is composed of points in three-dimensional space and is used for the pt2pt distance method. The point normal map is composed of points in three-dimensional space and their normal vectors and is used for the pt2pl distance method. The point covariance map is composed of points in three-dimensional space and their covariances and is used for the pt2d and d2d distance methods. By introducing a robust loss function commonly used in linear regression methods, the original geometric distance is redefined as the robust distance. （3） in, Let be the robust loss function. In this case, the Jacobian of the robust distance measurement is: （4） The influence function and weight function of the robust loss function are as follows: （5） The Jacobian of robust distance measurement is obtained by the chain rule as follows: （6）； The state is updated using both geometric distance measurement and robust distance measurement: ,or (9) Jacobian solutions for observation equations include Jacobian for geometric distance measurements and Jacobian for robust distance measurements: （10） in, （11） （12） The formula for calculating residuals is: （13） In the formula, It is a point in laser observation. It is a point on the map. It is the state prediction value, that is, the output of the state evolution module, h d (·) indicates distance calculation; Detailed update calculation steps: （14） in, It is the covariance of the measurement, that is, the variance of the distance in distance-based EKF, which is a scalar; It is the state covariance, which is one. The matrix, It is the Kalman gain, with a value of , The Jacobian matrix of the measurement equation has a size of . , It is a residual.

2. The distance-based EKF positioning method according to claim 1, characterized in that, The input information of the inertial navigation unit is represented by pose transformation, which is the output information of a single wheel speed or IMU, or the result of the fusion of IMU and wheel speed, or the output of pose estimation sensor; the 3D point cloud map is a point map, a point normal map, or a point covariance map; the 3D laser scanning points are directly acquired through multi-line laser.

3. The distance-based EKF positioning method according to claim 1, characterized in that, The data association process involves using the current pose state to transform the currently perceived 3D laser scanning point into a map coordinate system, and then using the k-NN fast nearest neighbor search algorithm to find the corresponding point.

4. The distance-based EKF positioning method according to claim 1, characterized in that, The map representation is obtained offline first. In the map construction stage, multiple frames of point clouds and their poses are obtained first, and then stitched together based on the poses. The point normal map is obtained by calculating the normal vector of each point after obtaining the point map. The point covariance map is obtained by calculating the covariance of each point based on the point map. The methods for calculating covariance in point covariance maps are k-NN Search, Range Search, and Voxel Grid. The k-NN Search method finds the K nearest points and then calculates the covariance. The Range Search method finds all points whose distance from the query point does not exceed r and then calculates the covariance. The VoxelGrid method rasterizes the map and then calculates the mean and covariance of all points in each grid cell.

5. The distance-based EKF positioning method according to claim 1, characterized in that, Based on the information from the inertial measurement unit, predict the pose state at the current moment: Define the state as ,in, Represents the rotation matrix. Represents the translation vector; control is defined as... Then, given the result of the previous frame... Predict the state at the current moment. （7） in, , , representing rotational and translational noise, respectively, which follows a Gaussian distribution. （8）。 6. A distance-based EKF positioning system, characterized in that, include: The data acquisition module is used to collect information from the inertial measurement unit, 3D laser scanning points, and 3D point cloud map information. The data association module is used to find the corresponding points of 3D laser scanning points in the 3D point cloud map to achieve data association; The distance calculation module is used to calculate the distance between corresponding points after data association, and obtain geometric distance measurement and robust distance measurement. The state evolution module is used to predict the pose state at the current moment based on the information from the inertial measurement unit. The state update module is used to update the pose state at the current moment using geometric distance measurement and robust distance measurement, and then output the results; Solving for the distance between corresponding points yields geometric distance measurements and robust distance measurements: The distance between two points in three-dimensional space is divided into four types: point-to-point distance pt2pt, point-to-surface distance pt2pl, point-to-distribution distance pt2d, and distribution-to-distribution distance d2d. It is a point in laser observation. It is a point on the map. It is the normal vector of the map point. and They are points and points The covariance; assuming and There is a transformation relationship between them. （1） This is the pose prediction value; the distance is calculated separately in the vehicle coordinate system and the world coordinate system: in: （2） Three map representations are used for the four types of distances: point map, point normal map, and point covariance map. The point map is composed of points in three-dimensional space and is used for the pt2pt distance method. The point normal map is composed of points in three-dimensional space and their normal vectors and is used for the pt2pl distance method. The point covariance map is composed of points in three-dimensional space and their covariances and is used for the pt2d and d2d distance methods. By introducing a robust loss function commonly used in linear regression methods, the original geometric distance is redefined as the robust distance. （3） in, Let be the robust loss function. In this case, the Jacobian of the robust distance measurement is: （4） The influence function and weight function of the robust loss function are as follows: （5） The Jacobian of robust distance measurement is obtained by the chain rule as follows: （6）； The state is updated using both geometric distance measurement and robust distance measurement: ,or (9) Jacobian solutions for observation equations include Jacobian for geometric distance measurements and Jacobian for robust distance measurements: （10） in, （11） （12） The formula for calculating residuals is: （13） In the formula, It is a point in laser observation. It is a point on the map. It is the state prediction value, that is, the output of the state evolution module, h d (·) indicates distance calculation; Detailed update calculation steps: （14） in, It is the covariance of the measurement, that is, the variance of the distance in distance-based EKF, which is a scalar; It is the state covariance, which is one. The matrix, It is the Kalman gain, with a value of , The Jacobian matrix of the measurement equation has a size of . , It is a residual.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the distance-based EKF positioning method as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the distance-based EKF positioning method as described in any one of claims 1 to 5.

Images