A method for acquiring the three-dimensional spatial position and attitude of a ship

Abstract

The present invention provides a method for acquiring the three-dimensional spatial position and attitude of a ship, comprising the following steps: generating a contour model of the ship based on three-dimensional spatial coordinates and ship size information; rotating the contour model of the ship along the X-axis, the Y-axis and the Z-axis respectively based on the radian expression of the ship's heading information, the roll angle information and the pitch angle information, and designing a roll angle information rotation matrix, a pitch angle information rotation matrix and a heading information rotation matrix for rotating the ship contour model according to the ship's heading information, the roll angle information and the pitch angle information; merging the heading information rotation matrix, the roll angle information rotation matrix and the pitch angle information rotation matrix to obtain a final ship rotation matrix; and rotating the contour model of the ship based on the final ship rotation matrix to obtain the three-dimensional spatial position and attitude information of the ship. The present invention can more intuitively show the position and attitude of the ship in the three-dimensional coordinate system under the rectangular parallelepiped model.

Description

Technical Field

[0001] The invention belongs to the technical field of ship positioning and relates to a method for acquiring the three-dimensional spatial position and posture of a ship. Background Art

[0002] Currently, the position determined by satellite navigation systems simulates the ship as a point, known as a "point position." This system cannot objectively, accurately, or in real time provide information such as the ship's actual spatial location and attitude. This has significant limitations in improving ship safety, enhancing maritime management efficiency, and adapting to the development of intelligent shipping. A ship's spatial position, on the other hand, better reflects its motion and shape, and is of great practical significance for collision avoidance and ship management.

[0003] The existing method still makes corrections to the ship's position and still regards the ship's position as a point, which still has certain limitations for ship navigation safety. However, for example, when a ship is sailing in a narrow waterway and performing collision avoidance operations, treating the ship as a point will bring certain safety hazards to the ship. Therefore, how to sail more safely and more easily obtain the ship's dynamic data and its current situation during navigation is still a key issue in navigation technology research. Summary of the Invention

[0004] In order to solve the above problems, the technical solution adopted by the present invention is: a method for obtaining the three-dimensional spatial position and posture of a ship, comprising the following steps:

[0005] Obtain the ship's latitude and longitude information, roll angle, pitch angle, heading and ship antenna position coordinates;

[0006] Based on the radius of the earth, the longitude and latitude coordinates of the ship are converted into radians, and then the longitude and latitude coordinates of the ship converted into radians are converted into three-dimensional spatial coordinates using the Cartesian coordinate system;

[0007] Convert the ship's heading information, roll angle information, and pitch angle information into radians;

[0008] Generate a ship's outline model based on the three-dimensional spatial coordinates and ship size information;

[0009] Based on the radian expression of the ship's heading information, roll angle information, and pitch angle information, the ship's profile model is rotated along the X-axis, Y-axis, and Z-axis respectively, and a roll angle information rotation matrix, a pitch angle information rotation matrix, and a heading information rotation matrix are designed for rotating the ship's profile model according to the ship's heading information, roll angle information, and pitch angle information;

[0010] Combine the heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix to obtain the final ship rotation matrix;

[0011] Based on the final ship rotation matrix and the ship antenna position coordinates, the ship's contour model is rotated to obtain the three-dimensional spatial position and attitude information of the ship.

[0012] Furthermore: the outline model adopts a rectangular outline.

[0013] Furthermore, the roll angle information rotation matrix is ​​obtained by rotating the ship profile model along the X-axis. The expression of the roll angle information rotation matrix is ​​as follows:

[0014] Rx=[1,0,0;0,cos(θ),-sin(θ);0,sin(θ),cos(θ)]

[0015] Where: θ is the ship's longitude in radians, and Rx is the rotation matrix of the ship's roll.

[0016] Furthermore, the pitch angle information rotation matrix is ​​obtained by rotating the ship profile model along the Y axis. The expression of the pitch angle information rotation matrix is ​​as follows:

[0017] Ry=[cos(θ),0,sin(θ);0,1,0-sin(θ),0,cos(θ)]

[0018] Where: θ is the ship's longitude in radians, and Ry is the rotation matrix of the ship's pitch.

