Ship battery replacement control method and system

Abstract

The invention discloses a ship battery replacement control method and system, is applied to the technical field of new energy ships, and is used for solving the problem that in the prior art, the ship battery replacement precision and the environment adaptability are poor. The method specifically comprises the following steps: constructing an initial environment map according to initial radar point cloud data, identifying obstacles in the initial environment map, and extracting coordinates of the obstacles in the initial environment map as initial obstacle coordinates; a battery box identifier is obtained according to battery box image recognition, and coordinates of the battery box identifier are extracted as initial grabbing coordinates; and according to the initial obstacle coordinates and the initial grabbing coordinates, a current battery replacement path of the mechanical arm is generated, and in the battery replacement process, the current battery replacement path is adjusted according to inertial measurement data of the ship and / or current radar point cloud data of the ship berthing position. Inertial measurement data and current radar point cloud data are introduced in real time in the battery replacement process to adjust the battery replacement path, the influence generated by the external environment is compensated, and the accuracy and environment adaptability of the ship battery replacement are improved.

Description

Technical Field

[0001] This application relates to the technical field of new energy ships, and particularly relates to a ship battery replacement control method and system. Background Art

[0002] As one of the important technologies in the field of new energy ships, ship battery replacement has received extensive attention and rapid development in recent years. In the existing technology, ship battery replacement mainly completes the battery replacement operation through automated or semi-automated equipment to achieve the fast endurance ability of the ship.

[0003] Currently, ship battery replacement technology has been applied to various scenarios, such as inland river shipping, offshore transportation, and port operations, etc. However, in actual applications, the accuracy of the battery replacement system is affected by various factors, such as wind and wave environment, ship sway, and equipment mechanical errors, etc. At the same time, due to the large differences in environmental conditions in different waters (such as wave undulations, water flow speed changes, etc.), the existing battery replacement equipment has deficiencies in adapting to complex environments, resulting in a low battery replacement success rate or the need for frequent manual intervention, thus affecting the efficiency and reliability of the entire battery replacement process. In addition, the existing ship battery replacement solutions mostly rely on the infrastructure support of fixed docks or specific areas, and their adaptability is relatively limited for non-standard environments or scenarios with large dynamic changes. These factors together lead to the problems of poor accuracy and environmental adaptability of the existing ship battery replacement. Summary of the Invention

[0004] This application provides a ship battery replacement control method and system to solve the problems of poor accuracy and environmental adaptability of ship battery replacement existing in the prior art.

[0005] The technical solutions provided by this application are as follows:

[0006] In a first aspect, the present invention provides a ship battery replacement control method, including:

[0007] Obtain the initial radar point cloud data of the ship's docking position and the battery box image of the ship;

[0008] Construct an initial environment map based on the initial radar point cloud data, identify the obstacles in the initial environment map, and extract the coordinates of each obstacle in the initial environment map as the initial obstacle coordinates;

[0009] Identify the battery box identifier in the battery box image, and extract the actual coordinates of the battery box identifier as the initial grasping coordinates;

[0010] Generate the current battery replacement path of the robotic arm according to the initial obstacle coordinates and the initial grasping coordinates, and control the robotic arm to perform ship battery replacement according to the current battery replacement path.

[0011] During the process of the robotic arm replacing the battery of the ship according to the current circuit replacement path, obtain the sensor measurement data, and adjust the current circuit replacement path according to the sensor measurement data until the battery replacement of the ship is completed; wherein, the sensor measurement data is the inertial measurement data of the ship and / or the current radar point cloud data of the ship's docking position.

[0012] Optionally, when the sensor measurement data is the inertial measurement data of the ship, obtaining the sensor measurement data and adjusting the current circuit replacement path according to the sensor measurement data includes:

[0013] Obtain the inertial measurement data of the first time period; wherein, the first time period starts from the time before the current time and at an interval of a preset number of inertial acquisition cycles from the current time, and ends at the current time;

[0014] Calculate the attitude angle of the ship within the first time period according to the inertial measurement data within the first time period;

[0015] Input the attitude angle of the ship within the first time period into the attitude angle prediction model to obtain the predicted attitude angle output by the attitude angle prediction model;

[0016] When the angle difference between the predicted attitude angle and the attitude angle at the current time is greater than the preset threshold, generate a compensation amount for the grasping coordinates according to the predicted attitude angle;

[0017] Adjust the initial grasping coordinates according to the compensation amount of the grasping coordinates to obtain the target grasping coordinates;

[0018] Adjust the sub-path closest to the battery box in the current circuit replacement path according to the target grasping coordinates.

[0019] Optionally, when the sensor measurement data is the current radar point cloud data of the ship's docking position, obtaining the sensor measurement data and adjusting the current circuit replacement path according to the sensor data measurement includes:

[0020] Obtain the current radar point cloud data of the ship's docking position and the current position coordinates of the robotic arm;

[0021] Update the initial environment map according to the current radar point cloud data to obtain the current environment map;

[0022] Identify the obstacles in the current environment map and extract the coordinates of each obstacle in the current environment map;

[0023] Based on the coordinates of each obstacle in the current environmental map and the current position coordinates of the robotic arm, when it is determined that there is an obstacle in the current environmental map that meets the distance condition, the coordinates of the obstacle in the current environmental map that meets the distance condition are used as the target obstacle coordinates; wherein, the distance condition is that the distance between the obstacle and the robotic arm is less than a preset first distance threshold, or the perpendicular distance between the obstacle and the upcoming running path of the robotic arm is less than a preset second distance threshold;

[0024] Adjust the current circuit-changing path according to the target obstacle coordinates.

[0025] Optionally, adjusting the current circuit-changing path according to the target obstacle coordinates includes:

[0026] Taking the sub-path where the target obstacle coordinates are located in the current circuit-changing path as the target sub-path;

[0027] Determine the starting coordinates and ending coordinates of the target sub-path;

[0028] Generate an updated sub-path according to the target obstacle coordinates, the starting coordinates and the ending coordinates of the target sub-path;

