A marine automatic charging data communication system based on optical positioning

Abstract

The present invention relates to the technical field of charging positioning, and discloses a marine automatic charging data communication system based on optical positioning, comprising: an area division unit, an optical positioning unit, an offset calculation unit, a correction unit, a data fusion unit and a docking unit. Through the linkage of optical positioning and multiple units, the problem of automatic charging of unmanned ships in dense fog environments is solved. The visibility attenuation coefficient is calculated by laser data, and the laser power is adjusted as needed to ensure stable signal transmission in dense fog. Closed-loop collaboration from optical positioning to docking units uses positioning coordinates, attitude offset and other data to control the entire charging process, which can accurately offset the influence of water surface fluctuations. The safe area setting and dynamic adjustment of contact force during trajectory planning avoid rigid docking and reduce the loss of charging equipment. Compared with visual recognition relying on light, this solution can still achieve precise docking under low visibility, thereby improving the accuracy and stability of automatic charging.

Description

Technical Field

[0001] The present invention relates to the technical field of charging positioning, and in particular to a marine automatic charging data communication system based on optical positioning. Background Art

[0002] With the development of the times, various ships have become more intelligent, especially unmanned ships for various purposes. These unmanned ships are powered by electricity and equipped with robots, which enable them to automatically reach the dock and connect the charging gun to the charging pile with the assistance of the robot for automatic charging.

[0003] Existing automatic charging of unmanned boats usually uses visual recognition technology to correct the hull of the unmanned boat to adapt to the fluctuating water surface environment, and through high-precision visual recognition, the robotic driving arm of the unmanned boat drives the charging gun to connect to the charging port of the charging pile and automatically charge. However, in foggy environments, visual recognition technology still has significant technical defects in automatic charging. Specifically: in dense fog, the dense fog will severely attenuate light, causing the overall brightness of the image captured by the camera to be greatly reduced, resulting in the loss of key feature information during recognition, making it difficult to effectively identify the position of the unmanned boat and the position of the charging port and perform automatic charging. Summary of the Invention

[0004] In view of the deficiencies in the prior art, the present invention provides a marine automatic charging data communication system based on optical positioning, which solves the above problems.

[0005] The above technical objectives of the present invention are achieved through the following technical solutions:

[0006] A marine automatic charging data communication system based on optical positioning, comprising:

[0007] An area division unit is used to divide the charging pile into a circular charging area with a radius R, obtain laser data at the edge of the circular charging area, and calculate the current visibility attenuation coefficient of the circular charging area based on the laser data;

[0008] The optical positioning unit is used to correct the laser emission power according to the current visibility attenuation coefficient after the target object enters the circular charging area, and obtain the positioning coordinates of the target object, which is an unmanned vessel;

[0009] An offset calculation unit is used to obtain the hull data of the target object in the circular charging area and determine the hull attitude offset based on the positioning coordinates of the target object and the hull data;

[0010] A correction unit is used to analyze the hull attitude offset and the radius R of the circular charging area to generate a three-dimensional heading correction vector of the target object;

[0011] The data fusion unit is used to obtain the distance value between the charging gun and the charging port of the charging pile, and fuse the distance value, the positioning coordinates of the target object and the three-dimensional heading correction vector to obtain the motion trajectory of the charging gun;

[0012] The docking unit is used to control the charging gun to dock with the charging interface with a constant contact force according to the motion trajectory.

[0013] Furthermore, a circular charging area with a radius of R is divided according to the charging pile, and laser data of the edge of the circular charging area is obtained, including:

[0014] Take the charging pile as the center of the circle, and divide the water area based on the center of the charging pile, and divide the circular charging area with a radius of R between the charging pile and the water area;

[0015] The edge of the circular charging area is divided into multiple points, marked as edge points, and the data of the laser signal from emission to reflection after passing through the edge points and returning to the sensor is recorded in real time to obtain the laser data of the edge of the circular charging area.

[0016] Furthermore, the current visibility attenuation coefficient of the circular charging area is calculated based on the laser data, including:

[0017] Analyze the laser data to obtain multiple sets of laser round-trip time differences;

[0018] The actual propagation distance of the laser from the charging station to each edge point is calculated based on the round-trip time difference of multiple groups of lasers and the propagation speed of light in water;

[0019] Extract the laser data to obtain the intensity attenuation value of each group of laser signals;

[0020] Based on each set of laser signal intensity attenuation values, the light attenuation rate of each edge point is calculated;

[0021] Build a three-dimensional coordinate system based on the location of the charging pile, obtain the positioning coordinates of each edge point, calculate the spatial distance between any two edge points, and build a spatial weight matrix of the edge points based on the spatial distance;

[0022] The light attenuation rates of all edge points are fused based on the spatial weight matrix to obtain the weighted light attenuation rate. The weighted light attenuation rate is corrected according to the actual propagation distance to obtain the current visibility attenuation coefficient of the circular charging area.

[0023] Furthermore, the laser emission power is corrected according to the current visibility attenuation coefficient, and the positioning coordinates of the target object are obtained, including:

[0024] Determine the power correction factor based on the current visibility attenuation factor;

[0025] Correcting the initial laser emission power based on the power correction coefficient to obtain a corrected laser emission power;

[0026] Recording the data of the corrected laser from the time it is emitted to the time it is reflected by the edge point and then returned to the sensor, to obtain first data;

[0027] The first data is analyzed, and the target object is located to obtain the location coordinates of the target object.

[0028] Furthermore, based on the positioning coordinates of the target object and the hull data, the hull attitude offset is determined, including:

[0029] Analyze the positioning coordinates to generate the tangent direction vector of the target object's motion trajectory;

[0030] Establish a dynamic coordinate system based on the hull data and tangent direction vector, and generate the hull coordinate system transformation matrix;

[0031] Based on the hull coordinate system transformation matrix, time and space, the hull data is corrected to obtain the comprehensive attitude eigenvalue;

[0032] Analyze the comprehensive posture eigenvalues ​​to obtain the three-dimensional angle offset vector and the three-dimensional position offset vector;

[0033] The 3D angle offset vector and the 3D position offset vector are fused to obtain the hull attitude offset.

[0034] Furthermore, the ship attitude offset and the radius R of the circular charging area are analyzed to generate a three-dimensional heading correction vector of the target object, including:

[0035] Get water data of the circular charging area;

[0036] Based on the water body data, the weight of each deviation parameter in the hull attitude offset on the heading is adjusted to obtain the adaptive attitude influence matrix;

[0037] With the charging pile as the center, a three-dimensional space constraint field is constructed based on the radius R of the operation area;

[0038] Based on the real-time distance between the target object and the center of the charging area, the strength of the adaptive posture influence matrix and the regional constraint vector field are dynamically adjusted;

[0039] The adjusted adaptive attitude influence matrix is ​​fused with the regional constraint vector field to generate a three-dimensional heading correction vector.

[0040] Furthermore, the distance value, the positioning coordinates of the target object, and the three-dimensional heading correction vector are integrated to obtain the motion trajectory of the charging gun, including:

[0041] Synchronize and calibrate the distance value from the charging gun to the charging port of the charging pile, the positioning coordinates of the target object, and the acquisition timestamp of the three-dimensional heading correction vector to generate the calibrated distance value, positioning coordinates, and three-dimensional heading correction vector;

[0042] Analyze water body data to obtain error distribution characteristics under different water qualities;

[0043] Based on the error distribution characteristics, a dynamic noise adjustment matrix is ​​constructed;

[0044] The calibrated distance value, positioning coordinates and three-dimensional heading correction vector are fused based on the dynamic noise adjustment matrix to generate comprehensive position information.

[0045] Furthermore, the distance value, the positioning coordinates of the target object, and the three-dimensional heading correction vector are integrated to obtain the motion trajectory of the charging gun, which also includes:

[0046] Analyze the hull data and the motion limits of the robot drive arm to obtain the trajectory curvature limit standard;

[0047] According to the dynamic noise adjustment matrix, the trajectory curvature limit standard, the comprehensive position information and the three-dimensional heading correction vector are combined to generate a smooth preliminary trajectory;

[0048] According to the trajectory curvature limit standard, the contact force requirements corresponding to different curvature segments are calculated to obtain the contact force compensation at each point on the trajectory;

[0049] Fine-tune the preliminary trajectory based on the contact force compensation amount so that each segment of the trajectory can match the contact force requirements under the corresponding curvature, and obtain the compensated trajectory;

