A method for calibrating installation errors of water laser radar and inertial navigation

By collecting data on lidar and inertial navigation equipment at multiple locations in the water environment, and using the global search algorithm to calculate the installation error, the problem of difficulty in calibration of lidar and inertial navigation equipment in the water environment is solved, achieving efficient and low-cost Error calibration.

CN115200608BActive Publication Date: 2024-11-05BEIJING INST OF AEROSPACE CONTROL DEVICES
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210658082.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-06-10
Publication Date
2024-11-05
Estimated Expiration
2042-06-10

AI Technical Summary

Technical Problem

The existing calibration methods for installation errors of lidar and inertial navigation equipment are difficult to apply in water environments, especially because there are few absolutely stationary targets, which leads to high difficulty and low accuracy.

Method used

By acquiring the lidar point cloud data of relatively static targets and the position and attitude data of inertial navigation equipment at N different acquisition locations, the installation error of lidar and inertial navigation equipment is calculated using global search algorithms and statistical methods, and the installation error of lidar and inertial navigation equipment is reduced. Calibration difficulty and improves environmental adaptability.

Benefits of technology

It has achieved simplified calibration process in a water environment, reduced calculation ability requirements, improved calibration accuracy and adaptability, and can perform effective calibration under lower site requirements.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115200608B_ABST
    Figure CN115200608B_ABST
Patent Text Reader

Abstract

The present invention discloses a method for calibrating installation errors of a water laser radar and an inertial navigation device, comprising the following steps: (1) collecting laser radar point cloud data relative to a static target A and position and attitude data of an inertial navigation device at N different collection positions; recording the laser radar point cloud data relative to the static target A and the position and attitude data of the inertial navigation device collected at the kth collection position as C and C respectively; k and I k ; (2) According to C k and I k , obtain the position coordinates #imgabs0# of the relative static target A in the laser radar coordinate system and the position coordinates #imgabs1# of the kth acquisition position in the navigation coordinate system (3) Based on #imgabs2# and #imgabs3# and the global search algorithm, obtain the installation error E of the laser radar and the inertial navigation device. The present invention achieves the calibration of the laser radar and inertial navigation installation errors by sampling simple scenes and applying search algorithms and statistical methods, thereby improving the adaptability of the calibration scene to the environment and reducing the difficulty of calibration.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a method for calibrating installation errors of an on-water laser radar and an inertial navigation, and belongs to the field of unmanned boat intelligence and marine intelligent equipment. Background Art

[0002] At present, unmanned intelligent equipment is booming, and its development is inseparable from the improvement of the detection equipment. However, in order to improve the accuracy of the detection position, in addition to the use of laser radar with a measurement accuracy better than 1 cm, the calibration method of laser radar and inertial navigation equipment also plays an important role. The existing installation error calibration of laser radar and inertial navigation is already a mature technology in the field of unmanned vehicles. By controlling the vehicle to move along a specific trajectory, the on-board computer collects data on the surrounding static environment. The calibration personnel calibrate the point cloud data matching method. This method not only requires a lot of computing power for the computer, but also is not suitable for the situation where there are basically absolutely stationary targets in the environment. There are few absolutely stationary targets on the water. Summary of the invention

[0003] The purpose of the present invention is to overcome the above-mentioned defects and provide a method for calibrating the installation error of a water laser radar and an inertial navigation device, comprising the following steps: (1) collecting laser radar point cloud data relative to a static target A and position and attitude data of an inertial navigation device at N different collection positions; wherein the laser radar point cloud data relative to the static target A and the position and attitude data of the inertial navigation device collected at the kth collection position are respectively recorded as C k and I k , represents an integer; (2) the laser radar point cloud data C relative to the static target A obtained at the kth acquisition position k and the position and attitude data of the inertial navigation device I k , get the position coordinates of the relative static target A in the laser radar coordinate system and the position coordinates of the kth acquisition position in the navigation coordinate system Where lidar represents the laser radar coordinate system, and navi represents the navigation coordinate system; (3) According to step (2) and Based on the global search algorithm, the installation error E of the laser radar and the inertial navigation device is obtained. The present invention achieves the calibration of the laser radar and inertial navigation installation error by sampling simple scenes and applying the search algorithm and statistical methods, thereby improving the adaptability of the calibration scene to the environment and reducing the difficulty of calibration.

