Autonomous mobile system outdoor positioning method and device fusing laser radar and RTK

Abstract

The invention discloses an autonomous mobile system outdoor positioning method and device fusing a laser radar and RTK, and the method comprises the steps: obtaining laser radar positioning data and RTK positioning data, the laser radar positioning data being positioning data obtained by calculating point cloud data obtained by scanning the surrounding environment through a laser radar by a laser radar positioning module, and the RTK positioning data being positioning data obtained by calculating point cloud data obtained by scanning the surrounding environment through a RTK positioning module; the RTK positioning data is positioning data obtained by using an RTK module, and the laser radar and the RTK module are carried on a target autonomous moving system; fusing the laser radar positioning data and the RTK positioning data to obtain a fused positioning result; and the abnormal state of the laser radar positioning module is judged, and when it is judged that the laser radar positioning module is in the abnormal state, the RTK pose is used to re-initialize the laser radar positioning data, and repositioning is carried out. The method has the advantages of being simple in implementation method, low in cost, high in environmental adaptability, stable, reliable and the like, has a repositioning function and can ensure positioning continuity.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous mobile system positioning, and particularly to an outdoor positioning method and device for an autonomous mobile system integrating a lidar and RTK. Background Art

[0002] During the autonomous movement of autonomous mobile systems such as mobile robots and driverless vehicles, high-precision and real-time navigation and positioning technologies are required. As an active sensor, a lidar can obtain accurate three-dimensional information of the surrounding environment and is widely used in scenarios such as robot navigation and autonomous driving. However, the lidar data alone is often affected by factors such as noise, occlusion, and dynamic objects, resulting in limited positioning accuracy. The RTK (Real-Time Kinematic) module is a high-precision positioning module based on carrier phase differential technology. By receiving satellite signals, it can provide high-precision absolute positioning data. However, RTK positioning is highly dependent on the environment, requires satellite signals, and has a limited coverage range. It cannot reliably provide accurate positioning data when the signal is blocked or the environment is harsh.

[0003] Using SLAM (Simultaneous Localization and Mapping) technology enables a robot or an autonomous mobile system to estimate its own position in real time and construct an environmental map in an unknown environment. For example, the LOAM (Lidar Odometry and Mapping) method estimates the motion trajectory of the robot by continuously scanning lidar data and using the geometric relationship between point clouds for matching. However, the LOAM method is prone to drift in large-scale tests, especially in long-term navigation tasks, and its positioning accuracy will gradually decrease. To reduce drift, one method can be to introduce a global map for scan matching to increase the accuracy of matching. However, this type of method will increase the computational load and affect the real-time performance. Another method is to use an Extended Kalman Filter (EKF) for sensor fusion, but the EKF method has certain limitations when dealing with nonlinear problems and is difficult to handle the constraint relationships between multiple sensors.

[0004] Fusing lidar data with multi-sensor data such as RTK data can improve the above problems and achieve high-precision positioning. To achieve multi-sensor fusion, in the prior art, a fusion method based on factor graph optimization is usually adopted, that is, by integrating lidar odometry constraints, IMU pre-integration factors, and RTK global constraints into the factor graph, and then dynamically adjusting the weights of each sensor, the constraint relationship between multi-sensor data can be efficiently processed, and high-precision and real-time positioning and navigation can be achieved. However, the implementation complexity and cost of the above method are relatively high, and the fusion effect highly depends on the reliability of the sensors. There are still problems such as limited applicable scenarios and reliability. For example, when the RTK signal is blocked or the environment is harsh, the positioning environmental adaptability is poor and the reliability is not high. And the outdoor environment is complex and changeable. When the above factor graph optimization-based fusion method is applied to the outdoor environment, the positioning accuracy and reliability cannot be ensured. Summary of the Invention

[0005] The technical problem to be solved by the present invention lies in: aiming at the technical problems existing in the prior art, the present invention provides an outdoor positioning method and device for an autonomous mobile system that fuses lidar and RTK, which has a simple implementation method, low cost, high positioning accuracy, strong environmental adaptability, and is stable and reliable. It has a relocalization function and can adapt to the complex and changeable outdoor environment to ensure the reliability and stable continuity of positioning.

