An intelligent ship augmented reality navigation assistance information display method

Abstract

The present invention discloses a method for displaying augmented reality navigation assistance information for intelligent ships, including: a multi-sensor data communication module, a camera extrinsic parameter estimation module, and an AR navigation assistance information display module under the ROS computational graph. The multi-sensor data communication module is used for data communication and topic publishing of sensors such as on-board LiDAR, cameras, IMU / RTK, etc. The camera extrinsic parameter estimation module is used to fuse the measurement data of LiDAR and IMU / RTK by the ESKF (Error State Kalman Filter) method to accurately estimate the camera extrinsic parameters and publish topics. The AR navigation assistance information display module is used to convert the three-dimensional AR navigation assistance information to the camera coordinate system and complete the accurate rendering and display of the AR navigation assistance information through virtual-real camera matching. The present invention provides a method for displaying augmented reality navigation assistance information for intelligent ships, which solves the problems in the prior art that the camera extrinsic parameter estimation method is relatively single and has low accuracy, the data communication between multi-sensors is complicated, and the rendering data does not correspond to the real camera parameters in the display of navigation assistance information.

Description

Technical Field

[0001] The present invention relates to the field of intelligent ship navigation, and particularly to an intelligent ship augmented reality navigation aid information display method. Background Art

[0002] With the continuous expansion of the scale of the shipping industry, the number of ships sailing on the sea and inland rivers has increased, and the problem of ship navigation safety has become increasingly prominent. Moreover, the analysis results of a series of safety accidents show that although the causes of ship collision accidents are necessarily related to the navigation environment and machinery and equipment, negligence in lookout or improper operation are still the main factors. Maintaining a stable visual perception is crucial for navigation safety.

[0003] Augmented Reality (AR) technology is a new technology that "seamlessly" integrates the information of the real world and the virtual world. It is to simulate and superimpose the physical information (visual information, sound, taste, touch, etc.) that is difficult to experience within a certain time and space range in the real world through computer technology, and apply the virtual information to the real world, which can be perceived by the human senses, so as to achieve a sensory experience beyond reality. 2018 was the first year when AR technology began to be applied, and it has achieved professional and popular demonstration applications in industries such as industry, medical care, culture, and entertainment. When a ship sails at night or in poor visibility conditions, it is impossible to ensure stable situation awareness relying on the driver's visual observation and camera sensors. Generally, the driver and the shore-based safety officer judge the situation through two-dimensional information such as radar, nautical charts, and AIS. However, AR technology can assist safe navigation with an intuitive visual display, which is of great significance in ensuring the safe navigation of ships. However, in the prior art, the method for estimating the external parameters of the camera is relatively single and the accuracy is low, the data communication between multiple sensors is complicated, and there is a problem that the rendering data does not correspond to the real camera parameters in the display of navigation aid information. Summary of the Invention

[0004] The present invention provides an intelligent ship augmented reality navigation aid information display method to overcome the problems in the prior art that the method for estimating the external parameters of the camera is relatively single and the accuracy is low, the data communication between multiple sensors is complicated, and the rendering data does not correspond to the real camera parameters in the display of navigation aid information.

[0005] To achieve the above object, the technical solution of the present invention is as follows:

[0006] An intelligent ship augmented reality navigation aid information display method includes a multi-sensor data communication module, a camera external parameter estimation module, and an AR navigation aid information display module based on the ROS computational graph; the multi-sensor data communication module includes a LiDAR data communication node, an IMU / RTK data communication node, a three-dimensional navigation aid information communication node, and a camera data communication node, and includes the following steps:

[0007] Step S1: The LiDAR data communication node reads the three-dimensional distance data measured by the shipborne LiDAR to obtain three-dimensional point cloud information, and publishes a three-dimensional point cloud topic. The IMU / RTK data communication node reads the IMU / RTK data information measured by the shipborne IMU / RTK integrated navigation device, and publishes an IMU topic and a latitude / longitude / altitude position topic. The three-dimensional navigation information communication node reads the navigation information measured by AIS, ECDIS, and RADAR navigation devices, and publishes a three-dimensional coordinate group topic. The camera data communication node reads the real-time video stream parameters information and the camera internal parameters information of the camera, and publishes an RGB image topic and a camera internal parameters topic;

[0008] Step S2: The camera extrinsic parameter estimation module is used to initialize the origin of the world coordinate system, and fuse the three-dimensional distance data of the three-dimensional point cloud topic with the IMU / RTK data information of the IMU topic and the latitude / longitude / altitude position topic to obtain estimated extrinsic parameter information, and publishes a world coordinate system origin topic and an estimated extrinsic parameter topic;

[0009] Step S3: The AR navigation information display module includes a spatial coordinate system transformation node and an AR navigation information rendering and display node. The spatial coordinate system transformation node subscribes to the three-dimensional coordinate group topic, the world coordinate system origin topic, and the estimated extrinsic parameter topic to analyze and obtain a rendered three-dimensional coordinate group, and publishes a rendered three-dimensional coordinate group topic;

[0010] Step S4: The AR navigation information rendering and display node subscribes to the RGB image topic, the camera internal parameters topic, and the rendered three-dimensional coordinate group topic to complete the display of the AR navigation information.

[0011] Further, in step S1, the IMU / RTK data information includes the three-axis acceleration vector information, the three-axis angular velocity vector information, the attitude quaternion information, and the latitude / longitude / altitude position information of the IMU / RTK integrated navigation device. The IMU / RTK data communication node integrates the three-axis acceleration vector information, the three-axis angular velocity vector information, and the attitude quaternion information to obtain an IMU message, and publishes an IMU topic. The IMU / RTK data communication node publishes a latitude / longitude / altitude position topic based on the latitude / longitude / altitude position information;

[0012] The three-dimensional navigation information communication node is used to read the navigation information conforming to the NMEA 0183 protocol sent by AIS, ECDIS, and RADAR navigation devices, and parse the three-dimensional geographical coordinate information in the conforming navigation information through the NMEA 0183 protocol. The three-dimensional geographical coordinate information is defined based on the ROS standard three-dimensional coordinate group message to publish a three-dimensional coordinate group topic;

[0013] The camera data communication node is used to read the real-time video stream parameters of the camera and the internal parameters of the camera. The real-time video stream parameters of the camera publish the RGB image topic based on the ROS standard image message. The ROS standard image message is the Image message under the sensor_msgs function package in ROS. The internal parameters of the camera publish the internal camera parameter topic based on the ROS standard internal camera parameter message. The ROS standard internal camera parameter message refers to the CameraInfo message under the sensor_msgs function package in ROS.

[0014] Further, the publishing of the world coordinate system origin topic and the estimated external parameter topic in step S2 is specifically as follows:

[0015] Step S2.1: The latitude, longitude, and altitude position topic published by the multi-sensor data communication module initializes the world coordinate system origin based on the ESKF fused camera external parameter estimation node through the latitude, longitude, and altitude position at the initial moment, and publishes the world coordinate system origin topic with the ROS standard three-dimensional coordinate message.

[0016] Step S2.2: The three-dimensional point cloud topic and the IMU topic data obtain the estimated camera external parameters based on the ESKF fused camera external parameter estimation node, and the ESKF fused camera external parameter estimation node publishes the camera external parameter topic with the ROS standard odometry message as the content.

[0017] Further, the publishing of the world coordinate system origin topic is specifically as follows:

[0018] Step S2.1.1: In the camera external parameter estimation module, the camera sensor, the IMU / RTK sensor, and the LiDAR sensor are all fixedly connected to the hull, and the spatial transformation matrix between any two of the camera sensor, the IMU / RTK sensor, and the LiDAR sensor can be obtained through physical installation parameters or joint calibration. A ship-fixed coordinate system O is constructed with the position of the GPS antenna on the ship as the origin, the bow direction as the ox axis, the starboard direction as the oy axis, and the vertical downward direction as the oz axis.

[0019] If the relative translation between the installation position of the IMU / RTK sensor and the origin O is the vector t OI , and the relative rotation is the matrix R OI , then the physical installation parameters of the IMU / RTK coordinate system relative to the ship-fixed coordinate system O obtained by the IMU / RTK sensor can be described by the spatial transformation matrix T OI as follows:

[0020]

[0021] If both the LiDAR sensor and the camera sensor know the physical installation parameters T OL and TOC , where L and C respectively represent the LiDAR coordinate system and the camera coordinate system, and the spatial transformation matrix T between the camera sensor and the IMU / RTK sensor CI , and the spatial transformation matrix T between the camera sensor and the LiDAR sensor CL can both be obtained through matrix operations:

[0022] T CI = T CO ·T OI

[0023] T CL = T CO ·T OL

[0024] If the physical installation parameters of the IMU / RTK sensor, LiDAR sensor, and camera sensor relative to the ship fixed coordinate system O are unknown, then through the first joint calibration tool, the spatial transformation matrix T between the camera sensor and the IMU / RTK sensor is directly obtained CI , and using the second joint calibration tool, the spatial transformation matrix T between the camera sensor and the LiDAR sensor is directly obtained CL ; the extrinsic parameters of the IMU / RTK sensor, LiDAR sensor, and camera sensor are converted through the spatial transformation matrix:

[0025] T CW = T CL ·T LW = T CI ·T IW

[0026] Among them, T CW represents the spatial transformation matrix from the world coordinate system W to the camera coordinate system C, T CL represents the spatial transformation matrix from the radar coordinate system L to the camera coordinate system C, T LW represents the spatial transformation matrix from the world coordinate system W to the radar coordinate system L, T CI represents the spatial transformation matrix from the IMU coordinate system I to the camera coordinate system C, T IW represents the spatial transformation matrix from the world coordinate system W to the IMU coordinate system I;

[0027] Step S2.1.2: The ESKF fusion camera extrinsic parameter estimation node receives the 3D point cloud topic, IMU topic, and geodetic position topic, and initializes the world coordinate system using the geodetic position at the initial moment, that is, using the longitude, latitude, and altitude at the initial moment as the origin of the northeast-up world coordinate system W, and performs geographical position conversion between the geodetic position topic and the origin of the northeast-up world coordinate system W;

[0028] Step S2.1.3: In the geographical location conversion, the origin of the ENU (East-North-Up) world coordinate system is set using the library functions reset and the longitude, latitude, and altitude at the initial moment. The metric coordinates relative to the origin coordinates are obtained using the library function forward. The longitude, latitude, and altitude at the initial moment are encoded in the ROS standard three-dimensional vector message format, and the world coordinate system origin topic is published.

[0029] Further, the specific operation of publishing the camera extrinsic parameter topic is as follows:

[0030] Step S2.2.1: In the ESKF (Extended Kalman Filter) fusion camera extrinsic parameter estimation node, the system state to be estimated is divided into the true state the nominal state and the error state δx, and the relationship is as follows:

[0031]

[0032] Step S2.2.2: To estimate the camera extrinsic parameters, a prediction model of the error state δx in continuous time is constructed with the IMU (Inertial Measurement Unit) attitude matrix R, velocity vector v, displacement vector p, acceleration bias vector b a and angular velocity bias vector b g as parameters.

[0033]

[0034] Among them, F t is the linear state transition matrix in continuous time, B t is the measurement noise matrix in continuous time, and w is the measurement noise;

[0035] Step S2.2.3: Based on the latitude-longitude-altitude position topic and the LiDAR point cloud topic, an error state observation model for attitude construction is obtained:

[0036] y = G t ·δx + C t ·n

[0037] Among them, G t is the linear observation matrix in continuous time, C t is the observation noise matrix in continuous time, n is the observation noise, y is the error state observable in continuous time, and the displacement error observable δp is provided by the metric coordinates converted from the latitude-longitude-altitude position topic, and the Lie algebra δθ of the attitude error observable is provided by the LiDAR point cloud matching. Among them, the LiDAR point cloud matching uses a variant algorithm to calculate the attitude change at the current observation moment relative to the previous observation moment:

[0038]

[0039] Among them, nδp and n δθ respectively represent the observation errors of displacement and attitude, and I 3 represents the 3D identity matrix;

[0040] Step S2.2.4: Discretize the ESKF prediction model and the observation model y to obtain the discretized model:

[0041] δx k = F k-1 δx k-1 + B k-1 w k

[0042] F k-1 = I 15 + F t · T,

[0043] y k = G k δxk + C k n k

[0044] where, δx k , δx k-1 represent the error states recursively at times k and k - 1 in discrete time, F k-1 is the linear state transition matrix at time k - 1 in discrete time, B k-1 is the measurement noise matrix at time k - 1 in discrete time, w k is the measurement noise at time k in discrete time, I 15 represents the 15D identity matrix, R k-1 represents the attitude matrix at time k - 1 in discrete time, y k is the error state observation at time k in discrete time, G k is the linear observation matrix at time k in discrete time, C k is the observation noise matrix at time k in discrete time, n k is the observation noise at time k in discrete time;

[0045] Step S2.2.5: Recursively update and correct the error state and its covariance based on the above discretized model:

[0046]

