Ship hydrogenation mechanical arm system and hydrogenation docking positioning control method

Abstract

The present application belongs to the technical field of ship hydrogenation, and specifically provides a ship hydrogenation mechanical arm system and a hydrogenation docking positioning control method, which comprises: initial calibration; multi-sensor data fusion through a federal learning framework by using vision, radar and IMU; calculation of ship body sway displacement based on IMU secondary integral, and solution of mechanical arm end target pose containing compensation; segmented trajectory planning by using RRT algorithm and quintic polynomial interpolation algorithm; real-time compensation of joint gap error based on LSTM neural network; and optimization of long-term accuracy through self-calibration iterative learning control. The present application aims to solve the problems of low hydrogenation docking accuracy, poor efficiency and insufficient safety caused by ship sway and mechanical arm error in the prior art, and realizes full-process automatic and accurate docking of ship hydrogenation.

Description

Technical Field

[0001] This invention belongs to the field of ship hydrogen refueling technology, specifically, it relates to a ship hydrogen refueling robotic arm system and a hydrogen refueling docking and positioning control method. Background Technology

[0002] Currently, ship hydrogen refueling operations mostly rely on manual labor. Existing technologies have many shortcomings and cannot meet the requirements of large-scale commercial operation and safe operation, as follows: 1. Low level of automation and low operating efficiency: The existing ship hydrogen refueling robotic arms have limited automation levels. The core docking process requires full human intervention, which is not only labor-intensive but also affected by factors such as operator experience, ambient light, and weather (such as rain, fog, and strong light). The docking process is cumbersome and the time required for a single hydrogen refueling is long, which cannot meet the needs of continuous and rapid hydrogen refueling for multiple ships and restricts the large-scale promotion of hydrogen-powered ships.

[0003] 2. Insufficient positioning accuracy and low docking success rate: Ships are affected by natural factors such as wind, waves and currents in the water, which will produce continuous rolling, pitching and heave. It is difficult for manual operation to capture and compensate for this dynamic displacement in real time, which will easily cause deviation when the hydrogen refueling nozzle docks with the hydrogen refueling port on the side of the ship. This will not only cause docking failure, but may also cause the hydrogen refueling nozzle to collide with the hull, damaging the hydrogen refueling equipment or the hull structure.

[0004] 3. Insufficient safety guarantees and potential safety hazards: The existing robotic arm lacks a sound safety protection mechanism. It has neither effective anti-collision warning and braking measures nor a reliable locking device for the hydrogen refueling process. Once docking is misoperated or a misoperation occurs, it may lead to safety accidents such as hydrogen leakage and equipment damage, which seriously threaten the safety of personnel and ships.

[0005] 4. Weak error compensation capability and unstable long-term accuracy: During the movement of the robotic arm, factors such as joint clearance, friction loss, and load deformation will generate positioning errors, and the errors will accumulate after multiple dockings. Existing technologies lack effective error compensation and self-calibration mechanisms, which leads to a continuous decline in the positioning accuracy of the robotic arm after long-term operation, making it unable to stably meet the requirements of high-precision hydrogen refueling docking.

[0006] 5. Poor environmental adaptability and limited applicable scenarios: The existing robotic arm's perception system mostly uses a single sensor, which is prone to perception failure in complex operating environments such as fog, backlight, and seawater fog, leading to the interruption of the docking process and making it unsuitable for various port and ship hydrogen refueling scenarios under various weather conditions. Summary of the Invention

[0007] The technical problem to be solved by the present invention is to provide a ship hydrogen refueling robotic arm system and a hydrogen refueling docking and positioning control method to achieve precise positioning and docking of the hydrogen refueling nozzle and improve hydrogen refueling efficiency.

[0008] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: a method for controlling the hydrogen refueling docking and positioning of a ship hydrogen refueling robotic arm, comprising the following steps: S1. Initial calibration: For the robotic arm that is put into use for the first time, the DH parameters are calibrated. The corrected DH parameters, the standard three-dimensional coordinate model of the hydrogen filling hole of the robotic arm, and the reference value of the coordinate system of the robotic arm base are used as the initial reference for subsequent real-time calculations. S2. Real-time perception and data fusion of multiple sensors: Real-time acquisition of environmental data through visual cameras, lidar and IMU, construction of state space equations using federated learning framework, fusion of multi-source observation data from visual cameras, lidar and IMU, and output of accurate preset values ​​of robotic arm end pose. S3. Calculation of real-time hull rolling displacement and dynamic pose determination: The real-time hull rolling displacement ΔP is obtained by performing a second integral calculation on the hull acceleration data collected by the IMU. ship (t); Establish the pose equation of the robotic arm end effector, including hull sway compensation, and solve for the real-time target pose P of the robotic arm end effector docking with the hydrogen refueling port. end ; S4. Segmented trajectory planning: using the real-time target pose P end For the target pose, a segmented trajectory planning process is performed on the robotic arm, first coarsely and then finely, to generate the complete motion path plan for the robotic arm. S5. Real-time compensation of kinematic errors: Establish a joint gap error model, predict the joint motion trend based on LSTM neural network, calculate the angle compensation value Δθi of each joint, and superimpose the compensation value into the target motion angle of each joint of the robotic arm; S6. Self-calibration iterative learning control: Record the end position error of the (k-1)th docking, calculate the joint angle correction, and add the joint angle correction to the joint target angle of the kth docking.

[0009] In a preferred embodiment, step S2 includes the following steps: S201. Perform time synchronization calibration on the vision camera, lidar and IMU; S202. The infrared positioning mark of the hydrogen refueling port on the side of the ship is identified by a vision camera to obtain the two-dimensional visual coordinates of the hydrogen refueling port on the side of the ship; the original ranging value is obtained by scanning with lidar, and the ranging error is corrected by combining the ship's roll angle monitored by IMU and using the mapping relationship to obtain the accurate ranging value; the ship's roll, pitch, heading angle and the vibration data of the robotic arm base are collected in real time by IMU, and the original data of the ship's attitude angle and acceleration are output. S203. Input the visual coordinates, laser ranging, and IMU angle data into the state space equation of the federated learning framework for fusion.