[0006] To solve the above technical problems, the technical solution proposed by the present invention is:

[0007] An outdoor positioning method for an autonomous mobile system that fuses lidar and RTK, the steps include:

[0008] Obtain lidar positioning data and RTK positioning data respectively. The lidar positioning data is the positioning data calculated from the point cloud data obtained by the lidar positioning module scanning the surrounding environment with the lidar, and the RTK positioning data is the positioning data obtained by using the RTK module. The lidar and RTK module are carried on the target autonomous mobile system;

[0009] Fuse the lidar positioning data and the RTK positioning data to obtain a fused positioning result;

[0010] Judge the abnormal state of the lidar positioning module. When it is judged that the lidar positioning module is in an abnormal state, use the RTK pose to re-initialize the lidar positioning data and perform relocalization.

[0011] Further, the judging of the abnormal state of the lidar includes:

[0012] Calculate the abnormality value of the current lidar. The abnormality value is based on J T The minimum eigenvalue k of Jmin It is calculated that \(J\) represents the Jacobian matrix of the nonlinear least squares problem \(f(z)\) with respect to the lidar pose \(z\), and the nonlinear least squares problem \(f(z)\) is constructed for pose estimation using lidar positioning data;

[0013] Judge the current \(k\) min Whether it is less than the preset anomaly threshold. If so, it is determined that the current lidar is in an abnormal positioning state; otherwise, it is determined that the current lidar is in a normal positioning state.

[0014] Furthermore, the anomaly degree value is calculated according to the formula where \(\varepsilon_g\) represents the distance that the constructed constraint \(p_0\) moves in the normal direction, represents the optimal solution of the pose increment corresponding to the constructed constraint \(p_0\).

[0015] Furthermore, the preset anomaly threshold \(h\) n is determined according to the formula \(\varepsilon_g\) max / \(\varepsilon\) 跑飞 where \(\varepsilon_g\) max \(=\max(f - J\Delta z\) * ), \(\varepsilon_g\) max represents the maximum value that the current added constraint moves in the normal direction, \(\Delta z\) * represents the optimal solution of the nonlinear least squares problem \(f\) constructed for pose estimation using lidar positioning data, \(\varepsilon\) 异常 \(=k\|\Delta z\|\), \(\varepsilon\) 异常 represents the position accuracy requirement, \(\Delta z\) represents the pose increment to be optimized, that is, the pose between adjacent frames, and \(k\) represents a constant parameter.

[0016] Furthermore, it also includes that in the initialization stage, RTK positioning data is used as the first frame pose of the lidar positioning module; when the lidar constraint is in a normal state, the lidar odometer uses IMU pre-integration for initial estimation optimization.

[0017] Furthermore, when it is determined that the lidar positioning module is in an abnormal positioning state, the pose of the RTK positioning module at the previous moment is used as a reference, and the IMU data from the previous moment to the current moment is used for integration operation to predict the RTK pose at the current moment. According to the predicted RTK pose at the current moment and the extrinsic parameters of the RTK positioning module and the lidar, the pose of the lidar at the current moment is obtained and used as the repositioning initial pose of the lidar;

[0018] The lidar positioning data is re-initialized using the RTK pose according to the following formula:

[0019]

[0020] where, is the initial estimate under index m, is the optimal pose under index m - 1, is the RTK and IMU pose under index m.

[0021] Furthermore, the lidar positioning data and the RTK positioning data are fused according to the following formula to obtain the fused positioning result:

[0022]

[0023] where P fusion represents the fused positioning result, P RTK represents the positioning result of the RTK module, P LiDAR represents the positioning result of the lidar positioning module, ω RTK and ω lidar respectively represent the RTK positioning weight coefficient and the radar positioning weight coefficient, HDOP represents the horizontal dilution of precision, ε is a constant, and ||∑ lidar || is the covariance matrix of the lidar positioning result.

[0024] An electronic device includes a processor and a memory. The memory is used to store a computer program, and the processor is used to execute the computer program to perform the method as described above.

[0025] A computer-readable storage medium storing a computer program, where the computer program, when executed by a processor, implements the method as described above.

[0026] Compared with the prior art, the advantages of the present invention are as follows:

[0027] 1. Through the fusion of lidar positioning and RTK positioning, the present invention can achieve seamless switching between indoor and outdoor positioning in specific scenarios, has high-precision and high-frequency positioning capabilities, can use lidar for stable and reliable positioning in the absence of RTK signals, and also has a relocalization function, which can automatically recover when lidar positioning is abnormal to ensure positioning continuity.

[0028] 2. During the operation of the lidar positioning module, the present invention continuously detects the abnormal state of the lidar positioning module. If it is detected that the lidar module is in an abnormal state, the RTK pose is used to re-initialize the lidar positioning data and perform relocalization, so that when the lidar positioning module is in an abnormal state, it can quickly perform relocalization with the help of the RTK pose, restore the lidar positioning module to normal positioning, and achieve the relocalization function in the abnormal state, thereby improving the positioning accuracy and stability in the entire positioning process. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1It is a schematic structural diagram of the outdoor positioning system of the autonomous mobile system that integrates lidar and RTK in this embodiment.

[0030] Figure 2 It is a schematic flowchart of the implementation of the outdoor positioning method of the autonomous mobile system that integrates lidar and RTK in this embodiment. Specific implementation manners

[0031] The present invention will be further described below in conjunction with the accompanying drawings of the specification and specific preferred embodiments, but the protection scope of the present invention is not limited thereby.

[0032] As Figure 1 shown, the steps of the outdoor positioning method of the autonomous mobile system that integrates lidar and RTK in this embodiment include:

[0033] Step S01. Obtain lidar positioning data and RTK positioning data respectively. The lidar positioning data is the positioning data calculated from the point cloud data obtained by the lidar scanning the surrounding environment by the lidar positioning module, and the RTK positioning data is the positioning data obtained by using the RTK module. The lidar and the RTK module are carried on the target autonomous mobile system.

[0034] Specifically, the autonomous mobile system can be a mobile robot, an unmanned vehicle, etc. The lidar positioning module can obtain point cloud data by scanning the surrounding environment with the lidar, construct a map based on the SLAM method for the scanned point cloud data, and estimate its own position. The lidar positioning module specifically includes a front-end odometer, a back-end optimization unit, a loop detection unit, a map construction unit, etc. Among them, the front-end odometer is used to estimate the motion trajectory of the mobile system through the continuously scanned point cloud data, and the back-end optimization unit optimizes the trajectory estimated by the front-end odometer to form an optimized trajectory to reduce the cumulative error. The loop detection unit is used for loop detection to detect whether the robot returns to a previously visited position to correct the cumulative error, and the map construction unit is used to construct a three-dimensional map of the environment according to the optimized trajectory. The RTK module provides high-precision absolute positioning data by receiving satellite signals.

[0035] Step S02. Fuse the lidar positioning data and the RTK positioning data to obtain a fused positioning result.

[0036] In this embodiment, the final positioning data is determined by fusing the lidar positioning data and the RTK positioning data to combine the advantages of lidar and RTK positioning, and improve the accuracy and reliability of positioning.

[0037] In this embodiment, fusing the lidar positioning data and the RTK positioning data to obtain a fused positioning result includes: in the mapless mode, the pose of the RTK positioning data is preferentially output, and when there is no RTK positioning data, the lidar positioning data is switched to be output; in the mapped mode, the lidar pose is preferentially output, and when the lidar positioning is abnormal, the RTK pose is switched to be output. By configuring a positioning fusion module, the lidar positioning data and the RTK positioning data are obtained and fused to obtain a fused positioning result as the final positioning data.

[0038] As an alternative implementation, it further includes that at the system initialization stage, the RTK positioning data is used as the first-frame pose of the lidar positioning module, which can avoid the estimation error brought by the traditional online coordinate system estimation method and reduce the influence of the lidar cumulative error on the coordinate system transformation. When the lidar constraint is in a normal state, the lidar odometer uses IMU pre-integration for initial estimation optimization.