[0019] Furthermore, the heading information rotation matrix is ​​obtained by rotating the ship profile model along the Z axis, and the pitch angle information rotation matrix is ​​expressed as follows:

[0020] Rz=[cos(θ),-sin(θ),0; sin(θ),cos(θ),0,0,0,1];

[0021] Where: θ is the ship's longitude in radians, and Rz is the rotation matrix of the ship's heading.

[0022] Furthermore, the process of generating the ship's outline model based on the three-dimensional spatial coordinates and the ship's size information is as follows:

[0023] Design the first array, which contains the six faces of a cuboid in three-dimensional space, each face consists of four vertices;

[0024] A second array is designed, which contains the eight vertex coordinates of a cuboid in three-dimensional space. The vertex coordinates are defined relative to a coordinate system with the three-dimensional coordinates of the ship as the center point and the length, width, and height of the ship, so as to generate the outline model of the ship.

[0025] Furthermore, the heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix are merged to obtain a final ship rotation matrix; the final ship rotation matrix merging formula is as follows:

[0026] R=Rz*Ry*Rx

[0027] A device for acquiring the three-dimensional spatial position and attitude of a ship, comprising:

[0028] Acquisition module: used to obtain the ship's latitude and longitude information, roll angle, pitch angle, heading and ship antenna position coordinates;

[0029] Conversion module I: used to convert the longitude and latitude coordinates of the ship into radians based on the radius of the earth, and then convert the longitude and latitude coordinates of the ship converted into radians into three-dimensional spatial coordinates using the Cartesian coordinate system;

[0030] Conversion module II: used to convert the ship's heading information, roll angle information, and pitch angle information into radian expression;

[0031] Generation module: used to generate the outline model of the ship based on the spatial three-dimensional coordinates and ship size information;

[0032] Preliminary rotation matrix: Based on the radian expression of the ship's heading information, roll angle information, and pitch angle information, the ship's outline model is rotated along the X-axis, Y-axis, and Z-axis respectively. The roll angle information rotation matrix, pitch angle information rotation matrix, and heading information rotation matrix are designed to rotate the ship's outline model according to the ship's heading information, roll angle information, and pitch angle information;

[0033] Final rotation matrix: used to combine the heading information rotation matrix, roll angle information rotation matrix, and pitch angle information rotation matrix to obtain the final ship rotation matrix;

[0034] Obtaining module: used to rotate the ship's contour model based on the final ship rotation matrix and the ship's antenna position coordinates to obtain the 3D spatial position and attitude information of the ship.

[0035] The present invention provides a method for acquiring the three-dimensional spatial position and attitude of a ship. The method utilizes the longitude and latitude of the ship, the position of the ship's antenna and the ship's attitude to better represent the position of the ship in space.

[0036] The present invention provides the longitude and latitude of the ship, the three-dimensional coordinates of the ship's antenna, and the ship's dynamic data. The longitude and latitude coordinates are converted into three-dimensional coordinates using a Cartesian coordinate system. The ship's dynamic data, such as heading, roll, and pitch, are converted into radians. Three rotation matrices, X, Y, and Z, are established, and the changes in the ship's dynamic data are simulated by the rotation matrices. A rectangular parallelepiped model is constructed according to the size of the ship. The matrix is ​​applied and combined with the rectangular parallelepiped to obtain the spatial position of the ship in three-dimensional coordinates with the center of the earth as the origin. This technical solution is used to solve the problem that traditional positioning methods can only obtain the position of the ship point but cannot obtain the spatial position and attitude information of the ship. The method of the present application can obtain the spatial position of the ship and the ship's dynamic data, such as heading, while positioning the ship. It is helpful in improving the safety of ships and the development of intelligent navigation.

[0037] This has guiding significance in the fields of intelligent navigation of ships and ship collision avoidance. Specifically, the present invention has the following advantages: the present invention can more intuitively see the position and posture of a ship in a three-dimensional coordinate system with the center of the earth as the coordinate origin under a rectangular parallelepiped model. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0039] Figure 1 A flow chart of the method of this application;

[0040] Figure 2 This is the simulation heading result diagram;

[0041] Figure 3 This is the simulation pitch result diagram;

[0042] Figure 4 This is the simulation roll result diagram. DETAILED DESCRIPTION

