A weak inertial navigation dependence difference mapping and track mapping method

Abstract

The application discloses a weak inertial navigation dependence difference mapping and track mapping method, comprising the following steps: S1, data acquisition: using a data acquisition system to collect and acquire point cloud data around a track; S2, defining a system coordinate system L and a world coordinate system W; S3, track point cloud mileage estimation: point cloud re-projection; extracting a local feature descriptor; calculating a pose transformation matrix; calculating a pose transformation matrix; converging the pose transformation matrix and the pose transformation matrix to obtain a final pose transformation matrix; iterative convergence solving; S4, mapping track mapping: S41, world coordinate system W unification; S42, track prior mapping. The method has the advantages of high sparse tolerance, low drift, low complexity and high detail restoration degree, greatly reduces the dependence on auxiliary hardware, and has higher track scene reconstruction accuracy, economic efficiency and application prospect.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of high-precision railway measurement, and in particular relates to a weak inertial navigation dependence difference mapping and track mapping method. BACKGROUND

[0002] In the railway operation and maintenance stage, obtaining high-precision scene data and corresponding feature geometric parameters is an important data input for railway state detection and monitoring, maintenance and repair evaluation, and operation safety maintenance. At present, China is in a period of rapid development of intelligent equipment replacing manual work, especially the relatively mature track inspection instrument and mobile three-dimensional scanning system, which have made great achievements in the field of high-precision railway measurement. However, in actual application, in areas such as tunnels, underground, and blind areas, satellite positioning and navigation signals may be disturbed, lost, or have no signal, and high-precision inertial navigation is usually needed as a data source and correction in the direction of the distance. However, this method mainly has the following problems:

[0003] (1) The accumulation of inertial navigation errors makes the distance data obtained in the measurement process gradually deviate from the true value, and the track-related application scenarios are not closed-loop conditions, so the traditional model cannot be used to eliminate or weaken the errors, resulting in large measurement data errors and failing to meet the high-precision requirements in engineering applications;

[0004] (2) In the mobile acquisition process, the small incremental motion changes over time are mainly integrated, which may cause data drift, resulting in holes or distortion of the point cloud results, directly affecting the authenticity of track measurement and endangering the safety of train operation;

[0005] (3) In the pose estimation process, the inertial navigation output is mainly used to compensate the estimated data, and for laser radar and other high two-degree-of-freedom acquisition devices, there are problems such as regional boundary limitation, missing dead zone, and field distortion, and the accuracy, precision, and confidence of the original data are low.

[0006] In view of the above problems and weaknesses, it is urgent to develop a weak inertial navigation dependence difference mapping and track mapping method. SUMMARY

[0007] In order to solve the problems existing in the prior art, the present application provides a weak inertial navigation dependence difference mapping and track mapping method with high sparse tolerance, low drift, and low complexity. The method of the present application realizes accurate estimation of speed, position, and attitude and fine registration and mapping of local point clouds, thereby reducing the dependence on positioning and inertial measurement technology and greatly reducing the hardware cost and application limitations.

[0008] The technical solutions and steps of the present application are as follows:

[0009] S1, Data Acquisition: Use a data acquisition system to collect and acquire point cloud data along the track perimeter. The point cloud data acquired during the k-th scan of the data acquisition system is denoted as point cloud P. k The point cloud data acquired in each scan is added to set P. Therefore, after N scans, the data acquisition system obtains set P = {P1, P2, ..., P...} k-1 ,P k ,P k+1 ,…P N};

[0010] Where k takes values ​​from k=1,2…N, and N is the total number of consecutive scans performed by the data acquisition system; point cloud P k Satisfies the coplanar geometric relationship;

[0011] S2, define the system coordinate system L and the world coordinate system W:

[0012] System coordinate system L: The origin of system coordinate system L is the geometric center of the lidar in the data acquisition system. The positive z-axis of system coordinate system L is the same as the initial attitude and travel direction of the data acquisition system. The y-axis of system coordinate system L is perpendicular to the z-axis and the positive y-axis is the direction away from the earth's center. The x-axis, y-axis and z-axis of system coordinate system L form a right-handed coordinate system.