[0039] As an alternative implementation, in the mapped mode, the lidar positioning data and the RTK positioning data can be fused by a weighted fusion method, which can quickly and efficiently fuse the lidar and RTK positioning data, make full use of the rich environmental information provided by the lidar for accurate positioning in complex environments, and at the same time use the high-precision absolute positioning provided by the RTK to correct the cumulative error of the lidar, reduce the complexity of the fusion, and improve the efficiency, stability, reliability, and accuracy of the positioning, so as to achieve high-precision and high-frequency positioning. For example, the lidar positioning data and the RTK positioning data can be fused to obtain a fused positioning result according to the following formula:

[0040]

[0041] where HDOP represents the horizontal dilution of precision, that is, the amplification factor of the satellite geometric distribution on the positioning error, σ RTK is the nominal accuracy of the RTK receiver, ||∑ lidar || is the covariance matrix of the lidar positioning result, ε is a very small constant (such as ε = 10 -6 ) to prevent the denominator from being zero. P fusion represents the fused positioning result, P RTK represents the positioning result of the RTK module, and P lidar represents the positioning result of the lidar positioning module.

[0042] Step S03. Determine the abnormal state of the lidar positioning module. When it is determined that the lidar positioning module is in an abnormal state, re-initialize the lidar positioning data with the RTK pose and perform re-positioning.

[0043] During the operation of the lidar positioning module in this embodiment, the abnormal state of the lidar positioning module is continuously detected. If the lidar module is detected to be in an abnormal state, for example, when there is a phenomenon of running away, if there is an RTK pose currently, the lidar positioning data is re-initialized using the RTK pose and re-positioning is performed, so that it can be recognized that the lidar positioning module can quickly perform re-positioning with the help of the RTK pose when in an abnormal state, enabling the lidar positioning module to resume normal positioning and realizing the re-positioning function under abnormal states.

[0044] Specifically, a re-positioning module with the above-mentioned re-positioning function can be loaded into the lidar positioning module. During the operation of the lidar positioning module, the re-positioning module continuously detects the abnormal state of the lidar positioning module. If the lidar module is detected to be in an abnormal state, the lidar positioning data is re-initialized using the RTK pose and re-positioning is performed.

[0045] To implement the re-positioning mechanism, it is necessary to accurately judge whether the lidar is in an abnormal state. In this embodiment, the principle of lidar abnormal state detection is first analyzed:

[0046] When using lidar positioning data for pose estimation, a non-linear least squares problem can be constructed. For the solution of the non-linear least squares problem, the linear least squares method can be used. Taking the Gauss-Newton method as an example, f(z) is a non-linear function, z is the lidar pose, and for minimizing ||f(z + Δz)|| 2 , the linear least squares problem can be expressed as follows:

[0047]

[0048] J(z)Δz = -f(z) (5)

[0049] Δz * = -(J(z) T J(z)) -1 J(z)f(z) (6)

[0050] In the formula, Δz ∈ se(3) is the pose increment to be optimized, that is, the pose between adjacent frames, and Δz * ∈ se(3) is the optimal solution of the linear least squares problem f(z), ||·|| represents the magnitude of the vector, is the Jacobian matrix of f(z) with respect to the lidar pose z.

[0051] To detect lidar positioning anomalies, this embodiment introduces a wrong constraint p, which will cause at the optimal point Δz *An obvious offset εz is generated, and then an anomaly factor G can be constructed to quantitatively describe the degree of anomaly. By comparing the size of the anomaly factor G with the preset anomaly threshold, it is possible to quickly determine whether the lidar positioning module has an anomaly.

[0052] Specifically, first, a constraint p0 is constructed to pass through the optimal solution Δz * and satisfy the following equation:

[0053]

[0054] After adding the constraint condition p0, the linear least squares problem can be converted into Equation (8). Since the constraint p0 passes through the optimal solution Δz * , the least squares solution of Equation (8) is still equal to Δz * , that is, Equation (9). Therefore, the constraint p0 is considered to be correct.

[0055]

[0056] When the constraint p0 moves a certain distance εg (a small amount) in the normal direction, another constraint is obtained and the linear least squares problem and the solution can be expressed as:

[0057]

[0058] Since there is a deviation between the solution and Δz * , the constraint p is considered to be an incorrect constraint, and thus εz p is the displacement of εz along the p direction, ||p|| = 1, εz p =(p T ·εz) / ||p|| = p T εz. Therefore, the anomaly factor G can be defined as:

[0059]

[0060] As shown in the above formula, when the direction of p is the eigenvector corresponding to k min , εz p is the largest, and the anomaly factor G can be calculated as:

[0061]

[0062] where k min is the minimum eigenvalue of J T J, so the runaway degree of the lidar is only related to the k T of J minRelated. When εg is a constant, a smaller runaway degree G corresponds to a larger This indicates that the problem of least squares estimating the pose increment of lidar is more vulnerable to interference. That is to say, the lidar constraint is less sufficient, making it easier for lidar degradation to occur. Therefore, for a given threshold, when the minimum eigenvalue is less than the threshold, it can be determined that the lidar has abnormal states such as runaway.

[0063] Based on the above analysis, the steps of determining the abnormal state of the lidar in this embodiment specifically include:

[0064] Step S301, calculate the abnormality value of the current lidar, and the abnormality value is calculated according to J T The minimum eigenvalue k of J min Calculated, for example, the abnormality value can be specifically calculated according to the above formula (13);

[0065] Step S301, determine whether the current k min Is less than the preset abnormality threshold, if so, it is determined that the current lidar is in an abnormal positioning state, otherwise it is determined that the current lidar is in a normal positioning state.

[0066] Through the above method, this embodiment can quickly and accurately determine whether the lidar positioning module has abnormal states such as runaway in combination with the minimum eigenvalue k min Thereby promptly starting repositioning to ensure the stability and reliability of positioning.

[0067] The determination of the preset abnormality threshold will directly affect the accuracy of the abnormal state determination. This embodiment further adopts a dynamic threshold determination method based on the position accuracy requirement to achieve dynamic identification of the lidar abnormal state. The position accuracy requirement is denoted as ε 跑飞 , which represents the accuracy requirement for the positioning algorithm to ensure that the carrier can move safely and accurately from one place to another. That is, in the positioning task, the position error of the algorithm should always be less than ε 跑飞 .

[0068] When adding a constraint p, the offset of the optimal estimate ||εz|| should be less than the position accuracy requirement ε 跑飞 . For any given εg, Is:

[0069]

[0070] Let εg be the maximum value, Still following formula (11), then the lower bound of G is given by the following formula:

[0071] G≥εg max / ε 跑飞 =h n (15)

[0072] where εg max represents the maximum value of the current movement in the normal direction of the added constraint, that is, εg max = max(f - JΔz * ), h n is the calculated dynamic anomaly threshold, and the position accuracy ε 跑飞 can be expressed as:

[0073] ε 跑飞 = k||Δz|| (16)

[0074] where Δz represents the pose between adjacent frames, and k represents a constant parameter. For example, k can be taken as 0.7 - 0.85, and the value of k can be determined according to actual requirements.

[0075] Specifically, the preset anomaly threshold h n can be determined according to the formula εg max / ε 跑飞 where εg max = max(f - JΔz * ). When it is determined that G is less than the anomaly threshold h n , it is determined that there is a positioning anomaly in the lidar positioning module. It is necessary to use the RTK pose to initialize the position of the lidar and start repositioning, which can effectively solve the problem of positioning stability and reliability when the lidar has a positioning anomaly.

[0076] Furthermore, it also includes that when it is determined that the speed or position of the current lidar positioning is too large, the system determines that the lidar positioning may be abnormal. At this time, if there is an RTK pose, the RTK pose is used to set the initial position of the lidar, and the lidar positioning module is re-initialized.

[0077] As an optional implementation manner, when it is determined that the lidar positioning module is in an abnormal positioning state, the pose of the RTK positioning module at the previous moment is used as a reference, and the IMU data from the previous moment to the current moment is used for integration operation to predict the RTK pose at the current moment. According to the predicted RTK pose at the current moment and the extrinsic parameters of the RTK positioning module and the lidar, the pose of the lidar at the current moment is obtained, and this pose is used as the repositioning initial pose of the lidar. For example, the lidar positioning data can be re-initialized using the RTK pose according to the following formula:

[0078]