[0029] Replace the target sub-path in the current circuit-changing path with the updated sub-path.

[0030] Optionally, identifying the battery box identification in the battery box image and extracting the actual coordinates of the battery box identification as the initial grasping coordinates includes:

[0031] Identifying the battery box identification in the battery box image to obtain the first pixel coordinates and the second pixel coordinates of the battery box identification;

[0032] Determine the three-dimensional coordinates of the battery box identification according to the first pixel coordinates, the second pixel coordinates and the camera parameters;

[0033] Convert the three-dimensional coordinates of the battery box identification according to a preset first calibration matrix to obtain the actual coordinates of the battery box identification in the robotic arm coordinate system, and use the actual coordinates as the initial grasping coordinates.

[0034] Optionally, it further includes:

[0035] Obtain the contact force data between the robotic arm and the battery box;

[0036] If it is determined that the contact force data within the second time period all exceed the preset contact force safety threshold, then control the corresponding emergency stop switch module to act to cut off the power supply of the robotic arm; wherein, the second time period starts from the time before the current time and is separated from the current time by a preset number of contact force acquisition cycles, and ends at the current time.

[0037] Optionally, it further includes:

[0038] After the robotic arm completes the installation of the battery, obtain an image of the battery box interface of the ship;

[0039] Extract the first coordinates of the battery box interface and the first coordinates of the battery interface in the battery box interface image;

[0040] Determine the first offset between the first coordinates of the battery box interface and the first coordinates of the battery interface;

[0041] When the first offset is greater than the offset threshold, adjust the pose of the robotic arm according to the first offset.

[0042] Optionally, after adjusting the pose of the robotic arm according to the first offset, it further includes:

[0043] Obtain the radar point cloud data of the battery box interface and the battery interface;

[0044] According to the radar point cloud data of the battery box interface and the battery interface, determine the second coordinates of the battery box interface and the second coordinates of the battery interface;

[0045] Determine the second offset between the second coordinates of the battery box interface and the second coordinates of the battery interface;

[0046] When the second offset is greater than the offset threshold, adjust the pose of the robotic arm according to the second offset.

[0047] In a second aspect, the present invention provides a ship battery swapping system, including: a robotic arm, a binocular camera, a lidar, an inertial measurement module, and an industrial control device;

[0048] The industrial control device is respectively connected to the robotic arm, the binocular camera, the lidar, and the inertial measurement module;

[0049] The lidar is used to collect the radar point cloud data of the ship's docking position in real time and send the radar point cloud data of the ship's docking position to the industrial control device;

[0050] The binocular camera is used to collect the battery box image of the ship in real time and send the battery box image of the ship to the industrial control device;

[0051] The inertial measurement module is used to collect the inertial measurement data of the ship in real time and send the inertial measurement data of the ship to the industrial control device;

[0052] The industrial control device is used to execute the ship battery swapping control method as described above to control the robotic arm to complete the battery replacement of the ship.

[0053] Optionally, it further includes: a contact force measurement module and an emergency stop switch module; the contact force measurement module is connected to the industrial control device, the emergency stop switch module is serially arranged between the power supply end of the robotic arm and the power supply, and the control end of the emergency stop switch module is connected to the industrial control device;

[0054] A contact force measurement module, which is used to collect the contact force data between the robotic arm and the battery box in real time and send the contact force data to the industrial control device;

[0055] The industrial control device is used to send an emergency stop signal to the emergency stop switch module when it is determined that the contact force data within the second time all exceed the preset contact force safety threshold;

[0056] The emergency stop switch module is used to cut off the connection between the robotic arm and the power supply when receiving the emergency stop signal; and connect the robotic arm and the power supply when receiving the power supply signal.

[0057] The beneficial effects of this application are as follows:

[0058] In this application, by combining the environmental map constructed from radar point cloud data and the battery box image recognition result, the system can accurately locate the position of the battery box and the distribution of obstacles, effectively improving the accuracy of ship battery replacement. During the battery replacement process, inertial measurement data and current radar point cloud data are introduced in real time, which can compensate for the position offset of the ship caused by dynamic disturbances such as wind waves and water currents, and avoid the influence caused by entering new obstacles in the battery replacement path, ensuring the precise docking of the end effector of the robotic arm with the battery box interface, and further improving the accuracy of ship battery replacement and the environmental adaptability.

[0059] Other features and advantages of this application will be described in the subsequent specification, and part of them can be made obvious from the specification, or understood by implementing this application. The objectives and other advantages of this application can be achieved and obtained through the structures specifically pointed out in the written specification, claims, and drawings. Description of the Drawings

[0060] The drawings described herein are used to provide a further understanding of this application, and constitute a part of this application. The schematic embodiments of this application and their descriptions are used to explain this application, and do not constitute an improper limitation to this application. In the drawings:

[0061] Figure 1 is a schematic diagram of the framework of the ship battery replacement system in the embodiment of this application;

[0062] Figure 2 is a schematic diagram of the overall framework of the ship battery replacement control method in the embodiment of this application;

[0063] Figure 3 is a schematic diagram of the specific process of the first current battery replacement path adjustment method in the embodiment of this application;

[0064] Figure 4 is a schematic diagram of the specific process of the second current battery replacement path adjustment method in the embodiment of this application. Detailed Embodiments

[0065] In order to make the objectives, technical solutions and beneficial effects of the present application clearer and more understandable, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without creative efforts shall fall within the protection scope of the present application.

[0066] The embodiment of the present application provides a ship battery swapping system 100. Refer to Figure 1 As shown, the ship battery swapping system 100 provided by the embodiment of the present application at least includes: a robotic arm 101, a binocular camera 102, a lidar 103, an inertial measurement module 104, and an industrial control device 105; the industrial control device 105 is respectively connected to the robotic arm 101, the binocular camera 102, the lidar 103, and the inertial measurement module 104;

