Ship inertial navigation equipment position and attitude calibration method based on two-dimensional code scanning

Abstract

The invention discloses a ship inertial navigation equipment position and attitude calibration method based on two-dimensional code scanning, and the method comprises the following steps: printing a positioning two-dimensional code on a plane mirror, and installing the plane mirror at the bow and stern line of a ship deck or the position of a GNSS antenna; a high-precision camera is installed and calibrated, and an installation deflection angle of the inertial navigation equipment relative to the high-precision camera is obtained, so that a coordinate transformation matrix between the inertial navigation equipment and the high-precision camera is obtained; shooting the two-dimensional code through a high-precision camera to obtain a corresponding position and posture relationship between the two-dimensional code and the high-precision camera, thereby obtaining a coordinate transformation matrix between the two-dimensional code and the high-precision camera; calculating an attitude transfer matrix of the inertial navigation equipment relative to the two-dimensional code so as to calculate an installation deflection angle between the inertial navigation equipment and the bow and tail lines of the ship; the two-dimensional code is shot through the high-precision camera, the distance between the high-precision camera and the two-dimensional code is obtained, and then the outer lever arm value of the two-dimensional code relative to the inertial navigation equipment is calculated. The method has wider applicability.

Description

Technical Field

[0001] The present invention relates to the field of navigation equipment, and in particular to a method for calibrating the position and attitude of a ship inertial navigation device based on two-dimensional code scanning. Background Art

[0002] The calibration of the installation error of ship inertial navigation has always been a long-standing problem. Most of the calibration of ship inertial navigation requires the use of gyrotheodolites or optical equipment and prisms. A gyrotheodolite is an instrument that combines a gyroscope and a theodolite through a connecting mechanism to measure the true north azimuth. It uses the physical properties of the gyroscope itself (fixed axis and precession) and uses a metal belt to suspend the sensitive part of the gyroscope with the center of gravity moved downward to sense the horizontal component of the earth's rotation angular velocity. Under the action of gravity, a torque precessing toward the north is generated, causing the main axis of the gyroscope to swing back and forth around the meridian plane of the earth, thereby determining the true north azimuth. Gyrotheodolites can usually only be used in static conditions. For ship calibration, this process needs to be implemented in the dock, which has certain limitations. Summary of the invention

[0003] In order to solve the problems existing in the above-mentioned prior art, the present invention provides a method for calibrating the position and attitude of a ship inertial navigation device based on QR code scanning. By utilizing a high-precision camera and a visual QR code, the method can meet the calibration of ship inertial navigation devices in static and non-static conditions, and has a wider scope of application.