[0043] It should be noted that, unless there is any conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0044] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is in no way intended to limit the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0045] Figure 1 A flow chart of the method of this application;

[0046] A method for acquiring the three-dimensional spatial position and attitude of a ship, characterized by:

[0047] S1: Obtain the ship's latitude and longitude information, roll angle, pitch angle, heading and ship antenna position coordinates;

[0048] S2: Based on the radius of the earth, the longitude and latitude coordinates of the ship are converted into radians, and then the longitude and latitude coordinates of the ship converted into radians are converted into three-dimensional spatial coordinates using the Cartesian coordinate system;

[0049] S3: converting the ship's heading information, roll angle information, and pitch angle information into radians;

[0050] S4: Generate a ship's outline model based on the three-dimensional spatial coordinates and ship size information;

[0051] S5: Based on the radian expression of the ship's heading information, roll angle information, and pitch angle information, the ship's profile model is rotated along the X-axis, Y-axis, and Z-axis, respectively, and a roll angle information rotation matrix, a pitch angle information rotation matrix, and a heading information rotation matrix are designed for rotating the ship's profile model according to the ship's heading information, roll angle information, and pitch angle information;

[0052] S6: Merge the heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix to obtain the final ship rotation matrix;

[0053] S7: Based on the final ship rotation matrix and the ship antenna position coordinates, the ship's contour model is rotated to obtain the three-dimensional spatial position and attitude information of the hull.

[0054] Steps S1 / S2 / S3 / S4 / S5 / S6 / S7 are performed in sequence;

[0055] This application is implemented using MATLAB software; the expression for defining a function that can accept input is as follows:

[0056] "function draw_ship()"

[0057] The outline model adopts a rectangular outline.

[0058] Furthermore, the obtained ship antenna position coordinate information ensures that the antenna position relative to the ship remains unchanged after the rotation, and the antenna coordinates are taken as a reference to ensure that the antenna coordinates remain unchanged relative to the ship after the rotation;

[0059] Furthermore, the radius of the earth is defined as follows:

[0060] R e arth=6371e3;

[0061] where R e arth is the radius of the Earth.

[0062] The expression for defining the latitude and longitude coordinates of the ship is as follows:

[0063] lat s hip=lat;

[0064] lon s hip=lon;

[0065] where lat s hip indicates the latitude of the ship, lon s hip represents the latitude of the ship; lat represents the specific latitude coordinate of the ship, and lon represents the specific longitude coordinate of the ship.

[0066] The expression for converting longitude and latitude into radians is as follows:

[0067] lat r ad=deg2rad(lat s hip);

[0068] lon r ad=deg2rad(lon s hip);

[0069] where lat r ad represents the ship's latitude in radians, lon r ad represents the ship's longitude information in radians.

[0070] Furthermore, the process of coordinate transformation design using the Cartesian coordinate system is as follows:

[0071] Under the conditions of the ship's longitude and latitude, use the following formula to convert the longitude and latitude into radians, and then use the formula for converting spherical coordinates to three-dimensional coordinates to express the ship as a three-dimensional space position with the center of the earth as the coordinate origin.

[0072] The formula for converting longitude and latitude into radians is as follows:

[0073] α=deg2rad(β);

[0074] Where: α is the longitude and latitude of the ship in radians, and β is the longitude of the ship.

[0075] Furthermore, the expression for converting from spherical coordinates to Cartesian coordinates is as follows: Spherical coordinates (r, θ, φ) can be converted to Cartesian coordinates (x, y, z) by the following formula:

[0076] x=rsin(φ)cos(θ)

[0077] y=rsin(φ)sin(θ)

[0078] z=rcos(φ)

[0079] Where: r is the radius of the earth, φ is the ship's latitude in radians, and θ is the ship's longitude in radians.

[0080] Then, substitute the defined earth radius and the arc latitude and longitude coordinates to obtain:

[0081] x s hip=R e arth*cos(lat r ad)*cos(lon r ad);

[0082] y s hip=R e arth*cos(lat r ad)*sin(lon r ad);

[0083] z s hip=R e arth*sin(lat r ad);

[0084] The formula for converting the ship's heading, roll angle, and pitch angle into radians is as follows:

[0085] α=deg2rad(β);