[0010] In the preferred embodiment, the formula for calculating the precise distance measurement value in step S202 is: d corr =d measured (1+k sin(θ)); Where: d corr Indicates the precise distance measurement value; d measured θ represents the original distance measurement value; θ represents the hull roll angle monitored by the IMU; k is the sway coefficient, with a value of 0.01~0.03.

[0011] In the preferred embodiment, the real-time sway displacement ΔP of the hull in step S3 is... ship The specific calculation of (t) is as follows: The three-dimensional acceleration data of the ship's hull acquired by the IMU is subjected to low-pass filtering; the filtered acceleration data is then integrated once in the time dimension to obtain the real-time velocity of the ship's motion. By performing a second integration of the ship's real-time velocity along the time dimension and combining it with the ship's initial position reference value, the three-dimensional displacement of the ship at time t is obtained, i.e., the real-time rolling displacement ΔP of the ship. ship (t).

[0012] In a preferred embodiment, the expression for the pose equation of the robotic arm's end effector in step S3 is: P end =T base T arm (q) P tool +ΔP ship (t); Among them, P end Let T be the target pose of the robotic arm's end effector at time t; base T is the transformation matrix from the robot arm's base coordinate system to the world coordinate system; arm (q) is the kinematic transformation matrix corresponding to the joint angle q of the robotic arm, q=[q1, q2, ..., q3]. n ] T n is the number of joints in the robotic arm, calculated from the real-time angles of each joint; P tool Here, ΔP represents the coordinates of the origin of the hydrogen refueling nozzle tool coordinate system relative to the end flange of the robotic arm; ship (t) represents the real-time swaying displacement of the ship at time t.

[0013] In a preferred embodiment, in step S4, when the distance between the end of the robotic arm and the hydrogen refueling port on the side of the ship is greater than a set distance, RRT is used. The algorithm generates the optimal path; when the distance is less than the set distance, it switches to the fine-tuning stage and uses a fifth-order polynomial interpolation algorithm to generate a local path.

[0014] In the preferred embodiment, the RRT The specific execution steps of the algorithm include: 1) Initialization: Using the current end-effector pose of the robotic arm as the starting node and the Pend calculated in step S3 as the target node, construct a spatial constraint boundary that includes the obstacle region; 2) Random sampling and node expansion: Randomly generate sampling points in the motion space, and extend from the nearest node of the current random tree to the sampling points to generate new nodes; 3) Reselect parent node: Calculate the cost of all neighboring nodes of the new node, select the node with the lowest cost as the parent node of the new node, and optimize the path length; 4) Path smoothing and termination: When the random tree expands to the vicinity of the target node, i.e. the distance is ≤ the set distance, sampling stops, the starting node and the target node are connected, and a global collision-free optimal path is generated in the coarse adjustment stage.

[0015] In the preferred embodiment, the trajectory equation of the fifth-order polynomial interpolation algorithm in step S4 is: S t = a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5； Wherein: S t The purpose is to fine-tune the displacement / angle of a certain joint of the robotic arm at time t; t is the motion time of the fine-tuning stage, with a value range of [0,T], and T is the total motion time of the fine-tuning stage; a0, a1, a2, a3, a4, and a5 are the coefficients of the fifth-degree polynomial, which are the parameters to be solved.

[0016] In the preferred embodiment, the constraints of the trajectory equation are: at the start of fine-tuning, the relative displacement of the joint is 0; at the end of fine-tuning, the joint completes all target relative displacements; at the start and end of fine-tuning, the joint motion velocity is 0; at the start and end of fine-tuning, the joint motion acceleration is 0, expressed as: S(0) = 0; S(1) = 1; S′(0)=S′(1)=0; S′′(0)=S′′(1)=0; Where S(t) is the displacement or angle of a joint of the fine-tuning robot arm at time t, S′(t) represents the joint motion velocity, and S′′(t) represents the joint motion acceleration. Substituting the six constraints into the trajectory equation yields six linear equations. By solving these linear equations, the values ​​of a0, a1, a2, a3, a4, and a5 are uniquely determined.

[0017] In a preferred embodiment, the joint gap error model in step S5 is: Δθi = αi·sgn(vi) + βi·|vi|; Where Δθi is the angle compensation value of the i-th joint, αi is the clearance characteristic coefficient of the i-th joint, sgn(vi) is the sign function representing the direction of joint movement, βi is the velocity characteristic coefficient of the i-th joint, and vi is the real-time movement velocity of the i-th joint.

[0018] In a preferred embodiment, in step S6, the joint angle correction amount is calculated using the joint angle correction amount formula, which is: Δqk = Γ·∫θ^T e(k-1)(τ)·Ψ(τ)dτ; Where Δqk is the joint angle correction matrix for the k-th docking, Γ is the learning gain matrix, ∫ represents the integral over the total time of a single docking [0,T], and ek 1(τ) represents the end position error at time τ during the (k-1)th docking, and Ψ(τ) is the basis function matrix, which represents the mapping relationship between the end position error and the angles of each joint.

[0019] The present invention also provides a ship hydrogenation robotic arm system for performing the above-described method, comprising: The robotic arm body adopts a two-stage control structure, including a coarse adjustment module for large-scale position adjustment and a fine adjustment module for precise alignment with the hydrogen filling port; The sensing module includes a hydrogen refueling port positioning marker installed on the side of the ship's hydrogen refueling port, a vision camera and a lidar installed at the front end of the robotic arm, and pressure probes installed on the top and side of the robotic arm, which are used to acquire images and distance information of the hydrogen refueling port positioning marker. An IMU installed on the hull is used to collect the hull's roll, pitch and heading angle data in real time. The sensor fusion module fuses multi-source observation data from vision cameras, lidar, and IMU to output accurate end-effector pose observations. The control module is used to perform dynamic pose calculation based on the data from the sensor fusion module and control the movement of the robotic arm.