[0004] In order to achieve the above-mentioned object of the invention, the present invention provides the following technical solutions:

[0005] A method for calibrating installation errors of a water laser radar and an inertial navigation system comprises the following steps:

[0006] (1) At N different acquisition positions, the laser radar point cloud data and the position and attitude data of the inertial navigation device relative to the static target A are collected; the laser radar point cloud data and the position and attitude data of the inertial navigation device relative to the static target A collected at the kth acquisition position are respectively denoted as C k and I k , Represents an integer;

[0007] (2) Based on the laser radar point cloud data C of the relative static target A collected at the kth collection position k and the position and attitude data of the inertial navigation device I k , get the position coordinates of the relative static target A in the laser radar coordinate system and the position coordinates of the kth acquisition position in the navigation coordinate system Among them, lidar represents the laser radar coordinate system, and navi represents the navigation coordinate system;

[0008] (3) According to step (2) and Based on the global search algorithm, the installation error E between the lidar and the inertial navigation device is obtained.

[0009] Furthermore, in step (1), the laser radar point cloud data relative to the static target A in When it is the kth acquisition position, the coordinate values ​​of each component of the i-th point of the laser radar point cloud relative to the static target A in the laser radar coordinate system, M k is the total number of lidar point clouds at the kth acquisition position;

[0010] Position and attitude data of inertial navigation device I k =(lat k ,lng k ,θ k ,ψ k ,φ k ), where lat k , lng k ,θ k , k ,φ k are the latitude, longitude, pitch angle, heading angle, and roll angle of the kth acquisition position respectively.

[0011] Furthermore, in step (1), the total number of acquisition positions N is not less than 20, and the relative static target A is located in the area surrounded by the 1st to Nth acquisition positions.

[0012] Furthermore, the specific method of step (2) is as follows:

[0013] (2.1) The longitude and latitude of the first acquisition location are used as the origin of the navigation coordinate system;

[0014] (2.2) Let k = 1;

[0015] (2.3) Based on the laser radar point cloud data C relative to the static target A k Get the position coordinates of the relative static target A in the laser radar coordinate system

[0016] (2.4) Based on the latitude lat1 and longitude lng1 of the first collection location and the latitude lat of the kth collection location k and longitude lng k Get the position coordinates of the kth acquisition position in the navigation coordinate system

[0017] (2.7) If k = N, end the calculation; otherwise, set k = k + 1 and return to step (2.3).

[0018] Furthermore, in the step (2.3), The calculation formula is as follows:

[0019]

[0020] Furthermore, in the step (2.4), The calculation formula is as follows:

[0021]

[0022] Among them, R M and R N They are the radius of curvature of the earth's meridian plane and the radius of curvature of the meridian plane at latitude lat1 respectively.

[0023] Furthermore, in step (3), the installation error E between the laser radar and the inertial navigation device is:

[0024] E=(Δθ,Δψ,Δφ,Δx imu ,Δy imu ,Δz imu )

[0025] Where Δθ, Δψ, and Δφ represent the pitch angle installation error, heading angle installation error, and roll angle installation error of the laser radar and the inertial navigation device, respectively. imu , Δy imu , Δz imuIt represents the position installation error of each component of the laser radar and the inertial navigation device in the inertial navigation device coordinate system, and imu represents the inertial navigation device coordinate system.

[0026] Furthermore, in step (3), according to the step (2) and Based on the global search algorithm, find the point that minimizes the J value, which is the installation error E between the laser radar and the inertial navigation device:

[0027]

[0028] in,

[0029]

[0030] Represents the attitude conversion matrix from the laser radar coordinate system to the inertial navigation device coordinate system, Represents the attitude conversion matrix from the inertial navigation device coordinate system to the navigation coordinate system, θ k , k ,φ k are the pitch angle, heading angle, and roll angle of the kth acquisition position, respectively. cov represents the covariance matrix, det represents the determinant of the matrix, S(E) is θ, Δψ, Δφ, Δx imu , Δy imu , Δz imu The value space of .