[0067] The lidar 103 is configured to collect the radar point cloud data of the ship's docking position in real time and send the radar point cloud data of the ship's docking position to the industrial control device 105;

[0068] The binocular camera 102 is configured to collect the battery box image of the ship in real time and send the battery box image of the ship to the industrial control device 105;

[0069] The inertial measurement module 104 is configured to collect the inertial measurement data of the ship in real time and send the inertial measurement data of the ship to the industrial control device 105;

[0070] The industrial control device 105 is configured to execute a ship battery swapping control method to control the robotic arm 101 to complete the battery replacement of the ship.

[0071] In practical applications, the process of replacing the battery of a ship is that the industrial control device 105 adopts the ship battery replacement control method proposed in this application to control the robotic arm 101 to grab the battery in the ship battery box and place it at a specified position, and grab a new battery and place it in the ship battery box. The robotic arm 101 includes a six-degree-of-freedom robotic arm 101 body and a composite gripper configured at the end to adapt to batteries of different sizes. The fixed end of the robotic arm 101 is fixed to the dock or mobile platform through a base, and the mobile platform supports multi-directional movements in the horizontal and vertical directions. The lidar 103 is fixed to the dock or the base of the robotic arm 101, and the scanning range of the lidar 103 covers the ship docking position. Specifically, the lidar 103 can scan the deck of the ship, the ship battery box, and the obstacles around the ship. The lidar 103 can specifically select a 16-line lidar 103, and the scanning frequency of the lidar 103 can be set to 20 Hz. The binocular camera 102 can be set at the same position as the lidar 103, or the binocular camera 102 can also be set at other positions that can satisfy capturing a complete image of the battery box. The baseline distance of the binocular camera 102 can be set to 0.5 m, the focal length can be set to 1000 pixels, and the image resolution can be set to 1920×1080 pixels. The inertial measurement module 104 can be set on a rigid component close to the center of gravity of the ship, and it is also necessary to avoid components with strong vibrations in the ship. The inertial measurement module 104 includes a six-axis IMU and a communication sub-module. The six-axis IMU is a sensor integrating a 3-axis accelerometer and a 3-axis gyroscope. The communication module is used to send the x-axis acceleration, y-axis acceleration, z-axis acceleration, x-axis angular velocity, y-axis angular velocity, and z-axis angular velocity collected by the six-axis IMU in real time to the industrial control device 105. The industrial control device 105 can adopt an industrial-grade embedded computer, such as Jetson Xavier, and be combined with a real-time operating system RTOS.

[0072] In a possible implementation manner, the ship battery replacement system 100 further includes: a contact force measurement module 106 and an emergency stop switch module 107; the contact force measurement module 106 is connected to the industrial control device 105, and the emergency stop switch module 107 is serially arranged between the power supply end of the robotic arm 101 and the power supply, and the control end of the emergency stop switch module 107 is connected to the industrial control device 105;

[0073] The contact force measurement module 106 is used to collect the contact force data between the robotic arm 101 and the battery box in real time, and send the contact force data to the industrial control device 105;

[0074] The industrial control device 105 is used to send an emergency stop signal to the emergency stop switch module 107 when it is determined that the contact force data within the second time all exceed the preset contact force safety threshold;

[0075] The emergency stop switch module 107 is used to cut off the connection between the robotic arm 101 and the power supply when receiving an emergency stop signal; and connect the robotic arm 101 to the power supply when receiving a power supply signal.

[0076] In practical applications, the contact force measurement module 106 may include a six-axis force sensor and a communication sub-module. The six-axis force sensor is arranged on the composite gripper configured at the end of the robotic arm 101 to detect the contact force and contact torque during the grasping process in real time. The communication sub-module sends the contact force and contact torque as contact force data to the industrial control device 105. The emergency stop switch module can be a relay switch.

[0077] Based on the above embodiments, the embodiments of the present application provide a ship battery swapping control method. Refer to Figure 2 As shown, the general process of the ship battery swapping control method provided by the embodiments of the present application is as follows:

[0078] Step 101: Obtain the initial radar point cloud data of the ship's docking position and the image of the ship's battery box.

[0079] In practical applications, the initial radar point cloud data is the radar point cloud data detected by a lidar fixed on the dock or the base of the robotic arm when the ship starts to dock. The image of the ship's battery box is obtained by a binocular camera photographing the ship's battery box.

[0080] Step 102: Construct an initial environment map based on the initial radar point cloud data, identify the obstacles in the initial environment map, and extract the coordinates of each obstacle in the initial environment map as the initial obstacle coordinates.

[0081] In practical applications, after obtaining the initial radar point cloud data, preprocessing operations including denoising processing and ground segmentation are performed on the initial radar point cloud data in sequence. The denoising processing can use statistical filtering to remove the outliers in the initial radar point cloud data. The specific program code is as follows:

[0082] pcl::StatisticalOutlierRemoval <pcl::pointxyz>sor;

[0083] sor.setInputCloud(cloud);

[0084] sor.setMeanK(50);

[0085] sor.setStddevMulThresh(1.0);

[0086] sor.filter(*cloud_filtered);

[0087] The denoising process corresponding to the above code is as follows: First, create a StatisticalOutlierRemoval object with the template parameter PointXYZ, indicating that the processed point cloud is of the XYZ type. Then, set the input point cloud, where the input point cloud is the initial radar point cloud data. Next, set the number of nearest neighbors considered when calculating the neighborhood of each point and the standard deviation multiple threshold. Here, setMeanK(50) means considering 50 nearest neighbors around each point. The 1.0 in setStddevMulThresh(1.0) is the threshold of the standard deviation multiple, that is, if the average distance of a point exceeds the global average plus 1 times the standard deviation, it will be removed. Finally, perform filtering and store the filtered initial radar point cloud data in cloud_filtered.

[0088] Ground segmentation is performed using the RANSAC algorithm for plane fitting to extract the ground point cloud, and then non-ground objects including ships and obstacles are separated. The specific program code for ground segmentation is as follows:

[0089] pcl::SACSegmentation <pcl::pointxyz>seg;

[0090] seg.setOptimizeCoefficients(true);

[0091] seg.setModelType(pcl::SACMODEL_PLANE);

[0092] seg.setMethodType(pcl::SAC_RANSAC);

[0093] seg.setDistanceThreshold(0.01);

[0094] seg.segment(*inliers,*coefficients);

[0095] The ground segmentation process corresponding to the above code is as follows: First, a SACSegmentation object is created with the template parameter PointXYZ, indicating that the processed point cloud is of the XYZ type. Then, setOptimizeCoefficients(true) is used to set the optimization of model coefficients to improve the accuracy of plane fitting. setModelType sets the segmentation type to plane. setMethodType selects the RANSAC algorithm as the segmentation algorithm. setDistanceThreshold sets the distance threshold from points to the plane, and here the distance threshold is set to 0.01 meters. Finally, the segmentation is performed through the segment method, and the inlier indices and plane equation coefficients are output. After segmenting the ground point cloud using the RANSAC algorithm, the points not in *inliers can be selected as non-ground point clouds, and the Euclidean clustering method can be used to cluster the non-ground point clouds to determine the coordinates of each obstacle.

[0096] In practical applications, based on the initial radar point cloud data after denoising, the LOAM (Lidar Odometry and Mapping) algorithm or the Cartographer algorithm in the SLAM algorithm can be used to construct an initial environment map. The initial radar point cloud data after denoising is aligned with the ship CAD model through the Iterative Closest Point (ICP) algorithm to further correct the initial coordinates of each obstacle and obtain the target coordinates of each obstacle. The target coordinates of each obstacle are transformed in the coordinate system to obtain the target coordinates of each obstacle in the robotic arm coordinate system as the initial obstacle coordinates.

[0097] Step 103: Identify the battery box identifier in the battery box image and extract the actual coordinates of the battery box identifier as the initial grasping coordinates.

[0098] In practical applications, the battery box identifier is a reflective identifier of a preset shape, and the reflective identifier is set at the center position of the charging interface of the battery box. The battery box identifier can be an AprilTag with a size of 5 cm * 5 cm. The reflective identifier on the battery box is captured by a binocular camera to obtain a battery box image, where the battery box image includes a total of two battery box images taken by the left camera and the right camera. Calculate the coordinates of the center position of the reflective identifier and perform coordinate conversion to obtain the initial grasping coordinates. Specifically, to identify the battery box identifier in the battery box image and extract the actual coordinates of the battery box identifier as the initial grasping coordinates, the following methods can be used but are not limited to:

[0099] First, identify the battery box identifier in the battery box image to obtain the first pixel coordinates and the second pixel coordinates of the battery box identifier.

[0100] Then, determine the three-dimensional coordinates of the battery box identifier according to the first pixel coordinates, the second pixel coordinates, and the camera parameters.

[0101] Finally, convert the three-dimensional coordinates of the battery box identifier according to the preset first calibration matrix to obtain the actual coordinates of the battery box identifier in the robotic arm coordinate system, and use the actual coordinates as the initial grasping coordinates.

[0102] Specifically, in implementation, the first pixel coordinates are the pixel coordinates of the battery box identifier in the battery box image taken by the left camera, and the second pixel coordinates are the pixel coordinates of the battery box identifier in the battery box image taken by the right camera. The pixel coordinates of the battery box identifier in the left and right images are detected through image processing algorithms such as threshold segmentation and template matching to obtain the left image coordinates (x1, y1) and the right image coordinates (x2, y2) of the battery box identifier. Calculate the distance between the x-axes of the left image coordinates and the right image coordinates to obtain the disparity d, that is, d = x1 - x2; according to the baseline distance B, focal length f, and disparity d of the binocular camera, substitute them into the calculation formula of depth Z, Z = f·B / d, to calculate the depth Z, and calculate the three-dimensional coordinates (X, Y, Z) of the battery box identifier in the left camera coordinate system according to the following formulas (1) and (2). Among them, cx is the abscissa of the center point of the image, and cy is the ordinate of the center point of the image. Finally, multiply the three-dimensional coordinates (X, Y, Z) of the battery box identifier in the left camera coordinate system by the first calibration matrix to obtain the actual coordinates of the battery box identifier in the robotic arm coordinate system. Among them, the first calibration matrix is used to realize the coordinate conversion between the binocular camera coordinate system and the robotic arm coordinate system.

[0103] X = f·(x1 - cx)·Z (1)

[0104] Y = f·(y1 - cy)·Z (2)

[0105] In addition, during the process of extracting the coordinates of the battery box identifier, the SGBM algorithm can be used to improve the accuracy of parallax and reduce the error of depth. The distortion is corrected by the Zhang Zhengyou calibration method to reduce the error.

[0106] Step 104: Generate the current circuit-changing path of the robotic arm based on the initial obstacle coordinates and the initial grasping coordinates, and control the robotic arm to perform battery replacement on the ship according to the current circuit-changing path.

[0107] In practical applications, after determining the initial obstacle coordinates and the initial grasping coordinates, an existing obstacle avoidance algorithm improved based on RRT (Rapidly-exploring Random Tree) can be used to generate the current circuit-changing path of the robotic arm, so as to control the robotic arm to grasp the battery in the ship's battery box and place it at a specified position through the current circuit-changing path, and grasp a new battery through the current circuit-changing path and place it in the ship's battery box.

[0108] Step 105: During the process of the robotic arm performing battery replacement on the ship according to the current circuit-changing path, obtain the sensor measurement data, and adjust the current circuit-changing path according to the sensor measurement data until the battery replacement of the ship is completed; wherein, the sensor measurement data is the inertial measurement data of the ship and / or the current radar point cloud data of the ship's docking position.

[0109] Specifically, as shown in Figure 3 When the sensor measurement data is the inertial measurement data of the ship, obtaining the sensor measurement data and adjusting the current circuit-changing path according to the sensor measurement data includes:

[0110] Step 201: Obtain the inertial measurement data of the first time period; wherein, the first time period starts from the time that is before the current time and has a preset number of inertial acquisition cycles with the current time as the end, and ends at the current time.

[0111] In practical applications, the inertial measurement data is the data collected in real time by a three-axis accelerometer and a three-axis gyroscope, including the x-axis acceleration, y-axis acceleration, z-axis acceleration, x-axis angular velocity, y-axis angular velocity, and z-axis angular velocity.

[0112] Step 202: Calculate the attitude angle of the ship within the first time based on the inertial measurement data within the first time.

[0113] In practical applications, after receiving the inertial measurement data within the first period of time, a first-order low-pass filter is used to perform low-pass filtering on the inertial measurement data to eliminate high-frequency noise. The cut-off frequency of the first-order low-pass filter can be set to 5 Hz. Multiple filtered inertial measurement data are converted to the global coordinate system of the ship. Among them, the X-axis of the global coordinate system of the ship is the bow direction, the Y-axis is the port side, and the Z-axis is vertically upward. The quaternion method is used to fuse the inertial measurement data, and the attitude angles of the ship are calculated from the quaternions. Each inertial measurement data corresponds to an attitude angle, where the attitude angles include the roll angle Roll, the pitch angle Pitch, and the heave angle Heave.

[0114] Specifically, the quaternion method is used to fuse the inertial measurement data and extract the attitude angles. The corresponding program code is as follows:

[0115]

[0116] The quaternion method fusion process corresponding to the above code is as follows: The quaternion_update function, which accepts the current quaternion q, the three-axis gyroscope data gyro, and the time interval dt. Inside the function, the omega array is calculated, where the first element is 0, and the next three are the angular velocities of the x-axis, y-axis, and z-axis detected by the gyroscope. Then q_dot, the derivative of the quaternion, is calculated by multiplying q and omega using quaternion multiplication and then multiplying by 0.5. Next, the forward Euler method: q_new = q + q_dot * dt is used to update the quaternion to obtain q_new, and normalization is performed. The quaternion_multiply function is used to implement quaternion multiplication. Finally, the roll angle Roll and pitch angle Pitch in the attitude angles are extracted. The heave angle Heave can be calculated based on the data collected by the accelerometer.

[0117] Step 203: Input the attitude angles of the ship within the first period of time into the attitude angle prediction model to obtain the predicted attitude angles output by the attitude angle prediction model.

[0118] In practical applications, the attitude angle prediction model is an ARIMA model (Autoregressive Integrated Moving Average Model). The attitude angle prediction model can be obtained by training the model based on the continuous historical attitude angles in time. Specifically, first, an ARIMA model with the autoregressive order, the difference order, and the moving average order all being 1 is created as the initial prediction model. The continuous historical attitude angles in time are used as the training parameters of the initial prediction model. The method of maximum likelihood estimation is used to determine the model parameters of the initial prediction model, and the initial prediction model is adjusted according to the model parameters to obtain the attitude angle prediction model. The model parameters include the variance of the error term, the constant term, the moving average coefficient, and the autoregressive coefficient.

[0119] Step 204: When the angle difference between the predicted attitude angle and the attitude angle at the current time is greater than the preset threshold, generate a compensation amount for the grasping coordinate according to the predicted attitude angle.

[0120] In practical applications, the angle differences between each type of attitude angle in the predicted attitude angle and the corresponding type of attitude angle in the attitude angle at the current time are calculated respectively. Among them, the angle differences include the roll angle difference, the pitch angle difference, and the heave angle difference. When the roll angle difference, the pitch angle difference, and the heave angle difference are all greater than the corresponding thresholds, substitute the predicted attitude angle into the following formulas (3)-(5) to generate the compensation amount for the grasping coordinate.

[0121] △x = Heave*sin(Pitch) (3)

[0122] △y =Heave*sin(Roll) (4)

[0123] △z = Heave*cos(Pitch)*cos(Roll) (5)

[0124] Where, △x is the compensation amount of the x coordinate, △y is the compensation amount of the y coordinate, △z is the compensation amount of the z coordinate, Roll is the roll angle, Pitch is the pitch angle, and Heave is the heave angle.

[0125] Step 205: Adjust the initial grasping coordinate according to the compensation amount of the grasping coordinate to obtain the target grasping coordinate.

[0126] Specifically, add the x coordinate compensation amount △x to the x coordinate of the initial grasping coordinate, add the y coordinate compensation amount △y to the y coordinate of the initial grasping coordinate, and add the z coordinate compensation amount △z to the z coordinate of the initial grasping coordinate, so as to obtain the target grasping coordinate.

[0127] Step 206: Adjust the sub-path closest to the battery box in the current circuit path according to the target grasping coordinate.

[0128] In practical applications, after determining the target grasping coordinates, in order to reduce the large amount of computation brought by path adjustment, when there are multiple sub-paths in the current circuit-changing path, only the sub-path closest to the ship's battery box is adjusted. Based on the target grasping coordinates, the obstacle coordinates in this sub-path, and the starting coordinates of this sub-path, an improved obstacle avoidance algorithm based on RRT is used to regenerate the sub-path closest to the ship's battery box, so as to update the sub-path closest to the battery box in the current sub-path to the sub-path regenerated according to the target grasping coordinates, the obstacle coordinates in this sub-path, and the starting coordinates of this sub-path.

[0129] During specific implementation, refer to Figure 4 As shown in the figure, when the sensor measurement data is the current radar point cloud data of the ship's docking position, obtain the sensor measurement data and adjust the current circuit-changing path according to the sensor measurement data, including:

[0130] Step 301: Obtain the current radar point cloud data of the ship's docking position and the current position coordinates of the robotic arm. Among them, the current radar point cloud data of the ship's docking position is the radar point cloud data of the ship's docking position detected in real time by a lidar fixed on the dock or the robotic arm base.