[0079] where, is the initial estimate at index m, that is, the initial estimate at the current moment, is the optimal pose at index m - 1, that is, the optimal pose at the previous moment, It is the RTK and IMU poses at index m, that is, the RTK and IMU poses at the current moment.

[0080] Through the above method, in this embodiment, when the lidar positioning module is in an abnormal state, relocalization can be achieved using the RTK pose, and the coordinate systems of lidar positioning and RTK positioning can be unified, thereby improving the positioning accuracy and stability during the entire positioning process.

[0081] Figure 2 The system architecture applicable to this embodiment. The system may include: a lidar positioning module, an RTK module, an IMU, and an outdoor positioning device for an autonomous mobile system. The lidar positioning module is used to obtain point cloud data of the surrounding environment and calculate positioning data. The RTK module is used to receive satellite signals and provide high-precision absolute positioning data. The outdoor positioning device for the autonomous mobile system includes a data receiving module, a fusion positioning module, and a relocalization module, which are respectively connected to the lidar positioning module and the RTK module, and are used to receive lidar positioning data and RTK positioning data, and fuse the lidar positioning and RTK positioning data to output the final fused positioning result; the fusion positioning module is also used to continuously detect the abnormal state of the lidar positioning module during the operation of the lidar positioning module. If it is detected that the lidar module is in an abnormal state and there is a current RTK pose, the RTK pose is used to re-initialize the lidar positioning data and perform relocalization to make the lidar positioning module resume normal positioning and achieve the relocalization function under abnormal conditions.

[0082] Through the fusion of lidar positioning and RTK positioning, the above system can achieve seamless switching between indoor and outdoor positioning in a specific scenario, have high-precision and high-frequency positioning capabilities, and can use the lidar for stable and reliable positioning in the absence of RTK signals. At the same time, it also has a relocalization function and can automatically recover when the lidar positioning is abnormal to ensure positioning continuity.

[0083] It can be understood that the above description of the devices that can be included in the system is not limited but only for illustrative purposes. Optionally, the system may further include:

[0084] An industrial computer to serve as the core processor of the system, responsible for running the positioning algorithm and providing necessary computing resources.

[0085] A navigation module, used to achieve navigation based on the positioning data of the outdoor positioning device and provide ros speed information to the robot chassis.

[0086] In summary, through the effective integration of lidar and RTK positioning technology and combined with the relocalization mechanism, the present invention can achieve seamless switching between indoor and outdoor positioning, improve positioning accuracy and stability, and thus achieve high-precision and highly reliable outdoor positioning and navigation. The present invention can be used in outdoor navigation and positioning tasks of autonomous mobile systems such as mobile robots and driverless vehicles.

[0087] This embodiment further provides an electronic device, including a processor and a memory. The memory is used to store a computer program, and the processor is used to execute the computer program to perform the method as described above.

[0088] It can be understood that the above method of this embodiment can be executed by a single device, such as a computer or a server, etc., or can also be applied to a distributed scenario where multiple devices cooperate with each other to complete. In the case of a distributed scenario, one of the multiple devices can only execute one or more steps of the above method of this embodiment, and the multiple devices interact with each other to complete the above method. The processor can be implemented in the form of a general-purpose CPU, a microprocessor, an application-specific integrated circuit, or one or more integrated circuits, etc., and is used to execute relevant programs to implement the above method of this embodiment. The memory can be implemented in the form of a read-only memory ROM, a random access memory RAM, a static storage device, and a dynamic storage device, etc. The memory can store an operating system and other application programs. When implementing the above method of this embodiment through software or firmware, the relevant program codes are stored in the memory and called by the processor for execution.

[0089] This embodiment further provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, it implements the method as described above.

[0090] Those skilled in the art should understand that the above embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to disk memories, CD-ROMs, optical memories, etc.) containing computer-usable program codes. The present application is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate for realizing in the processFigure 1 means for the functions specified in one or more processes and / or boxes Figure 1 These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory produce a manufactured article including an instruction means that implements the functions in the process Figure 1 means for the functions specified in one or more processes and / or boxes Figure 1 These computer program instructions may also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are performed on the computer or other programmable device to produce a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide for implementing the functions in the process Figure 1 means for the functions specified in one or more processes and / or boxes Figure 1 steps for the functions specified in one or more boxes or a plurality of boxes.