[0050] With the charging port as the center, a dynamically changing ellipsoidal safety zone is set according to the water flow direction and the length and diameter of the charging gun. The compensated trajectory is checked. If the distance between any point on the trajectory and the boundary of the safety zone is less than the safety threshold, secondary fine-tuning is performed to ensure that the trajectory is always within the ellipsoidal safety zone, and the motion trajectory of the charging gun is finally obtained.

[0051] Furthermore, the charging gun is controlled to dock with the charging interface with a constant contact force according to the motion trajectory, including:

[0052] Extract the instantaneous velocity, acceleration and curvature change rate of the charging gun's motion trajectory;

[0053] Obtain the material hardness of the charging gun and charging interface, and combine the instantaneous velocity, acceleration, trajectory curvature change rate, and the material hardness of the charging gun and charging interface to obtain the contact force reference value;

[0054] The contact force reference value is combined with the end buffer distance of the charging gun motion trajectory to obtain a contact force correction value, and a constant contact force is calculated based on the contact force reference value and the contact force correction value.

[0055] Furthermore, controlling the charging gun to dock with the charging interface with a constant contact force according to the motion trajectory also includes:

[0056] Obtain the contact pressure between the charging gun and the charging port in real time;

[0057] Calculate the contact pressure and constant contact force to obtain the force deviation parameter;

[0058] The motion drive parameters are dynamically adjusted according to the force deviation parameters until the charging gun is docked with the charging interface.

[0059] In summary, the present invention mainly has the following beneficial effects:

[0060] Through laser optical positioning and visibility attenuation coefficient calculation, the limitation of dense fog environment on the automatic charging of unmanned boats has been broken through, and the core defect of visual recognition technology has been solved. The visibility assessment method constructed by the m-domain division unit based on laser data can quantify the degree of attenuation of light by fog in real time, providing an accurate basis for the power correction of the optical positioning unit, ensuring that the laser signal can still be stably transmitted in low visibility. Compared with the visual recognition method that relies on image brightness and feature extraction, this solution can maintain positioning accuracy in dense fog environment through direct calculation of laser round-trip time difference and signal attenuation value, avoiding positioning failure caused by loss of key features. The entire process from laser data collection at the edge of the charging area to target object coordinate generation is independent of ambient light conditions, which improves the adaptability of unmanned boats to automatic charging in bad weather.

[0061] Through the closed-loop control system of multi-unit collaboration, the whole process from area identification to charging docking is precisely controlled, which greatly improves the reliability of automatic charging. Among them, the offset calculation unit combines the hull data with the dynamic coordinate system conversion to capture the posture offset caused by the surface fluctuation in real time. The data fusion unit effectively integrates the multi-source data errors through timestamp calibration and dynamic noise matrix to generate a smooth charging gun motion trajectory. The docking unit is based on the dynamic adjustment of the force deviation parameters to ensure that the charging gun completes the docking with a constant contact force. This kind of positioning, offset and dynamic control is realized. Correction Comprehensive control from trajectory to docking enables precise calculation of each link through parameterization, avoiding the uncertainty of subjective feature judgment in visual recognition. Even in complex environments with superposition of water flow disturbance and dense fog, docking accuracy can still be maintained.

[0062] By setting the ellipsoidal safety area and curvature limitation standards in the trajectory planning stage, the rigid collision between the charging gun and the interface can be effectively avoided. In addition, the contact force graded adjustment strategy in the docking stage can dynamically adapt the driving parameters of the robot driving arm to drive the charging gun according to the deviation level. Through data analysis, this solution can predict risks in advance and actively correct them, and control the contact force fluctuations between the charging gun and the interface, ensuring the stability of the unmanned boat during automatic charging, and can perform charging positioning through optics. In this process, data communication also ensures the accuracy of data analysis when the unmanned boat is preparing to charge. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 It is a schematic diagram of the marine automatic charging data communication system based on optical positioning of the present invention. DETAILED DESCRIPTION

[0064] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0065] refer to Figure 1 , a marine automatic charging data communication system based on optical positioning, comprising:

[0066] An area division unit is used to divide the charging pile into a circular charging area with a radius R, obtain laser data at the edge of the circular charging area, and calculate the current visibility attenuation coefficient of the circular charging area based on the laser data;

[0067] The optical positioning unit is used to correct the laser emission power according to the current visibility attenuation coefficient after the target object enters the circular charging area, and obtain the positioning coordinates of the target object, which is an unmanned vessel;

[0068] An offset calculation unit is used to obtain the hull data of the target object in the circular charging area and determine the hull attitude offset based on the positioning coordinates of the target object and the hull data;

[0069] A correction unit is used to analyze the hull attitude offset and the radius R of the circular charging area to generate a three-dimensional heading correction vector of the target object;

[0070] The data fusion unit is used to obtain the distance value between the charging gun and the charging port of the charging pile, and fuse the distance value, the positioning coordinates of the target object and the three-dimensional heading correction vector to obtain the motion trajectory of the charging gun;

[0071] The docking unit is used to control the charging gun to dock with the charging interface with a constant contact force according to the motion trajectory.

[0072] The visibility attenuation coefficient is calculated in real time through the area division unit, and the laser emission power of the optical positioning unit is dynamically corrected to ensure the accurate positioning of the unmanned vessel. The attitude offset is calculated in combination with the hull data and a three-dimensional heading correction vector is generated. The motion trajectory is obtained by integrating the charging gun distance data, and the charging gun is docked with a constant contact force, which effectively solves the problems of light attenuation and feature loss caused by dense fog, and improves the accuracy of automatic charging of unmanned vessels in complex water environments.

[0073] In one case of this embodiment, a circular charging area with a radius R is divided according to the charging pile, and laser data of the edge of the circular charging area is obtained, including:

[0074] Take the charging pile as the center of the circle, and divide the water area based on the center of the charging pile, and divide the circular charging area with a radius of R between the charging pile and the water area;

[0075] The edge area of ​​the circular charging area is divided into multiple points, marked as edge points, and the data of the laser signal from emission to reflection after passing through the edge point and returning to the sensor is recorded in real time. The laser data of the edge of the circular charging area can be obtained. Specifically, with the charging pile as the center of the circle, the 360-degree circumference of the edge of the circular charging area is divided into multiple sector-shaped areas at a fixed angle interval (5 degrees). The intersection of the edge line of each sector and the circumference is the edge point. The laser sensor is used to emit laser to each edge point, and the laser emission time, the time when the laser returns to the sensor after reflection after passing through the edge point, and the reflected signal strength are recorded. The above data is the laser data.

[0076] By precisely dividing the circular charging area and acquiring edge point laser data at fixed angle intervals, the light attenuation in the area can be captured in real time. Compared with solutions that rely on visual recognition, this can effectively deal with the problem of light attenuation in dense fog, avoid positioning failures due to feature loss, improve the system's adaptability in complex foggy waters, and ensure the stability of the charging docking process.

[0077] In one aspect of this embodiment, calculating the current visibility attenuation coefficient of the circular charging area based on the laser data includes:

[0078] Analyze the laser data to obtain multiple sets of laser round-trip time differences, specifically including: extracting the laser emission time and reflection return time corresponding to each edge point from the laser data, subtracting the laser emission time from the reflection return time of each edge point to obtain the laser round-trip time difference of a single edge point, and calculating all edge points once to obtain multiple sets of laser round-trip time differences;

[0079] The actual propagation distance of the laser from the charging pile to each edge point is calculated based on the round-trip time differences of multiple groups of lasers combined with the propagation speed of light in water. Specifically, the round-trip time difference of each group of lasers is divided by 2 to obtain the one-way propagation time. The one-way propagation time is multiplied by the propagation speed of light in water to obtain the actual propagation distance of the laser from the charging pile to the corresponding edge point. The round-trip time differences of the lasers at all edge points are calculated according to the above calculation steps to obtain the actual propagation distance of all edge points.

[0080] Extracting the laser data to obtain each group of laser signal intensity attenuation values, specifically including: extracting the laser initial emission signal intensity and the reflected received signal intensity corresponding to each edge point from the laser data, subtracting the reflected received signal intensity from the initial emission signal intensity of each edge point to obtain the laser signal intensity attenuation value of the single edge point, performing this operation for all edge points to obtain each group of laser signal intensity attenuation values;

[0081] Based on each set of laser signal intensity attenuation values, the light attenuation rate of each edge point is calculated, specifically comprising: dividing each set of laser signal intensity attenuation values ​​by the initial laser emission signal intensity of the corresponding edge point to obtain the light attenuation rate of the single edge point. This calculation is repeated for all edge points to obtain the light attenuation rate of each edge point;