[0131] Step 302: Update the initial environment map according to the current radar point cloud data to obtain the current environment map.

[0132] In practical applications, follow the statistical filtering method in step 102 to remove the outliers in the current radar point cloud, and the filtering parameters are the same as those in step 102. Fit the ground plane through the RANSAC algorithm and extract the non-ground point cloud. The code is the same as that in step 102, but the input is the current radar point cloud data. Use NDT or ICP registration to align the non-ground point cloud extracted from the current radar point cloud data with the existing map, and then use octomap to merge the new point cloud into the initial environment map to obtain the current environment map.

[0133] Step 303: Identify the obstacles in the current environment map and extract the coordinates of each obstacle in the current environment map.

[0134] In practical applications, use the Euclidean clustering method to cluster the processed non-ground point cloud extracted from the current radar point cloud data to obtain the coordinates of each obstacle in the current environment map.

[0135] Step 304: Based on the coordinates of each obstacle in the current environmental map and the current position coordinates of the robotic arm, when it is determined that there is an obstacle in the current environmental map that meets the distance condition, the coordinates of the obstacle in the current environmental map that meets the distance condition are used as the target obstacle coordinates; wherein, the distance condition is that the distance between the obstacle and the robotic arm is less than a preset first distance threshold, or the vertical distance between the obstacle and the upcoming running path of the robotic arm is less than a preset second distance threshold.

[0136] In practical applications, the first distance threshold in the distance condition is the minimum safety distance between the obstacle and the robotic arm, and the second distance threshold in the distance condition is the minimum vertical distance at which the obstacle will not hinder the upcoming running path of the robotic arm. When there is an obstacle in the current environmental map that meets the distance condition, it corresponds to the existence of an obstacle in the current environmental map that poses a direct collision risk or a path obstruction risk. At this time, it is necessary to adjust the current circuit-changing path according to the target obstacle coordinates; when there is no obstacle in the current environmental map that meets the distance condition, it corresponds to the situation where although new obstacles are added in the current environmental map, there are no obstacles that pose a direct collision risk or a path obstruction risk. At this time, there is no need to adjust the current circuit-changing path.

[0137] Step 305: Adjust the current circuit-changing path according to the target obstacle coordinates.

[0138] In practical applications, the adjustment of the current circuit-changing path is not the regeneration of the entire path. To reduce the computational amount, only the sub-path blocked by the target obstacle coordinates in the current circuit-changing path can be adjusted. Specifically, adjusting the current circuit-changing path according to the target obstacle coordinates can be achieved by, but not limited to, the following methods:

[0139] First, the sub-path where the target obstacle coordinates are located in the current circuit-changing path is used as the target sub-path;

[0140] Then, determine the starting coordinates and ending coordinates of the target sub-path;

[0141] Next, generate an updated sub-path according to the target obstacle coordinates, the starting coordinates, and the ending coordinates of the target sub-path;

[0142] Finally, replace the target sub-path in the current circuit-changing path with the updated sub-path.

[0143] In practical applications, according to the target obstacle coordinates, the starting coordinates, and the ending coordinates of the target sub-path, an improved obstacle avoidance path planning algorithm based on RRT is used to generate the updated sub-path of the robotic arm, and the target sub-path in the current circuit-changing path is replaced with the updated sub-path to achieve targeted updating of the current circuit-changing path.

[0144] In a possible implementation manner, the ship battery replacement control method further includes:

[0145] First, obtain the contact force data between the robotic arm and the battery box.

[0146] Then, if it is determined that the contact force data within the second time period all exceed the preset contact force safety threshold, control the corresponding emergency stop switch module to act to cut off the power supply of the robotic arm; wherein, the second time period starts from the time before the current time and at an interval of a preset number of contact force acquisition cycles from the current time and ends at the current time.

[0147] In practical applications, the contact force data is measured in real time by a six-axis force sensor of a composite gripper configured at the end of the robotic arm. The contact force data includes the x-axis contact force Fx, the y-axis contact force Fy, the z-axis contact force Fz, the x-axis contact moment Mx, the y-axis contact moment My, and the z-axis contact moment Mz. When the contact force and contact moment in each axial direction in the contact force data within the second time period are all greater than the preset threshold, it corresponds to the situation where the robotic arm is jammed or collided. At this time, an emergency stop is triggered to cut off the power supply of the robotic arm to protect the robotic arm.

[0148] It is worth noting that for the preset gripping force threshold range of the composite gripper in the robotic arm, when there is at least one axial contact force exceeding the gripping force threshold range among the contact forces in each axial direction, control the robotic arm to perform pose adjustment, or use an impedance control model to dynamically adjust the stiffness of the robotic arm. Specifically, when the contact force in any axial direction exceeds the gripping force threshold range, preferentially trigger the pose adjustment algorithm based on the Jacobian matrix to control the robotic arm to perform pose adjustment and achieve rapid force unloading through joint space compensation; if the continuous adjustment fails, switch to the impedance control mode, use the impedance control model to dynamically decay the target stiffness matrix, soften the characteristics of the robotic arm according to the exponential law, adjust the stiffness of the robotic arm, and at the same time maintain the critical damping ratio to ensure system stability. In addition, when the change amount of at least one axial contact force within the preset time among the contact forces in each axial direction exceeds the preset change amount threshold, it corresponds to the situation where the battery slides in the corresponding axial direction. At this time, increase the upper limit of the gripping force threshold range in the corresponding direction and control the robotic arm to increase the gripping force in the corresponding direction.

[0149] In a possible implementation manner, after identifying the obstacles in the current environmental map, it includes:

[0150] When the obstacles in the current environmental map include a human body, control the corresponding emergency stop switch module to act to cut off the power supply of the robotic arm.

[0151] In practical applications, when using the Euclidean clustering method to cluster non-ground point clouds, if a human body contour is detected, that is, when it is determined that the obstacles include a human body, the corresponding emergency stop switch module is controlled to act to stop the robotic arm, so as to protect the personnel who stray into the working area of the robotic arm.