[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 a position and attitude of a ship's inertial navigation device based on two-dimensional code scanning, the method comprising the following steps:

[0006] Step 1, plane mirror installation: print the positioning QR code on the plane mirror, and install the plane mirror on the bow and stern line of the ship deck; Step 2, high-precision camera installation: install the high-precision camera on the ship inertial navigation device, take the side plane of the ship inertial navigation device as the reference plane, align the installation surface of the high-precision camera with the reference plane for installation, establish a coordinate system with the horizontal plane as the X-axis, the horizontal plane as the Y-axis, the vertical direction as the Z-axis, and the center of the side of the ship inertial navigation device as the origin, and calibrate in the laboratory to obtain the installation deflection angle between the ship inertial navigation device and the high-precision camera, and obtain the coordinate conversion matrix between the ship inertial navigation device and the high-precision camera according to the installation deflection angle;

[0007] Step 3, obtaining the coordinate conversion matrix: photographing the QR code of the plane mirror with a high-precision camera, obtaining the corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror, thereby obtaining the coordinate conversion matrix between the QR code of the plane mirror and the high-precision camera;

[0008] Step 4: Calibrate the installation deflection angle: Calculate the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror, thereby calculating the installation deflection angle between the ship's inertial navigation device and the ship's bow and stern line, and completing the installation calibration of the ship's inertial navigation device.

[0009] A method for calibrating a position and attitude of a ship's inertial navigation device based on two-dimensional code scanning, the method comprising the following steps:

[0010] Step S1, plane mirror installation: print the positioning QR code on the plane mirror, and install the plane mirror at the position of the GNSS antenna; Step S2, high-precision camera installation: install the high-precision camera on the ship inertial navigation device, take the side plane of the ship inertial navigation device as the reference plane, align the installation surface of the high-precision camera with the reference plane for installation, establish a coordinate system with the horizontal plane as the X-axis, the horizontal plane as the Y-axis, the vertical direction as the Z-axis, and the center of the side of the ship inertial navigation device as the origin, and calibrate in the laboratory to obtain the installation deflection angle between the ship inertial navigation device and the high-precision camera, and obtain the coordinate conversion matrix between the ship inertial navigation device and the high-precision camera according to the installation deflection angle;

[0011] Step S3, obtaining a coordinate conversion matrix: photographing the two-dimensional code of the plane mirror with a high-precision camera, obtaining a corresponding position and posture relationship between the high-precision camera and the two-dimensional code of the plane mirror, thereby obtaining a coordinate conversion matrix between the two-dimensional code of the plane mirror and the high-precision camera;

[0012] Step S4, calibrating the outer arm value: photographing the QR code of the plane mirror with a high-precision camera to obtain the distance between the high-precision camera and the QR code of the plane mirror, and then calculating the outer arm value of the QR code of the plane mirror relative to the ship's inertial navigation device based on the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror.

[0013] Furthermore, the installation angle between the ship's inertial navigation device and the high-precision camera in the X-axis direction is α, the installation angle between the ship's inertial navigation device and the high-precision camera in the Y-axis direction is β, and the installation angle between the ship's inertial navigation device and the high-precision camera in the Z-axis direction is γ. Then, the coordinate transformation matrix between the ship's inertial navigation device coordinate system and the high-precision camera is:

[0014]

[0015] Furthermore, the corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror follows the following formula:

[0016]

[0017] Among them, s is the pixel coordinate, ui 、v i is the projection coordinate of the i-th target point on the image plane, x i ,y i is the coordinate of the i-th target point, and H is the coordinate mapping matrix;

[0018] Expanding the above formula yields:

[0019]

[0020] Using the information of multiple target points, the elements of the H matrix can be obtained through the least squares method;

[0021] Then normalize the H matrix unit to obtain the coordinate transformation matrix T between the plane mirror's QR code and the high-precision camera:

[0022]

[0023] Furthermore, the attitude transfer matrix between the ship's inertial navigation device and the two-dimensional code of the plane mirror is The calculation formula is as follows:

[0024]

[0025] Furthermore, the projection of the two-dimensional code of the plane mirror relative to the high-precision camera in the three directions of the X-axis, Y-axis, and Z-axis follows the following formula:

[0026]

[0027] Where L is the distance between the high-precision camera and the QR code of the plane mirror, x l is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the X-axis direction; l is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the Y-axis direction, z l It is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the Z-axis direction.

[0028] Based on the above technical solution, this application has the following advantages compared with the prior art:

[0029] 1. This application places several QR codes in the bow and stern direction of the ship, and then uses a high-precision camera to obtain images of the QR codes. After relevant calculations, the attitude and heading angle of the high-precision camera relative to the QR code are obtained, thereby obtaining the installation error of the ship's inertial navigation equipment. This calibration method is different from the gyrotheodolite, which can only be calibrated in a static state, is not limited to implementation in the dock, and has a wider applicability;

[0030] 2. This application places a QR code at the position of the GNSS antenna, obtains the QR code image through a high-precision camera, and after data solution, the outer arm value of the ship's inertial navigation device, that is, the position information of the ship's inertial navigation device relative to the GNSS antenna, can be obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 Schematic diagram of the installation position of the plane mirror in the embodiment of the present invention.

[0032] Figure 2 It is a working principle diagram of the present invention.

[0033] Figure 3 It is an installation relationship diagram among the ship inertial navigation device, camera and QR code in the present invention. DETAILED DESCRIPTION

[0034] In order to make the purpose, technical scheme and advantages of the present invention clearer, the specific embodiments shown below are used to describe the present invention. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the present invention. In addition, in the following description, the description of known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.

[0035] The terms used in this disclosure are for the purpose of describing specific embodiments only and are not intended to limit the disclosure. The singular forms of "a", "said" and "the" used in this disclosure and the appended claims are also intended to include plural forms unless the context clearly indicates otherwise. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0036] It should be understood that although the terms first, second, third, etc. may be used in the present disclosure to describe various information, such information should not be limited to these terms and should not be understood as indicating or implying relative importance. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of the present disclosure, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0037] In the description of the present invention, unless otherwise specified and limited, it should be noted that the terms "installed", "connected" and "connected" should be understood in a broad sense. For example, it can be a mechanical connection or the internal connection between two components. It can be a direct connection or an indirect connection through an intermediate medium. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to the specific circumstances.