[0013] As the data acquisition system moves, a corresponding system coordinate system needs to be constructed for each scan. The system coordinate system constructed for the k-th scan is denoted as L. k ; point cloud P k Any point A inside i In the system coordinate system L k The following is represented as i = 1, 2, ..., M, where M is the point cloud P k The total number of interior points;

[0014] World coordinate system W: The world coordinate system W completely coincides with the system coordinate system L1 constructed during the first scan of the data acquisition system, where the point cloud P... k Any point A inside i In the world coordinate system W k The following is represented as

[0015] S3, orbital point cloud odometry, specifically includes the following steps:

[0016] S31, Point Cloud Reprojection: Reprojecting the set P = {P1, P2, ..., P...} k-1 ,P k ,P k+1 ,…P N Each point cloud in the array is reprojected to its corresponding scan termination time to obtain a set.

[0017] wherein, the scanning start time of the point cloud P k is t k , the scanning end time of the point cloud P k is t k+1 , the point cloud P k is re-projected to the time stamp t k+1 to obtain the point cloud P and added to the set , thus, the start time to the end time of the k+1th scanning of the data acquisition system includes the point cloud P and P k+1 ;

[0018] S32, extracting a local feature descriptor, and obtaining a boundary point set φ k+1 and a smooth point set Ω k+1 in the point cloud P k+1 according to the registration mapping relationship, the boundary point set φ and the smooth point set Ω in the point cloud P

[0019] S321, selecting a feature point A k in the point cloud P j of the set P as an initial reference point, adding all point clouds containing the initial reference point A j in the set P to the set S1={P k ,…,P l}, selecting a feature point A l ′ in the point cloud P j of the set P as a reference point, adding all point clouds containing the reference point A j ′ in the set P to the set S2={P l ,…,P n}, repeating the above process until the division of all point clouds in the set P is completed, obtaining the sets S1, S2…S c ;

[0020] wherein, the values of k, l, n and c are all 1, 2…N, and l>k, n>l;

[0021] S322, calculating the local feature descriptor value η of each point in the point cloud P k in the set S1 except the initial reference point A j with A j as the initial reference point; taking the point with the largest local feature descriptor value in the point cloud P k as the candidate boundary point, and taking the point with the smallest local feature descriptor value as the candidate smooth point;

[0022] from the point cloud Pk and the registration mapping relationship between the point cloud P k+1 k+1 k+1 and the smooth point set Ω k+1 ; by the registration mapping relationship between the point cloud P and the point cloud P k+1 and the smooth point set

[0023] wherein, for each point in the point cloud P k except the initial reference point, the local feature descriptor value η of the initial reference point A j is calculated according to the following formula:

[0024]

[0025] wherein, A j , A i ∈ S, j≠i, and M is the total number of points in the point cloud P k

[0026] S323, if the number of the candidate boundary points or the number of the candidate smooth points is zero or M, then re-perform S321-S322, that is, re-select the initial reference point and re-calculate; otherwise, perform S33;

[0027] S33, calculate the pose transformation matrix T The specific steps are as follows:

[0028] S331, define the pose transformation matrix as T Under the current timestamp t k+2 , let the pose transformation matrix of the data acquisition system in the time period [t k+1 , t k+2 ] be T wherein, t x , t y , t z represent the translation transformation amounts of the x, y, z axes in the L coordinate system respectively, θ x , θ y , θ z represent the rotation transformation amounts of the x, y, z axes in the L coordinate system respectively;

[0029] S332, solve the pose transformation matrix T

[0030]