[0020] In a preferred embodiment, the coarse adjustment module includes a coarse adjustment hydraulic drive device, a coarse adjustment rotary actuator, and a coarse adjustment robotic arm. The coarse adjustment hydraulic drive device is mounted on the base of the ship hydrogenation robotic arm, drives the coarse adjustment rotary actuator to rotate, and the coarse adjustment robotic arm is mounted on the coarse adjustment rotary actuator. The fine-tuning module includes a fine-tuning servo drive device, a fine-tuning rotation execution component, and a fine-tuning robotic arm. The fine-tuning servo drive device is installed at the tail end of the coarse-tuning robotic arm. The fine-tuning servo drive device drives the fine-tuning rotation execution component to rotate. The fine-tuning robotic arm is installed on the fine-tuning rotation execution component. The hydrogen refueling nozzle is fixed to the tail end of the fine-tuning robotic arm by a fastener, and the vision camera and lidar are installed at the tail end of the fine-tuning robotic arm.

[0021] In a preferred embodiment, the sensing module further includes a ship attitude monitoring camera installed on the base of the ship's hydrogen refueling robotic arm, used to observe whether the ship has docked and the ship's real-time position and attitude.

[0022] The present invention provides a ship hydrogen refueling robotic arm system and a hydrogen refueling docking positioning control method, which have the following beneficial effects: 1. By using multi-sensor fusion, dynamic compensation for hull sway, real-time error compensation, and self-calibration iterative learning control, the positioning deviation caused by ship sway and joint gaps is effectively solved, which greatly improves the docking accuracy and success rate of the hydrogen refueling nozzle and the hydrogen refueling port, and avoids equipment collision damage to the hull.

[0023] 2. By combining the joint gap error model, LSTM neural network prediction and self-calibration iterative learning mechanism, we can achieve instant error compensation for single docking and optimization of cumulative error for multiple dockings, solve the problem of error accumulation, and ensure the stability of positioning accuracy of the robotic arm during long-term operation.

[0024] 3. The robotic arm adopts a two-stage control structure of coarse adjustment and fine adjustment. Coarse adjustment enables rapid approximation over a wide range, while fine adjustment enables high-precision alignment. The trajectory planning adopts a segmented design, which balances work efficiency and positioning accuracy, and improves the overall work experience.

[0025] 4. Achieve fully automated control of the entire process from initial calibration, real-time perception, trajectory planning to docking completion, without the need for manual intervention, reducing the impact of human factors, simplifying the docking process, shortening the time for a single hydrogen refueling, meeting the continuous and rapid hydrogen refueling needs of multiple ships, and helping to promote the large-scale promotion of hydrogen-powered ships. Attached Figure Description

[0026] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, do not constitute an undue limitation of the invention. In the drawings: Figure 1 This is a schematic diagram of the structure of the robotic arm body of the present invention; Figure 2 This is a hardware architecture diagram of a robotic arm system; Figure 3 Flowchart of the docking control method; In the diagram: 1. Ship hydrogen refueling robotic arm base; 2. Ship attitude monitoring camera; 3. Coarse adjustment hydraulic drive device; 4. Coarse adjustment rotation actuator; 5. Coarse adjustment robotic arm; 6. Fine adjustment servo drive device; 7. Fine adjustment rotation actuator; 8. Fine adjustment robotic arm; 9. Vision camera; 10. Fixture; 11. Pressure probe; 12. Safety lock; 13. Hydrogen refueling nozzle; 14. Ship side pressure probe; 15. Safety lock interface; 16. Ship side hydrogen refueling port; 17. Hydrogen refueling port positioning marker; 18. LiDAR. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0028] Example 1: The ship hydrogen refueling robotic arm system provided by this invention is used to execute the following hydrogen refueling docking and positioning control method. It includes a robotic arm body, a sensing module, a sensor fusion module, and a control module. These modules work together to achieve precise docking of the hydrogen refueling nozzle with the ship's hydrogen refueling port and safe hydrogen refueling. The specific structure is as follows: 1. Robotic arm body like Figure 1 As shown, a two-stage control structure is adopted, including a coarse adjustment module and a fine adjustment module, which takes into account both work efficiency and positioning accuracy.

[0029] (1) Coarse adjustment module: It consists of a coarse adjustment hydraulic drive device, a coarse adjustment rotation actuator and a coarse adjustment robotic arm. The coarse adjustment hydraulic drive device is installed on the base of the ship hydrogenation robotic arm and is used to drive the coarse adjustment rotation actuator to rotate. The coarse adjustment robotic arm is installed on the coarse adjustment rotation actuator and achieves a wide range of rotation under the drive of the coarse adjustment rotation actuator to complete the initial positioning of the hydrogenation nozzle.

[0030] It adopts electro-hydraulic servo drive (servo motor power 7.5kW, hydraulic pump displacement 16mL / r), and the joint stiffness is ≥500N. m / rad, repeatability ±0.5mm.

[0031] (2) Fine adjustment module: It consists of a fine adjustment servo drive device, a fine adjustment rotation execution component and a fine adjustment robotic arm. The fine adjustment servo drive device is installed at the tail end of the coarse adjustment robotic arm and is used to drive the fine adjustment rotation execution component to rotate. The fine adjustment robotic arm is installed on the fine adjustment rotation execution component to realize high-precision fine adjustment of the hydrogen refueling nozzle.

[0032] Adopting a direct-drive servo motor (torque 3N) m, encoder resolution 23 bits), paired with a harmonic reducer (reduction ratio 100:1), end positioning accuracy ±0.05mm.

[0033] The arm body is made of 7075 aerospace aluminum alloy (density 2.8g / cm³), with anodized surface treatment, and the total weight is ≤80kg (including end effector).

[0034] The end effector includes a hydrogen refueling nozzle and an electromagnetic safety lock. The hydrogen refueling nozzle is fixed to the tail end of the fine-tuning robotic arm by a fastener. The hydrogen refueling nozzle has a built-in electromagnetic safety lock. Pressure probes are installed on the top and side of the robotic arm to achieve safe locking and collision warning during the hydrogen refueling process.