[0047] where, is the predicted error state at time k, is the posterior error state at time k - 1, is the predicted error state covariance at time k, is the posterior error state covariance at time k-1, The superscript T in represents the transpose of the matrix, and Q k is the covariance of the measurement noise at time k, and K k is the Kalman gain at time k, and R k is the covariance of the observation noise at time k, is the corrected posterior error state, is the corrected posterior error state covariance, and I is the identity matrix of the same dimension as. At each time when the k observable quantity y is obtained, the posterior error state

[0048] The nominal state is extrapolated according to the median integral as follows:

[0049]

[0050] where, represents the posterior attitude matrix at time k-1, represents the predicted attitude matrix at time k, φ is the three-dimensional rotation vector from time k-1 to k, and the symbol (·) ∧ represents the skew-symmetric matrix, and ω k , ω k-1 represents the angular velocity at times k and k-1 in discrete time, represents the deviation of the angular velocity at times k and k-1 in discrete time, and Δt is the time interval, represents the posterior velocity vector at time k-1, represents the predicted velocity vector at time k, and a k , a k-1 represents the acceleration at times k and k-1 in discrete time, represents the deviation of the acceleration at times k and k-1 in discrete time, and g is the local gravitational acceleration, represents the posterior displacement vector at time k-1, represents the predicted displacement vector at time k;

[0051] According to the superposition relationship of the true state, nominal state, and error state, the nominal state is corrected to obtain the true state at the current time

[0052]

[0053] where all physical quantities are at time k, is the nominal displacement vector, is the posterior correction value of the displacement vector error, is the true displacement vector, is the nominal velocity vector, is the posterior correction value of the velocity vector error, is the true velocity vector, is the nominal attitude matrix, is the skew-symmetric matrix of the posterior correction value of the rotation vector error, is the true rotation matrix, is the nominal acceleration deviation, is the posterior correction value of the acceleration deviation error, is the true acceleration deviation, is the nominal angular velocity deviation, is the posterior correction value of the angular velocity deviation error, is the true angular velocity deviation;

[0054] The true displacement vector and the true attitude matrix together constitute the external camera parameter matrix T k at time k:

[0055]

[0056] where T CW,k is the transformation matrix of the camera coordinate system C relative to the ENU world coordinate system W at time k;

[0057] Step S2.2.6: The camera coordinate system C encodes T CW,k in the ROS standard odometry message format nav_msgs / Odometry and publishes the external camera parameter topic.

[0058] Furthermore, the specific operation of publishing the rendered three-dimensional coordinate group topic in step S3 is as follows:

[0059] Step S3.1: For any geometric vertex data coordinate P V in the three-dimensional coordinate group topic, the library function forward in the open-source geocomputation library GeographicLib is used to obtain the metric coordinate P V of this vertex data coordinate in the world coordinate system W W , where x W , y W , z W represent the metric coordinates in the three directions of E, N, and U. The transformation matrix T CW of the camera system C relative to the ENU world coordinate system W in the external camera parameter topic data is used to perform three-dimensional coordinate transformation on the geometric vertex data:

[0060]

[0061] Among them, R CW represents the rotation matrix of the camera system C relative to the ENU world system E, and t CW represents the displacement vector of the camera system C relative to the ENU world system W, and 0 T represents the transpose of the three-dimensional zero vector. Apply the three-dimensional space transformation T CW to all the coordinate data in the three-dimensional coordinate group topic to obtain the rendered three-dimensional coordinate group, P C represents any geometric vertex data in the rendered three-dimensional coordinate group in the camera coordinate system;

[0062] Step S3.2: Encode the categories of the geometric vertex data in the rendered three-dimensional coordinate group based on the same type message format of the three-dimensional coordinate group, and publish the rendered three-dimensional coordinate group topic.

[0063] Furthermore, the display of the AR navigation assistance information in step S4 is specifically as follows:

[0064] Step S4.1: The AR navigation assistance information rendering and display node reads the camera internal parameters f x , f y , c x , c y , k i , p i in the camera internal parameter topic, where f x , f y is the scaling transformation coefficient from any three-dimensional space coordinate point in the camera system C to the pixel plane, and c x , c y is the translation transformation coefficient from the space point in the camera system to the pixel plane; k i represents the radial distortion correction parameter, and p i represents the tangential distortion correction parameter;

[0065] Step S4.2: The AR navigation assistance information rendering and display node reads the current frame image in the RGB image topic, and obtains the height h and width w of this frame of image;

[0066] Step S4.3: Using image processing technology, perform distortion correction on the original RGB image according to the radial distortion correction parameter k i and the tangential distortion correction parameter p i ;

[0067] Step S4.4: Using the camera internal parameters f x , f y , c x , c y , the height h and width w of the image, and the near clipping plane Z n and the far clipping plane Z of the perspective frustum in the three-dimensional rendering enginef Construct the rendering projection matrix K that conforms to the internal camera parameters and the 3D graphics engine:

[0068]

[0069] Step S4.5: Render any geometric vertex data P in the 3D coordinate group topic data C , and use the projection matrix K to transform the 3D geometric coordinates P to be rendered C to the pixel point P in the image plane coordinate system I :

[0070] P I = K · P C

[0071] Step S4.6: Select a predefined geometric drawing method according to the coordinate group category to obtain a pixel coordinate group, and superimpose the pixel coordinate group on the corrected RGB image to complete the rendering and display of the AR navigation assistance information.

[0072] Advantages of the present invention:

[0073] The present invention provides an intelligent ship augmented reality navigation assistance information display method. The multi-sensor data communication module under the ROS computational graph is used for data communication and topic publishing of on-board LiDAR, camera, and IMU / RTK sensors. The camera extrinsic parameter estimation module under the ROS computational graph is used to fuse the measurement data of the LiDAR sensor and the IMU / RTK sensor by the ESKF (Error State Kalman Filter) method to accurately estimate the camera extrinsic parameters and publish topics. The AR navigation assistance information display module under the ROS computational graph is used to convert the 3D AR navigation assistance information to the camera coordinate system and complete the accurate rendering and display of the AR navigation assistance information through virtual-real camera matching. It solves the problems in the prior art that the camera extrinsic parameter estimation method is relatively single and has low accuracy, the data communication between multiple sensors is complicated, and the rendering data in the navigation assistance information display does not correspond to the real camera parameters. Description of the Drawings

[0074] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following drawings are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0075] Figure 1 It is a flowchart of an intelligent ship augmented reality navigation assistance information display method of the present invention;

[0076] Figure 2This is a block diagram of an intelligent ship augmented reality navigation aid information display system according to the present invention. Detailed implementation manners