[0091] The above are only the preferred embodiments of the present invention and do not impose any formal limitations on the present invention. Although the present invention has been disclosed above in preferred embodiments, it is not intended to limit the present invention. Therefore, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the technical solution of the present invention shall fall within the scope of protection of the technical solution of the present invention.

Claims

1. An outdoor positioning method for an autonomous mobile system integrating lidar and RTK, characterized by the steps Including: Obtain lidar positioning data and RTK positioning data respectively. The lidar positioning data is the positioning data calculated from the point cloud data obtained by the lidar positioning module scanning the surrounding environment using a lidar, and the RTK positioning data is the positioning data obtained using an RTK module. The lidar and the RTK module are mounted on the target autonomous mobile system; Fuse the lidar positioning data and the RTK positioning data to obtain a fused positioning result; Judge the abnormal state of the lidar positioning module. When it is judged that the lidar positioning module is in an abnormal state, use the RTK pose to re-initialize the lidar positioning data and perform re-positioning.

2. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to claim 1, characterized in that, The judgment of the abnormal state of the lidar includes: Calculate the anomaly value of the current lidar, where the anomaly value is calculated according to the minimum eigenvalue k of J T of J min The nonlinear least squares problem f(z) is constructed by using lidar positioning data for pose estimation; J represents the Jacobian matrix of the nonlinear least squares problem f(z) with respect to the lidar pose z Determine the current k min Whether it is less than the preset abnormal threshold. If so, determine that the current lidar is in an abnormal positioning state; otherwise, determine that the current lidar is in a normal positioning state.

3. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to claim 2, wherein The abnormality value is calculated according to the formula where εg represents the distance that the constructed constraint p0 moves in the normal direction, represents the optimal solution of the pose increment corresponding to the constraint p0 of the structure.

4. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to claim 2, characterized in that, The preset abnormal threshold h n According to the formula εg max / ε 跑飞 It is determined, where εg max = max(f - JΔz * ), εg max represents the maximum value of the movement in the normal direction of the currently added constraint, Δz * represents the optimal solution of the nonlinear least squares problem f constructed by using the lidar positioning data for pose estimation, ε 异常 = k||Δz||, ε 异常 represents the position accuracy requirement, Δz represents the pose increment to be optimized, that is, the pose between adjacent frames, and k represents a constant parameter.

5. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to claim 1, characterized in that, It also includes that in the initialization stage, the RTK positioning data is used as the first frame pose of the lidar positioning module; when the lidar constraint is in a normal state, the lidar odometer uses IMU pre-integration for initial estimation optimization.

6. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to any one of claims 1 to 5, characterized in that When it is judged that the lidar positioning module is in a positioning abnormal state, use the pose of the RTK positioning module at the previous moment as a reference, perform integration operation using the IMU data from the previous moment to the current moment, predict the RTK pose at the current moment, and obtain the pose of the lidar at the current moment according to the predicted RTK pose at the current moment and the extrinsic parameters of the RTK positioning module and the lidar, and use it as the initial pose for lidar re-positioning.

7. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to claim 6, characterized in that, Use the RTK pose to re-initialize the lidar positioning data according to the following formula: Among them, is the initial estimate at index m, is the optimal pose at index m - 1, is the RTK and IMU poses at index m.

8. The outdoor positioning method of the autonomous mobile system integrating lidar and RTK according to any one of claims 1 to 5, characterized in that, Fuse the lidar positioning data and the RTK positioning data according to the following formula to obtain a fused positioning result: Among them, P fusion represents the fused positioning result, P RTK represents the positioning result of the RTK module, P LiDAR represents the positioning result of the lidar positioning module, ω RTK , ω lidar respectively represent the RTK positioning weight coefficient and the radar positioning weight coefficient, HDOP represents the horizontal dilution of precision, ε is a constant, ||∑ lidar || is the covariance matrix of the lidar positioning result.

9. An electronic device, comprising a processor and a memory, the memory being used for storing a computer program, characterized in that, The processor is used to execute the computer program to execute the method according to any one of claims 1 to 8.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, the method according to any one of claims 1 to 8 is implemented.

Images