[0035] 2. Sensing Module The hardware architecture diagram of the robotic arm system is as follows: Figure 2 As shown, the sensing module is used to collect various environmental data, equipment status data, and ship attitude data during the hydrogen refueling operation, providing data support for precise docking. It includes hydrogen refueling port positioning markers, a vision camera, lidar, an IMU (inertial measurement unit), a ship attitude monitoring camera, and a pressure probe, as described in detail below: (1) Hydrogen refueling port positioning marker: installed on the side of the ship's hydrogen refueling port to provide a clear positioning reference for the vision camera.

[0036] (2) Vision camera: An industrial-grade binocular camera is used and installed at the tail end of the fine-tuning robotic arm. It is equipped with an 850nm infrared filter to identify hydrogen refueling port positioning markers and output the two-dimensional visual coordinates of the hydrogen refueling port on the ship side.

[0037] In this embodiment, an industrial-grade binocular camera (Basler ace 2, resolution 1624×1240, frame rate 60fps) is used, paired with an 850nm infrared filter, adaptable to day and night and foggy scenes, with a field of view of 65°×50°, a baseline distance of 120mm, and a 3D reconstruction accuracy of ≤0.15mm.

[0038] (3) LiDAR: An Ouster OS0-128 solid-state radar (range range 0.1-50m, angular resolution 0.1°×0.1°) is used, generating 128×3600 point cloud data per second. Redundant points are reduced through voxel filtering, and the point cloud registration error is ≤0.2mm. It is installed at the tail end of the fine-tuning robotic arm to scan the hydrogen refueling port and surrounding area to obtain raw ranging data.

[0039] (4) IMU (Inertial Measurement Unit): Installed on the hull, it is used to collect the hull's roll, pitch and yaw angles in real time and output the raw data of the hull's attitude angles and accelerations; (5) Ship attitude monitoring camera: installed on the base of the ship hydrogen refueling robotic arm, used to observe whether the ship is docked in place and the ship's real-time attitude, providing a basis for judgment for starting hydrogen refueling operations; (6) Pressure probes: Two are installed on the top and two on the side of the robotic arm, with a range of 0-10N, an accuracy of 0.05N, and a response time of ≤10ms. The hydrogen refueling nozzle has a built-in 6-axis force sensor (ATI Nano17), with a range of ±50N and a resolution of 0.01N, which is used to monitor the contact pressure between the hydrogen refueling nozzle and the hull and the force changes during the docking process in real time.

[0040] 3. Sensor Fusion Module: Employing a federated learning framework, this module fuses observation data from multiple sources, including visual cameras, LiDAR, and IMU, eliminating observation noise from individual sensors and outputting accurate pose observations of the robotic arm's end effector, providing reliable data input for subsequent pose calculation.

[0041] 4. Control Module: The core is used to receive data output from the sensor fusion module, perform dynamic pose calculation, trajectory planning, error compensation and safety control, drive the robotic arm to complete hydrogen docking and hydrogen refueling operations, and integrate a motion controller, fine-tuning servo drive, safety monitoring module and limit switch to realize full-process automated control.

[0042] Example 2: The hydrogen refueling docking positioning control method provided in this embodiment is based on the aforementioned ship hydrogen refueling robotic arm system. It is the core control logic for the precise docking of the ship hydrogen refueling robotic arm. The overall design revolves around the entire process of the robotic arm from its initial state to completing the docking at the hydrogen refueling port, and then to continuous self-calibration and optimization. The specific steps are as follows: S1, Initial Calibration For the robotic arm being used for the first time, an industrial-grade calibration plate with an accuracy of ±0.05mm is used, which is positioned to match the actual installation location of the hydrogen filling port on the side of the ship. The DH parameters of the robotic arm are calibrated, and the actual coordinates of each joint of the robotic arm in different poses are collected. The deviation between the theoretical and actual values ​​of the DH parameters such as the link length, joint angle, and link offset is corrected, and an accurate inverse kinematic model of the robotic arm is established. The corrected DH parameters, the standard three-dimensional coordinate model of the hydrogen filling port, and the reference values ​​of the coordinate system of the robotic arm base are pre-stored into the robotic arm control system as the initial reference for subsequent real-time calculations.

[0043] S2, Real-time Sensing with Multiple Sensors and Data Fusion The system collects environmental data in real time using visual cameras, LiDAR, and IMU. A state-space equation is constructed using a federated learning framework. Multi-source observation data from these devices are then fused to output precise preset pose values ​​for the robotic arm's end effector. The specific steps include: S201, Time Synchronization Calibration: Adopting the IEEE 1588 precision clock protocol, the timestamps of the vision camera (30fps), lidar (100Hz), and IMU (1000Hz) are unified to the main clock of the robotic arm control system, ensuring that the observation data of different sensors at the same time node can be fused, and the timestamp error is ≤1ms.

[0044] S202, Multimodal Positioning and Perception: The visual camera identifies the infrared positioning markers of the hydrogen refueling port on the side of the ship through an improved YOLOv8 network (with the addition of CBAM attention mechanism), and outputs the two-dimensional visual coordinates and recognition confidence of the hydrogen refueling port; the lidar scans the hydrogen refueling port and surrounding area to obtain the raw ranging value d. measured Combining the ship's roll angle θ monitored by the IMU, and through the mapping relationship d corr =d measured (1+k sin(θ) (where k is the sway coefficient, ranging from 0.01 to 0.03, calibrated according to the actual sea conditions at the port) is used to correct the ranging error and obtain the accurate ranging value d. corr The IMU collects real-time data on the ship's roll, pitch, and heading angles, as well as the vibration data of the robotic arm base, and outputs raw data on the ship's attitude angles and accelerations.

[0045] S203. Multi-sensor fusion: A federated learning framework is used to construct a state-space equation, which fuses multi-source observation data from vision, lidar, and IMU to eliminate observation noise from a single sensor and obtain an estimated value of the hydrogen refueling port on the ship's side. This value is used as a preset value for the end-effector pose, providing a data foundation for subsequent dynamic pose calculation.