[0031] wherein, is the point cloud P​​​​​k+1 the boundary point set Φ k+1 or the smooth point set Ω k+1 the pose transformation matrix of any point q in P q denotes the timestamp of point q, is the point cloud P in the L coordinate system k denotes the coordinates of any point q in P, and the function σ(*, n, m) is the value of the nth to mth term of * denotes the matrix the vector composed of the nth to mth solution of the corresponding homogeneous linear equation system), and the Rodrigues formula of R is as follows:

[0032] R = e ωθ = I + ω sin θ + ω 2 (1 - cos θ)

[0033]

[0034]

[0035] wherein the function Ψ(*) is a skew-symmetric matrix taking the variable *, and I is the unit matrix;

[0036] S34, replacing the boundary point set Φ k+1 and the smooth point set Ω k+1 in S33 with the boundary point set and the smooth point set performing step S33 to calculate the corresponding pose transformation matrix replacing the pose transformation matrix with the pose transformation matrix to obtain the final pose transformation matrix;

[0037] S35, iteratively converging to solve: traversing the set S2…S c , performing S322-S34 to obtain the final pose transformation matrix corresponding to each set, and further obtaining the spatial motion estimation curve of the data acquisition system corresponding to the set P, wherein the horizontal coordinate of the spatial motion estimation curve is the acquisition timestamp, and the vertical coordinate is the final pose transformation matrix in S34;

[0038] S36, repeating S1-S35 until the point cloud P final corresponding to the spatial motion estimation curve of the data acquisition system is obtained;

[0039] S4, mapping track mapping:

[0040] S41, Unification of World Coordinate System W: In the spatial motion estimation curve obtained in step S36, each time stamp includes corresponding point cloud data containing pose information. All point cloud data are registered to the world coordinate system W using the real spatial mapping relationship.

[0041] S42, Orbit Prior Mapping:

[0042] Based on prior feature information in the track application scenario, a fine point cloud registration algorithm is used to fuse point clouds with adjacent timestamps in the point cloud under the world coordinate system W to realize track mapping.

[0043] In S1, the data acquisition system uses a self-moving RC-SLAM high-precision 3D scanning system, which can also be replaced by other devices with the same function, and the replacement devices must include at least a calibrated dual-axis lidar.

[0044] In S321, in point cloud P l The method for selecting a reference point is as follows: for the point cloud P l After performing Delaunay triangulation, the average Euclidean distance d is obtained in the point cloud P. l Select the initial reference point A. j Euclidean distance ≥ Feature point A j ′ is used as a reference point, wherein the initial reference point A is... j Both the reference point and the reference point are points outside a plane that are approximately parallel to the laser projection direction during the scanning of the data acquisition system.

[0045] In S42, the prior feature information includes spatial seamless continuity and local consistency of rail contour, and the logical information weight is considered to be higher than the measurement information weight within a certain time period.

[0046] The update frequency of S41 should be 10 times higher than that of S42, and the field of view overlap during the update process should be higher than 70%.

[0047] In S322, n = 1 and m = 3.

[0048] Compared with the prior art, the present invention has the following beneficial effects:

[0049] 1. The difference mapping and orbit mapping method of the present invention adopts a point cloud data classification and coordinate system registration method based on local feature descriptors, which greatly reduces the dependence on the hardware parameters of auxiliary equipment such as inertial navigation, IMU and gyroscope, making it more economical and applicable. At the same time, it achieves better theoretical mapping accuracy with higher computing power.

[0050] 2. The difference mapping and track mapping method of the present application adopts parallel calculation of world coordinate system I and track prior mapping mapping, ensuring the convergence feasibility of engineering practical problems, while the difference double-frequency mapping process ensures the accuracy and restoration of local details, and weakens the cumulative error in practical application.

[0051] 3. The difference mapping and track mapping method of the present application adopts local feature descriptor to more effectively extract local details and geometric features of point cloud, further improving the accuracy of point cloud fine registration result, and realizing high-precision reconstruction of track scene. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 is a flowchart of the present application;

[0053] Figure 2 is a point cloud data projection schematic diagram of the present application;

[0054] Figure 3 is a difference mapping and track mapping schematic diagram of the present application. DETAILED DESCRIPTION