[0077] To make the objectives, 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 with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0078] This embodiment provides an intelligent ship augmented reality navigation aid information display method, as Figure 1 and Figure 2 shown, including a multi-sensor data communication module, a camera extrinsic parameter estimation module, and an AR navigation aid information display module based on the ROS (Robot Operation System) computational graph; the multi-sensor data communication module includes a LiDAR (Light Detection and Ranging) data communication node, an IMU / RTK (Inertial measurement unit / Real—time kinematic) data communication node, a three-dimensional navigation aid information communication node, and a camera data communication node, and includes the following steps:

[0079] Step S1: The LiDAR data communication node reads the three-dimensional distance data measured by the on-board LiDAR to obtain three-dimensional point cloud information, and publishes a three-dimensional point cloud topic with the ROS standard three-dimensional point cloud message sensor_msgs / PointCloud2 as the content. The IMU / RTK data communication node reads the IMU / RTK data information measured by the on-board IMU / RTK integrated navigation device, and publishes an IMU topic and a latitude / longitude / altitude position topic. The three-dimensional navigation aid information communication node reads the navigation information measured by AIS (Automatic Identification System), ECDIS (Electronic Chart Display and Information System), and RADAR (Radar) navigation devices, and publishes a three-dimensional coordinate group topic. The camera data communication node reads the real-time video stream parameter information and the camera intrinsic parameter information of the camera, and publishes an RGB image topic and a camera intrinsic parameter topic;

[0080] Step S2: The camera extrinsic parameter estimation module is used to initialize the origin of the world coordinate system, and fuse the three-dimensional distance data measured by the on-board LiDAR and the IMU / RTK data information measured by the on-board IMU / RTK integrated navigation device to obtain estimated extrinsic parameter information, and publishes a world coordinate system origin topic and an estimated extrinsic parameter topic;

[0081] Step S3: The AR navigation assistance information display module includes a spatial coordinate system transformation node and an AR navigation assistance information rendering and display node. The spatial coordinate system transformation node subscribes to the three-dimensional coordinate group topic, the world coordinate system origin topic, and the estimated external parameter topic to analyze and obtain the rendered three-dimensional coordinate group, and publishes the rendered three-dimensional coordinate group topic;

[0082] Step S4: The AR navigation assistance information rendering and display node subscribes to the RGB image topic, the camera internal parameter topic, and the rendered three-dimensional coordinate group topic to complete the display of the AR navigation assistance information.

[0083] The entire AR navigation assistance framework is constructed based on the ROS (Robot Operation System) computational graph, which solves the problem of a large number of sensor types such as cameras, lidars, IMUs, and RTKs, and complex data communication. The ROS nodes in the multi-sensor communication module publish topic data in the ROS standard message format. After replacing different models of sensors, it does not affect the entire AR navigation assistance framework, ensuring the convenience of hardware deployment and upgrade. In this method, only ROS topic data is used for communication between modules, and the functions of the modules are decoupled, improving the convenience of algorithm deployment and upgrade. Based on the ESKF (Error State Kalman Filter) method, the measurement data of the LiDAR, a high-precision distance sensor, and the IMU / RTK are fused, integrating the advantages of high measurement accuracy of the IMU in a short time, strong distance perception ability of the LiDAR, and small measurement data drift, improving the accuracy and accuracy of camera external parameter estimation. Compared with the existing method of constructing camera external parameters using on-board GPS and attitude meters, the ESKF-based method proposed in the present invention takes into account sensor errors, measurement, and observation noise, and maintains the estimation of error and noise terms in real-time calculation, avoiding the cumulative error problem caused by long-term operation. Moreover, ESKF only estimates the error state of the external parameters, which can greatly reduce the computational load and ensure the real-time performance of AR navigation assistance information rendering. To ensure that the projection relationship in the AR navigation assistance information rendering process is consistent with the real camera, the present invention proposes a rendering projection matrix that conforms to both the camera internal parameters and the three-dimensional graphics engine, and uses this matrix for projection transformation in the AR navigation assistance information rendering and display node, ensuring the consistency of the navigation assistance information rendering and solving the problem that the virtual landmarks and real landmarks in the existing AR navigation assistance system cannot be completely consistent.

[0084] In a specific embodiment, the IMU / RTK data information in step S1 includes the triaxial acceleration vector information, triaxial angular velocity vector information, attitude quaternion information, and latitude / longitude / altitude position information of the IMU / RTK integrated navigation device. The IMU / RTK data communication node packs the triaxial acceleration vector information, triaxial angular velocity vector information, and attitude quaternion information and publishes an IMU topic with the ROS standard IMU message sensor_msgs / Imu as the content. The IMU / RTK data communication node publishes a latitude / longitude / altitude position topic with the ROS standard GNSS positioning message sensor_msgs / NavSatFix as the content based on the latitude / longitude / altitude position information;

[0085] The three-dimensional navigation aid information communication node is used to read the navigation information conforming to the NMEA 0183 protocol sent by AIS, ECDIS, and RADAR navigation devices, and parse the three-dimensional geographical coordinate information in the conforming navigation information through the NMEA 0183 protocol. The three-dimensional geographical coordinate information is defined based on the ROS standard three-dimensional coordinate group message and publishes a three-dimensional coordinate group message containing the navigation aid information category based on the ROS standard three-dimensional coordinate message geometry_msgs / Vector3Stamped, and publishes a three-dimensional coordinate group topic with this message as the content;

[0086] The camera data communication node is used to read the real-time video stream parameters and internal camera parameters of the camera. The real-time video stream parameters of the camera publish an RGB image topic with the ROS standard image message as the content. The ROS standard image message is the Image message under the sensor_msgs function package in ROS. The internal camera parameters publish an internal camera parameter topic with the ROS standard internal camera parameter message as the content. The ROS standard internal camera parameter message refers to the CameraInfo message under the sensor_msgs function package in ROS.

[0087] In a specific embodiment, the publishing of the world coordinate system origin topic and the estimated external parameter topic in step S2 is specifically as follows:

[0088] Step S2.1: The latitude / longitude / altitude position topic published by the multi-sensor data communication module initializes the world coordinate system origin through the ESKF fusion camera external parameter estimation node based on the latitude / longitude / altitude position at the initial moment, and publishes the world coordinate system origin topic with the ROS standard three-dimensional coordinate message geometry_msgs / Vector3Stamped;

[0089] Step S2.2: The data of the 3D point cloud topic and the IMU topic are used to estimate the extrinsic camera parameters based on the ESKF fusion camera extrinsic parameter estimation node, and the ESKF fusion camera extrinsic parameter estimation node publishes a camera extrinsic parameter topic with the content of the ROS standard odometry nav_msgs / Odometry message.

[0090] In a specific embodiment, the publishing of the world system origin topic is specifically as follows:

[0091] Step S2.1.1: In the camera extrinsic parameter estimation module, the camera sensor, the IMU / RTK sensor, and the LiDAR sensor are all fixedly connected to the hull, and the spatial transformation matrix between any two of the camera sensor, the IMU / RTK sensor, and the LiDAR sensor can be obtained through physical installation parameters or joint calibration. A ship-fixed coordinate system O is constructed with the position of the GPS antenna on the ship as the origin, the bow direction as the ox axis, the starboard direction as the oy axis, and the vertical downward direction as the oz axis;

[0092] If the relative translation of the installation position of the IMU / RTK sensor to the origin O is a vector t OI , and the relative rotation is a matrix R OI , then the physical installation parameters of the IMU / RTK coordinate system obtained by the IMU / RTK sensor relative to the ship-fixed coordinate system O can be described by the spatial transformation matrix T OI :

[0093]

[0094] If both the LiDAR sensor and the camera sensor know the physical installation parameters T OL and T OC , where L and C represent the LiDAR coordinate system and the camera coordinate system respectively, then the spatial transformation matrix T CI between the camera sensor and the IMU / RTK sensor, and the spatial transformation matrix T CL between the camera sensor and the LiDAR sensor can both be obtained through matrix operations:

[0095] T CI =T CO ·T OI

[0096] T CL =T CO ·T OL

[0097] If the physical installation parameters of the IMU / RTK sensor, LiDAR sensor, and camera sensor relative to the ship's fixed coordinate system O are unknown, the spatial transformation matrix T between the camera sensor and the IMU / RTK sensor is directly obtained through the first joint calibration tool. CI The first joint calibration tool is the kalibr camera-IMU joint calibration tool; the spatial transformation matrix T between the camera sensor and the LiDAR sensor is directly obtained using the second joint calibration tool. CL The second joint calibration tool is the autoware camera-LiDAR joint calibration tool.

[0098] The extrinsic parameters of the IMU / RTK sensor, LiDAR sensor, and camera sensor are converted through the spatial transformation matrix:

[0099] T CW = T CL · T LW = T CI · T IW

[0100] where T CW represents the spatial transformation matrix from the world coordinate system W to the camera coordinate system C, T CL represents the spatial transformation matrix from the radar coordinate system L to the camera coordinate system C, T LW represents the spatial transformation matrix from the world coordinate system W to the radar coordinate system L, T CI represents the spatial transformation matrix from the IMU coordinate system I to the camera coordinate system C, T IW represents the spatial transformation matrix from the world coordinate system W to the IMU coordinate system I;

[0101] Step S2.1.2: The ESKF fusion camera extrinsic parameter estimation node receives the three-dimensional point cloud topic, IMU topic, and geodetic position topic, and initializes the world coordinate system using the geodetic position at the initial moment, that is, using the longitude, latitude, and altitude at the initial moment as the origin of the north-east-down world coordinate system W. The geodetic position topic and the origin of the north-east-down world coordinate system W are converted for geographical location using the open-source geographic calculation library GeographicLib.

[0102] Step S2.1.3: The geographical location conversion uses the library functions reset (longitude, latitude, altitude) and the longitude, latitude, and altitude at the initial moment to set the origin of the Northeast-East-Up (ENU) world coordinate system. The library function forward (longitude, latitude, altitude, E, N, U) is used to obtain the metric coordinates E, N, U relative to the origin's longitude, latitude, and altitude. The longitude, latitude, and altitude at the initial moment are encoded in the ROS standard three-dimensional vector message format geometry_msgs / Vector3Stamped, and the world coordinate system origin topic is published.

[0103] In a specific embodiment, the publishing of the camera extrinsic parameter topic is specifically as follows:

[0104] Step S2.2.1: The ESKF fusion camera extrinsic parameter estimation node divides the system state to be estimated into the true state the nominal state and the error state δx, and the relationship is as follows:

[0105]

[0106] Step S2.2.2: To estimate the camera extrinsic parameters, a prediction model of the error state δx in continuous time is constructed with the IMU attitude matrix R, velocity vector v, displacement vector p, and acceleration bias vector b a and the angular velocity bias vector b g as parameters.

[0107]

[0108] where F t is the linear state transition matrix in continuous time, B t is the measurement noise matrix in continuous time, and w is the measurement noise;

[0109] Step S2.2.3: Based on the latitude-longitude-altitude position topic and the LiDAR point cloud topic, an error state observation model for attitude construction is obtained:

[0110] y = G t ·δx + C t ·n

[0111] where G t is the linear observation matrix in continuous time, C tis the observation noise matrix in continuous time, n is the observation noise, y is the error state observation in continuous time, the displacement error observation δp is provided by the metric coordinates obtained from the conversion of the latitude, longitude and altitude position topic, and the Lie algebra δθ of the attitude error observation is provided by the LiDAR point cloud matching. Among them, the LiDAR point cloud matching uses any one of the variant algorithms of ICP, NDT or GICP, Fast-VGICP, PTPLICP, NDT-omp to calculate the attitude change at the current observation moment relative to the previous observation moment:

[0112]

[0113] where n δp and n δθ represent the observation errors of displacement and attitude respectively, and I 3 represents the 3-dimensional identity matrix;

[0114] Step S2.2.4: Discretize the ESKF prediction model and the observation model y to obtain the discretized model:

[0115] δx k = F k-1 δx k-1 + B k-l W k

[0116] F k-1 = I 15 + F t ·T,

[0117] y k = G k δx k + C k n k

[0118] where, δx k , δx k-1 represent the error states recursively at times k and k-1 in discrete time, F k-1 is the linear state transition matrix at time k-1 in discrete time, B k-1 is the measurement noise matrix at time k-1 in discrete time, w k is the measurement noise at time k in discrete time, I 15 represents the 15-dimensional identity matrix, R k-1 represents the attitude matrix at time k-1 in discrete time, y k is the error state observation at time k in discrete time, G k is the linear observation matrix at time k in discrete time, C kis the observation noise matrix at discrete time instant k, n k is the observation noise at discrete time instant k;

[0119] Step S2.2.5: Recursively calculate and correct the error state and its covariance based on the above discretized model:

[0120]

[0121] where, is the predicted error state at time instant k, is the posterior error state at time instant k - 1, is the predicted error state covariance at time instant k, is the posterior error state covariance at time instant k - 1, the superscript T in represents the transpose of a matrix, Q k is the covariance of the measurement noise at time instant k, K k is the Kalman gain at time instant k, R k is the covariance of the observation noise at time instant k, is the corrected posterior error state, is the corrected posterior error state covariance, I is the identity matrix of the same dimension as . At each time instant when the measurement y k is obtained, calculate the posterior error state

[0122] Estimate the nominal state according to the midpoint integration:

[0123]