[0082] Construct a three-dimensional coordinate system based on the location of the charging pile, obtain the positioning coordinates of each edge point, calculate the spatial distance between any two edge points, and construct the spatial weight matrix of the edge points based on the spatial distance. Specifically, the following steps are used: take the location of the charging pile as the origin of the three-dimensional coordinate system, set The axis is along the direction of water flow in the water area. The axis is in the horizontal plane Axis vertical, The axis is perpendicular to the horizontal plane and upward; according to the angle of the previously divided sector area, each edge point corresponds to an angle value (starting from 0 degrees and increasing by 5 degrees), and the radius R of the circular charging area is multiplied by the cosine and sine of the angle to obtain Coordinates and coordinate, The coordinates are set to 0 (i.e., water surface height), and then the three-dimensional positioning coordinates of each edge point are determined; the distance between any two edge points is calculated. axis, axis, The coordinate difference on the axis, square the three differences respectively, add the three square results, and take the square root of the sum to get the spatial distance between any two edge points; set edge points, then construct a OK The rows and columns of the matrix correspond to each edge point. Rank Column position ( and represent different edge points respectively), when equal When , that is, the position corresponds to the same edge point, its weight value is set to 0; when Not equal to When edge points and The spatial distance between the edge points is calculated, and then the weight value of the position is obtained by dividing the spatial distance by 1. The weight values ​​of all positions in the matrix are calculated in sequence to construct the spatial weight matrix of the edge points.

[0083] Based on the spatial weight matrix, the light attenuation rates of all edge points are fused to obtain a weighted light attenuation rate, which is then corrected according to the actual propagation distance to obtain the current visibility attenuation coefficient of the circular charging area. Specifically, the light attenuation rate of each edge point is multiplied by all the weight values ​​of the corresponding row in the spatial weight matrix, and then all the products are added together to obtain the weighted attenuation value of the edge point; the weighted attenuation values ​​of all edge points are summed up, and then divided by the sum of the weights in the spatial weight matrix to obtain the weighted light attenuation rate; the actual propagation distance of each edge point is added up and divided by the total number of edge points to obtain the average actual propagation distance of the laser; and the weighted light attenuation rate is divided by the average actual propagation distance of the laser to obtain the current visibility attenuation coefficient of the circular charging area.

[0084] The visibility attenuation coefficient of the circular charging area is accurately calculated through laser data, which eliminates the dependence on visual images. Even when the light is severely attenuated due to thick fog, the position information and light attenuation data of the edge points can still be accurately obtained based on the propagation characteristics and attenuation laws of the laser signal. This process does not rely on the image features captured by the camera, avoiding the problem of being unable to identify the position of the unmanned boat and the charging interface due to reduced image brightness and loss of key information, ensuring that the unmanned boat charging robot can still stably complete charging docking in complex foggy environments, and improving the adaptability of the automatic charging system in severe weather.

[0085] By accurately converting the laser round-trip time difference to the actual propagation distance, deriving the light attenuation rate from the signal intensity attenuation value, and combining the weighted fusion of multiple edge point data with the three-dimensional coordinate system and spatial weight matrix, a complete quantitative analysis method is formed. This weighted correction method based on spatial correlation fully considers the spatial distribution characteristics of each edge point, effectively reduces the impact of single data errors on the results, and makes the calculated visibility attenuation coefficient more in line with the actual water environment, providing high-precision environmental parameter support for the path planning and docking control of unmanned boat charging, and further ensuring the stability and efficiency of the automatic charging process.

[0086] In one case of this embodiment, the laser emission power is corrected according to the current visibility attenuation coefficient, and the positioning coordinates of the target object are obtained, including:

[0087] Determining a power correction coefficient based on the current visibility attenuation coefficient specifically includes: setting a reference visibility attenuation coefficient and a reference laser emission power corresponding to the reference, calculating a ratio of the current visibility attenuation coefficient to the reference attenuation coefficient, dividing the reference laser emission power by the reference visibility attenuation coefficient to obtain a power base, multiplying the ratio by the reference laser emission power and then dividing the result by the power base corresponding to the reference laser emission power to obtain the power correction coefficient;

[0088] Correcting the initial laser emission power based on the power correction coefficient to obtain a corrected laser emission power, specifically comprising: obtaining the initial laser emission power of the laser transmitter, multiplying the power correction coefficient by the initial laser emission power, and obtaining a result that is the corrected laser emission power;

[0089] Recording data from the time the corrected laser light is emitted until it is reflected by an edge point and then returned to the sensor to obtain first data, specifically comprising: causing the laser transmitter to emit laser light at the corrected power, synchronously triggering a timer to record the emission time, and when the laser light is reflected by each edge point and received by the sensor, recording the reception time and the signal strength at that time, and sequentially associating the emission time, reception time, and signal strength corresponding to each edge point. The resulting data set is the first data;

[0090] Analyze the first data and locate the target object to obtain the positioning coordinates of the target object, specifically including: finding the emission time and reception time of the laser reflected by the target object from the first data, subtracting the emission time from the reception time to obtain the time difference of the laser's round trip propagation, dividing the time difference by 2 to obtain half the time it takes for the laser to travel from the charging pile to the target object and then return, that is, the one-way propagation time, multiplying the one-way propagation time by the speed of light in water to obtain the straight-line distance from the charging pile to the target object, determining the angle corresponding to the laser emission direction from the first data, and in a three-dimensional coordinate system with the charging pile as the origin, the angle corresponds to the direction on the horizontal plane, and multiplying the straight-line distance by the cosine value of the angle to obtain the target object's position. The coordinate value on the axis; multiply the straight-line distance by the sine of the angle to get the coordinate value of the target object on the axis. The coordinate value on the axis, because the target object is on the water surface, The axis coordinates are set to 0, and the combination of the three coordinate values ​​is the positioning coordinates.

[0091] By using a power correction method calculated based on the visibility attenuation coefficient, the laser emission intensity can be automatically adjusted according to the real-time environment, ensuring that even in the case of heavy fog that severely attenuates the light, the reflected signal can still be clearly captured by the sensor. This adaptive adjustment method avoids the problem of signal loss due to being too weak or interference caused by being too strong at a fixed power, ensures the accuracy of the emission time, reception time and signal strength in the first data, and improves the environmental adaptability of laser detection during the charging process of the unmanned boat.

[0092] Through precise time difference calculation and spatial coordinate conversion, the target object is located with high precision, overcoming the technical limitations of visual recognition in foggy conditions. By using the corrected one-way propagation time and emission angle of the laser signal, combined with three-dimensional coordinate conversion, the spatial coordinates of the target object can be directly obtained without relying on image feature extraction. This positioning method is unaffected by light brightness and can stably output the position information of the unmanned vessel and charging port in dense fog, providing precise guidance for the charging robot's docking operation, significantly reducing the docking failure rate caused by positioning errors, and improving the efficiency of the unmanned vessel's automatic charging.

[0093] In one case of this embodiment, determining the hull attitude offset based on the positioning coordinates of the target object and the hull data includes:

[0094] Hull data includes: length, width, height, geometry, heading angle, roll angle, pitch angle, sway frequency, sway period, hull center of gravity offset and sway amplitude, etc.

[0095] The positioning coordinates are analyzed to generate the tangent direction vector of the target object's motion trajectory, specifically including: continuously collecting the positioning coordinates of the target object at different times, constructing a motion trajectory point set containing timestamps and three-dimensional coordinates, selecting continuous sampling points (5 adjacent points) from the motion trajectory point set, and performing curve fitting through the least squares method, specifically: setting a curve that can reflect the changing trend of the motion trajectory point set, and then adjusting the characteristic parameters such as the curvature and inclination angle of the curve to minimize the sum of the squares of the deviations from each sampling point to the curve, and obtaining a smooth curve representing the motion trend, selecting two adjacent points on the smooth curve, calculating the coordinate difference and time difference between the two points, dividing the coordinate difference by the time difference, and obtaining the average slope in the interval, which represents the tangent slope of the curve at this point. In this three-dimensional coordinate system, since the target object is on the water surface, The axis coordinate is always 0, and the tangent direction vector only needs to be considered 、 Axis component, the tangent slope is the trajectory curve in In-plane relative to The inclination of the axis, i.e. Axis variation and The ratio of the axis change is taken Taking the unit increment (1 meter) in the axial direction as the basis, multiply the tangent slope by The unit increment in the axial direction is obtained Axis increment, both The 0 increments of the axis form an ordered array ( Increment, Increment, 0), and then generate the tangent direction vector of the target object's motion trajectory;