[0055] The technical solutions of the present application will be further described in detail below in combination with the drawings.

[0056] A difference mapping and track mapping method with weak inertial navigation dependence, the flow is as shown in Figure 1 , comprising the following steps:

[0057] S1, data acquisition: using a data acquisition system to acquire and obtain point cloud data around the track, the point cloud data acquired by the data acquisition system in the kth scanning is denoted as point cloud P k , and the point cloud data acquired in each scanning is added to the set P, then the data acquisition system is scanned for N times to obtain the set P={P1, P2,…, P k-1 ,P k ,P k+1 ,…P N}.

[0058] Among them, the data acquisition system uses a self-moving RC-SLAM high-precision three-dimensional scanning system, which can also be replaced by other devices with the same function and the replacement device at least includes a calibrated double-axis laser radar; the value range of k is k=1, 2…N, N is the total number of continuous scanning of the data acquisition system; the point cloud P k satisfies the coplanar geometric relationship.

[0059] S2, define system coordinate system L and world coordinate system W:

[0060] System coordinate system L: The origin of system coordinate system L is the geometric center of the lidar in the data acquisition system. The positive z-axis of system coordinate system L is the same as the initial attitude and travel direction of the data acquisition system. The y-axis of system coordinate system L is perpendicular to the z-axis and the positive y-axis is the direction away from the Earth's center. The x-axis, y-axis and z-axis of system coordinate system L form a right-handed coordinate system.

[0061] As the data acquisition system moves, a corresponding system coordinate system needs to be constructed for each scan. The system coordinate system constructed for the k-th scan is denoted as L. k ; point cloud P k Any point A inside i In the system coordinate system L k The coordinates below are i = 1, 2, ..., M, where M is the point cloud P k The total number of interior points.

[0062] World coordinate system W: The world coordinate system W completely coincides with the system coordinate system L1 constructed during the first scan of the data acquisition system, where the point cloud P... k Any point A inside i In the world coordinate system W k The coordinates below are

[0063] S3, orbital point cloud odometry, specifically includes the following steps:

[0064] S31, Point Cloud Reprojection: Reprojecting the set P = {P1, P2, ..., P...} k-1 ,P k ,P k+1 ,…P N Each point cloud in the array is reprojected to its corresponding scan termination time to obtain a set.

[0065] Among them, such as Figure 2 As shown, point cloud P k The scan start time is t k Point cloud P k The scan termination time is t k+1 P point cloud k Reprojection to t k+1 Timestamps get point clouds and add to the collection Therefore, the start time to end time of the (k+1)th scan of the data acquisition system includes the point cloud. and P k+1 .

[0066] S32, Extracting local feature descriptors:

[0067] S321, in the point cloud P of set P. k Select a feature point A j As the initial reference point, set P will contain the initial reference point A. j Add all point clouds to set S1 = {P k ,…,P l In the point cloud P of set P; l Select a feature point A j As a reference point, set P contains reference point A. j All point clouds of ' are added to set S2 = {P l ,…,P n Repeat the above process until all point clouds in set P are partitioned, resulting in sets S1, S2...S... c .

[0068] Where k, l, n and c all take values ​​from 1, 2…N, and l>k, n>l;

[0069] Among them, in point cloud P l The method for selecting a reference point is as follows: for the point cloud P l After performing Delaunay triangulation, the average Euclidean distance d is obtained in the point cloud P. l Select the initial reference point A. j Euclidean distance ≥ Feature point A j ′ is used as a reference point, wherein the initial reference point A is... j Both the reference point and the reference point are points outside a plane that are approximately parallel to the laser projection direction during the scanning of the data acquisition system.

[0070] S322, Calculate the point cloud P in set S1. k Except for the initial reference point A j In addition, with A j For each point of the initial reference point, set a local feature descriptor value η; then convert the point cloud P... k The point with the largest local feature descriptor value (not unique) is selected as the candidate boundary point, and the point with the smallest local feature descriptor value (not unique) is selected as the candidate smoothing point.

[0071] From adjacent point clouds (point cloud P) k And point cloud P k+1 The registration mapping relationship between the two points is used to obtain the point cloud P. k+1 The boundary point set Φ in k+1 and smooth point set Ω k+1 From point clouds With point cloud P k+1 The registration mapping relationship between them is used to obtain the point cloud. Boundary point set in and smooth point set

[0072] wherein the point cloud P k is calculated j The formula for calculating the local feature descriptor value η of each point except the initial reference point is as follows:

[0073]

[0074] wherein A j ,A i ∈S,j≠i,M is the total number of points in the point cloud P k .

[0075] S323, if the number of candidate boundary points or the number of candidate smoothing points is zero or M, then re-perform S321-S322, i.e. re-select the initial reference point and re-calculate; otherwise, perform S33.

[0076] S33, calculate the pose transformation matrix corresponding to the points in the boundary point set φ k+1 and the smoothing point set Ω k+1 . The specific steps are as follows:

[0077] S331, define the pose transformation matrix as At the current timestamp t k+2 , let the pose transformation matrix of the data acquisition system in the time period [t k+1 , t k+2 ] be wherein t x , t y , t z represent the translation transformation amounts of the x, y, z axes in the L coordinate system, respectively, θ x , θ y , θ z represent the rotation transformation amounts of the x, y, z axes in the L coordinate system, respectively.

[0078] S332, by simultaneously solving formula (2) and formula (3), the pose transformation matrix is

[0079]

[0080] wherein, is the pose transformation matrix of any point q in the boundary point set Φ k+1 or the smoothing point set Ω k+1 of the point cloud P k+1 , t q represents the timestamp (acquisition time) of the point q, is the pose transformation matrix of the point q in the L coordinate system, and kcoordinates of any point q, and σ(*, n, m) is the value of the nth to mth element of * representation matrix the vector consisting of the nth to mth element of the solution of the corresponding homogeneous linear equations), n = 1, m = 3, and the Rodrigues formula of R is as follows:

[0081] R = e ωθ = I + ω sin θ + ω 2 (1 - cos θ)

[0082]

[0083] where Ψ(*) is a skew-symmetric matrix taking variable *, and I is the identity matrix.

[0084] S34, replacing the boundary point set Φ k+1 and the smooth point set Ω k+1 in S33 with the boundary point set and the smooth point set performing step S33 to calculate the corresponding pose transformation matrix the pose transformation matrix and the pose transformation matrix converge to obtain the final pose transformation matrix.

[0085] S35, iterative convergence solution: traversing the set S2…S c , performing S322-S34 to obtain the final pose transformation matrix corresponding to each set, and further obtaining the spatial motion estimation curve of the data acquisition system corresponding to the set P, wherein the horizontal coordinate of the spatial motion estimation curve is the acquisition timestamp, and the vertical coordinate is the final pose transformation matrix in S34.

[0086] S36, repeating S1-S35 until the point cloud P final corresponding to the spatial motion estimation curve of the data acquisition system is obtained.

[0087] S4, mapping track mapping:

[0088] S41, world coordinate system W unification: in the spatial motion estimation curve obtained in step S36, each timestamp includes corresponding point cloud data containing pose information, and all point cloud data is registered to the world coordinate system W using the real space mapping relationship.

[0089] S42, track prior mapping:

[0090] According to the prior feature information in the track application scene, the point cloud fine registration algorithm is used to fuse the point clouds of adjacent time stamps in the world coordinate system W, to realize track mapping, such asFigure 3 is shown.

[0091] wherein the prior feature information includes spatial seamless continuity and local consistency of rail profile, and the logical information is considered to have higher weight than the measured information (obtained by point cloud fine registration algorithm) in a certain time period.

[0092] wherein the update frequency of step S41 is generally 10 times higher than the update frequency of step S42, and the field of view coincidence degree of the update process is higher than 70%.