[0046] The state-space equations are: x k = Fx k-1 +Gw k-1 ; z k = Hx k +v k ; Where: x k x represents the pose state of the robotic arm's end effector at the k-th time point; k-1 The state of the robotic arm's end effector at time point k-1 (the calculated value from the previous time step); F is the state transition matrix, representing the change in the robotic arm's end effector pose from k... The motion changes from time 1 to time k; G is the process noise transfer matrix, used to correct random errors in the robot arm joint motion; w k 1 represents process noise, which is the random error generated by joint friction, hydraulic fluctuations, etc. during the movement of the robotic arm, and follows a Gaussian distribution; z krepresents the multi-sensor fusion observation at the k-th time point (integrating visual coordinates, laser-corrected ranging, and IMU angle data); H is the observation noise transfer matrix, used to correct sensor observation errors; v k The observation noise is the inherent observation error of the vision, lidar, and IMU sensors, and follows a Gaussian distribution.

[0047] The execution logic is as follows: using the pose state x from the previous moment... k 1. Predict the current pose x k Then, by fusing the sensor observations z at the current moment... k The predicted values ​​are corrected, and the final output is an accurate observation of the robot arm's end-effector pose, providing a data foundation for subsequent dynamic pose calculation.

[0048] S3. Calculation of real-time swaying displacement and dynamic pose determination of the hull This step is the core calculation process for the robotic arm's motion control. It calculates the real-time swaying displacement of the hull through IMU integration, and establishes the pose equation of the robotic arm's end effector by combining the kinematic transformation matrix. The calculation results provide direct data guidance for the movement of the robotic arm's coarse adjustment module and fine adjustment module, providing a quantified pose target for the robotic arm to accurately align with the hydrogen refueling port.

[0049] The real-time sway displacement ΔP of the ship is obtained by performing a second integral calculation on the hull acceleration data collected by the IMU. ship (t); Establish the pose equation of the robotic arm end effector, including hull sway compensation, and solve for the real-time target pose P of the robotic arm end effector docking with the hydrogen refueling port. end Specifically, it includes the following steps: S301. Real-time sway displacement calculation: The three-dimensional acceleration data of the hull acquired by the IMU is low-pass filtered to remove high-frequency noise caused by seawater turbulence and sea wind vibration; the filtered acceleration data is integrated once in the time dimension to obtain the real-time motion velocity of the hull in the x, y, and z directions; the real-time velocity of the hull is integrated again, and combined with the hull position reference value at the initial moment, the three-dimensional displacement of the hull at time t is obtained, that is, the real-time sway displacement of the hull; the integrated displacement is corrected by the ranging results of the lidar to eliminate the cumulative error in the integration process.

[0050] The specific steps are as follows: Raw data preprocessing: Low-pass filtering is performed on the three-dimensional acceleration data of the ship hull acquired by the IMU to remove high-frequency noise caused by seawater turbulence and sea wind vibration; First integration: Integrate the filtered acceleration data along the time dimension to obtain the real-time velocity v of the hull in the x, y, and z directions. x (t), v y (t), vz (t); Double integration: Integrate the ship's real-time velocity again along the time dimension, and combine it with the ship's initial position reference value to obtain the ship's three-dimensional displacement at time t, i.e., the ship's real-time rolling displacement. ΔP ship (t)=[x ship (t), y ship (t), z ship (t)] T ; Error correction: The integral displacement is corrected by using the ranging results of the lidar to eliminate the accumulated error in the integration process and ensure the accuracy of the displacement calculation.

[0051] S302. Dynamic pose calculation: Establishing the pose equation of the robotic arm end effector considering hull sway: P end =T base T arm (q) P tool +ΔP ship (t); Among them, P end Let T be the target pose of the robotic arm's end effector at time t; base T is the transformation matrix from the robot arm's base coordinate system to the world coordinate system; arm (q) is the kinematic transformation matrix corresponding to the joint angle q of the robotic arm, q=[q1, q2, ..., q3]. n ] T n is the number of joints in the robotic arm, calculated from the real-time angles of each joint; P tool Here, ΔP represents the coordinates of the origin of the hydrogen refueling nozzle tool coordinate system relative to the end flange of the robotic arm; ship (t) represents the real-time sway displacement of the ship at time t, achieving dynamic compensation for the sway of the ship.

[0052] S4, Segmented Trajectory Planning P calculated in step S3 end To determine the target pose, the robotic arm undergoes segmented trajectory planning, first with coarse adjustments and then with fine adjustments, to generate a collision-free, smooth, complete motion path. Specifically, this includes the following steps: S401, Coarse Adjustment Stage: When the distance between the end of the robotic arm and the hydrogen inlet exceeds the set threshold (0.5m), RRT is used. The algorithm uses the current end-effector pose of the robotic arm as the starting node, P end For the target node, a spatial constraint boundary is constructed that includes obstacle areas such as the ship hull and port equipment. Paths are generated by random sampling and optimized to generate the global collision-free optimal path in the coarse adjustment stage, so as to achieve large-scale and rapid approach of the hydrogen refueling nozzle.

[0053] RRT The specific execution steps of the algorithm include: ① Initialization: Using the current end-effector pose of the robotic arm as the starting node, and using the P calculated in step three... end For the target node, construct the spatial constraint boundary for the movement of the robotic arm (including the obstacle area such as the ship hull and port equipment). ② Random sampling: Randomly generate sampling points within the motion space, with a sampling step size of 0.1m (balancing planning efficiency and path accuracy). ③ Node expansion: Extend from the nearest node in the current random tree to the sampling point to generate a new node. Determine whether the new node is within the obstacle area. If it is, discard it; otherwise, add it to the random tree. ④ Reselect parent node: Calculate the cost of all neighboring nodes of the new node, select the node with the lowest cost as the parent node of the new node, and optimize the path length; ⑤ Path smoothing: The generated random tree path is smoothed by setting the path smoothness parameter κ=0.8 to eliminate sharp corners in the path and ensure the smoothness of the robotic arm's movement; ⑥ Termination condition: When the random tree expands to the vicinity of the target node (distance ≤ 0.5m), sampling stops, the starting node and the target node are connected, and the global collision-free optimal path in the coarse adjustment stage is generated. The obstacle avoidance safety distance is set to 0.3m (to ensure that the robotic arm does not collide with the ship or obstacles during its movement).