[0124] where, represents the posterior attitude matrix at time instant k - 1, represents the predicted attitude matrix at time instant k, φ is the three-dimensional rotation vector from time instant k - 1 to k, the symbol (·) ∧ represents the skew-symmetric matrix, ω k , ω k-1 represents the angular velocity at discrete time instants k and k - 1, represents the deviation of the angular velocity at discrete time instants k and k - 1, Δt is the time interval, represents the posterior velocity vector at time instant k - 1, represents the predicted velocity vector at time instant k, a k , a k-1 represents the acceleration at discrete time instants k and k - 1, represents the deviation of the acceleration at discrete time instants k and k - 1, g is the local gravitational acceleration, denotes the posterior displacement vector at time k-1, denotes the predicted displacement vector at time k;

[0125] According to the superposition relationship of the true state, nominal state, and error state, the nominal state is corrected to obtain the true state at the current time

[0126]

[0127] where all physical quantities are at time k, is the nominal displacement vector, is the posterior correction value of the displacement vector error, is the true displacement vector, is the nominal velocity vector, is the posterior correction value of the velocity vector error, is the true velocity vector, is the nominal attitude matrix, is the skew-symmetric matrix of the posterior correction value of the rotation vector error, is the true rotation matrix, is the nominal acceleration deviation, is the posterior correction value of the acceleration deviation error, is the true acceleration deviation, is the nominal angular velocity deviation, is the posterior correction value of the angular velocity deviation error, is the true angular velocity deviation;

[0128] The true displacement vector and the true attitude matrix together constitute the external camera parameter matrix T at time k k :

[0129]

[0130] where T CW,k is the transformation matrix of the camera coordinate system C relative to the ENU world coordinate system W at time k;

[0131] Step S2.2.6: The camera coordinate system C encodes T CW,k in the ROS standard odometry message format nav_msgs / Odometry and publishes the external camera parameter topic.

[0132] In a specific embodiment, the step of publishing the rendered three-dimensional coordinate group topic in step S3 is specifically:

[0133] Step S3.1: For any geometric vertex data coordinate P in the three-dimensional coordinate group topicV , which contains longitude, latitude, and altitude information, and uses the library function forward in the open-source Geographic Computing Library GeographicLib to obtain the vertex data coordinates P V The metric coordinates P in the world coordinate system W W , where x W , y W , z W represent the metric coordinates in the three directions of E, N, and U. Using the transformation matrix T of the camera coordinate system C relative to the ENU world coordinate system W in the camera extrinsic parameter topic data CW , perform a three-dimensional coordinate transformation on the geometric vertex data:

[0134]

[0135] Among them, R CW represents the rotation matrix of the camera coordinate system C relative to the ENU world coordinate system W, and t CW represents the displacement vector of the camera coordinate system C relative to the ENU world coordinate system W, and 0 T represents the transpose of the three-dimensional zero vector. Apply the three-dimensional space transformation T to all coordinate data in the three-dimensional coordinate group topic CW , to obtain the rendered three-dimensional coordinate group, P C represents any geometric vertex data in the rendered three-dimensional coordinate group in the camera coordinate system;

[0136] Step S3.2: Encode the categories of the geometric vertex data in the rendered three-dimensional coordinate group based on the same type message format of the three-dimensional coordinate group, and publish the rendered three-dimensional coordinate group topic.

[0137] In a specific embodiment, the display of the AR navigation assistance information in step S4 is specifically as follows:

[0138] Step S4.1: The AR navigation assistance information rendering and display node reads the camera internal parameters f x , f y , c x , c y , k i , p i , where f x , f y is the scaling transformation coefficient from any three-dimensional space coordinate point in the camera coordinate system C to the pixel plane, and c x , c y is the translation transformation coefficient from the space point in the camera coordinate system to the pixel plane; k i represents the radial distortion correction parameter, and p i represents the tangential distortion correction parameter;

[0139] Step S4.2: The AR navigation assistance information rendering and display node reads the current frame image in the RGB image topic, and obtains the height h and width w of this frame of image;

[0140] Step S4.3: Using image processing technology, according to the radial distortion correction parameter k i and the tangential distortion correction parameter p i perform distortion correction on the original RGB image. The distortion correction is a well-known technology in the art and is not the inventive point of this application, so it will not be elaborated here;

[0141] Step S4.4: Using the camera internal parameters f x , f y , c x , c y , the height h and width w of the image, and the near clipping plane Z of the perspective frustum in the 3D rendering engine n and the far clipping plane Z f construct a rendering projection matrix K that conforms to the camera internal parameters and the 3D graphics engine:

[0142]

[0143] Step S4.5: Render any geometric vertex data P in the 3D coordinate group topic data C , and use the projection matrix K to transform the 3D geometric coordinate P to be rendered C to the pixel point P in the image plane coordinate system I :

[0144] P I = K·P C

[0145] Step S4.6: Select a predefined geometric drawing method according to the coordinate group category to obtain a pixel coordinate group, and superimpose the pixel coordinate group on the corrected RGB image to complete the rendering and display of the AR navigation assistance information.

[0146] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An intelligent ship augmented reality navigation aid information display method, characterized in that, it includes a multi-sensor data communication module, a camera extrinsic parameter estimation module, and an AR navigation aid information display module based on the ROS computational graph; the multi-sensor data communication module includes a LiDAR data communication node, an IMU / RTK data communication node, a three-dimensional navigation aid information communication node, and a camera data communication node, and includes the following steps: Step S1: The LiDAR data communication node reads the three-dimensional distance data measured by the shipborne LiDAR to obtain three-dimensional point cloud information, and publishes a three-dimensional point cloud topic. The IMU / RTK data communication node reads the IMU / RTK data information measured by the shipborne IMU / RTK integrated navigation device, and publishes an IMU topic and a latitude / longitude / altitude position topic. The three-dimensional navigation aid information communication node reads the navigation information measured by AIS, ECDIS, and RADAR navigation devices, and publishes a three-dimensional coordinate group topic. The camera data communication node reads the real-time video stream parameters information and the camera intrinsic parameter information of the camera, and publishes an RGB image topic and a camera intrinsic parameter topic; Step S2: The camera extrinsic parameter estimation module is used to initialize the origin of the world coordinate system, and fuse the three-dimensional distance data of the three-dimensional point cloud topic with the IMU / RTK data information of the IMU topic and the latitude / longitude / altitude position topic to obtain estimated extrinsic parameter information, and publishes a world coordinate system origin topic and an estimated extrinsic parameter topic; Step S3: The AR navigation aid information display module includes a spatial coordinate system transformation node and an AR navigation aid information rendering and display node. The spatial coordinate system transformation node subscribes to the three-dimensional coordinate group topic, the world coordinate system origin topic, and the estimated extrinsic parameter topic to analyze and obtain a rendered three-dimensional coordinate group, and publishes a rendered three-dimensional coordinate group topic; Step S4: The AR navigation aid information rendering and display node subscribes to the RGB image topic, the camera intrinsic parameter topic, and the rendered three-dimensional coordinate group topic to complete the display of AR navigation aid information.