[0096] A dynamic coordinate system is established based on the hull data and the tangent direction vector, and a hull coordinate system conversion matrix is ​​generated, specifically including: establishing the hull coordinates with the geometric center of the hull as the origin: the longitudinal axis corresponds to the length direction of the hull, the transverse axis corresponds to the width direction of the hull, the vertical axis is perpendicular to the water surface and upward, and the tangent direction vector is used as the reference direction of the dynamic coordinate system, so that the longitudinal axis of the dynamic coordinate system is consistent with the tangent direction vector, and then the transverse axis and vertical axis of the dynamic coordinate system are respectively perpendicular to the longitudinal axis to determine the dynamic coordinate system; for the heading angle, the longitudinal axis of the dynamic coordinate system is used as the reference, and the angle between the longitudinal axis of the hull coordinate system and it is measured. This angle is the rotation angle corresponding to the heading angle; for the roll angle, the transverse axis of the dynamic coordinate system is used as the reference, and the roll angle is measured. Measure the angle between the horizontal axis of the hull coordinate system and its tilt to obtain the rotation angle corresponding to the roll angle; for the pitch angle, take the vertical axis of the dynamic coordinate system as the reference, measure the angle between the vertical axis of the hull coordinate system and its tilt to obtain the rotation angle corresponding to the pitch angle, and then obtain the three rotation angles; measure the straight-line distance between the origin of the hull coordinate system and the origin of the dynamic coordinate system in the three axis directions, and use these three distances as the translation parameters of the three axes respectively. Integrate the rotation parameters corresponding to the three rotation angles in the order of the longitudinal axis, transverse axis, and vertical axis to form the rotation matrix part, and then integrate the three translation parameters into the translation matrix part. Finally, combine the rotation matrix part and the translation matrix part to obtain the hull coordinate system transformation matrix;

[0097] Based on the hull coordinate system transformation matrix, time and space, the hull data is corrected to obtain the comprehensive attitude characteristic value, specifically including: the length and width parameters in the hull data are respectively corresponded to the rotation angle and translation distance in the transformation matrix. For example, the length parameter is adjusted in direction according to the rotation angle, and the position is moved according to the translation distance. After conversion one by one, it is summarized into the hull data in the dynamic coordinate system. The converted data of 10 consecutive moments are collected, and the data difference between each moment and the previous moment is calculated. If the difference exceeds the threshold (2 times the average data difference), it is determined to be high-frequency fluctuation data. The average value obtained by adding the data of the previous moment and the data of the next moment and dividing it by 2 directly replaces this high-frequency data. The width parameter also adjusts the lateral orientation according to the rotation angle, and changes the lateral orientation according to the translation distance. The position and length parameters use the same conversion logic, and the width and length data are summarized to obtain the hull data in the dynamic coordinate system after time correction; the correction values ​​of the five neighboring points around the hull are selected, and the closer the distance, the greater the weight. Among them, the weight of the nearest point is set to 0.3, the weight of the second closest point is set to 0.25, and it decreases in sequence to 0.1 of the farthest point. The correction value of each point is multiplied by the corresponding weight and added together. The sum obtained is the spatial deviation correction. The spatial deviation correction is added to the hull data in the dynamic coordinate system after time correction to obtain the hull data in the dynamic coordinate system after time and space correction. The weights of the time-corrected data and the space-corrected data are both set to 0.5. The two data are multiplied by their corresponding weights and added together to obtain the comprehensive attitude feature value;

[0098] The comprehensive attitude eigenvalues ​​are analyzed to obtain the three-dimensional angle offset vector and the three-dimensional position offset vector, specifically including: setting the reference angle of the target object for the ideal docking when achieving precise charging docking, that is, the standard heading angle, roll angle and pitch angle that the hull should maintain are the heading angle consistent with the direction facing the charging pile, the roll angle and pitch angle are 0 degrees, the hull is horizontal and facing the charging pile, and the center of the unmanned boat charging gun is determined in the global coordinate system. Axis coordinates, which are the reference coordinates, separate the current heading angle, roll angle, and pitch angle from the comprehensive attitude eigenvalues, and use the current three angle values ​​to subtract the reference angles at the ideal docking time to obtain three angle differences. Arrange them in the order of heading angle difference, roll angle difference, and pitch angle difference, which is the three-dimensional angle offset vector. Extract the current angle from the comprehensive attitude eigenvalues. The position values ​​on the axes are obtained by subtracting the corresponding reference coordinates from the current position values ​​of the three axes to obtain the three position differences. Axis difference, Axis difference, By arranging the axis differences in order, we can get the three-dimensional position offset vector;

[0099] The 3D angle offset vector and the 3D position offset vector are fused to obtain the hull attitude offset, which specifically includes: fusing the 3D angle offset vector (heading angle difference, roll angle difference, pitch angle difference) and the 3D position offset vector ( Axis difference, Axis difference, Axis difference) is assigned a weight of 0.5 to each, and the three components of the angle offset vector are multiplied by 0.5 respectively. The three components of the position offset vector are multiplied by 0.5 respectively. Then, the product of the first component of the angle is added to the product of the first component of the position, and so on. Six fusion results are obtained and arranged in the order of angle and position, which is the hull attitude offset.

[0100] By analyzing the dynamic coordinate system and trajectory, the posture of the unmanned ship's hull is accurately captured, effectively offsetting the interference of water surface fluctuations on charging and docking. The dynamic coordinate system established based on the tangent direction vector calculation of continuous trajectory points and the combination of hull geometric parameters and motion characteristics can reflect the instantaneous motion trend of the hull in real time. By fitting curves and smoothing high-frequency fluctuation data, the posture measurement error caused by factors such as water surface swaying and water flow impact is reduced, making the comprehensive posture feature value more in line with the actual state of the hull. This method breaks through the limitation of visual recognition relying on static images in foggy days, and can continuously output stable hull posture data even in harsh environments, facilitating timely adjustment of the hull posture later.

[0101] Through the weighted fusion of three-dimensional angles and position offset vectors, the comprehensive quantification of the hull posture offset is achieved, the accuracy of charging and docking is improved, the angular deviations such as heading angle and roll angle are uniformly evaluated with the three-dimensional spatial position deviation, and the integration of multi-dimensional data is completed through the dynamic coordinate system conversion matrix, which takes into account both the directional offset of the hull and the spatial position error. This fusion method avoids the one-sidedness of single parameter evaluation and can accurately identify subtle posture changes of the hull in complex water flow. Compared with the method of visual recognition relying on feature matching, this scheme can still maintain the calculation accuracy of the offset in dense fog environment, thereby improving the docking success rate of automatic charging of unmanned boats.

[0102] In one case of this embodiment, the hull attitude offset and the radius R of the circular charging area are analyzed to generate a three-dimensional heading correction vector of the target object, including:

[0103] Obtain water data in the circular charging area, including water temperature, water velocity, flow direction, water turbidity, wave height, and wave frequency;