[0054] S402, Fine-tuning Phase: When the distance between the robotic arm's end effector and the hydrogen refueling port is less than the set threshold (0.5m), switch to fine-tuning mode and use a fifth-order polynomial interpolation algorithm to generate a local path. The trajectory equation is: S t = a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 ; Wherein: S t The purpose is to fine-tune the displacement / angle of a certain joint of the robotic arm at time t; t is the motion time of the fine-tuning stage, with a value range of [0,T], and T is the total motion time of the fine-tuning stage; a0, a1, a2, a3, a4, and a5 are the coefficients of the fifth-degree polynomial, which are the parameters to be solved.

[0055] The constraints of the trajectory equation are: at the start of fine-tuning, the relative displacement of the joint is 0; at the end of fine-tuning, the joint has completed all target relative displacements; at the start and end of fine-tuning, the joint motion velocity is 0; at the start and end of fine-tuning, the joint motion acceleration is 0. The expression is: S(0)=0, the relative displacement of the joint is 0 at the start of fine-tuning; When S(1)=1, the joint completes all target relative displacements when the fine-tuning ends; S′(0)=S′(1)=0, the joint movement speed is 0 at the beginning and end of the fine adjustment (no start-up or stop impact); S′′(0)=S′′(1)=0, the joint motion acceleration is 0 at the beginning and end of the fine adjustment (no sudden acceleration, ensuring smooth motion).

[0056] Substituting the six constraints into the trajectory equation yields six linear equations. By solving these linear equations, the values ​​of a0, a1, a2, a3, a4, and a5 are uniquely determined. The specific solution is as follows: ① Substituting S(0)=0, we get a0=0; ② For S t Find the first derivative: S ′ t =a1+2a2t+3a3t 2 +4a4t 3 +5a5t 4 Substitute S ′ (0)=0, therefore a1=0; ③ For S t Find the second derivative: S ′′ t =2a 2 +6a3t+12a4t 2 +20a5t 3 Substitute S ′′ (0)=0, therefore a2=0; ④ Substitute S(1)=1, S... ′ (1) = 0, S ′′ (1)=0, we get a system of three linear equations in three variables a3, a4, a5, and solve them to get a3, a4, a5; ⑤ Finally, the fifth-order polynomial trajectory equation for the fine-tuning stage is obtained: S t =10t 3 15t 4 +6t 5 The equation is mapped to the actual motion displacement / angle of each joint of the robotic arm, generating a precise fine-tuning trajectory for each joint.

[0057] S5, Real-time compensation for kinematic errors A joint gap error model is established, the joint motion trend is predicted based on the LSTM neural network, the angle compensation value Δθi of each joint is calculated, and the compensation value is superimposed on the target motion angle of each joint of the robotic arm.

[0058] This step involves online real-time error compensation during a single docking operation of the robotic arm. It corrects joint clearance errors generated during the robotic arm's movement in real time. This complements the subsequent self-calibration iterative learning control, which follows a "single-time real-time compensation - multiple-iteration optimization" model. Real-time kinematic error compensation focuses on the instantaneous and static errors generated during joint movement in a single docking operation (such as joint clearance, load deformation, and instantaneous vibration), providing a safety net for the accuracy of the single docking. Self-calibration iterative learning control focuses on the accumulated and recurring systematic errors after multiple docking operations (such as wear on the robotic arm links and slight offsets in the hydrogen filling hole position). Through learning and iteration, it continuously optimizes the overall docking accuracy. The combination of these two approaches provides a dual guarantee for the robotic arm's docking accuracy.

[0059] In this embodiment, the expression for the joint gap error model is established as follows: Δθi = αi·sgn(vi) + βi·|vi|; Where Δθi is the angle compensation value of the i-th joint, αi is the clearance characteristic coefficient of the i-th joint, sgn(vi) is the sign function representing the direction of joint movement, βi is the velocity characteristic coefficient of the i-th joint, and vi is the real-time movement velocity of the i-th joint.

[0060] Based on the LSTM neural network, the gap error trend of each joint in the current motion state is predicted by taking the historical motion speed, angle and load change of the joint as input. The real-time motion speed vi of the joint is substituted into the error model to calculate the angle compensation value Δθi of each joint, which is superimposed on the target motion angle of each joint of the robotic arm to realize online real-time compensation of kinematic error.

[0061] The specific steps for error compensation are as follows: 1) Error prediction: Based on the LSTM neural network, the gap error trend of each joint in the current motion state is predicted by taking the historical motion speed, angle and load change of the joint as input, and the error prediction value is output in advance. 2) Error calculation: Substitute the real-time joint motion velocity vi into the joint gap error model to calculate the angle compensation value Δθi for each joint; 3) Real-time compensation: The angle compensation value Δθi is superimposed on the target motion angle of each joint of the robotic arm to correct the actual motion command of the joint and realize online real-time compensation of kinematic errors; 4) Precision assurance: After compensation, the joint positioning accuracy of the robotic arm is improved to ±0.05°, ensuring the motion accuracy of the robotic arm during a single docking process.

[0062] S6, Self-calibrating iterative learning control Record the end position error of the (k-1)th docking, calculate the joint angle correction, and add the joint angle correction to the joint target angle of the kth docking.

[0063] This step is an iterative correction process after multiple dockings of the robotic arm. Based on the end position error of the previous docking, the joint angle correction amount for this docking is calculated. Through "error learning-parameter correction" of multiple dockings, the motion trajectory of the robotic arm is continuously optimized, eliminating the accumulated systematic error after multiple dockings. It is suitable for the repeated docking needs of the same ship and the same hydrogen filling port in the marine hydrogen refueling scenario.

[0064] Specifically, record the end-effector position error e(k-1)(τ) at each time point during the (k-1)th docking process of the robotic arm, calculate the joint angle correction for the kth docking using the joint angle correction formula, and superimpose Δqk onto the joint target angle for the kth docking to correct the robotic arm's motion trajectory. Repeat the above steps, and through iterative learning from multiple dockings, gradually converge the end-effector position error to the minimum value, achieving self-optimization of accuracy.