[0031] Furthermore, the maximum value of S(E) is:

[0032] Δψ∈[-0, 2π), Δφ∈[-π, π)

[0033]

[0034] in represents the space of all real numbers.

[0035] Furthermore, S(E) is:

[0036]

[0037] in, are the maximum pitch angle installation error range, heading angle installation error range, and roll angle installation error range estimated according to the angular deviation of the actual installation positions of the laser radar and the inertial navigation. For example, in general installation, the coordinate systems of the laser radar and the inertial navigation are basically consistent, that is, the directions pointed by the corresponding components in the two coordinate systems are basically coincident (that is, less than 10°). In this case,

[0038]

[0039] are the maximum position installation error ranges of the laser radar and the inertial navigation device on each component in the inertial navigation device coordinate system estimated based on the angular deviation of the actual installation positions of the laser radar and the inertial navigation device; for example, in the navigation coordinate system, the installation position of the laser radar is approximately At the coordinate point, the measurement error is (e x ,e y ,e z )but:

[0040]

[0041]

[0042]

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

[0044] (1) The present invention provides a method for calibrating the installation error of a water laser radar and an inertial navigation system, which realizes the calibration of the installation error of the laser radar and the inertial navigation system by sampling a simple scene and applying a search algorithm and a statistical method;

[0045] (2) The present invention only needs to collect a relatively static obstacle (such as a buoy) on the water for calibration, thereby greatly reducing the requirements for the calibration site and improving the adaptability of the calibration scene to the environment;

[0046] (3) The present invention reduces the difficulty of calibration while ensuring the calibration accuracy and meets the computing power requirements brought by a large number of points, thus having practical application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 The present invention is a flow chart of a method for calibrating installation errors of an underwater laser radar and an inertial navigation. DETAILED DESCRIPTION

[0048] The following detailed description of the present invention will make the features and advantages of the present invention more clear and explicit.

[0049] The word "exemplary" is used exclusively herein to mean "serving as an example, embodiment, or illustration." Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments. Although various aspects of the embodiments are shown in the drawings, the drawings are not necessarily drawn to scale unless otherwise noted.

[0050] The present invention provides a method for calibrating the installation error of a water laser radar and an inertial navigation system, which can be calculated by using a small relatively static target already on the water as a reference. First, the data of the laser radar and the inertial navigation device at different positions around the static target are collected; then, the detection results of the static target position at different positions are calculated; finally, according to the detection results obtained at different positions, the installation error between the two devices is calculated by a regional search method. The method has the advantages of simple operation, low site requirements, and simple data processing method.

[0051] like Figure 1 The present invention provides a method for calibrating installation errors of a water laser radar and an inertial navigation system, and the specific steps are as follows:

[0052] (1) At N different acquisition locations, for a relatively static target A, collect the lidar point cloud data on the ship and the position and attitude data of the inertial navigation device, and combine the two into a data set D:

[0053]

[0054] The laser radar point cloud data at the kth acquisition position is C k , the position and attitude data of the inertial navigation device is I k , N represents the total number of sampling locations, Represents an integer;

[0055] in is the coordinate value of the x, y, and z components of the i-th point of the lidar point cloud relative to the static target A at the k-th acquisition position in the lidar coordinate system. lidar represents the lidar coordinate system, and M k is the total number of lidar point clouds at the kth acquisition position;

[0056] Position and attitude data of inertial navigation device I k =(lat k ,lng k ,θ k ,ψ k ,φ k ), where lat k , lng k ,θ k , k ,φ k They are the latitude, longitude, pitch angle, heading angle, and roll angle of the kth collection position, which are actually the latitude, longitude, pitch angle, heading angle, and roll angle of the unmanned boat / ship.

[0057] The more collection locations, the better but should not be less than 20, and the area enclosed by each collection location in sequence should include target A.

[0058] (2) Based on the laser radar point cloud data C relative to the static target A k , and the position and attitude data of the inertial navigation device I k Get the position of the relative static target A in the laser radar coordinate system and the position of the acquisition position in the navigation coordinate system Where navi represents the navigation coordinate system:

[0059] (a) Let the positions of lat1 and lng1 be the origin of the navigation coordinate system. The coordinates of the origin are

[0060] (b) Initialize parameter k to 1 and calculate the radius of curvature R of the Earth’s meridian plane M and the radius of curvature of the mortise and tenon surface R N .

[0061]

[0062]

[0063] Where R e is the radius of the Earth’s major axis, and f is the Earth’s ellipticity (R in the WGS84 model e =6356752, f=1 / 298.257).

[0064] (c) Calculate the position of A in the laser radar coordinate system at the kth acquisition position

[0065]

[0066] (d) Calculate the coordinates of the kth acquisition position in the navigation coordinate system The calculation method is as follows.

[0067]

[0068] (e) If k is equal to N, then end; otherwise, k is equal to k+1 and return to step (c).

[0069] (3) According to step (2) and Based on the global search algorithm, the installation error E between the laser radar and the inertial navigation device is obtained. The installation error E is defined as follows:

[0070] E=(Δθ,Δψ,Δφ,Δx imu ,Δy imu,Δz imu )

[0071] Where Δθ, Δψ, and Δφ represent the installation errors of pitch angle, heading angle, and roll angle, respectively, and Δx imu , Δy imu , Δz imu They respectively represent the installation errors of various components in the coordinate system where the inertial navigation device is located (ie, the inertial navigation device coordinate system).

[0072] Through global search algorithms (such as bee colony method, genetic algorithm, annealing algorithm, etc.), in Δθ, Δψ, Δφ, Δx imu , Δy imu , Δz imu In the possible value space S(E), find the point that minimizes the J value, which is the installation error E between the laser radar and the inertial navigation device.

[0073]

[0074] Where cov represents the covariance matrix, det represents the determinant of the matrix, The definition of is as follows.

[0075]

[0076] in is the attitude conversion matrix from the laser radar coordinate system to the inertial navigation device coordinate system, It is the attitude conversion matrix from the inertial navigation device coordinate system to the navigation coordinate system.

[0077]

[0078]

[0079] Δv is defined as follows.

[0080]

[0081] S(E) is Δθ, Δψ, Δφ, Δx imu , Δy imu , Δz imu The value space of S(E) is at most

[0082] Δψ∈[-0, 2π), Δφ∈[-π, π)

[0083]

[0084] in represents the space of all real numbers.

[0085] In order to speed up the global search, S(E) can be set by estimating the maximum error angle and error distance of the actual installation in advance. At this time, the value space of S(E) is

[0086]

[0087] They are the estimated maximum pitch angle installation error range, heading angle installation error range, and roll angle installation error range, respectively. They are the estimated maximum installation error ranges in the x, y, and z directions in the inertial navigation device coordinate system.

[0088] The present invention only needs to collect a relatively static obstacle (such as a buoy) on the water for calibration, thereby improving the adaptability of the calibration scene to the environment. And by extracting the target position from the point cloud data of each collection point, the calibration parameters are finally calculated through a mature global search algorithm and statistical method. Compared with the past, the computational complexity of the calibration algorithm is reduced, the requirement for computing power is reduced, and it has practical application value.

[0089] The present invention has been described in detail above in conjunction with specific implementations and exemplary examples, but these descriptions cannot be understood as limiting the present invention. Those skilled in the art understand that, without departing from the spirit and scope of the present invention, a variety of equivalent substitutions, modifications or improvements may be made to the technical solution of the present invention and its implementation methods, all of which fall within the scope of the present invention. The scope of protection of the present invention shall be subject to the attached claims.