[0086] Where α represents the ship's heading, roll angle, and pitch angle in radians; β represents the ship's specific heading, roll angle, and pitch angle.

[0087] Define the ship's heading, roll, and pitch as bow_angle, roll_angle, and pitch_angle respectively. And convert them into radians:

[0088] bow_angle_rad=deg2rad(bow_angle);

[0089] roll_angle_rad=deg2rad(roll_angle);

[0090] pitch_angle_rad=deg2rad(pitch_angle);

[0091] Where: bow_angle_rad is the converted ship heading in radians, roll_angle_rad is the converted ship roll, and pitch_angle_rad is the converted ship pitch.

[0092] The roll angle information rotation matrix is ​​obtained by rotating the ship profile model along the X-axis. The expression of the roll angle information rotation matrix is ​​as follows:

[0093] Rx=[1,0,0;0,cos(θ),-sin(θ);0,sin(θ),cos(θ)]

[0094] Rx=[1,0,0;0,cos(roll_angle_rad),-sin(roll_angle_rad);

[0095] 0,sin(roll_angle_rad),cos(roll_angle_rad)];

[0096] Where: θ is the ship roll angle in radians, Rx is the rotation matrix of the ship roll, and roll_angle_rad is the ship roll in radians;

[0097] The pitch angle information rotation matrix is ​​obtained by rotating the ship profile model along the Y axis. The expression of the pitch angle information rotation matrix is ​​as follows:

[0098] Ry=[cos(θ),0,sin(θ);0,1,0-sin(θ),0,cos(θ)]

[0099] Ry=[cos(pitch_angle_rad),0,sin(pitch_angle_rad);0,1,0;

[0100] -sin(pitch_angle_rad),0,cos(pitch_angle_rad)];

[0101] Where: θ is the pitch angle of the ship in radians, Ry is the rotation matrix of the ship's pitch, and pitch_angle_rad is the pitch angle of the ship in radians.

[0102] The heading information rotation matrix is ​​obtained by rotating the ship profile model along the Z axis. The expression of the pitch angle information rotation matrix is ​​as follows:

[0103] Rz=[cos(θ),-sin(θ),0; sin(θ),cos(θ),0,0,0,1];

[0104] Rz=[cos(bow_angle_rad),-sin(bow_angle_rad),0;

[0105] sin(bow_angle_rad),cos(bow_angle_rad),0;

[0106] 0,0,1]

[0107] Where: θ is the ship heading in radians, Rz is the rotation matrix of the ship heading, and bow_angle_rad is the ship heading in radians.

[0108] The process of generating the ship's outline model based on the three-dimensional spatial coordinates and the ship's size information is as follows:

[0109] Design the first array, which contains the six faces of a cuboid in three-dimensional space, each face consists of four vertices; the specific expression of the first array is as follows:

[0110] faces=[1,2,6,5;% bottom face

[0111] 2,3,7,6;% side

[0112] 3,4,8,7;% side

[0113] 4,1,5,8;% side

[0114] 1,2,3,4;% top surface

[0115] 5,6,7,8];% top surface

[0116] Design a second array, which contains the coordinates of eight vertices of a cuboid in three-dimensional space. These vertex coordinates are defined relative to a coordinate system with the three-dimensional coordinates of the ship as the center point and the length, width and height of the ship.

[0117] The eight vertices of the cuboid are defined according to the ship size. The second array expression is as follows:

[0118] half size =[116,18,8.35];

[0119] vertices[

[0120] x ship -half size(1) y ship -half size(2) z ship -half size(3) ;

[0121] x ship +half size(1) y ship -half size(2) z ship -half size(3) ;

[0122] x ship +half size(1) y ship +half size(2) z ship -half size(3) ;

[0123] x ship -half size(1) y ship +half size(2) z ship -half size(3) ;

[0124] x ship -half size(1) y ship -half size(2) z ship +half size(3) ;

[0125] x ship +half size(1) y ship -half size(2) z ship +half size(3) ;

[0126] x ship +half size(1) y ship +half size(2) z ship +half size(3) ;

[0127] x ship -half size(1) y ship +half size(2) z ship +half size(3) ;]

[0128] The first array can generate a cuboid with six faces of no specific length, and the second array determines the length of each side according to the size of the ship;