[0104] Based on the water body data, the weight of each deviation parameter in the hull attitude offset on the heading is adjusted to obtain the adaptive attitude influence matrix, which specifically includes: the six deviation parameters of the hull attitude offset (heading angle difference, roll angle difference, pitch angle difference, Axis difference, Axis difference, The initial weight of each deviation parameter is set to 1, and the influence coefficient of each data in the water body data on each deviation parameter is set. Among them, the influence coefficient of water temperature on all deviation parameters is 1.0; the influence coefficient of water flow velocity on The influence coefficient of the axial difference is 1.5, and the other deviation parameters are all 1.0; The influence coefficient of the axial difference is 1.3, The influence coefficient of axis difference is 1.2, and the other deviation parameters are all 1.0; the influence coefficient of turbidity on all deviation parameters is 1.1; the influence coefficient of wave height on pitch The influence coefficient of the roll angle difference is 1.4. The influence coefficient of the axis difference is 1.3, and the other deviation parameters are all 1.0; the wave frequency has an influence on the pitch The influence coefficient of the roll angle difference is 1.2. The influence coefficient of the axis difference is 1.1, and the other deviation parameters are all 1.0. The initial weight of each deviation parameter is multiplied by the corresponding influence coefficient to obtain the adjusted weight. The six adjusted weights are arranged in the order of the deviation parameters to obtain the adaptive posture influence matrix;

[0105] With the charging pile as the center, a three-dimensional space constraint field is constructed based on the radius R of the operating area. The three-dimensional space constraint field is constructed, specifically including:

[0106] The distance constraint force is used to guide the UAV to move towards the center of the charging area. The farther the distance, the greater the force. The range of the distance constraint force is 0-5N (the farther from the center, the greater the force. The distance from the center is 5N, and the distance from the center is 0).

[0107] Boundary constraint force is used to prevent the unmanned ship from approaching the edge of the area. The closer to the edge, the greater the repulsive force. The range of boundary constraint force is 0-5E (the closer to the edge, the greater the force, 5E at the edge, 0 at the center);

[0108] The depth constraint is used to adjust the wading depth of the unmanned boat according to the water depth. The depth constraint range is 2U, to ensure the charging interface height matches. Specifically: the depth constraint is based on the UAV's wading depth. When the UAV's wading depth fluctuates (such as tidal changes or waves causing the hull to float up and down), the difference between the current UAV's wading depth and the charging interface's reference height is calculated to generate the corresponding constraint force value. The constraint force range is set to 2U, where U represents the unit adjustment amount that matches the charging interface.

[0109] The regional constraint vector field is formed by superimposing three kinds of constraint forces to provide spatial navigation guidance;

[0110] Based on the real-time distance between the target object and the center of the charging area, the strength of the adaptive posture influence matrix and the regional constraint vector field is dynamically adjusted, including:

[0111] Get the real-time distance between the target object and the center of the charging area, and calculate the distance in units of R;

[0112] When the target object is in the far field phase (real-time distance 0.8R), increase the weight of the regional constraint vector field to quickly guide the target object into the effective range. At this time, the weight of the regional constraint vector field is set to 0.8, and the weight of the adaptive attitude influence matrix is ​​set to 0.2. This is because when the target object is in the far field, it needs to be quickly homed. At this time, the target object is far away from the center and may be close to the boundary. The primary task is to pull it to the center area through the regional constraint vector field to avoid being in the dangerous boundary or deviating from the target for a long time. Therefore, the weight of the regional constraint vector field is relatively high, 0.8. The relative attitude of the target in the far field has little effect on the overall control effect. Therefore, the weight of the adaptive attitude influence matrix is ​​relatively low, 0.2, to avoid attitude adjustment interfering with the homing efficiency of the main direction;

[0113] When the target object enters the mid-game phase (0.5R Real-time distance When the value of the adaptive attitude influence matrix and the regional constraint vector field is 0.8R, the weights of the adaptive attitude influence matrix and the regional constraint vector field are balanced, and the weights of both are 0.5. Because the target object is close to the center area, it is necessary to simultaneously consider position and attitude correction. If the regional constraint is still emphasized, the attitude deviation may accumulate when the target object is close to the center. If the attitude adjustment is emphasized, the final position accuracy may be affected. Therefore, the weights of the two need to be balanced, and the weights of both are 0.5.

[0114] When the target object reaches the near field stage (real-time distance 0.5R), the weight of the regional constraint vector field is set to 0.2, and the weight of the adaptive attitude influence matrix is ​​set to 0.8. Because the target object is close to the center (position deviation is small), the attitude accuracy has a great influence on the final control effect. Therefore, the attitude of the target object is corrected by the adaptive attitude influence matrix first. Therefore, the weight of the adaptive attitude influence matrix is ​​relatively high, 0.8. The near-field position deviation is already very small. If the regional constraint force is further strengthened, it may lead to over-adjustment, such as causing the target object to oscillate back and forth near the center. Therefore, the weight of the regional constraint vector field is relatively low, 0.2, just to maintain basic stability.

[0115] The adjusted adaptive attitude influence matrix is ​​fused with the regional constraint vector field to generate a three-dimensional heading correction vector containing a yaw angle correction (adjusting the bow direction to align with the charging port), a linear velocity correction (controlling the approach speed to avoid collision), and an angular velocity correction (adjusting the rotational attitude to compensate for roll and pitch effects). Specifically, the following steps are performed: multiplying each value in the adjusted adaptive attitude influence matrix by its current weight (0.2 in the far field, 0.5 in the midfield, and 0.8 in the near field) to obtain a weighted attitude matrix; multiplying each value in the regional constraint vector field by its current weight (0.8 in the far field, 0.5 in the midfield, and 0.2 in the near field) to obtain a weighted constraint vector; and adding the corresponding components of the two (for example, adding the first number to the first number, and the second number to the second number). In the resulting vector, the first component is the yaw angle correction, the second is the linear velocity correction, and the third is the angular velocity correction. Combining these three vectors gives the three-dimensional heading correction vector.

[0116] By introducing water body data to construct an adaptive attitude influence matrix, dynamic weight adjustment of the hull attitude deviation is achieved, which improves the heading correction accuracy of the unmanned ship in complex water environments. In view of the differentiated influence of water factors such as water temperature, water flow, and waves on different deviation parameters, the weight of each parameter is adjusted in real time by setting the influence coefficient, so that the heading correction is more in line with the actual environment. Even in scenes where thick fog causes visual failure, the relationship between water body data and attitude offset can still be used to obtain accurate correction basis, breaking through the limitations of ambient light on heading control.

[0117] By constructing a three-dimensional spatial constraint field through the radius of the circular charging area and combining the dynamic weight fusion strategy of the far, mid and near fields, precise control of the entire process of the unmanned ship from long-distance navigation to close-range docking is achieved. The superposition of distance constraint force and boundary constraint force forms an efficient regional navigation guide, preventing the unmanned ship from deviating from the charging range. The dynamic weight distribution mechanism prioritizes rapid homing in the far field and focuses on posture fine-tuning in the near field, taking into account both navigation efficiency and docking accuracy. This multi-field fusion control logic does not need to rely on boundary judgment of visual recognition. Even in dense fog, it can still guide the unmanned ship to stably enter the docking area through the constraint force vector, greatly reducing the heading deviation caused by environmental interference.

[0118] In one case of this embodiment, the distance value, the positioning coordinates of the target object, and the three-dimensional heading correction vector are integrated to obtain the motion trajectory of the charging gun, including:

[0119] The distance value from the charging gun to the charging port of the charging pile, the positioning coordinates of the target object, and the acquisition timestamp of the three-dimensional heading correction vector are synchronously calibrated to generate the calibrated distance value, positioning coordinates, and three-dimensional heading correction vector. Specifically, the acquisition time of the distance value, positioning coordinates, and three-dimensional heading correction vector are extracted based on the internal clock of the charging pile. The earliest acquisition time and the latest acquisition time are found. The range between these two times is the time interval that needs to be calibrated. This time interval is evenly divided into several equal small segments, each of which corresponds to a calibration time point. For each calibration time point, check whether there is data collected at this time point in the three sets of data. If so, use this data directly; if not, check the two most recent acquisition values ​​of the data before and after this calibration time point. According to the size of these two values ​​and the time difference between them and the calibration time point, calculate the value corresponding to this calibration time point. Specifically, assume that the calibration time point is , first find The most recent collection time and corresponding data , and then find The most recent collection time and corresponding data ,calculate and The time difference is recorded as ;calculate and The time difference is recorded as ;Bundle arrive The data changes between are considered uniform, then Calculated value at time , so that each calibration time point has three sets of data corresponding to the value, these values ​​​​compose the calibrated distance value, positioning coordinates and three-dimensional heading correction vector;

[0120] The water body data is analyzed to obtain the error distribution characteristics under different water qualities, specifically including: based on the turbidity, flow rate and water temperature in the water body data, the water quality is divided into multiple levels, the first level is low turbidity and low speed, the second level is medium turbidity and medium speed, and the third level is high turbidity and high speed. For each level, the distance value and positioning coordinates under the water quality of that level are continuously collected as measurement data, and the true values ​​of the distance value and positioning coordinates are simultaneously obtained as the reference true value. For each level, the difference between each set of measurement data and the corresponding reference true value is calculated to obtain multiple error values ​​to form an error sample set for that level. The error sample set is divided into intervals according to a fixed numerical interval (0.1 meter interval), and the number of occurrences (frequency) of error samples in each interval is counted. The interval with the highest frequency and the frequency ratio of the interval are determined, and then the central tendency and discrete range of the error can be obtained. The central tendency and discrete range are the error distribution characteristics corresponding to different water quality levels;

[0121] Based on the error distribution characteristics, a dynamic noise adjustment matrix is ​​constructed. Specifically, the following steps are taken: for the error distribution characteristics of different water quality levels, the median of the error concentration interval is taken as the basic coefficient, and weights are assigned according to the frequency ratio (where the higher the frequency ratio, the greater the weight). The basic coefficient of each interval is multiplied by the weight corresponding to the frequency ratio of the interval, and all the product results are added up to obtain the noise coefficient of this water quality level. The noise coefficients are then arranged into a matrix in the order of the three levels. This matrix is ​​the dynamic noise adjustment matrix.

[0122] Based on the dynamic noise adjustment matrix, the calibrated distance value, positioning coordinates and three-dimensional heading correction vector are integrated to generate comprehensive location information, specifically including: determining the noise coefficient in the dynamic noise adjustment matrix corresponding to the current water quality level, and assigning weights to each data (calibrated distance value, positioning coordinates, three-dimensional heading correction vector) according to the noise coefficient. The smaller the noise coefficient, the higher the data reliability, and therefore the larger the weight. Each data is multiplied by the corresponding weight and summed up, and the result is the comprehensive location information.

[0123] Through the timestamp synchronization calibration mechanism, the spatiotemporal alignment of distance values, positioning coordinates and three-dimensional heading correction vectors is achieved. By dividing the time interval and performing interpolation calculations based on the charging pile clock, the time difference caused by different data acquisition delays is eliminated, ensuring that the calibrated data forms a matching relationship in the same time dimension. This synchronization processing avoids fusion deviations caused by timing misalignment. Even in scenarios where water flow disturbances cause fluctuations in data acquisition frequency, the temporal consistency of the data can still be maintained through uniform interpolation. Compared with the limitation of visual recognition relying on frame synchronization, this solution can stably output time-aligned multi-source data in dense fog environments, providing data support for subsequent charging gun trajectory planning.

[0124] A dynamic noise adjustment matrix is ​​constructed based on the error distribution characteristics of water quality levels, enabling adaptive weighted fusion of multi-source data and significantly improving the reliability of comprehensive location information. By dividing water quality levels and extracting error concentration trends and discrete ranges, differentiated weights are assigned to distance values ​​and positioning coordinates under different water qualities. This dynamic adjustment mechanism can specifically offset measurement errors caused by water quality interference. Even in extreme environments with dense fog and turbid water, the noise matrix can still be used to optimize data fusion accuracy and ensure the accuracy of planning the charging gun's motion trajectory.

[0125] In one case of this embodiment, the distance value, the positioning coordinates of the target object, and the three-dimensional heading correction vector are integrated to obtain the motion trajectory of the charging gun, which also includes:

[0126] The motion limits of the hull data and the robot drive arm are analyzed to obtain the trajectory curvature limitation standard, which specifically includes: extracting the maximum swing amplitude in the hull data and the maximum allowable rotation angle, joint length and moving distance of each joint of the robot drive arm, continuously collecting the three-dimensional position coordinates of the target object, and the collection unit is 1 second. The difference between each coordinate and the coordinate at the starting moment is calculated, and the maximum value of these differences is taken as the maximum offset per unit time. The maximum offset is divided by the unit time (1 second) to obtain the offset rate per unit time. For each joint of the robot drive arm, its maximum allowable rotation angle and joint length are known. The joint connection is taken as the origin, the joint length is the radius, and the maximum allowable rotation angle is the rotation angle. The sine function is used to calculate the displacement compensation of the joint in the direction perpendicular to the joint axis (sine value multiplied by joint length), and the cosine function is used to calculate the displacement compensation along the joint axis (cosine value multiplied by joint length), and then decomposed into The displacement compensation components of each joint on the three axes are obtained, and the displacement compensation components of all joints on the same axis are added together to obtain the displacement compensation components of the robot drive arm at the end of the The maximum compensable displacement range on the axis, for the X axis, take the positive and negative values ​​of the maximum swing amplitude of the hull as the interval endpoints (for example, if the maximum swing amplitude is 0.5 meters, the X axis interval is =Meters); The Y-axis and Z-axis are determined in the same way according to the maximum swing amplitude in their respective directions. For the X-axis, compare the X-axis compensation range of the robot drive arm and the X-axis fluctuation range of the hull, and take the interval composed of the smaller left endpoint and the larger right endpoint of the two intervals. The Y-axis and Z-axis are processed in the same way to obtain the intervals of the three axes. The range composed of the three intervals is the maximum position fluctuation range that the target object can tolerate; for the robot drive arm, obtain the motion trajectory range of the charging gun at the end of the robot drive arm when each joint rotates within its motion limit range, as well as the maximum steering angle and moving distance of the end of the robot drive arm per unit time (1 second), and obtain the robot drive arm. To determine the physical boundaries of boom motion, the charging gun's trajectory is broken down into multiple continuous trajectory points at 1-second intervals. For any two adjacent trajectory points, the straight-line distance between the two points is first measured. The turning angle from the first point to the second point (i.e., the difference between the angle between the line connecting the two points and the horizontal axis) is then calculated using the coordinates of the two points. The turning angle is then divided by the straight-line distance between the two points to obtain the curvature value of the trajectory segment. The curvature values ​​of all trajectory segments are then compared with the maximum position fluctuation range tolerable for the ship, the maximum turning angle at the end of the robot's drive arm, and the travel distance. The minimum curvature value is selected as the threshold, which is specified to be no more than the threshold for the curvature of all trajectory segments. This, in turn, forms a trajectory curvature limit standard.

[0127] According to the dynamic noise adjustment matrix, the trajectory curvature limit standard, the integrated position information, and the three-dimensional heading correction vector are combined to generate a smooth preliminary trajectory. Specifically, the process includes: determining the noise coefficient of each data according to the current dynamic noise adjustment matrix, assigning weights to the integrated position information and the three-dimensional heading correction vector according to the noise coefficient, giving higher weights to data with smaller noise coefficients, and obtaining the initial trajectory points by weighted summation. The curvature (curvature) of the trajectory between two adjacent initial trajectory points is calculated and compared with the trajectory curvature limit standard. If the curvature of the trajectory segment exceeds the limit standard, the position of the next trajectory point is adjusted so that the curvature of the trajectory segment is reduced to within the limit standard. Starting from the first trajectory point, five consecutive trajectory points are selected in sequence, and the average position of the five trajectory points in each direction is calculated. The average position of the five trajectory points is replaced by the average position. All trajectory points are processed in this way in sequence to finally form a smooth preliminary trajectory.

[0128] According to the trajectory curvature limit standard, the contact force requirements corresponding to different curvature segments are calculated to obtain the contact force compensation amount at each point on the trajectory. Specifically, according to the maximum allowable curvature in the trajectory curvature limit standard, the charging gun motion trajectory is divided into different curvature segments, specifically: the curvature from 0 to 30% of the maximum allowable curvature is the low curvature segment, 30% to 70% is the medium curvature segment, and 70% to 100% is the high curvature segment. For each curvature segment, the ratio of the actual curvature of the segment to the maximum allowable curvature is calculated. For example, if the actual curvature of a segment is 50% of the maximum allowable curvature, the ratio is 0.5. The basic compensation value and maximum compensation value of the contact force are set (the basic compensation value is 0, and the maximum compensation value is the load-bearing limit of the robot drive arm). The compensation amount of each segment is calculated according to the above ratio: the low curvature segment is calculated at 30% of the ratio, the medium curvature segment is calculated at 60% of the ratio, and the high curvature segment is calculated at 100% of the ratio, that is, the compensation amount Maximum compensation value Corresponding ratio (30%, 60% or 100%) Ratio, the compensation amount of each segment is evenly distributed to all the track points contained in the segment, and the compensation amount of each point is Compensation amount per segment The total number of points in this section can be used to obtain the contact force compensation amount of each point;

[0129] Fine-tune the preliminary trajectory based on the contact force compensation so that each segment of the trajectory can match the contact force requirements under the corresponding curvature, thereby obtaining the compensated trajectory. Specifically, the contact force compensation for each point on the trajectory is extracted, and the point adjustment amplitude is determined based on the compensation amplitude. The larger the compensation, the larger the adjustment amplitude. With the original trajectory point as the reference, when the contact force compensation is positive, the point is fine-tuned in the direction close to the charging interface. When the compensation is negative, the point is fine-tuned in the direction away from the charging interface. After adjustment, the curvature between adjacent points is recalculated to check whether it meets the trajectory curvature limit standard. If it does not meet the standard, the point position is fine-tuned again until the curvature meets the standard. Adjustment is performed point by point in this way to finally obtain the compensated trajectory.

[0130] With the charging port as the center, a dynamically changing ellipsoidal safety zone is set according to the water flow direction and the length and diameter of the charging gun, and the compensated trajectory is checked. If the distance between any point on the trajectory and the boundary of the safety zone is less than the safety threshold (0.3 times the diameter of the charging gun), the trajectory is always kept within the ellipsoidal safety zone through secondary fine-tuning, and the motion trajectory of the charging gun is finally obtained. Specifically, with the charging port as the center, the major axis of the ellipsoidal safety zone is set according to the water flow direction, and the length of the major axis is set to 1.5 times the length of the charging gun. The minor axis is set perpendicular to the water flow direction, and the length of the minor axis is 2 times the diameter of the charging gun. The distance from each point on the compensated trajectory to the boundary of the safety zone is measured one by one. If the distance of a point is less than the safety threshold (0.3 times the diameter of the charging gun), the position of the point is fine-tuned towards the inside of the ellipsoidal safety zone. After fine-tuning, the distance is remeasured until the distance between all points and the boundary is not less than the safety threshold. The trajectory at this time is the motion trajectory of the charging gun.

[0131] By combining hull data with the motion limits of the robot drive arm to construct a trajectory curvature limitation standard, precise planning of the charging gun's motion trajectory is achieved, effectively adapting to the fluctuating water surface environment. Based on the maximum swing amplitude of the hull and the joint motion parameters of the drive arm, the tolerable position fluctuation range and physical motion boundaries are calculated to ensure that the trajectory curvature does not exceed the equipment limit. Through smoothing and dynamic adjustment, the preliminary trajectory not only meets the hull swing compensation requirements, but also matches the motion characteristics of the drive arm, avoiding docking jams caused by rigid trajectories. Even in dense fog, there is no need to rely on visual recognition motion prediction, and a flexible trajectory adapted to the hull dynamics can be generated through quantitative curvature standards, providing stable path guidance for charging gun docking.

[0132] Through contact force compensation and ellipsoidal safety zone settings, the safety of the charging gun movement and the reliability of docking are fully guaranteed. The dynamic allocation of contact force compensation for different curvature segments can offset the additional force generated by the hull swing and avoid rigid collision between the charging gun and the interface. The dynamic safety area centered on the charging interface ensures that the trajectory of the robot drive arm when driving the charging gun to move is always within a safe range through secondary fine-tuning. This dual protection mechanism breaks through the limitations of visual recognition boundary judgment in foggy days. Even in an environment with extremely low visibility, the stability of the charging gun movement can still be guaranteed through parameterized safety standards, ensuring the docking success rate of automatic charging of the unmanned ship.

[0133] In one case of this embodiment, controlling the charging gun to dock with the charging interface with a constant contact force according to the motion trajectory includes:

[0134] Extract the instantaneous velocity, acceleration and curvature change rate of the charging gun's motion trajectory, specifically including: selecting two adjacent trajectory points in the charging gun's motion trajectory, obtaining the coordinate values ​​of the two points in the three-dimensional coordinate system, and calculating The coordinate difference in the three directions is combined into a spatial displacement. The acquisition time of the two points is recorded and the time interval is calculated. The spatial displacement is divided by the time interval. The result is the average speed between the two points, which is used as the instantaneous speed of the next trajectory point. Three consecutive trajectory points are obtained. The instantaneous speed between the first two trajectory points and the next two trajectory points is obtained according to the above method to form two adjacent instantaneous speed vectors. The difference between the two instantaneous speed vectors is calculated and divided by the corresponding time interval. The result is the acceleration corresponding to the next instantaneous speed. Three consecutive points on the trajectory are selected, and the arc curve is fitted by the coordinates of the three points to determine the coordinates of the center of the arc and calculate the radius. The reciprocal of the radius is the curvature of the arc. The middle point is taken as its curvature value. The curvature values ​​of the three adjacent subsequent points are calculated in the same way. At the same time, the difference between the two adjacent curvature values ​​is calculated. The difference is divided by the corresponding time interval. The result is the trajectory curvature change rate corresponding to the next curvature value.

[0135] Obtain the material hardness of the charging gun and charging interface, combine the instantaneous velocity, acceleration, trajectory curvature change rate, and the material hardness of the charging gun and charging interface to obtain the contact force reference value. Specifically, set the instantaneous velocity weight to 0.3, the acceleration weight to 0.4, and the curvature change rate weight to 0.3. Multiply the instantaneous velocity, acceleration, and trajectory curvature change rate by their corresponding weights and add them together to obtain the dynamic impact value. Then, add the material hardness values ​​of the charging gun and charging interface and divide by 2 to obtain the average hardness value. Use the average hardness value as the static coefficient. Multiply the dynamic impact value by the static coefficient to obtain the contact force reference value.

[0136] The contact force reference value is combined with the end buffer distance of the charging gun's motion trajectory to obtain a contact force correction value. The constant contact force is calculated based on the contact force reference value and the contact force correction value. Specifically, in the charging gun's motion trajectory, a specific line segment extending from the end of the charging gun's motion trajectory (the side close to the charging port) toward the starting direction of the trajectory is designated as the end buffer segment, and the length of this line segment is the end buffer distance; the end buffer distance is divided into several sub-intervals at equal intervals, and a distance attenuation coefficient is set for each sub-interval (where the attenuation coefficient increases the closer to the charging port, and the attenuation coefficient ranges from 0 to 1). The straight-line distance between the current position of the charging gun and the charging port is obtained in real time, the sub-interval to which it belongs is determined, and the corresponding attenuation coefficient is extracted. The coefficient is multiplied by the contact force reference value, and the product is the contact force correction value (the correction value is 0 when the charging gun has not entered the end buffer segment). When the charging gun is outside the buffer segment, the contact force reference value is directly used as the constant contact force; after entering the end buffer segment, the contact force reference value is subtracted from the contact force correction value, and the resulting sum is the constant contact force.

[0137] By calculating constant contact force through multi-dimensional parameter fusion, accurate and stable docking of the charging gun and the charging interface is achieved, effectively solving the problem of unstable contact force caused by fluctuations in the water surface environment. By extracting the instantaneous velocity, acceleration and curvature change rate of the trajectory and combining the hardness of the two materials to construct a contact force baseline value, the impact of dynamic motion state and static physical properties on contact force is fully considered. This calculation method does not need to rely on visual recognition to judge the docking force. Even in dense fog, it can still output stable contact force parameters, providing a reliable force control basis for charging gun docking.

[0138] Through the integrated design of the terminal buffer distance and the attenuation coefficient, a gradual correction of the contact force is achieved, which greatly reduces the risk of hard contact. The buffer section is divided into multiple sub-intervals and a gradient attenuation coefficient is set, so that the contact force gradually adapts to the interface's tolerance range as the distance shortens, avoiding impact force overload during rigid docking. Based on the dynamic balance between the contact force baseline value and the correction value, it can maintain an efficient approach speed at a long distance and achieve flexible buffering when approaching docking. Compared with the method of visual recognition that relies on images to judge distance, this scheme directly calculates the buffer correction amount through trajectory parameters, which can still maintain distance perception accuracy in foggy environments and ensure the safety of charging docking.

[0139] In one case of this embodiment, controlling the charging gun to dock with the charging interface with a constant contact force according to the motion trajectory further includes:

[0140] Obtain the contact pressure between the charging gun and the charging port in real time;

[0141] Calculate the contact pressure and the constant contact force to obtain the force deviation parameter, specifically including: calculating the difference between the contact pressure and the constant contact force, taking the absolute value of the difference as the instantaneous deviation value, setting a fixed time window (0.5 seconds), adding all the instantaneous deviation values ​​in the window, and then dividing by the number of data in the window to obtain the average deviation value, dividing the average deviation value by the constant contact force to obtain the relative deviation rate, and integrating the instantaneous deviation value, the average deviation value and the relative deviation rate to form the force deviation parameter;

[0142] The motion drive parameters are dynamically adjusted according to the force deviation parameters until the charging gun is docked with the charging interface. Specifically, three thresholds (low, medium, and high) are preset, corresponding to the instantaneous deviation value, average deviation value, and relative deviation rate, respectively. The instantaneous deviation value, average deviation value, and relative deviation rate are compared with the three thresholds respectively. If it is lower than the low threshold, the deviation level is 1; if it is between the low and medium thresholds, the level is 2; if it is higher than the medium threshold, the level is 3. Then, the deviation level of each of the instantaneous deviation value, average deviation value, and relative deviation rate can be obtained, and a basic adjustment coefficient is set for each deviation level: 0.2 for level 1, 0.4 for level 2, and 0.6 for level 3, and a fine-tuning coefficient: 0.1 for level 1, and 0 for level 2. .2, 3 is 0.3; multiply the instantaneous deviation value by the corresponding basic adjustment coefficient, and add the average deviation value multiplied by the corresponding fine-tuning coefficient to obtain the speed basic adjustment amount. Multiply the instantaneous deviation value by the corresponding fine-tuning coefficient, and add the average deviation value multiplied by the corresponding basic adjustment coefficient. The sum of the two is the acceleration basic adjustment amount. Use the relative deviation rate as the correction coefficient and multiply it with the speed and acceleration basic adjustments respectively to obtain the final adjustment value. Use the final adjustment value to update the driving parameters of the robot drive arm in real time, and repeat the above process until it is detected in real time that the charging gun and the charging interface are fully fitted and the contact pressure is stable within the constant contact force range, that is, the docking is determined to be complete and the above process is stopped.

[0143] By monitoring contact pressure in real time and calculating force deviation parameters, dynamic correction of the charging gun's docking force is achieved, effectively offsetting contact force fluctuations caused by water surface fluctuations. The multi-dimensional integration of instantaneous deviation value, average deviation value, and relative deviation rate comprehensively reflects the degree of deviation between the actual contact force and the target value. Based on the deviation level divided by three thresholds, the adjustment coefficient can be output in a targeted manner. Through weighted calculation of the basic adjustment amount of speed and acceleration, the motion parameters of the robot drive arm are accurately corrected. This closed-loop control mechanism does not rely on visual recognition force feedback judgment. Even in dense fog, the docking force can still be calibrated in real time through force deviation parameters, ensuring that the charging gun fits the interface with constant contact force, avoiding docking failures due to unstable force.

[0144] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A marine automatic charging data communication system based on optical positioning, characterized in that: include: The area division unit is used to divide the charging pile into a circular charging area with a radius of R and obtain laser data at the edge of the circular charging area, including: Take the charging pile as the center of the circle, and divide the water area based on the center of the charging pile, and divide the circular charging area with a radius of R between the charging pile and the water area; The edge of the circular charging area is divided into multiple points, marked as edge points, and the data of the laser signal from emission to reflection at the edge point and then returned to the sensor is recorded in real time to obtain the laser data of the edge of the circular charging area; Calculates the current visibility reduction coefficient for the circular charging area based on laser data, including: Analyze the laser data to obtain multiple sets of laser round-trip time differences; The actual propagation distance of the laser from the charging station to each edge point is calculated based on the round-trip time difference of multiple groups of lasers and the propagation speed of light in water; Extract the laser data to obtain the intensity attenuation value of each group of laser signals; Based on each set of laser signal intensity attenuation values, the light attenuation rate of each edge point is calculated; Build a three-dimensional coordinate system based on the location of the charging pile, obtain the positioning coordinates of each edge point, calculate the spatial distance between any two edge points, and build a spatial weight matrix of the edge points based on the spatial distance; The light attenuation rates of all edge points are fused based on the spatial weight matrix to obtain the weighted light attenuation rate. The weighted light attenuation rate is corrected according to the actual propagation distance to obtain the current visibility attenuation coefficient of the circular charging area. The optical positioning unit is used to correct the laser emission power according to the current visibility attenuation coefficient after the target object enters the circular charging area, and obtain the positioning coordinates of the target object, including: Determine the power correction factor based on the current visibility attenuation factor; Correcting the initial laser emission power based on the power correction coefficient to obtain a corrected laser emission power; Recording the data of the corrected laser from the time it is emitted to the time it is reflected by the edge point and then returned to the sensor, to obtain first data; Analyzing the first data and locating the target object to obtain the location coordinates of the target object; The target object is an unmanned ship; An offset calculation unit is used to obtain the hull data of the target object in the circular charging area and determine the hull attitude offset based on the positioning coordinates of the target object and the hull data; A correction unit is used to analyze the hull attitude offset and the radius R of the circular charging area to generate a three-dimensional heading correction vector of the target object; The data fusion unit is used to obtain the distance value between the charging gun and the charging port of the charging pile, and fuse the distance value, the positioning coordinates of the target object and the three-dimensional heading correction vector to obtain the motion trajectory of the charging gun; The docking unit is used to control the charging gun to dock with the charging interface with a constant contact force according to the motion trajectory.

2. The marine automatic charging data communication system based on optical positioning according to claim 1, characterized in that: Based on the positioning coordinates of the target object and the hull data, the hull attitude offset is determined, including: Analyze the positioning coordinates to generate the tangent direction vector of the target object's motion trajectory; Establish a dynamic coordinate system based on the hull data and tangent direction vector, and generate the hull coordinate system transformation matrix; Based on the hull coordinate system transformation matrix, time and space, the hull data is corrected to obtain the comprehensive attitude eigenvalue; Analyze the comprehensive posture eigenvalues ​​to obtain the three-dimensional angle offset vector and the three-dimensional position offset vector; The 3D angle offset vector and the 3D position offset vector are fused to obtain the hull attitude offset.

3. The marine automatic charging data communication system based on optical positioning according to claim 2, characterized in that: Analyze the hull attitude offset and the radius R of the circular charging area to generate the three-dimensional heading correction vector of the target object, including: Get water data of the circular charging area; Based on the water body data, the weight of each deviation parameter in the hull attitude offset on the heading is adjusted to obtain the adaptive attitude influence matrix; With the charging pile as the center, a three-dimensional space constraint field is constructed based on the radius R of the operation area; Based on the real-time distance between the target object and the center of the charging area, the strength of the adaptive posture influence matrix and the regional constraint vector field are dynamically adjusted; The adjusted adaptive attitude influence matrix is ​​fused with the regional constraint vector field to generate a three-dimensional heading correction vector.

4. The marine automatic charging data communication system based on optical positioning according to claim 1, characterized in that: The distance value, the positioning coordinates of the target object, and the 3D heading correction vector are integrated to obtain the motion trajectory of the charging gun, including: Synchronize and calibrate the distance value from the charging gun to the charging port of the charging pile, the positioning coordinates of the target object, and the acquisition timestamp of the three-dimensional heading correction vector to generate the calibrated distance value, positioning coordinates, and three-dimensional heading correction vector; Analyze water body data to obtain error distribution characteristics under different water qualities; Based on the error distribution characteristics, a dynamic noise adjustment matrix is ​​constructed; The calibrated distance value, positioning coordinates and three-dimensional heading correction vector are fused based on the dynamic noise adjustment matrix to generate comprehensive position information.

5. The marine automatic charging data communication system based on optical positioning according to claim 4, characterized in that: The distance value, the positioning coordinates of the target object, and the 3D heading correction vector are integrated to obtain the motion trajectory of the charging gun, which also includes: Analyze the hull data and the motion limits of the robot drive arm to obtain the trajectory curvature limit standard; According to the dynamic noise adjustment matrix, the trajectory curvature limit standard, the comprehensive position information and the three-dimensional heading correction vector are combined to generate a smooth preliminary trajectory; According to the trajectory curvature limit standard, the contact force requirements corresponding to different curvature segments are calculated to obtain the contact force compensation at each point on the trajectory; Fine-tune the preliminary trajectory based on the contact force compensation amount so that each segment of the trajectory can match the contact force requirements under the corresponding curvature, and obtain the compensated trajectory; With the charging port as the center, a dynamically changing ellipsoidal safety zone is set according to the water flow direction and the length and diameter of the charging gun. The compensated trajectory is checked. If the distance between any point on the trajectory and the boundary of the safety zone is less than the safety threshold, secondary fine-tuning is performed to ensure that the trajectory is always within the ellipsoidal safety zone, and the motion trajectory of the charging gun is finally obtained.

6. The marine automatic charging data communication system based on optical positioning according to claim 5, characterized in that: Control the charging gun to dock with the charging port with a constant contact force according to the motion trajectory, including: Extract the instantaneous velocity, acceleration and curvature change rate of the charging gun's motion trajectory; Obtain the material hardness of the charging gun and charging interface, and combine the instantaneous velocity, acceleration, trajectory curvature change rate, and the material hardness of the charging gun and charging interface to obtain the contact force reference value; The contact force reference value is combined with the end buffer distance of the charging gun motion trajectory to obtain a contact force correction value, and a constant contact force is calculated based on the contact force reference value and the contact force correction value.

7. The marine automatic charging data communication system based on optical positioning according to claim 6, characterized in that: Controlling the charging gun to dock with the charging port with a constant contact force according to the motion trajectory also includes: Obtain the contact pressure between the charging gun and the charging port in real time; Calculate the contact pressure and constant contact force to obtain the force deviation parameter; The motion drive parameters are dynamically adjusted according to the force deviation parameters until the charging gun is docked with the charging interface.

Images