[0090] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A method for calibrating installation errors of a water laser radar and an inertial navigation system, characterized in that: include: At N different acquisition positions, the laser radar point cloud data relative to the static target A and the position and attitude data of the inertial navigation device are collected; the laser radar point cloud data relative to the static target A and the position and attitude data of the inertial navigation device collected at the kth acquisition position are respectively recorded as C k and I k , Represents an integer; According to the laser radar point cloud data C of the relative static target A collected at the kth collection position k and the position and attitude data of the inertial navigation device I k , get the position coordinates of the relative static target A in the laser radar coordinate system and the position coordinates of the kth acquisition position in the navigation coordinate system Among them, lidar represents the laser radar coordinate system, and navi represents the navigation coordinate system; according to and Based on the global search algorithm, the installation error E between the laser radar and the inertial navigation device is obtained; according to and Based on the global search algorithm, find the point that minimizes the J value, which is the installation error E between the laser radar and the inertial navigation device: in, Represents the attitude conversion matrix from the laser radar coordinate system to the inertial navigation device coordinate system, Represents the attitude conversion matrix from the inertial navigation device coordinate system to the navigation coordinate system, θ k , ψk, φ k are the pitch angle, heading angle, and roll angle of the kth acquisition position, respectively. cov represents the covariance matrix, det represents the determinant of the matrix, S(E) is θ, Δψ, Δφ, Δx imu , Δy imu , Δz imu The value space of Where Δθ, Δψ, and Δφ represent the pitch angle installation error, heading angle installation error, and roll angle installation error of the laser radar and the inertial navigation device, respectively. imu , Δy imu , Δz imu It represents the position installation error of each component of the laser radar and the inertial navigation device in the inertial navigation device coordinate system, and imu represents the inertial navigation device coordinate system.

2. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 1 is characterized in that: LiDAR point cloud data relative to static target A in When it is the kth acquisition position, the coordinate values ​​of each component of the i-th point of the laser radar point cloud relative to the static target A in the laser radar coordinate system, M k is the total number of lidar point clouds at the kth acquisition position; Position and attitude data of inertial navigation device I k =(lat k ,lng k ,θ k ,ψ k ,φ k ), where lat k , lng k ,θ k , k ,φ k are the latitude, longitude, pitch angle, heading angle, and roll angle of the kth acquisition position respectively.

3. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 1 or 2, characterized in that: The total number of collection positions N is not less than 20, and the relative static target A is located in the area surrounded by the 1st to Nth collection positions.

4. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 2 is characterized in that: Get the position coordinates of the relative static target A in the laser radar coordinate system and the position coordinates of the kth acquisition position in the navigation coordinate system The specific method is as follows: According to the laser radar point cloud data C relative to the static target A k Get the position coordinates of the relative static target A in the laser radar coordinate system The longitude and latitude of the first acquisition position are used as the origin of the navigation coordinate system; according to the latitude lat1 and longitude lng1 of the first acquisition position, and the latitude lat k and longitude lng k Get the position coordinates of the kth acquisition position in the navigation coordinate system 5. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 4 is characterized in that: The calculation formula is as follows:

6. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 4 is characterized in that: The calculation formula is as follows: Among them, R M and R N They are the radius of curvature of the earth's meridian plane and the radius of curvature of the meridian plane at latitude lat1 respectively.

7. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 1, characterized in that: The installation error E of the laser radar and inertial navigation equipment is: E=(Δθ,Δψ,Δφ,Δx imu ,Dy imu ,Δz imu ) Where Δθ, Δψ, and Δφ represent the pitch angle installation error, heading angle installation error, and roll angle installation error of the laser radar and the inertial navigation device, respectively. imu , Δy imu , Δz imu It represents the position installation error of each component of the laser radar and the inertial navigation device in the inertial navigation device coordinate system, and imu represents the inertial navigation device coordinate system.

8. The method for calibrating installation errors of a water laser radar and an inertial navigation system according to claim 1, characterized in that: The maximum value of S(E) is: in represents the space of all real numbers.

9. The method for calibrating installation errors of a water laser radar and an inertial navigation according to claim 8, characterized in that: S(E) is: in, They are the maximum pitch angle installation error range, heading angle installation error range, and roll angle installation error range estimated based on the angular deviation of the actual installation positions of the laser radar and the inertial navigation; They are respectively the maximum position installation error ranges of the laser radar and the inertial navigation device on each component in the inertial navigation device coordinate system estimated according to the angular deviation of the actual installation positions of the laser radar and the inertial navigation device.