[0129] A variable R is defined to store the product of the rotation matrices around the Z axis, Y axis, and X axis, meaning that the rotation is first rotated around the X axis, then the Y axis, and finally the Z axis. This order ensures that the rotations are accumulated and applied correctly, and the combined rotation matrix is ​​applied to the vertices of the object, and finally the rotated vertices are translated to a new position.

[0130] The heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix are combined to obtain a final ship rotation matrix; the final ship rotation matrix is ​​as follows:

[0131] R=Rz*Ry*Rx

[0132] Apply a rotation matrix:

[0133] vertices = (R*vertices′)′

[0134] Calculate the position of the vertex after rotation:

[0135] vertices=vertices+repmat([antenna_x,antenna_y,antenna_z],8,1)

[0136] Where: antenna_x is the antenna X coordinate, antenna_y is the antenna Y coordinate, and antenna_z is the antenna Z coordinate.

[0137] Draw a cuboid:

[0138] patch('Vertices',vertices,'Faces',faces,'FaceColor','blue');

[0139] A device for acquiring the three-dimensional spatial position and attitude of a ship, comprising:

[0140] Acquisition module: used to obtain the ship's latitude and longitude information, roll angle, pitch angle, heading and ship antenna position coordinates;

[0141] Conversion module I: used to convert the longitude and latitude coordinates of the ship into radians based on the radius of the earth, and then convert the longitude and latitude coordinates of the ship converted into radians into three-dimensional spatial coordinates using the Cartesian coordinate system;

[0142] Conversion module II: used to convert the ship's heading information, roll angle information, and pitch angle information into radian expression;

[0143] Generation module: used to generate the outline model of the ship based on the spatial three-dimensional coordinates and ship size information;

[0144] Preliminary rotation matrix: Based on the radian expression of the ship's heading information, roll angle information, and pitch angle information, the ship's outline model is rotated along the X-axis, Y-axis, and Z-axis respectively. The roll angle information rotation matrix, pitch angle information rotation matrix, and heading information rotation matrix are designed to rotate the ship's outline model according to the ship's heading information, roll angle information, and pitch angle information;

[0145] Final rotation matrix: used to combine the heading information rotation matrix, roll angle information rotation matrix, and pitch angle information rotation matrix to obtain the final ship rotation matrix;

[0146] Obtaining module: used to rotate the ship's contour model based on the final ship rotation matrix and the ship's antenna position coordinates to obtain the 3D spatial position and attitude information of the ship.

[0147] Example 1: In order to verify the effectiveness of the method of the present invention, the parameters used in this example are as follows:

[0148] Table 1 Main parameters of “Yukunlun”

[0149]

[0150]

[0151] By using the method of this application, the three-dimensional spatial position of the ship can be determined according to the coordinate axis of the obtained result, and the result of the ship's attitude can also be obtained. Figure 2-Figure 4 As shown:

[0152] Figure 2 This is the simulation heading result diagram;

[0153] Figure 3 This is the simulation pitch result diagram;

[0154] Figure 4 This is the simulation roll result diagram;