2. The intelligent ship augmented reality navigation aid information display method according to claim 1, characterized in that, in step S1, the IMU / RTK data information includes the three-axis acceleration vector information, three-axis angular velocity vector information, attitude quaternion information, and latitude / longitude / altitude position information of the IMU / RTK integrated navigation device. The IMU / RTK data communication node integrates the three-axis acceleration vector information, three-axis angular velocity vector information, and attitude quaternion information to obtain an IMU message, and publishes an IMU topic. The IMU / RTK data communication node publishes a latitude / longitude / altitude position topic based on the latitude / longitude / altitude position information; The three-dimensional navigation aid information communication node is used to read the navigation information conforming to the NMEA0183 protocol sent by AIS, ECDIS, and RADAR navigation devices, and parse the three-dimensional geographic coordinate information in the navigation information conforming to the NMEA 0183 protocol through the NMEA 0183 protocol. The three-dimensional geographic coordinate information is defined based on the ROS standard three-dimensional coordinate group message to publish a three-dimensional coordinate group topic; The camera data communication node is used to read the real-time video stream parameters of the camera and the internal parameters of the camera. The real-time video stream parameters of the camera publish an RGB image topic based on the ROS standard image message. The ROS standard image message is the Image message under the sensor_msgs function package in ROS. The internal parameters of the camera publish a camera internal parameter topic based on the ROS standard camera internal parameter message. The ROS standard camera internal parameter message refers to the CameraInfo message under the sensor_msgs function package in ROS.

3. An intelligent ship augmented reality navigation aid information display method according to claim 1, characterized in that in step S2, the specific content of publishing the world coordinate system origin topic and the estimated external parameter topic is as follows: Step S2.1: The latitude, longitude and altitude position topic published by the multi-sensor data communication module is based on the ESKF fusion camera external parameter estimation node. The origin of the world coordinate system is initialized through the latitude, longitude and altitude position at the initial moment, and the world coordinate system origin topic is published in the form of a ROS standard three-dimensional coordinate message; Step S2.2: The three-dimensional point cloud topic and the IMU topic data obtain the estimated camera external parameters based on the ESKF fusion camera external parameter estimation node, and the ESKF fusion camera external parameter estimation node publishes a camera external parameter topic with the ROS standard odometry message as the content.

4. An intelligent ship augmented reality navigation aid information display method according to claim 3, characterized in that the specific content of publishing the world coordinate system origin topic is as follows: Step S2.1.1: In the camera external parameter estimation module, the camera sensor, the IMU / RTK sensor and the LiDAR sensor are all fixedly connected to the hull, and the spatial transformation matrix between any two of the camera sensor, the IMU / RTK sensor and the LiDAR sensor can be obtained through physical installation parameters or joint calibration. A ship-fixed coordinate system O is constructed with the position of the GPS antenna on the ship as the origin, the bow direction as the ox axis, the starboard direction as the oy axis, and vertically downward as the oz axis; If the relative translation between the installation position of the IMU / RTK sensor and the origin O is known as vector t OI , and the relative rotation is matrix R OI , then the physical installation parameters of the IMU / RTK coordinate system obtained by the IMU / RTK sensor with respect to the ship-fixed coordinate system O can be described by the spatial transformation matrix T OI as follows: If both the LiDAR sensor and the camera sensor know the physical installation parameter T OL and T OC , where L and C represent the LiDAR coordinate system and the camera coordinate system respectively, then the spatial transformation matrix T CI between the camera sensor and the IMU / RTK sensor, and the spatial transformation matrix T CL between the camera sensor and the LiDAR sensor can both be obtained through matrix operations: T CI = T CO · T OI T CL = T CO · T OL If the physical installation parameters of the IMU / RTK sensor, LiDAR sensor, and camera sensor relative to the ship-fixed coordinate system O are unknown, the spatial transformation matrix T between the camera sensor and the IMU / RTK sensor is directly obtained through the first joint calibration tool CI , and the spatial transformation matrix T between the camera sensor and the LiDAR sensor is directly obtained by using the second joint calibration tool CL ; the external parameters of the IMU / RTK sensor, LiDAR sensor, and camera sensor are converted through the spatial transformation matrix: T CW = T CL · T LW = T CI · T IW Among them, T CW represents the spatial transformation matrix from the world coordinate system W to the camera coordinate system C, and T CL represents the spatial transformation matrix from the radar coordinate system L to the camera coordinate system C, and T LW represents the spatial transformation matrix from the world coordinate system W to the radar coordinate system L, and T CI represents the spatial transformation matrix from the IMU coordinate system I to the camera coordinate system C, and T IW represents the spatial transformation matrix from the world coordinate system W to the IMU coordinate system I; Step S2.1.2: The ESKF fusion camera external parameter estimation node receives the three-dimensional point cloud topic, the IMU topic, and the latitude, longitude and altitude position topic, and initializes the world coordinate system by using the latitude, longitude and altitude position at the initial moment, that is, uses the longitude, latitude and altitude at the initial moment as the origin of the northeast celestial world coordinate system W, and performs geographical position conversion between the latitude, longitude and altitude position topic and the origin of the northeast celestial world coordinate system W; Step S2.1.3: The geographical position conversion uses the library functions reset and the longitude, latitude and altitude at the initial moment to set the origin of the northeast celestial world coordinate system ENU, uses the library function forward to obtain the metric coordinates relative to the origin coordinates, and encodes the longitude, latitude and altitude at the initial moment in the form of a ROS standard three-dimensional vector message, and publishes the world coordinate system origin topic.