[0152] In a possible implementation manner, the ship battery swapping control method further includes:

[0153] First, after the robotic arm completes the installation of the battery, obtain an image of the battery box interface of the ship;

[0154] Then, extract the first coordinates of the battery box interface and the first coordinates of the battery interface in the image of the battery box interface;

[0155] Next, determine the first offset between the first coordinates of the battery box interface and the first coordinates of the battery interface;

[0156] Finally, when the first offset is greater than the offset threshold, adjust the pose of the robotic arm according to the first offset.

[0157] In practical applications, the image of the battery box interface of the ship is obtained by shooting with a binocular camera. Using the method in step 103, extract the first coordinates of the battery box interface and the first coordinates of the battery interface; the first offset is the distance between the first coordinates of the battery box interface and the first coordinates of the battery interface in each axis. When there is at least one axial distance in the first offset greater than the preset offset threshold, according to the first offset, generate a robotic arm joint adjustment amount by inverse kinematics solution, and adjust the pose of the robotic arm according to the robotic arm joint adjustment amount.

[0158] In a possible implementation manner, after adjusting the pose of the robotic arm according to the first offset, it further includes:

[0159] First, obtain the radar point cloud data of the battery box interface and the battery interface.

[0160] Then, according to the radar point cloud data of the battery box interface and the battery interface, determine the second coordinates of the battery box interface and the second coordinates of the battery interface.

[0161] Next, determine the second offset between the second coordinates of the battery box interface and the second coordinates of the battery interface.

[0162] Finally, when the second offset is greater than the offset threshold, adjust the pose of the robotic arm according to the second offset.

[0163] In practical applications, the radar point cloud data of the battery box interface and the battery interface are collected by a laser radar fixed on the dock or the base of the robotic arm. The denoising and ground segmentation in step 102 are used to separate the ground interference point cloud, and the Euclidean clustering method is used to separate the battery box interface from the battery interface point cloud; combined with the interface prior size information, the principal component analysis method is used to screen the candidate point cloud clusters that meet the geometric constraints. In the feature extraction stage, RANSAC plane fitting is performed on the battery box interface point cloud and the battery box interface point cloud respectively, and the three-dimensional center of mass coordinates of the battery box interface and the three-dimensional center of mass coordinates of the battery box interface are calculated. According to the first calibration matrix, the three-dimensional center of mass coordinates of the battery box interface and the three-dimensional center of mass coordinates of the battery box interface are converted to the robotic arm coordinate system to obtain the second coordinates of the battery box interface and the second coordinates of the battery interface. The second offset is the distance between the second coordinates of the battery box interface and the second coordinates of the battery interface in each axis. When there is at least one axial distance in the second offset that is greater than a preset offset threshold, the robot arm joint adjustment amount is generated by inverse kinematics solution according to the second offset, and the posture of the robot arm is adjusted according to the robot arm joint adjustment amount. The above process of adjusting the posture of the robot arm according to the radar point cloud data of the battery box interface and the battery interface is repeated until the distance between the second coordinate of the battery box interface and the second coordinate of the battery interface in each axial direction in the second offset is less than the preset minimum distance, and the minimum distance can be set to 1mm.

[0164] In this way, by integrating the dual perception advantages of vision and lidar, a high-precision closed-loop feedback system is constructed. First, binocular vision is used to quickly identify the interface coordinates and make preliminary posture adjustments, and then secondary calibration is performed in three-dimensional space through lidar point cloud data. This multimodal sensor collaboration not only overcomes the environmental limitations of a single sensor (such as light interference, feature occlusion, etc.), but also forms a continuously optimized control closed loop through cyclic iterative verification, significantly improving the reliability, environmental adaptability and docking accuracy of the power replacement process, and providing a technical guarantee for both efficiency and safety for ship power replacement scenarios. It should be noted that although several units or sub-units of the device are mentioned in the above detailed description, this division is merely exemplary and not mandatory. In fact, according to the embodiments of the present application, the features and functions of two or more units described above can be concretized in one unit. Conversely, the features and functions of a unit described above can be further divided into multiple units to be concretized.

[0165] In addition, although the operations of the method of the present application are described in a specific order in the drawings, this does not require or imply that the operations must be performed in this specific order, or that all the operations shown must be performed to achieve the desired results. Additionally or alternatively, some steps may be omitted, multiple steps may be combined into one step, and / or one step may be decomposed into multiple steps.

[0166] Although the preferred embodiments of the present application have been described, those skilled in the art can make additional changes and modifications once they learn the basic creative concepts. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications falling within the scope of the present application.

[0167] Obviously, those skilled in the art can make various changes and modifications to the embodiments of the present application without departing from the spirit and scope of the embodiments of the present application. Thus, if these modifications and variations of the embodiments of the present application fall within the scope of the claims of the present application and their equivalent technologies, the present application is also intended to include these changes and modifications.< / pcl::pointxyz> < / pcl::pointxyz>

Claims

1. A ship power exchange control method, characterized in that: include: Obtain initial radar point cloud data of the ship's docking location and images of the ship's battery box; constructing an initial environment map according to the initial radar point cloud data, identifying obstacles in the initial environment map, and extracting coordinates of each obstacle in the initial environment map as initial obstacle coordinates; Identify the battery box logo in the battery box image, and extract the actual coordinates of the battery box logo as initial grabbing coordinates; Generate a current battery replacement path of the manipulator according to the initial obstacle coordinates and the initial grasping coordinates, and control the manipulator to replace the battery of the ship according to the current battery replacement path; During the process of the robotic arm replacing the battery of the ship according to the current battery replacement path, sensor measurement data is obtained, and the current battery replacement path is adjusted according to the sensor measurement data until the battery replacement of the ship is completed; wherein the sensor measurement data is the inertial measurement data of the ship and / or the current radar point cloud data of the ship's docking position.