[0155] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for acquiring the three-dimensional spatial position and posture of a ship, characterized by: The following steps are involved: Obtain the ship's latitude and longitude information, roll angle, pitch angle, heading and ship antenna position coordinates; Based on the radius of the earth, the longitude and latitude coordinates of the ship are converted into radians, and then the longitude and latitude coordinates of the ship converted into radians are converted into three-dimensional spatial coordinates using the Cartesian coordinate system; Convert the ship's heading information, roll angle information, and pitch angle information into radians; Generate a ship's outline model based on the three-dimensional spatial coordinates and ship size information; Based on the radian expression of the ship's heading information, roll angle information, and pitch angle information, the ship's profile model is rotated along the X-axis, Y-axis, and Z-axis respectively, and a roll angle information rotation matrix, a pitch angle information rotation matrix, and a heading information rotation matrix are designed for rotating the ship's profile model according to the ship's heading information, roll angle information, and pitch angle information; Combine the heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix to obtain the final ship rotation matrix; Based on the final ship rotation matrix and the ship antenna position coordinates, the ship's contour model is rotated to obtain the 3D spatial position and attitude information of the ship. The roll angle information rotation matrix is ​​obtained by rotating the ship profile model along the X-axis. The expression of the roll angle information rotation matrix is ​​as follows: [ ); ] in: is the ship's longitude in radians, is the rotation matrix of the ship's roll; The pitch angle information rotation matrix is ​​obtained by rotating the ship profile model along the Y axis. The expression of the pitch angle information rotation matrix is ​​as follows: = [ ;0，1，0 ] in: is the ship's longitude in radians, is the rotation matrix of the ship's pitch; The heading information rotation matrix is ​​obtained by rotating the ship profile model along the Z axis. The expression of the pitch angle information rotation matrix is ​​as follows: = [ ； , ]； in: is the ship's longitude in radians, is the rotation matrix of the ship’s heading; The process of generating the ship's outline model based on the three-dimensional spatial coordinates and the ship's size information is as follows: Design the first array, which contains the six faces of a cuboid in three-dimensional space, each face consists of four vertices; Design a second array containing the coordinates of the eight vertices of a cuboid in three-dimensional space. The vertex coordinates are defined relative to a coordinate system centered on the three-dimensional coordinates of the ship and the length, width, and height of the ship, thereby generating a contour model of the ship. The heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix are combined to obtain a final ship rotation matrix. The final ship rotation matrix combination formula is as follows: 。 2. The method for acquiring the three-dimensional position and posture of a ship according to claim 1, characterized in that: The outline model adopts a rectangular outline.

3. A device for acquiring the three-dimensional position and attitude of a ship, characterized by: include: Acquisition module: used to obtain the ship's latitude and longitude information, roll angle, pitch angle, heading and ship antenna position coordinates; Conversion module I: used to convert the longitude and latitude coordinates of the ship into radians based on the radius of the earth, and then convert the longitude and latitude coordinates of the ship converted into radians into three-dimensional spatial coordinates using the Cartesian coordinate system; Conversion module II: used to convert the ship's heading information, roll angle information, and pitch angle information into radian expression; Generation module: used to generate the outline model of the ship based on the spatial three-dimensional coordinates and ship size information; Preliminary rotation matrix: Based on the radian expression of the ship's heading information, roll angle information, and pitch angle information, the ship's outline model is rotated along the X-axis, Y-axis, and Z-axis respectively. The roll angle information rotation matrix, pitch angle information rotation matrix, and heading information rotation matrix are designed to rotate the ship's outline model according to the ship's heading information, roll angle information, and pitch angle information; Final rotation matrix: used to combine the heading information rotation matrix, roll angle information rotation matrix, and pitch angle information rotation matrix to obtain the final ship rotation matrix; Obtaining module: used to rotate the ship's contour model based on the final ship rotation matrix and the ship's antenna position coordinates to obtain the 3D spatial position and attitude information of the ship; The roll angle information rotation matrix is ​​obtained by rotating the ship profile model along the X-axis. The expression of the roll angle information rotation matrix is ​​as follows: [ ); ] in: is the ship's longitude in radians, is the rotation matrix of the ship's roll; The pitch angle information rotation matrix is ​​obtained by rotating the ship profile model along the Y axis. The expression of the pitch angle information rotation matrix is ​​as follows: = [ ;0，1，0 ] in: is the ship's longitude in radians, is the rotation matrix of the ship's pitch; The heading information rotation matrix is ​​obtained by rotating the ship profile model along the Z axis. The expression of the pitch angle information rotation matrix is ​​as follows: = [ ； , ]； in: is the ship's longitude in radians, is the rotation matrix of the ship’s heading; The process of generating the ship's outline model based on the three-dimensional spatial coordinates and the ship's size information is as follows: Design the first array, which contains the six faces of a cuboid in three-dimensional space, each face consists of four vertices; Design a second array containing the coordinates of the eight vertices of a cuboid in three-dimensional space. The vertex coordinates are defined relative to a coordinate system centered on the three-dimensional coordinates of the ship and the length, width, and height of the ship, thereby generating a contour model of the ship. The heading information rotation matrix, the roll angle information rotation matrix, and the pitch angle information rotation matrix are combined to obtain a final ship rotation matrix. The final ship rotation matrix combination formula is as follows: 。

Images