[0038] In order to better understand the technical solution of the present invention, the technical solution of the present invention is described in detail below in conjunction with specific embodiments. The embodiments described below are only used to explain the present invention and are not intended to limit the present invention. Specific experimental methods not mentioned in the embodiments are usually carried out according to conventional experimental methods. The test instruments used without indicating the manufacturer are all conventional products that can be obtained commercially.

[0039] Embodiment 1

[0040] A method for calibrating a position and attitude of a ship's inertial navigation device based on two-dimensional code scanning, the method comprising the following steps:

[0041] Step 1: Installation of plane mirrors: Print the positioning QR codes on several plane mirrors respectively, and install the plane mirrors on the bow and stern lines of the ship deck;

[0042] Step 2, high-precision camera installation: install a high-precision camera on the ship's inertial navigation device, take the side plane of the ship's inertial navigation device as the reference plane, align the installation surface of the high-precision camera with the reference plane for installation, establish a coordinate system with the horizontal plane as the X-axis, the horizontal plane as the Y-axis, the vertical direction as the Z-axis, and the center of the side of the ship's inertial navigation device as the origin, and calibrate in the laboratory to obtain the installation deflection angle between the ship's inertial navigation device and the high-precision camera. According to the installation deflection angle, the coordinate conversion matrix between the ship's inertial navigation device and the high-precision camera is obtained. The installation deflection angle between the ship's inertial navigation device and the high-precision camera in the X-axis direction is α, the installation deflection angle between the ship's inertial navigation device and the high-precision camera in the Y-axis direction is β, and the installation deflection angle between the ship's inertial navigation device and the high-precision camera in the Z-axis direction is γ. Then the coordinate conversion matrix between the ship's inertial navigation device coordinate system and the high-precision camera is:

[0043]

[0044] Step 3, calculate the coordinate conversion matrix: use a high-precision camera to shoot the QR code of the plane mirror, obtain the corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror, and then calculate the coordinate conversion matrix between the QR code of the plane mirror and the high-precision camera. The corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror follows the following formula:

[0045]

[0046] Among them, s is the pixel coordinate, u i 、v i is the projection coordinate of the i-th target point on the image plane, x i ,y i is the coordinate of the i-th target point, and H is the coordinate mapping matrix;

[0047] Expanding the above formula yields:

[0048]

[0049] Using the information of multiple target points, the elements of the H matrix can be obtained through the least squares method;

[0050] Then normalize the H matrix unit to obtain the coordinate transformation matrix T between the plane mirror's QR code and the high-precision camera:

[0051]

[0052] Attitude transfer matrix between the QR code of the ship's inertial navigation device and the plane mirror The calculation formula is as follows:

[0053]

[0054] Step 4: Calibrate the installation deflection angle: Calculate the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror, thereby calculating the installation deflection angle between the ship's inertial navigation device and the ship's bow and stern line, and completing the installation calibration of the ship's inertial navigation device.

[0055] Embodiment 2

[0056] A method for calibrating a position and attitude of a ship's inertial navigation device based on two-dimensional code scanning, the method comprising the following steps:

[0057] Step S1, plane mirror installation: printing the positioning QR codes on several plane mirrors respectively, and installing the plane mirrors at the position of the GNSS antenna;

[0058] Step S2, high-precision camera installation: install the high-precision camera on the ship inertial navigation device, take the side plane of the ship inertial navigation device as the reference plane, align the installation surface of the high-precision camera with the reference plane for installation, establish a coordinate system with the horizontal plane as the X-axis, the horizontal plane as the Y-axis, the vertical direction as the Z-axis, and the center of the side of the ship inertial navigation device as the origin, calibrate in the laboratory, so as to obtain the installation deflection angle between the ship inertial navigation device and the high-precision camera, and obtain the coordinate conversion matrix between the ship inertial navigation device and the high-precision camera according to the installation deflection angle;

[0059] Step S3, obtaining a coordinate conversion matrix: photographing the two-dimensional code of the plane mirror with a high-precision camera, obtaining a corresponding position and posture relationship between the high-precision camera and the two-dimensional code of the plane mirror, thereby obtaining a coordinate conversion matrix between the two-dimensional code of the plane mirror and the high-precision camera;

[0060] Step S4, calibrating the outer arm value: photographing the QR code of the plane mirror with a high-precision camera to obtain the distance between the high-precision camera and the QR code of the plane mirror, and then calculating the outer arm value of the QR code of the plane mirror relative to the ship's inertial navigation device based on the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror.

[0061] Assume that the installation deflection angle between the ship's inertial navigation device and the high-precision camera in the X-axis direction is α, the installation deflection angle between the ship's inertial navigation device and the high-precision camera in the Y-axis direction is β, and the installation deflection angle between the ship's inertial navigation device and the high-precision camera in the Z-axis direction is γ, then the coordinate transformation matrix between the ship's inertial navigation device coordinate system and the high-precision camera is:

[0062]

[0063] Then, the corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror follows the following formula:

[0064]

[0065] Among them, s is the pixel coordinate, u i 、v i is the projection coordinate of the i-th target point on the image plane, x i ,y i is the coordinate of the i-th target point, and H is the coordinate mapping matrix;

[0066] Expanding the above formula yields:

[0067]

[0068] Using the information of multiple target points, the elements of the H matrix can be obtained through the least squares method;

[0069] Then normalize the H matrix unit to obtain the coordinate transformation matrix T between the plane mirror's QR code and the high-precision camera:

[0070]

[0071] Then, the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror is The calculation formula is as follows:

[0072]

[0073] Then, the projection of the two-dimensional code of the plane mirror relative to the high-precision camera in the three directions of X-axis, Y-axis and Z-axis follows the following formula:

[0074]

[0075] Where L is the distance between the high-precision camera and the QR code of the plane mirror, x l is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the X-axis direction; l is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the Y-axis direction, z l It is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the Z-axis direction.

[0076] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, ordinary technicians in the field should understand that the specific implementation methods of the invention can still be modified or some technical features can be replaced by equivalents without departing from the spirit of the technical solution of the present invention, which should be included in the scope of the technical solution for protection of the present invention.

Claims

1. A method for calibrating the position and attitude of a ship's inertial navigation device based on two-dimensional code scanning, characterized in that: The method comprises the following steps: Step 1: Install the plane mirror: print the positioning QR code on the plane mirror and install the plane mirror on the bow and stern line of the ship deck; Step 2, high-precision camera installation: install the high-precision camera on the ship's inertial navigation device, use the side plane of the ship's inertial navigation device as the reference plane, align the installation surface of the high-precision camera with the reference plane for installation, establish a coordinate system with the horizontal plane as the X-axis, the horizontal plane as the Y-axis, the vertical direction as the Z-axis, and the center of the side of the ship's inertial navigation device as the origin, and calibrate in the laboratory to obtain the installation deflection angle between the ship's inertial navigation device and the high-precision camera, and obtain the coordinate conversion matrix between the ship's inertial navigation device and the high-precision camera according to the installation deflection angle; Step 3, obtaining the coordinate conversion matrix: photographing the QR code of the plane mirror with a high-precision camera, obtaining the corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror, thereby obtaining the coordinate conversion matrix between the QR code of the plane mirror and the high-precision camera; Step 4: Calibrate the installation deflection angle: Calculate the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror, thereby calculating the installation deflection angle between the ship's inertial navigation device and the ship's bow and stern line, and completing the installation calibration of the ship's inertial navigation device.