2. The ship power exchange control method according to claim 1, characterized in that: When the sensor measurement data is inertial measurement data of a ship, the acquiring the sensor measurement data and adjusting the current battery exchange path according to the sensor measurement data include: Acquire inertial measurement data of a first time period; wherein the first time period is a time period starting from a time before the current time and separated from the current time by a preset number of inertial acquisition cycles and ending at the current time; Calculating the attitude angle of the ship within the first time according to the inertial measurement data within the first time; Inputting the attitude angle of the ship within the first time into an attitude angle prediction model to obtain a predicted attitude angle output by the attitude angle prediction model; When the angle difference between the predicted posture angle and the posture angle at the current time is greater than a preset threshold, generating a compensation amount of the grasping coordinates according to the predicted posture angle; Adjusting the initial grasping coordinates according to the compensation amount of the grasping coordinates to obtain target grasping coordinates; Adjust the sub-path in the current battery replacement path that is closest to the battery box according to the target grabbing coordinates.

3. The ship power exchange control method according to claim 1, characterized in that: When the sensor measurement data is current radar point cloud data of the ship's berthing position, the acquiring the sensor measurement data and adjusting the current battery exchange path according to the sensor measurement data include: Acquire the current radar point cloud data of the ship's docking position and the current position coordinates of the mechanical arm; Update the initial environment map according to the current radar point cloud data to obtain a current environment map; Identify obstacles in the current environment map, and extract coordinates of each obstacle in the current environment map; When it is determined that there is an obstacle satisfying the distance condition in the current environment map based on the coordinates of each obstacle in the current environment map and the current position coordinates of the robotic arm, the coordinates of the obstacle satisfying the distance condition in the current environment map are used as the target obstacle coordinates; wherein the distance condition is that the distance between the obstacle and the robotic arm is less than a preset first distance threshold, or that the vertical distance between the obstacle and the path that the robotic arm is about to run is less than a preset second distance threshold; Adjust the current battery exchange path according to the target obstacle coordinates.

4. The ship power exchange control method according to claim 3, characterized in that: The adjusting the current battery swapping path according to the target obstacle coordinates includes: The subpath where the target obstacle coordinates are located in the current battery swapping path is taken as the target subpath; Determine the starting coordinates and the ending coordinates of the target subpath; Generate an updated subpath according to the target obstacle coordinates, the starting coordinates and the ending coordinates of the target subpath; Replace the target sub-path in the current battery replacement path with the updated sub-path.

5. The ship power exchange control method according to claim 1, characterized in that: The identifying the battery box logo in the battery box image and extracting the actual coordinates of the battery box logo as the initial grabbing coordinates includes: Identify a battery box identifier in the battery box image to obtain a first pixel coordinate and a second pixel coordinate of the battery box identifier; Determine the three-dimensional coordinates of the battery box mark according to the first pixel coordinates, the second pixel coordinates and camera parameters; The three-dimensional coordinates of the battery box logo are transformed according to a preset first calibration matrix to obtain the actual coordinates of the battery box logo in the robot arm coordinate system, and the actual coordinates are used as the initial grasping coordinates.

6. The ship power exchange control method according to any one of claims 1 to 5, characterized in that: Also includes: Acquiring contact force data between the robotic arm and the battery box; If it is determined that the contact force data within the second time period exceeds the preset contact force safety threshold, the corresponding emergency stop switch module is controlled to operate to cut off the power supply of the robotic arm; wherein the second time period is a time period starting from the time of a preset number of contact force collection cycles before the current time and separated from the current time interval, and ending with the current time.

7. The ship power exchange control method according to claim 6, characterized in that: Also includes: After the robot arm completes the installation of the battery, acquiring a battery box interface image of the ship; Extracting the first coordinate of the battery box interface and the first coordinate of the battery interface in the battery box interface image; Determining a first offset between a first coordinate of the battery box interface and a first coordinate of the battery interface; When the first offset is greater than an offset threshold, the posture of the robotic arm is adjusted according to the first offset.

8. The ship power exchange control method according to claim 7, characterized in that: After adjusting the posture of the robotic arm according to the first offset, the method further includes: Acquire radar point cloud data of the battery box interface and the battery interface; Determine the second coordinate of the battery box interface and the second coordinate of the battery interface according to the radar point cloud data of the battery box interface and the battery interface; Determining a second offset between a second coordinate of the battery box interface and a second coordinate of the battery interface; When the second offset is greater than an offset threshold, the posture of the robotic arm is adjusted according to the second offset.

9. A ship power exchange system, characterized in that: include: Robotic arms, binocular cameras, lidar, inertial measurement modules and industrial control equipment; The industrial control equipment is respectively connected to the mechanical arm, the binocular camera, the laser radar and the inertial measurement module; The laser radar is used to collect radar point cloud data of the ship's berthing position in real time, and send the radar point cloud data of the ship's berthing position to the industrial control equipment; The binocular camera is used to collect the battery box image of the ship in real time and send the battery box image of the ship to the industrial control equipment; The inertial measurement module is used to collect the inertial measurement data of the ship in real time and send the inertial measurement data of the ship to the industrial control equipment; The industrial control equipment is used to execute the ship battery replacement control method as described in any one of claims 1 to 8, so as to control the robotic arm to complete the battery replacement of the ship.

10. The ship power exchange system according to claim 9, characterized in that: Also includes: A contact force measurement module and an emergency stop switch module; the contact force measurement module is connected to the industrial control device, the emergency stop switch module is arranged in series between the power supply end of the robot arm and the power supply, and the control end of the emergency stop switch module is connected to the industrial control device; The contact force measurement module is used to collect the contact force data between the robot arm and the battery box in real time, and send the contact force data to the industrial control equipment; The industrial control device is used to send an emergency stop signal to the emergency stop switch module when it is determined that the contact force data within the second time exceeds a preset contact force safety threshold; The emergency stop switch module is used to cut off the connection between the robot arm and the power supply when receiving the emergency stop signal; and to connect the connection between the robot arm and the power supply when receiving the power supply signal.

Images