[0065] The formula for joint angle correction is: Δqk = Γ·∫θ^T e(k-1)(τ)·Ψ(τ)dτ; Where Δqk is the joint angle correction matrix for the k-th docking; Γ is the learning gain matrix (n×n diagonal matrix), with diagonal elements ranging from 0.01 to 0.1, calibrated according to the motion response speed of the robotic arm; the larger the element value, the faster the learning iteration speed; ∫ represents the integration over the total time of a single docking [0,T], and ∫θ^T represents the integration of time τ over the total time of a single docking [0,T], representing the cumulative calculation of the error over the entire docking process; ek 1(τ) is the end position error (3D spatial coordinate error, 3×1) at time τ of the (k-1)th docking, which is collected in real time by the vision camera and lidar, i.e. the deviation between "actual position of hydrogen refueling nozzle and target position of hydrogen refueling port"; Ψ(τ) is the basis function matrix (n×3), which represents the mapping relationship between the end position error at time τ and the angles of each joint. It is derived from the Jacobian matrix of the robotic arm kinematics and is a fixed value; τ is the real-time time variable (unit: s) in a single docking process, with a value range of [0,T].

[0066] The specific execution steps of iterative learning control include: 1) Error Acquisition: Record the k-th error of the robotic arm. During a single docking process, the end position error at each time point τ is e(k-1)(τ). 2) Integral calculation: for e(k-1)(τ) Integrating Ψ(τ) over the time interval [0,T] yields the cumulative error value; 3) Solution of correction amount: Multiply the cumulative error value with the learning gain matrix Γ to obtain the joint angle correction amount Δqk for the kth docking; 4) Trajectory correction: Δqk is superimposed on the joint target angle of the kth docking to correct the motion trajectory of the robotic arm and eliminate the error generated by the previous docking; 5) Iterative Loop: Repeat the above steps and through multiple docking iterations, gradually reduce the end position error of the robotic arm to a minimum, thereby continuously improving docking accuracy.

[0067] S7, Security Control After the hydrogen refueling nozzle is successfully docked, the electromagnetic safety lock automatically locks with the hydrogen refueling port to prevent accidental detachment. The pressure probe monitors the contact pressure in real time. When the pressure exceeds the preset threshold or the force gradient exceeds 1 N / s, the system automatically triggers an alarm and retracts the hydrogen refueling nozzle. At the same time, the electromagnetic brake is activated to achieve emergency stop protection. During the movement of the robotic arm, soft and hard limits provide dual protection to prevent the robotic arm from exceeding its range of motion and colliding with the hull.

[0068] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for controlling the hydrogen refueling docking and positioning of a ship hydrogen refueling robotic arm, characterized in that, Includes the following steps: S1. Initial calibration: For the robotic arm that is put into use for the first time, the DH parameters are calibrated. The corrected DH parameters, the standard three-dimensional coordinate model of the hydrogen filling hole of the robotic arm, and the reference value of the coordinate system of the robotic arm base are used as the initial reference for subsequent real-time calculations. S2. Real-time perception and data fusion of multiple sensors: Real-time acquisition of environmental data through visual cameras, lidar and IMU, construction of state space equations using federated learning framework, fusion of multi-source observation data from visual cameras, lidar and IMU, and output of accurate preset values ​​of robotic arm end pose. S3. Calculation of real-time hull rolling displacement and dynamic pose determination: The real-time hull rolling displacement ΔP is obtained by performing a second integral calculation on the hull acceleration data collected by the IMU. ship (t); Establish the pose equation of the robotic arm end effector, including hull sway compensation, and solve for the real-time target pose P of the robotic arm end effector docking with the hydrogen refueling port. end ; S4. Segmented trajectory planning: using the real-time target pose P end For the target pose, a segmented trajectory planning process is performed on the robotic arm, first coarsely and then finely, to generate the complete motion path plan for the robotic arm. S5. Real-time compensation of kinematic errors: Establish a joint gap error model, predict the joint motion trend based on LSTM neural network, calculate the angle compensation value Δθi of each joint, and superimpose the compensation value into the target motion angle of each joint of the robotic arm; S6. Self-calibration iterative learning control: Record the end position error of the (k-1)th docking, calculate the joint angle correction, and add the joint angle correction to the joint target angle of the kth docking.

2. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 1, characterized in that, Step S2 includes the following steps: S201. Perform time synchronization calibration on the vision camera, lidar and IMU; S202. The infrared positioning mark of the hydrogen refueling port on the side of the ship is identified by a vision camera to obtain the two-dimensional visual coordinates of the hydrogen refueling port on the side of the ship; the original ranging value is obtained by scanning with lidar, and the ranging error is corrected by combining the ship's roll angle monitored by IMU and using the mapping relationship to obtain the accurate ranging value; the ship's roll, pitch, heading angle and the vibration data of the robotic arm base are collected in real time by IMU, and the original data of the ship's attitude angle and acceleration are output. S203. Input the visual coordinates, laser ranging, and IMU angle data into the state space equation of the federated learning framework for fusion.

3. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 2, characterized in that, In step S202, the formula for calculating the precise distance measurement value is: d corr =d measured (1+k sin(θ); Where: d corr Indicates the precise distance measurement value; d measured θ represents the original distance measurement value; θ represents the hull roll angle monitored by the IMU; k is the sway coefficient.

4. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 1, characterized in that, In step S3, the real-time sway displacement ΔP of the hull ship The specific calculation of (t) is as follows: The three-dimensional acceleration data of the ship's hull acquired by the IMU is subjected to low-pass filtering; the filtered acceleration data is then integrated once in the time dimension to obtain the real-time velocity of the ship's motion. By performing a second integration of the ship's real-time velocity along the time dimension and combining it with the ship's initial position reference value, the three-dimensional displacement of the ship at time t is obtained, i.e., the real-time rolling displacement ΔP of the ship. ship (t).

5. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 1, characterized in that, The expression for the pose equation of the robotic arm's end effector in step S3 is as follows: P end =T base T arm (q) P tool +ΔP ship (t); Among them, P end Let T be the target pose of the robotic arm's end effector at time t; base T is the transformation matrix from the robot arm's base coordinate system to the world coordinate system; arm (q) is the kinematic transformation matrix corresponding to the joint angle q of the robotic arm, q=[q1, q2, ..., q3]. n ] T n is the number of joints in the robotic arm, calculated from the real-time angles of each joint; P tool Here, ΔP represents the coordinates of the origin of the hydrogen refueling nozzle tool coordinate system relative to the end flange of the robotic arm; ship (t) represents the real-time swaying displacement of the ship at time t.

6. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 1, characterized in that, In step S4, when the distance between the end of the robotic arm and the hydrogen refueling port on the side of the ship is greater than a set distance, RRT is used. The algorithm generates the optimal path; when the distance is less than the set distance, it switches to the fine-tuning stage and uses a fifth-order polynomial interpolation algorithm to generate a local path.

7. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 6, characterized in that, The RRT The specific execution steps of the algorithm include: 1) Initialization: Using the current end-effector pose of the robotic arm as the starting node, and using the P calculated in step S3... end For the target node, construct a spatial constraint boundary that includes the obstacle region; 2) Random sampling and node expansion: Randomly generate sampling points in the motion space, and extend from the nearest node of the current random tree to the sampling points to generate new nodes; 3) Reselect parent node: Calculate the cost of all neighboring nodes of the new node, select the node with the lowest cost as the parent node of the new node, and optimize the path length; 4) Path smoothing and termination: When the random tree expands to the vicinity of the target node, i.e. the distance is ≤ the set distance, sampling stops, the starting node and the target node are connected, and a global collision-free optimal path is generated in the coarse adjustment stage.

8. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 6, characterized in that, The trajectory equation of the fifth-order polynomial interpolation algorithm in step S4 is: S t = a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 ; Wherein: S t The purpose is to fine-tune the displacement / angle of a certain joint of the robotic arm at time t; t is the motion time of the fine-tuning stage, with a value range of [0,T], and T is the total motion time of the fine-tuning stage; a0, a1, a2, a3, a4, and a5 are the coefficients of the fifth-degree polynomial, which are the parameters to be solved.

9. A method for controlling the hydrogen refueling docking and positioning of a ship hydrogen refueling robotic arm according to claim 8, characterized in that, The constraints of the trajectory equation are: at the start of fine-tuning, the relative displacement of the joint is 0; at the end of fine-tuning, the joint has completed all target relative displacements; at the start and end of fine-tuning, the joint motion velocity is 0; at the start and end of fine-tuning, the joint motion acceleration is 0. The expression is: S(0)=0； S(1)=1； S′(0)=S′(1)=0; S′′(0)=S′′(1)=0; Where S(t) is the displacement or angle of a joint of the fine-tuning robot arm at time t, S′(t) represents the joint motion velocity, and S′′(t) represents the joint motion acceleration. Substituting the six constraints into the trajectory equation yields six linear equations. By solving these linear equations, the values ​​of a0, a1, a2, a3, a4, and a5 are uniquely determined.

10. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 1, characterized in that, The joint gap error model in step S5 is as follows: Δθi = αi·sgn(vi) + βi·|vi|; Where Δθi is the angle compensation value of the i-th joint, αi is the clearance characteristic coefficient of the i-th joint, sgn(vi) is the sign function representing the direction of joint movement, βi is the velocity characteristic coefficient of the i-th joint, and vi is the real-time movement velocity of the i-th joint.

11. The method for hydrogen refueling docking and positioning control of a ship hydrogen refueling robotic arm according to claim 1, characterized in that, In step S6, the joint angle correction is calculated using the joint angle correction formula, which is as follows: Δqk = Γ·∫θ^T e(k-1)(τ)·Ψ(τ)dτ; Where Δqk is the joint angle correction matrix for the k-th docking, Γ is the learning gain matrix, ∫ represents the integral over the total time of a single docking [0,T], and ek 1(τ) represents the end position error at time τ during the (k-1)th docking, and Ψ(τ) is the basis function matrix, which represents the mapping relationship between the end position error and the angles of each joint.

12. A ship hydrogen refueling robotic arm system, characterized in that, For performing the method according to claims 1-11, comprising: The robotic arm body adopts a two-stage control structure, including a coarse adjustment module for large-scale position adjustment and a fine adjustment module for precise alignment with the hydrogen filling port; The sensing module includes a hydrogen refueling port positioning marker installed on the side of the ship's hydrogen refueling port, a vision camera and a lidar installed at the front end of the robotic arm, and pressure probes installed on the top and side of the robotic arm, which are used to acquire images and distance information of the hydrogen refueling port positioning marker. An IMU installed on the hull is used to collect the hull's roll, pitch and heading angle data in real time. The sensor fusion module fuses multi-source observation data from vision cameras, lidar, and IMU to output accurate end-effector pose observations. The control module is used to perform dynamic pose calculation based on the data from the sensor fusion module and control the movement of the robotic arm.

13. A ship hydrogen refueling robotic arm system according to claim 12, characterized in that, The coarse adjustment module includes a coarse adjustment hydraulic drive device, a coarse adjustment rotation actuator, and a coarse adjustment robotic arm. The coarse adjustment hydraulic drive device is mounted on the base of the ship hydrogenation robotic arm, and drives the coarse adjustment rotation actuator to rotate. The coarse adjustment robotic arm is mounted on the coarse adjustment rotation actuator. The fine-tuning module includes a fine-tuning servo drive device, a fine-tuning rotation execution component, and a fine-tuning robotic arm. The fine-tuning servo drive device is installed at the tail end of the coarse-tuning robotic arm. The fine-tuning servo drive device drives the fine-tuning rotation execution component to rotate. The fine-tuning robotic arm is installed on the fine-tuning rotation execution component. The hydrogen refueling nozzle is fixed to the tail end of the fine-tuning robotic arm by a fastener, and the vision camera and lidar are installed at the tail end of the fine-tuning robotic arm.

14. A ship hydrogen refueling robotic arm system according to claim 12, characterized in that, The sensing module also includes a ship attitude monitoring camera installed on the base of the ship's hydrogen refueling robotic arm, used to observe whether the ship has docked and the ship's real-time position and attitude.

Images