2. A method for calibrating the position and attitude of a ship's inertial navigation device based on two-dimensional code scanning, characterized in that: The method comprises the following steps: Step S1, plane mirror installation: print the positioning QR code on the plane mirror, and install the plane mirror at the position of the GNSS antenna; Step S2, high-precision camera installation: install the high-precision camera on the ship inertial navigation device, take the side plane of the ship inertial navigation device as the reference plane, align the installation surface of the high-precision camera with the reference plane for installation, establish a coordinate system with the horizontal plane as the X-axis, the horizontal plane as the Y-axis, the vertical direction as the Z-axis, and the center of the side of the ship inertial navigation device as the origin, calibrate in the laboratory, so as to obtain the installation deflection angle between the ship inertial navigation device and the high-precision camera, and obtain the coordinate conversion matrix between the ship inertial navigation device and the high-precision camera according to the installation deflection angle; Step S3, obtaining a coordinate conversion matrix: photographing the two-dimensional code of the plane mirror with a high-precision camera, obtaining a corresponding position and posture relationship between the high-precision camera and the two-dimensional code of the plane mirror, thereby obtaining a coordinate conversion matrix between the two-dimensional code of the plane mirror and the high-precision camera; Step S4, calibrating the outer arm value: photographing the QR code of the plane mirror with a high-precision camera to obtain the distance between the high-precision camera and the QR code of the plane mirror, and then calculating the outer arm value of the QR code of the plane mirror relative to the ship's inertial navigation device based on the attitude transfer matrix between the ship's inertial navigation device and the QR code of the plane mirror.

3. A method for calibrating the position and attitude of a ship inertial navigation device based on two-dimensional code scanning according to claim 1 or 2, characterized in that: The installation angle between the ship's inertial navigation device and the high-precision camera in the X-axis direction is α, the installation angle between the ship's inertial navigation device and the high-precision camera in the Y-axis direction is β, and the installation angle between the ship's inertial navigation device and the high-precision camera in the Z-axis direction is γ. The coordinate conversion matrix between the ship's inertial navigation device coordinate system and the high-precision camera is:

4. According to claim 3, a method for calibrating the position and attitude of a ship inertial navigation device based on two-dimensional code scanning is characterized in that: The corresponding position and posture relationship between the high-precision camera and the QR code of the plane mirror follows the following formula: Among them, s is the pixel coordinate, u i 、v i is the projection coordinate of the i-th target point on the image plane, x i ,y i is the coordinate of the i-th target point, and H is the coordinate mapping matrix; Expanding the above formula yields: Using the information of multiple target points, the elements of the H matrix can be obtained through the least squares method; Then normalize the H matrix unit to obtain the coordinate transformation matrix T between the plane mirror's QR code and the high-precision camera:

5. According to claim 4, a method for calibrating the position and attitude of a ship inertial navigation device based on two-dimensional code scanning is characterized in that: Attitude transfer matrix between the QR code of the ship's inertial navigation device and the plane mirror The calculation formula is as follows:

6. A method for calibrating the position and attitude of a ship inertial navigation device based on two-dimensional code scanning according to claim 5, characterized in that: The projection of the QR code of the plane mirror relative to the high-precision camera in the X-axis, Y-axis, and Z-axis directions follows the following formula: Where L is the distance between the high-precision camera and the QR code of the plane mirror, x l is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the X-axis direction; l is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the Y-axis direction, z l It is the outer arm value of the plane mirror's QR code relative to the ship's inertial navigation device in the Z-axis direction.

Images