[0093] It should be noted that although the operations of the methods of the present disclosure are described in a particular order in the accompanying drawings, this does not require or imply that the operations must be performed in this particular order, or that all of the illustrated operations must be performed to achieve desirable results. Rather, the steps depicted in the flowcharts can change order of execution. Additionally or alternatively, certain steps can be omitted, combined into a single step, and / or broken into multiple steps.

Claims

1. A differential mapping and trajectory mapping method with weak inertial navigation dependence, characterized in that... Includes the following steps: S1, Data Acquisition: Use a data acquisition system to collect and acquire point cloud data along the track perimeter. The point cloud data acquired during the k-th scan of the data acquisition system is denoted as point cloud P. k The point cloud data acquired in each scan is added to a set, resulting in a set P = {P1, P2, ..., P...} after N scans. k-1 ,P k ,P k+1 ,…P N }, k = 1, 2…N; S2, define the system coordinate system L and the world coordinate system W: System coordinate system L: The origin of system coordinate system L is the geometric center of the lidar in the data acquisition system. The positive z-axis of system coordinate system L is the same as the initial attitude and travel direction of the data acquisition system. The y-axis is perpendicular to the z-axis and takes the direction away from the earth's center as the positive direction. The x-axis forms a right-handed coordinate system with the y-axis and z-axis. Let L be the system coordinate system constructed during the k-th scan. k ; point cloud P k Any point A inside i In the system coordinate system L k The coordinates below are represented as M represents point cloud P k The total number of interior points; World coordinate system W: The world coordinate system W completely coincides with the system coordinate system L1 constructed during the first scan of the data acquisition system, where the point cloud P... k Any point A inside i In the world coordinate system W k The coordinates below are represented as S3, Orbital point cloud odometry estimation, includes the following steps: S31, Point Cloud Reprojection: Reprojecting the set P = {P1, P2, ..., P...} k-1 ,P k ,P k+1 ,…P N Each point cloud in the array is reprojected to its corresponding scan termination time to obtain a set. in: Point cloud P k The scan start time is t k The scan termination time is t. k+1 P point cloud k Reprojection to t k+1 Timestamp, to obtain point cloud Then, the point cloud is obtained from the start time to the end time of the (k+1)th scan of the data acquisition system. and P k+1 ; S32, extract local feature descriptors and obtain point cloud P based on the registration mapping relationship. k+1 The boundary point set Φ in k+1 and smooth point set Ω k+1 and point clouds Boundary point set in and smooth point set S321, in the point cloud P of set P. k Select a feature point A j As the initial reference point, set P will contain the initial reference point A. j Add all point clouds to set S1 = {P k ,…,P l In point cloud P l Select a feature point A j As a reference point, set P contains reference point A. j All point clouds of ' are added to set S2 = {P l ,…,P n Repeat the above process until all point clouds in set P are partitioned, resulting in sets S1, S2...S... c ; Where k, l, n and c all take values ​​from 1, 2…N, and l>k, n>l; In S321, in point cloud P l The method for selecting a reference point is as follows: for the point cloud P l After performing Delaunay triangulation, the average Euclidean distance d is obtained in the point cloud P. l Select the initial reference point A. j European distance Feature point A j ′ is used as a reference point, wherein the initial reference point A is... j Both the reference point and the reference point are points outside the plane that are approximately parallel to the laser projection direction during the scanning of the data acquisition system; S322, Calculate the point cloud P in set S1. k Except for the initial reference point A j In addition, with A j For each point of the initial reference point, set a local feature descriptor value η; then convert the point cloud P... k The point with the largest local feature descriptor value is selected as the candidate boundary point, and the point with the smallest local feature descriptor value is selected as the candidate smoothing point. From point cloud P k And point cloud P k+1 The registration mapping relationship between them yields the point cloud P k+1 The boundary point set Φ in k+1 and smooth point set Ω k+1 From point clouds With point cloud P k+1 The registration mapping relationship between them is used to obtain the point cloud. Boundary point set in and smooth point set Among them, the point cloud P is calculated k Except for the initial reference point, each point is assigned a value of A. j The formula for calculating the local feature descriptor value η for the initial reference point is as follows: Among them, A j A i ∈S, j≠i, M is the point cloud P k The total number of interior points; S323: If the number of candidate boundary points or candidate smoothing points is zero or M, then S321-S322 are executed again, that is, the initial reference point is reselected and the calculation is recalculated; otherwise, S33 is executed. S33, Calculate the pose transformation matrix The specific steps are as follows: S331, define the pose transformation matrix as follows: At the current timestamp t k+2 Next, order [t] k+1 ,t k+2 The pose transformation matrix of the data acquisition system within the time period is: Among them, t x t y t z Let θ represent the translation amounts along the x, y, and z axes in the L coordinate system. x ,θ y ,θ z These represent the rotational transformations of the x, y, and z axes in the L coordinate system, respectively. S332, Solving the pose transformation matrix by combining formulas (2) and (3) simultaneously gives: in, For point cloud P k+1 Boundary point set Φ k+1 or smooth point set Ω k+1 The pose transformation matrix of any point q in the matrix, t q This represents the timestamp of point q. Let P be the point cloud in the L coordinate system. k Let the coordinates of any point q in the equation be represented by the function σ(*,n,m), which takes the values ​​of the nth to mth terms of *. Representation matrix The vector formed by the solutions of the corresponding homogeneous linear system of equations from the nth to the mth term, and the Rodrigues formula for R are as follows: R=e ωθ =I+ωsinθ+ω 2 (1-cosθ) Wherein, the function Ψ(*) is a skew-symmetric matrix that takes the variable *, and I is the identity matrix; S34, the boundary point set Φ in S33 k+1 and smooth point set Ω k+1 Replace with boundary point set and smooth point set Execute step S33 to calculate the corresponding pose transformation matrix. pose transformation matrix and pose transformation matrix The final pose transformation matrix is ​​obtained by convergence; S35, Iterative convergence solution: Traverse sets S2…S c Execute S322-S34 to obtain the final pose transformation matrix corresponding to each set, and then obtain the spatial motion estimation curve of the data acquisition system corresponding to set P. The horizontal axis of the spatial motion estimation curve is the acquisition timestamp, and the vertical axis is the final pose transformation matrix in S34. S36, repeat S1-S35 until the point cloud P acquired by the data acquisition system in the last scan is obtained. final The corresponding spatial motion estimation curve of the data acquisition system; S4, mapping orbits for mapping: S41, Unification of World Coordinate System W: In the spatial motion estimation curve obtained in step S36, each time stamp includes corresponding point cloud data containing pose information. All point cloud data are registered to the world coordinate system W using the real spatial mapping relationship. S42, Orbit Prior Mapping: Based on prior feature information in the track application scenario, a fine point cloud registration algorithm is used to fuse point clouds with adjacent timestamps in the point cloud under the world coordinate system W to realize track mapping.

2. The differential mapping and trajectory mapping method with weak inertial navigation dependence according to claim 1, characterized in that: The data acquisition system in S1 uses a self-moving RC-SLAM high-precision 3D scanning system, or other 3D scanning systems with dual-axis lidar.

3. The differential mapping and trajectory mapping method with weak inertial navigation dependence according to claim 1, characterized in that: In S42, the prior feature information includes spatial seamless continuity and local consistency of rail contour, and the logical information weight is considered to be higher than the measurement information weight within a certain time period.

4. The differential mapping and trajectory mapping method with weak inertial navigation dependence according to claim 1, characterized in that: The registration frequency of S41 is 10 times that of S42 fusion frequency, and the field of view overlap during the update process is higher than 70%.

5. The differential mapping and trajectory mapping method with weak inertial navigation dependence according to claim 1, characterized in that: In S322, n = 1 and m = 3.

Images