5. An intelligent ship augmented reality navigation aid information display method according to claim 3, characterized in that the specific content of publishing the camera external parameter topic is as follows: Step S2.2.1: The ESKF fusion camera extrinsic parameter estimation node divides the system state to be estimated into the true state nominal state error state δx, and the relationship is as follows: Step S2.2.2: Construct a prediction model of the error state δx in continuous time with the IMU attitude matrix R, velocity vector v, displacement vector p, acceleration bias vector b a and angular velocity bias vector b g as parameters where, F t is the linear state transition matrix under continuous time, B t is the measurement noise matrix under continuous time, and w is the measurement noise; Step S2.2.3: Obtain an attitude construction error state observation model based on the latitude-longitude-altitude position topic and the LiDAR point cloud topic: y = G t ·δx + C t ·n where G t is a linear observation matrix in continuous time, C t is an observation noise matrix in continuous time, n is the observation noise, y is the error state observation in continuous time, the displacement error observation δp is provided by the metric coordinates obtained from the transformation of the geodetic position topic, and the Lie algebra δθ of the attitude error observation is provided by the LiDAR point cloud matching, where the LiDAR point cloud matching uses a variant algorithm to calculate the attitude change at the current observation moment relative to the previous observation moment: where n δp and n δθ represent the observation errors of displacement and attitude respectively, and I 3 represents the 3D identity matrix; Step S2.2.4: Discretize the ESKF prediction model and the observation model y with the state recursion period T to obtain a discretized model: δx k = F k-1 δx k-1 + B k-1 w k y k = G k δx k + C k n k where, δx k , δx k-1 represents the recursive error states at discrete times k and k - 1, F k-1 is the linear state transition matrix at discrete time k - 1, B k-1 is the measurement noise matrix at discrete time k - 1, w k is the measurement noise at discrete time k, I 15 represents the 15 - dimensional identity matrix, R k-1 represents the attitude matrix at discrete time k - 1, y k is the error state observable at discrete time k, G k is the linear observation matrix at discrete time k, C k is the observation noise matrix at discrete time k, n k is the observation noise at discrete time k; Step S2.2.5: Recursively calculate and correct the error state and its covariance based on the above discretized model: wherein, is the predicted error state at time k, is the posterior error state at time k-1, is the covariance of the predicted error state at time k, is the covariance of the posterior error state at time k-1, The superscript T in represents the transpose of the matrix, and Q k is the covariance of the measurement noise at time k, and K k is the Kalman gain at time k, and R k is the covariance of the observation noise at time k, is the corrected posterior error state, is the corrected covariance of the posterior error state, and I is the identity matrix with the same dimension as, and at each time when the k observation y is obtained, calculate the posterior error state Derivation of the nominal state according to the median integral : Among them, represents the posterior attitude matrix at time k-1, represents the predicted attitude matrix at time k, φ is the three-dimensional rotation vector from time k-1 to k, and the symbol (·) ∧ represents the skew-symmetric matrix, ω k , ω k-1 represent the angular velocities at discrete times k and k-1, represents the deviation of the angular velocities at discrete times k and k-1, Δt is the time interval, represents the posterior velocity vector at time k-1, represents the predicted velocity vector at time k, a k , a k-1 represent the accelerations at discrete times k and k-1, represents the deviation of the accelerations at discrete times k and k-1, g is the local acceleration due to gravity, represents the posterior displacement vector at time k-1, represents the predicted displacement vector at time k; According to the superposition relationship of the true state, nominal state, and error state, the nominal state is corrected to obtain the true state at the current moment Among them, all physical quantities are those at time k. is the nominal displacement vector, is the posterior correction value of the displacement vector error, is the true displacement vector, is the nominal velocity vector, is the posterior correction value of the velocity vector error, is the true velocity vector, is the nominal attitude matrix, is the skew-symmetric matrix of the posterior correction value of the rotation vector error, is the true rotation matrix, is the nominal acceleration deviation, is the posterior correction value of the acceleration deviation error, is the true acceleration deviation, is the nominal angular velocity deviation, is the posterior correction value of the angular velocity deviation error, is the true angular velocity deviation; The true displacement vector and the true attitude matrix together constitute the external camera parameter matrix T at time k k : where T CW, k is the transformation matrix of the camera coordinate system C relative to the ENU world coordinate system W at time k; Step S2.2.6: The camera coordinate system C encodes T in the ROS standard odometry message format nav_msgs / Odometry and publishes the camera extrinsic parameter topic. CW, k 6. An intelligent ship augmented reality navigation aid information display method according to claim 3, wherein, the specific process of publishing the rendered three-dimensional coordinate group topic in step S3 is as follows: Step S3.1: For any geometric vertex data coordinate P in the 3D coordinate group topic V , use the library function forward in the open-source geographic calculation library GeographicLib to obtain the metric coordinate P of this vertex data V in the metric system W in the world coordinate system W , where x W , y W , z W represent the metric coordinates in the three directions of E, N, and U. Use the transformation matrix T of the camera system C relative to the ENU world coordinate system W in the camera extrinsic parameter topic data CW to perform a 3D coordinate transformation on the geometric vertex data: where, R CW represents the rotation matrix of the camera system C with respect to the ENU world system W, and t CW represents the displacement vector of the camera system C with respect to the ENU world system W, 0 T represents the transpose of a three-dimensional zero vector. Applying the three-dimensional space transformation T CW to all the coordinate data in the three-dimensional coordinate group topic yields a rendered three-dimensional coordinate group, P C represents any geometric vertex data in the rendered three-dimensional coordinate group in the camera coordinate system; Step S3.2: Encode the categories of the geometric vertex data in the rendered three-dimensional coordinate group according to the same type message format of the three-dimensional coordinate group, and publish the rendered three-dimensional coordinate group topic.

7. An intelligent ship augmented reality navigation aid information display method according to claim 1, wherein, the specific display of the AR navigation aid information in step S4 is as follows: Step S4.1: The AR navigation information rendering and display node reads the camera internal parameters f in the camera internal parameter topic x , f y , c x , c y , k i , p i , where f x , f y is the scaling transformation coefficient from any three-dimensional space coordinate point in the camera system C to the pixel plane, and c x , c y is the translation transformation coefficient from the space point in the camera system to the pixel plane; k i represents the radial distortion correction parameter, and p i represents the tangential distortion correction parameter; Step S4.2: The AR navigation aid information rendering and display node reads the current frame image in the RGB image topic, and obtains the height h and width w of this frame of image; Step S4.3: Using image processing technology, according to the radial distortion correction parameter k i and the tangential distortion correction parameter p i perform distortion correction on the original RGB image; Step S4.4: Using the camera intrinsic parameter f x ,f y ,c x ,c y , the height h and width w of the image, and the near clipping plane Z of the projection frustum in the 3D rendering engine n and the far clipping plane Z f Construct a rendering projection matrix K that conforms to the camera's intrinsic parameters and the 3D graphics engine: Step S4.5: Render any geometric vertex data P in the 3D coordinate group topic data C , and use the projection matrix K to transform the 3D geometric coordinate P to be rendered C to the pixel point P in the image plane coordinate system I : P I = K·P C Step S4.6: Select a predefined geometric drawing method according to the coordinate group category to obtain a pixel coordinate group, and superimpose the pixel coordinate group on the corrected RGB image to complete the rendering and display of the AR navigation aid information.

Images