Autonomous positioning and docking control system and method for loading arm end effector

Abstract

The invention discloses an autonomous positioning and docking control system and method for a crane pipe end effector, and relates to the technical field of fluid loading and unloading automation, and the system comprises a global sensing unit which is fixedly installed above a crane position and is used for scanning the overall contour of a tank car to obtain vehicle global pose data; the local sensing and executing unit is integrated on an end effector and is used for collecting tank opening area images and three-dimensional point cloud and extracting and mapping three-dimensional coordinates of natural feature points; the calculation control unit receives the data, calculates the local position and posture of the tank opening, fuses the global posture and the local posture through a confidence weighted fusion algorithm to obtain corrected tank opening posture estimation, plans the movement track of the end effector, and controls the end effector to move when approaching the tank opening. The system is switched to an admittance control mode based on feedback of the six-dimensional force sensor, and smooth butt joint is achieved. The system does not need to transform the tank car, can adapt to the complex working conditions of random parking of vehicles and changeable tank opening states, and realizes safe and flexible butt joint.

Description

Technical Field

[0001] This invention relates to the field of automated fluid loading and unloading technology, specifically to an autonomous positioning and docking control system and method for the end effector of an arm. Background Technology

[0002] In the liquid material loading and unloading fields of industries such as petroleum and chemicals, loading arms are key equipment for realizing fluid transfer between tank trucks and storage and transportation facilities. Traditional loading arm docking operations heavily rely on manual operation, resulting in problems such as high labor intensity, high safety risks, and low efficiency. With the development of industrial automation and intelligent technologies, achieving autonomous positioning and docking of loading arms has become an inevitable trend to improve the inherent safety and operational efficiency of operations. Currently, related technological research and applications mainly focus on how to use sensors and control algorithms to replace manual labor and guide the end effector of the loading arm to accurately dock with the tank truck opening.

[0003] Existing technologies have proposed various automation solutions, but they generally suffer from drawbacks such as stringent requirements for operational conditions and insufficient robustness. For example, some existing technologies guide the movement of the loading arm by installing specific physical targets at the tank opening. While this method can achieve positioning, it requires modifications to all transport tank trucks, adding uniform markings, resulting in high implementation costs and poor universality in real-world open logistics scenarios. Other existing technologies perform closed-loop alignment by recognizing the tank opening contour. However, this method implicitly assumes that the vehicle is parked in a standard position and the tank opening is clean and neat. In actual operations, the parking posture of tank trucks is random, and the tank opening is often non-standardized due to oil stains, frost, or physical deformation. This makes such recognition methods based on a single vision and fixed geometric model prone to failure, unable to reliably complete docking tasks that require precise matching of position and orientation in three-dimensional space.

[0004] In summary, existing technologies either rely on pre-modification of the workpiece or can only operate under highly structured, ideal conditions. Their fundamental limitation lies in the singularity of perception and the lack of scenario adaptability, failing to systematically address the uncertainties of haphazard vehicle parking and variable tank opening conditions in real loading and unloading stations. Therefore, the critical technological bottleneck to be overcome in this field lies in how to enable the end effector of the loading arm to autonomously, with high precision, and robustly complete six-degree-of-freedom spatial positioning and alignment, ultimately achieving safe and smooth unmanned docking, while fully acknowledging the unstructured reality of random vehicle parking postures and variable tank opening physical states.

[0005] To address the aforementioned issues, existing technologies have not yet provided a systematic solution. It is necessary to invent a completely new perception and control system that is independent of any onboard auxiliary identification and can collaboratively process both the vehicle's global pose and the tank opening's local details, thereby adapting to complex on-site conditions. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of the prior art and provide an autonomous positioning and docking control system and method for the end effector of the loading arm. By constructing a multi-level perception and collaborative control system consisting of a station-level global perception and a loading arm-level local perception and computing control unit, it can effectively adapt to unstructured operating environments with random vehicle parking postures and variable physical states of tank openings without relying on any modifications to the tank truck, and achieve fully autonomous, high-precision positioning and safe and smooth docking of the loading arm end effector to the tank opening.

[0007] To solve the above-mentioned technical problems, the present invention provides the following technical solution: On one hand, a control system and method for autonomous positioning and docking of an arm-mounted loading arm end effector, the system comprising:

[0008] The station-level global perception unit is fixedly installed above the loading and unloading crane position to scan the overall outline of the parked tank truck in order to obtain global pose data that represents the parking posture of the vehicle.

[0009] The loading arm-level local sensing and actuation unit, integrated into the loading arm end effector, is used to image and measure the can opening area in three dimensions to extract multiple natural feature points on the can opening surface and their three-dimensional coordinates, and drive the end effector to move.

[0010] The computational control unit, which is communicatively connected to both the station-level global sensing unit and the arm-mounted local sensing and execution unit, is configured as follows:

[0011] Receive the global pose data and the three-dimensional coordinates of the natural feature points;

[0012] Based on the three-dimensional coordinates of the natural feature points, the local position and local orientation of the can opening are calculated;

[0013] By fusing the global pose data with the local pose data, a corrected estimate of the can opening pose is generated;

[0014] Based on the corrected estimate and the local position, the spatial motion trajectory of the end effector is planned;

[0015] The arm-mounted local sensing and execution unit is controlled to drive the end effector to move along the motion trajectory and achieve smooth docking with the tank opening.

[0016] Furthermore, the site-level global perception unit includes a two-dimensional LiDAR and a depth camera;

[0017] The two-dimensional lidar is used for horizontal scanning, and the horizontal yaw angle and lateral offset of the tanker are calculated by fitting the side contour line of the tanker body.

[0018] The depth camera is used to acquire depth images of the tanker area and estimate the pitch angle of the parking plane.

[0019] Furthermore, the loading arm-level local sensing and execution unit includes a visual sensor, a three-dimensional structured light sensor, and a multi-degree-of-freedom robotic arm;

[0020] The visual sensor is used to acquire images of the tank opening area;

[0021] The three-dimensional structured light sensor is used to project light spots onto the tank opening area and acquire its three-dimensional point cloud data;

[0022] The visual sensor identifies natural feature points in the image and performs spatial mapping with the three-dimensional point cloud data to obtain the three-dimensional coordinates of each natural feature point.

[0023] The multi-degree-of-freedom robotic arm is used to carry the vision sensor and the three-dimensional structured light sensor, and to execute the motion trajectory.

[0024] Furthermore, the computing control unit includes a data fusion module, which is configured to perform the following fusion calculations:

[0025]

[0026] in, This represents the corrected normal vector of the can opening plane, used to characterize the final can opening attitude estimate. This represents the local normal vector of the tank opening plane obtained by fitting a set of natural feature points. This represents the reference normal vector of the tank opening plane under the standard parking posture. This represents the spatial rotation transformation matrix composed of the horizontal yaw angle and pitch angle from the global pose data. The fusion weighting coefficient, with a value ranging from 0 to 1, is used to adjust the correction strength of global pose data for local pose estimation.

[0027] Furthermore, the fusion weight coefficients The confidence levels of the global pose data and the local pose estimation are dynamically adjusted. The confidence level of the global pose data is evaluated by the goodness of fit of the point cloud scanned by the field-level global sensing unit, and the confidence level of the local pose estimation is evaluated by the number and distribution uniformity of the extracted natural feature points.

[0028] Furthermore, the natural feature points include at least two of the following: non-standard edge points of the tank flange, center points of bolt caps, corner points of hinges, and corner points of nameplates;

[0029] The visual sensor is a pre-trained lightweight convolutional neural network used to identify and classify the natural feature points.

[0030] Furthermore, the computational control unit also includes a motion planning module, which is configured to:

[0031] Based on the corrected estimated tank opening attitude and position, the motion trajectory is generated using a spatiotemporal joint planning algorithm;

[0032] The motion trajectory includes the position sequence and attitude sequence of the end effector in three-dimensional space, and the attitude sequence is planned to actively compensate for vehicle skew represented by the global pose data.

[0033] Furthermore, the loading arm-level local sensing and execution unit also includes a six-dimensional force sensor, and the calculation and control unit is further configured as follows:

[0034] When the end effector approaches the tank opening to a preset distance, it switches to admittance control mode;

[0035] Based on the contact force and torque feedback from the six-dimensional force sensor, the position and attitude of the end effector are adjusted in real time to achieve compliant contact and insertion.

[0036] On the other hand, a method for autonomous positioning and docking control of an end effector of an arm loading device, applicable to the aforementioned autonomous positioning and docking control system for an end effector of an arm loading device, comprises the following specific steps:

[0037] Step 1: Using a fixedly installed site-level global perception device, scan the parked tanker truck to obtain global pose data including horizontal yaw angle, lateral offset, and pitch angle.

[0038] Step 2: Using a local sensing device mounted on the end effector of the loading arm, acquire images of the tank opening area and obtain its three-dimensional point cloud. Identify the natural feature points in the images and map them to the three-dimensional point cloud to obtain the three-dimensional coordinates of each natural feature point.

[0039] Step 3: Based on the three-dimensional coordinates of the natural feature points, calculate the local position and local orientation of the can opening;

[0040] Step 4: Merge the global pose data with the local pose data to generate a corrected estimate of the can opening pose;

[0041] Step 5: Based on the corrected estimate and the local position, plan the spatial motion trajectory of the end effector;

[0042] Step 6: Control the end effector to move along the motion trajectory, and perform compliant control based on force feedback when approaching the tank opening to complete the docking.

[0043] Furthermore, step four includes:

[0044] Based on the three-dimensional coordinates of the natural feature points, the local plane normal vector of the tank opening is determined by a point set registration algorithm;

[0045] Based on the horizontal yaw and pitch angles in the global pose data, determine the spatial rotation transformation;

[0046] The fusion weight coefficients are dynamically determined based on the confidence levels of the global pose data and the local plane normal vectors.

[0047] Based on the local plane normal vector, the reference normal vector under the standard parking posture, the spatial rotation transformation, and the fusion weight coefficient, the corrected tank opening plane normal vector is determined by weighted fusion calculation.

[0048] Compared with existing technologies, the autonomous positioning and docking control system and method for the end effector of an arm has the following advantages:

[0049] I. This invention constructs a multi-level perception system that coordinates a global perception unit at the field station level and a local perception unit at the loading arm level. This system effectively addresses the randomness of vehicle parking posture and the variability of the tank opening's physical state without relying on any onboard auxiliary markers. First, a fixed-mounted two-dimensional LiDAR and depth camera rapidly acquire global pose data such as the tank truck's horizontal yaw, lateral offset, and pitch angle, providing initial guidance and attitude reference for positioning. Then, vision and 3D structured light sensors integrated into the end effector extract the 3D coordinates of natural feature points such as the tank opening flange edge and bolt caps at close range, achieving precise perception of tank opening details. Finally, a confidence-weighted data fusion algorithm optimizes and integrates the global attitude information and local measurement results to generate a corrected tank opening attitude estimate. This enables the end effector to achieve high-precision, robust, autonomous positioning and alignment of the tank opening with six degrees of freedom in space, fully accommodating unstructured real-world operating environments.

[0050] II. This invention significantly improves the safety and reliability of the docking process by integrating a six-dimensional force sensor into the end effector and introducing an admittance control mode into the control logic. After completing trajectory planning and coarse positioning based on the sensing information, when the end effector approaches the tank opening to a preset distance, it automatically switches to the force feedback control stage. The controller reads the contact force and torque detected by the six-dimensional force sensor in real time and dynamically and smoothly adjusts the position and attitude of the end effector according to the admittance control law. This proactive control strategy that adapts to the contact state can effectively absorb and compensate for docking impacts caused by minor positioning deviations, slight deformation of the tank opening, or mechanical vibrations. It avoids the risk of damage to the tank opening sealing surface or the actuator structure that may be caused by rigid collisions. Thus, while achieving fully automatic docking, it ensures the physical safety of the operation process and the long-term stable operation of the equipment, and reduces maintenance costs.

[0051] Other advantages, objectives and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination or study, or may be learned from the practice of the invention. Attached Figure Description

[0052] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0053] Figure 1 This is a flowchart of the autonomous positioning and docking control method for the end effector of the loading arm according to the present invention;

[0054] Figure 2 This is a flowchart of the multi-level sensing and compliant control of the present invention;

[0055] Figure 3 This is a diagram of the autonomous positioning and docking control system architecture for the end effector of the loading arm of the present invention. Detailed Implementation

[0056] To further illustrate the technical means and effects of the present invention in achieving its intended purpose, the following detailed description of the specific implementation methods, structures, features, and effects of the present invention, in conjunction with the accompanying drawings and preferred embodiments, is provided below.

[0057] Example 1

[0058] like Figures 1 to 3 As shown, the autonomous positioning and docking control system for the end effector of the loading arm in this embodiment is mainly applied to liquid material loading and unloading stations in the petrochemical industry, and is designed for tank docking scenarios of standard railway or road transport tank cars. During system deployment, a unified world coordinate system must first be established, with the center position of the loading / unloading arm position as the origin, the horizontal direction as the XY plane, and the vertical upward direction as the Z-axis. All measurement data from sensors and actuators are converted and calculated based on this coordinate system.

[0059] The system's three core units are deployed as follows: the station-level global sensing unit is fixedly installed on a steel bracket above the loading and unloading arm position. The bracket height is determined according to the design height of the loading and unloading arm position to ensure that the scanning range can completely cover the tanker parking area; the loading arm-level local sensing and execution unit is integrated into the existing loading arm's end effector. The installation positions of each sensor and the robotic arm have been calibrated to ensure consistent measurement benchmarks; the computing and control unit uses an industrial control computer, installed in the station's control room, and establishes a communication connection with the first two units via Ethernet. The communication protocol uses TCP / IP to ensure the real-time performance and stability of data transmission.

[0060] In this embodiment, the station-level global perception unit consists of a two-dimensional lidar and a depth camera, which work together to acquire the global pose data of the tanker truck.

[0061] The two-dimensional lidar uses a low-cost industrial-grade model, with a scanning frequency set to a fixed value to meet the operational requirements of the site. The scanning angle range covers the entire side of the tanker truck. Its working principle involves rotating and emitting a laser beam, receiving reflected signals from the side of the tanker truck, and acquiring the polar coordinate data of the scanned point in the lidar coordinate system. Then, using preset calibration parameters, the polar coordinate data is converted into three-dimensional coordinates in the world coordinate system. For the acquired scanned point coordinates, the least squares method is used to fit the contour line of the tanker truck's side. Outliers are removed during the fitting process. The angle between the fitted straight line equation and the preset standard axis in the world coordinate system is calculated; this angle is the horizontal yaw angle of the tanker truck. Simultaneously, the distance between the fitted straight line and the standard position in the Y-axis direction is calculated, which is the lateral offset.

[0062] The depth camera employs active infrared depth imaging technology, with its shooting range partially overlapping with the scanning range of the 2D LiDAR, ensuring complementary data acquisition. After activation, the depth camera emits infrared light towards the tanker area, receives the reflected light, and calculates the depth information for each pixel, generating a depth image. Point cloud data of the tanker's parking plane is extracted from the depth image, and a plane fitting algorithm is used to process this point cloud data to obtain the equation of the parking plane. The angle between this plane and the XY plane of the world coordinate system is the pitch angle of the vehicle parking plane.

[0063] The global pose data is finally integrated into The data is transmitted in real time to the computing control unit in the form of [data / data].

[0064] In this embodiment, the loading arm-level local sensing and execution unit includes a vision sensor, a three-dimensional structured light sensor, a multi-degree-of-freedom robotic arm, and a six-dimensional force sensor. The specific implementation of each component is as follows:

[0065] The vision sensor employs a pre-trained lightweight convolutional neural network. The network structure is optimized based on the MobileNet series to meet the real-time requirements of industrial scenarios. The network training process is as follows: First, images of the tank opening area under different working conditions are collected as training samples. These conditions include different lighting conditions, different tank opening states, and different types of natural feature points, with a sample size of no less than 5000 images. Each sample image is labeled to clarify the location and category of the natural feature points. In this embodiment, the center point of the bolt cap and the corner point of the hinge are selected as two types of natural feature points. The labeled samples are divided into a training set, a validation set, and a test set in a ratio of 7:2:1. The gradient descent algorithm is used to train the network, optimizing the network's weights and bias parameters. The training objective is to minimize the classification error and localization error of feature point recognition. Finally, the trained network model is deployed in the embedded processor of the vision sensor. When the vision sensor is working, it first collects RGB images of the tank opening area, performs preprocessing such as grayscale conversion and Gaussian filtering for noise reduction, and then inputs the preprocessed images into the trained network. The network extracts features, maps features, and classifies the features, outputting the pixel coordinates and category information of the natural feature points.

[0066] The 3D structured light sensor employs random speckle projection technology, emitting infrared speckle patterns that are unique and stable, covering the entire tank opening area. During operation, the sensor projects the speckle pattern onto the tank opening area while simultaneously capturing images of the speckle using a built-in infrared camera. Based on triangulation principles, the 3D coordinates of each speckle point in the sensor's coordinate system are calculated, generating 3D point cloud data for the tank opening area. To achieve data fusion between the visual sensor and the 3D structured light sensor, both need to be calibrated beforehand: a checkerboard calibration board is used, and images of the board are captured in different poses. A calibration algorithm is then used to calculate the intrinsic parameter matrix and distortion coefficients of the visual sensor, as well as the extrinsic parameter matrix between the two sensors. Based on this calibration result, the pixel coordinates of the natural feature points identified by the visual sensor are converted into 3D coordinates in the world coordinate system using a coordinate transformation formula, thus acquiring the 3D coordinates of the natural feature points.

[0067] The multi-degree-of-freedom robotic arm utilizes a six-degree-of-freedom industrial arm, whose load capacity and range of motion meet the docking requirements of the loading arm's end effector. Vision sensors and 3D structured light sensors are fixed to the end flange of the robotic arm via customized mounting brackets. Their installation orientation has been calibrated to ensure that their measurement coordinate systems are consistent with the robotic arm's end effector coordinate system. The robotic arm is controlled using a closed-loop position control mode, receiving position and attitude commands from the computational control unit and driving the joints via servo motors to achieve precise movement of the end effector along the planned trajectory.

[0068] A six-dimensional force sensor is installed between the end flange of the robotic arm and the end effector of the loading arm. Its measurement range covers the contact forces and torques that may be generated during the docking process. The sensor establishes real-time communication with the computing control unit via a data cable, and can transmit the measured three-dimensional forces. and three-dimensional torque Data is transmitted to the control unit in real time, providing force feedback signals for compliant docking.

[0069] In this embodiment, the computing control unit is implemented based on an industrial control computer, with a built-in data fusion module and motion planning module, and various control functions are implemented through software programming.

[0070] The core function of the data fusion module is to fuse global pose data with local pose estimation to generate the corrected tank opening pose. The core fusion formula is as follows:

[0071]

[0072] The definitions and calculation processes for each parameter are as follows:

[0073] The corrected normal vector of the can opening plane is a three-dimensional vector. , is used to characterize the final orientation of the can opening in the world coordinate system, and its direction is perpendicular to the can opening plane.

[0074] The local tank opening plane normal vector is obtained by fitting the 3D coordinates of natural feature points. The specific calculation process is as follows: the obtained 3D coordinates of the natural feature points are input into a point set registration algorithm. In this embodiment, an iterative nearest-point simplification algorithm is used to fit the equation of the plane containing the tank opening. The normal vector of this plane is... After normalization, the following was obtained .

[0075] The reference normal vector for the tank opening plane in the standard parking posture is a pre-defined three-dimensional vector. When the tanker is parked in the standard posture, the tank opening plane is perpendicular to the ground, therefore, it is set as follows: .

[0076] : Spatial rotation transformation matrix, derived from the horizontal yaw angle in the global pose data. and pitch angle Composition, used to convert the reference normal vector Transform to the coordinate system of the actual parking posture of the tanker truck. The specific form of the rotation transformation matrix is:

[0077]

[0078] Among them, horizontal yaw angle It is the angle between the longitudinal axis of the tanker truck and the X-axis of the world coordinate system, with a value range of [value missing]. Pitch angle It is the angle between the plane where the tanker truck is parked and the XY plane of the world coordinate system, with a value range of... All angle values ​​are converted to radians for matrix calculation.

[0079] : Fusion weight coefficient, with a value ranging from 0 to 1, is used to adjust the correction strength of global pose data for local pose estimation. The value of is dynamically calculated based on the confidence scores of the global pose data and the local pose estimation. The calculation formula is as follows:

[0080]

[0081] in, The confidence level of the global pose data is evaluated using the goodness-of-fit of the point cloud scanned by the field-level global sensing unit. The goodness-of-fit is calculated by determining the average distance between the fitted line and each scanned point; the smaller the average distance, the higher the goodness-of-fit and the higher the confidence level. The closer the confidence level is to 1, the lower the confidence level is; conversely, the closer the confidence level is to 0.

[0082] The confidence level for local pose estimation is evaluated by the number and uniformity of the distribution of natural feature points. Good uniformity and high confidence are indicated when at least 10 natural feature points are extracted and evenly distributed across different regions of the can opening plane. The closer to 1, the lower the confidence level is; if the number of feature points is less than 8 or concentrated in a local area, the lower the confidence level is, and the closer it is to 0.

[0083] The above formula is used to calculate... Then, substituting these values ​​into the fusion formula yields the corrected normal vector of the tank opening plane. This completes the correction estimation of the can opening posture.

[0084] The motion planning module generates the spatial motion trajectory of the end effector based on the corrected tank opening posture and local position using a spatiotemporal joint planning algorithm.

[0085] The core idea of ​​the spatiotemporal joint planning algorithm is to generate smooth, collision-free motion trajectories while satisfying both spatial and temporal constraints. Spatial constraints include: the end effector's range of motion does not exceed the robotic arm's workspace; the motion trajectory maintains a preset safe distance from the tanker truck, loading arm support, and surrounding equipment. Temporal constraints include: the entire docking process time does not exceed a preset threshold to ensure operational efficiency; the end effector's speed and acceleration do not exceed the robotic arm's rated values ​​to avoid impact.

[0086] The motion trajectory generation process is as follows: First, the starting point and target point are determined. The starting point is the initial parking position of the end effector, and the target point is the docking center position of the tank opening. Then, the spatial path from the starting point to the target point is divided into multiple continuous small line segments. A cubic spline interpolation algorithm is used to interpolate the position and attitude of each line segment, resulting in a continuous position sequence and attitude sequence. The attitude sequence planning requires active compensation for vehicle skew, i.e., the normal vector of each attitude in the attitude sequence is interpolated with the corrected normal vector of the tank opening plane. Maintain consistency to ensure that the end effector matches the tank opening posture.

[0087] The calculation and control unit also has an admittance control mode to achieve smooth docking between the end effector and the tank opening. When the end effector moves along the planned trajectory to a preset distance from the tank opening, in this embodiment the preset distance is set to 5 centimeters, and the calculation and control unit automatically switches to the admittance control mode.

[0088] The core of admittance control is to convert force feedback signals into position and attitude adjustment quantities, and its control law is as follows:

[0089]

[0090] in, The position adjustment amount for the end effector includes Three directions, The actual contact force measured by the six-dimensional force sensor. The preset desired contact force, This is the stiffness coefficient.

[0091] When the end effector comes into contact with the tank opening, the six-dimensional force sensor provides real-time feedback of the contact force. The calculation control unit calculates the position adjustment amount based on the above control law. It also adjusts the position and orientation of the end effector in real time to ensure the actual contact force. Always close to the desired contact force This avoids excessive contact force that could damage the can opening or end effector, achieving smooth contact and insertion.

[0092] In this embodiment, the control method is executed according to the following steps:

[0093] Step 1: After the tanker truck is parked in its loading / unloading position, the site-level global perception unit starts operating. A 2D LiDAR performs a horizontal scan, acquiring the coordinates of scan points on the side of the tanker truck. After coordinate transformation and line fitting, the horizontal yaw angle and lateral offset are calculated. Simultaneously, a depth camera acquires depth images of the tanker truck area, extracts point cloud data of the parking plane, and performs plane fitting to calculate the pitch angle. The horizontal yaw angle, lateral offset, and pitch angle are integrated into global pose data and transmitted to the computational control unit.

[0094] Step Two: After receiving the global pose data, the computational control unit sends preliminary movement commands to the multi-degree-of-freedom robotic arm, driving the loading arm-level local sensing and execution unit to move to the approximate position of the can opening area. The vision sensor activates and acquires RGB images of the can opening area. After preprocessing, the images are input into a pre-trained lightweight convolutional neural network to identify and output the pixel coordinates of natural feature points. Simultaneously, the 3D structured light sensor projects a speckle pattern onto the can opening area, acquiring 3D point cloud data. Based on the pre-calibrated sensor extrinsic matrix, the pixel coordinates of the natural feature points are converted into 3D coordinates in the world coordinate system.

[0095] Step 3: After calculating the three-dimensional coordinates of the natural feature points received by the control unit, the iterative nearest-point simplification algorithm is used to fit these coordinates to obtain the local plane normal vector of the tank opening plane. Simultaneously, the average value of the three-dimensional coordinates of all natural feature points is calculated as the local position of the can opening.

[0096] Step 4: The data fusion module starts working. First, it determines the local plane normal vector based on the 3D coordinates of the natural feature points. Then, based on the horizontal yaw angle in the global pose data... and pitch angle Construct the spatial rotation transformation matrix Next, the confidence level of the global pose data is evaluated. and confidence level of local pose estimation The fusion weight coefficient is calculated using the formula. Finally, , , and Substituting into the fusion formula, the corrected normal vector of the tank opening plane is calculated. After completing the correction of the can opening posture, the estimation is performed.

[0097] Step 5: The motion planning module is based on the corrected can opening posture. Based on the local position, a spatiotemporal joint planning algorithm is used to generate the spatial motion trajectory of the end effector. The trajectory contains a continuous position sequence and attitude sequence, and the attitude sequence actively compensates for vehicle skew.

[0098] Step Six: The computational control unit sends motion trajectory commands to the multi-degree-of-freedom robotic arm, which drives the end effector to move along the planned trajectory. When the end effector approaches the can opening to within 5 cm, the computational control unit switches to admittance control mode. Based on the contact force and torque feedback from the six-dimensional force sensor, the position and attitude of the end effector are adjusted in real time through the admittance control law to achieve compliant contact and insertion, ultimately completing the docking with the can opening.

[0099] In this embodiment, to ensure the positioning and docking accuracy of the system, system calibration and parameter debugging are required before it is put into formal use:

[0100] Sensor calibration includes the calibration of the two-dimensional lidar with the world coordinate system, the calibration of the depth camera with the world coordinate system, and the calibration of the vision sensor with the three-dimensional structured light sensor. All calibrations are performed using standard calibration tools and methods. After calibration, the relevant calibration parameters are recorded and stored in the database of the computational control unit.

[0101] Parameter tuning includes fusion weight coefficients Calculation parameters for adjustment, stiffness coefficients for admittance control Debugging, preset distance debugging, etc. Through multiple simulated docking experiments, relevant parameters were adjusted to ensure that the system can achieve stable and accurate docking under different operating conditions.

[0102] This embodiment, through the coordinated operation of global and local perception, adapts to the actual working conditions of random vehicle parking postures and variable tank opening states without any modification to the tank truck, effectively improving the robustness of autonomous loading arm docking. By integrating global and local information through a data fusion algorithm and combining it with a spatiotemporal joint planning algorithm, the accuracy of positioning and docking is improved. Admittance control achieves compliant docking, reducing the risk of damage to the tank opening and actuators during the docking process. Compared to traditional manual operation, the system and method of this embodiment improve operational efficiency to a certain extent while ensuring operational safety.

[0103] Example 2

[0104] like Figures 1 to 3 As shown, the autonomous positioning and docking control system of the loading arm end effector in this embodiment is also applied to liquid material loading and unloading stations in the petrochemical industry. It is mainly designed for the docking scenario of tank trucks transporting goods by road. It is suitable for complex operating environments where there are no vehicle-mounted auxiliary signs, the tank trucks are parked randomly, and the tank openings are easily covered by oil or slightly deformed.

[0105] When deploying the system, a unified world coordinate system is first established, with the center position of the loading and unloading crane position as the origin, the horizontal direction as the XY plane, and the vertical direction as the Z axis. All measurement data from sensors and actuators are converted and calculated based on this coordinate system to ensure the consistency of data from each unit.

[0106] The deployment of the station-level global sensing unit, the loading arm-level local sensing and execution unit, and the computing control unit follows these steps: The station-level global sensing unit is fixedly installed on a steel bracket above the loading arm position. The bracket height is determined based on the actual loading arm position design parameters of the station to ensure that the scanning range can completely cover the parking area of ​​the highway tanker truck; the loading arm-level local sensing and execution unit is integrated into the end effector of the existing loading arm. The installation positions of each component are precisely calibrated to ensure a unified measurement benchmark; the computing control unit uses an industrial-grade control computer, installed in the station control room, and establishes a stable communication connection with the first two units via Ethernet. The TCP / IP communication protocol is used to ensure the real-time performance and reliability of data transmission.

[0107] In some optional implementations, the site-level global perception unit uses a three-dimensional lidar, which is an industrial-grade general-purpose lidar with a high scanning frequency and measurement accuracy, and can quickly acquire three-dimensional point cloud data of the overall outline of the tanker.

[0108] When the 3D LiDAR is working, it rotates and emits a laser beam to cover the entire area where the tanker is parked, and receives reflected signals from the tanker body and the area around the tank opening, directly generating 3D point cloud data in the world coordinate system. From the acquired 3D point cloud data, continuous point clouds of the tanker body side are extracted, and these point clouds are processed using a straight line fitting method to obtain the outline of the tanker body side. The angle between this outline and the preset standard axis in the coordinate system is calculated to obtain the horizontal yaw angle of the tanker; at the same time, the lateral distance between this outline and the standard parking position is calculated to obtain the lateral offset of the tanker.

[0109] To obtain the pitch angle of the vehicle's parking plane, point clouds of the top plane of the tanker or the plane around the tank opening are selected from the 3D point cloud data. A plane fitting method is used to process these point clouds to obtain the spatial attitude of the parking plane. The angle between this plane and the XY coordinate plane, i.e., the pitch angle, is then calculated. Global pose data are integrated into horizontal yaw angle, lateral offset, and pitch angle, and transmitted in real time to the computational control unit to provide global reference information for subsequent attitude correction.

[0110] The loading arm-level local sensing and execution unit includes a vision sensor, a 3D structured light sensor, a multi-degree-of-freedom robotic arm, and a six-dimensional force sensor. The specific implementation of each component is as follows:

[0111] The visual sensor employs a lightweight convolutional neural network based on transfer learning. This network uses a mature image recognition network as a pre-trained model and adapts to the task of recognizing natural feature points at the can opening through transfer learning.

[0112] During training, a large number of tank opening area images under different working conditions were collected as training samples. These conditions covered different lighting conditions, tank opening cleanliness levels, tank opening deformation levels, and different types of natural feature points. The training samples explicitly included non-standard edge points of the tank opening flange and nameplate corner points to ensure that the network could accurately identify these two types of natural feature points.

[0113] During training, the parameters of the pre-trained model are introduced as initial weights, and fine-tuning is performed using a small number of new samples to optimize the network's feature extraction capability and classification accuracy, shortening the training cycle while ensuring recognition performance. When the vision sensor is working, it first acquires RGB images of the tank opening area, performs grayscale conversion and noise reduction on the images, and then inputs the processed images into the trained network. The network performs feature extraction and classification recognition, outputting the pixel coordinates and category information of natural feature points.

[0114] The 3D structured light sensor employs striped structured light projection technology, projecting a continuous striped pattern onto the can opening area during operation. This striped pattern possesses high contrast and high stability, clearly covering the entire can opening area. The sensor's built-in infrared camera captures the striped image of the can opening area, and calculates the 3D coordinates of each stripe pixel based on the principle of triangulation, thereby generating 3D point cloud data of the can opening area.

[0115] To achieve data fusion between a visual sensor and a 3D structured light sensor, both were pre-calibrated using a standard checkerboard calibration board. Images of the calibration board were captured in different poses, and a calibration algorithm was used to determine the intrinsic parameters and distortion coefficients of the visual sensor, as well as the extrinsic parameters between the two sensors. Based on the calibration results, the pixel coordinates of natural feature points identified by the visual sensor were converted into 3D coordinates in the world coordinate system, thus achieving accurate acquisition of the 3D coordinates of the natural feature points.

[0116] The multi-degree-of-freedom robotic arm utilizes a seven-degree-of-freedom industrial arm, which offers greater motion flexibility compared to a six-degree-of-freedom arm, better adapting to complex spatial posture adjustments. Vision sensors and 3D structured light sensors are fixed to the end effector flange of the robotic arm via custom-designed mounting brackets. Their mounting orientation has been rigorously calibrated to ensure that their measurement coordinate systems are consistent with the end effector's coordinate system. The robotic arm employs a hybrid position-velocity control mode, receiving motion commands from the computational control unit and driving the joints via servo motors to achieve smooth movement of the end effector along a planned trajectory.

[0117] A six-dimensional force sensor is installed between the end flange of the robotic arm and the end effector of the loading arm. Its measurement range can cover the contact forces and torques that may be generated during the docking process. The sensor establishes real-time communication with the computing control unit via a data cable, continuously transmitting the measured three-dimensional force and torque data to the computing control unit, providing a reliable force feedback signal for compliant docking.

[0118] The computational control unit is based on an industrial control computer and has a built-in data fusion module and motion planning module. It implements various control functions through mature industrial control software programming.

[0119] The core function of the data fusion module is to fuse global pose data with local pose estimation results to generate the corrected pose of the tank opening.

[0120] Specifically, the data fusion module receives global pose data transmitted by the field-level global sensing unit and the three-dimensional coordinates of natural feature points transmitted by the loading arm-level local sensing and execution unit. Based on the three-dimensional coordinates of the natural feature points, the local plane normal vector of the tank opening is obtained by fitting a point set registration algorithm. This normal vector represents the local attitude of the tank opening.

[0121] Meanwhile, based on the horizontal yaw and pitch angles in the global pose data, a spatial rotation transformation matrix is ​​constructed to transform the reference normal vector of the tank opening plane under the standard parking posture to the coordinate system under the actual parking posture of the tank truck.

[0122] During the fusion process, the fusion weight coefficients are dynamically adjusted based on the confidence levels of the global pose data and the local pose estimation. The confidence level of the global pose data is assessed by the completeness and continuity of the point cloud data acquired by the 3D LiDAR; the more complete and continuous the point cloud data, the higher the confidence level. The confidence level of the local pose estimation is assessed by the recognition accuracy and distribution range of natural feature points; the higher the recognition accuracy and the wider the distribution, the higher the confidence level.

[0123] By using a weighted fusion method, the local plane normal vector is fused with the transformed reference normal vector to obtain the corrected tank opening plane normal vector. This normal vector accurately represents the actual posture of the tank opening and provides a reliable basis for subsequent motion planning.

[0124] The motion planning module generates the spatial motion trajectory of the end effector using a segmented planning strategy based on the corrected tank opening posture and local position.

[0125] It is understandable that the segmented planning strategy divides the entire motion process into a coarse positioning stage and a fine positioning stage. The coarse positioning stage mainly realizes the rapid movement of the end effector from the initial position to the area near the tank opening, while the fine positioning stage realizes the precise alignment of the end effector with the tank opening.

[0126] During trajectory generation, spatial and kinematic constraints are fully considered. Spatial constraints include ensuring that the movement range of the end effector does not exceed the working space of the robotic arm and that it maintains a safe distance from surrounding equipment. Kinematic constraints include ensuring that the movement speed and acceleration of the end effector do not exceed the rated values ​​of the robotic arm to avoid impact.

[0127] The generated motion trajectory includes a continuous sequence of position and attitude. The planning of the attitude sequence can actively compensate for vehicle skew, ensuring that the attitude of the end effector is consistent with the corrected attitude of the tank opening, thus laying the foundation for precise docking.

[0128] The computational control unit also features compliant control, enabling smooth docking between the end effector and the tank opening. When the end effector moves along the planned trajectory to a preset distance close to the tank opening, the computational control unit automatically switches to admittance control mode.

[0129] In admittance control mode, the computational control unit receives contact force and torque data from the six-dimensional force sensor in real time. Based on the preset desired contact force, it dynamically adjusts the position and attitude of the end effector. When the end effector comes into contact with the tank opening, if the actual contact force is greater than the desired contact force, the control unit will appropriately adjust the movement direction and speed of the actuator to reduce the contact force; if the actual contact force is less than the desired contact force, it will control the actuator to continue moving towards the tank opening until a suitable contact state is achieved.

[0130] This dynamic adjustment avoids excessive contact force during docking, effectively protecting the tank opening and end effector, and achieving compliant contact and insertion.

[0131] In this embodiment, the control method is executed according to the following steps:

[0132] Step 1: After the tanker truck is parked in its loading / unloading position, the station-level global perception unit starts working. The 3D lidar emits a laser beam towards the tanker truck's parking area to acquire 3D point cloud data of the overall outline of the tanker truck. From the point cloud data, the side profile of the truck body and the point cloud of the parking plane are extracted. The horizontal yaw angle, lateral offset, and pitch angle are obtained through fitting calculations, integrated into global pose data, and transmitted to the computing and control unit.

[0133] Step two: After receiving the global pose data, the computational control unit sends preliminary movement commands to the multi-degree-of-freedom robotic arm, driving the loading arm-level local sensing and execution unit to move to the approximate position of the tank opening area. A vision sensor acquires RGB images of the tank opening area, which, after preprocessing, are input into a trained neural network to identify the pixel coordinates of non-standard flange edge points and nameplate corner points. A 3D structured light sensor projects a stripe pattern onto the tank opening area, acquiring 3D point cloud data. Based on pre-calibrated sensor parameters, the pixel coordinates of natural feature points are converted into 3D coordinates in the world coordinate system.

[0134] Step 3: After the control unit receives the three-dimensional coordinates of the natural feature points, it uses a point set registration algorithm to fit these coordinates to obtain the local plane normal vector of the tank opening. At the same time, it calculates the average value of the three-dimensional coordinates of all natural feature points to determine the local position of the tank opening.

[0135] Step four: The data fusion module starts working. It obtains the local plane normal vector based on natural feature points, constructs a spatial rotation transformation matrix based on the global pose data, and transforms the reference normal vector under the standard parking posture. It evaluates the confidence levels of the global pose data and the local pose estimation, dynamically adjusts the fusion weight coefficients, and obtains the corrected tank opening plane normal vector through weighted fusion calculation, thus completing the corrected tank opening posture estimation.

[0136] Step 5: Based on the corrected tank opening posture and local position, the motion planning module uses a segmented planning strategy to generate the spatial motion trajectory of the end effector. The trajectory includes a position sequence and an attitude sequence, and the attitude sequence actively compensates for vehicle skewing.

[0137] Step six: The computational control unit sends motion trajectory commands to the multi-degree-of-freedom robotic arm, which drives the end effector to move along the planned trajectory. When the end effector approaches the tank opening to a preset distance, the computational control unit switches to admittance control mode. Based on the contact force and torque feedback from the six-dimensional force sensor, it adjusts the position and attitude of the end effector in real time to achieve compliant contact and insertion, ultimately completing the docking with the tank opening.

[0138] To ensure the system's positioning accuracy and docking stability, system calibration and parameter debugging are required before formal deployment. Sensor calibration includes the calibration of the 3D LiDAR and the world coordinate system, as well as the calibration of the vision sensor and the 3D structured light sensor. All calibrations are performed using standard calibration tools and methods, and the relevant parameters are stored in the database of the computational control unit after calibration.

[0139] Parameter debugging includes adjusting the fusion weight coefficient, adjusting the logic, adjusting the expected contact force of admittance control, and adjusting the preset distance. Through multiple simulations of docking experiments under different working conditions, the relevant parameters are optimized to ensure that the system can achieve stable and accurate docking under working conditions such as different vehicle tilt angles and different tank opening states.

[0140] This embodiment uses a 3D LiDAR as the site-level global perception unit, which can directly acquire the 3D point cloud data of the tanker truck, simplifying the calculation process of global pose data and improving the efficiency and accuracy of data acquisition.

[0141] The visual sensor employs a transfer learning training method, combined with the recognition of non-standard flange edge points and nameplate corner points, which enhances the robustness of natural feature point recognition and is applicable to more complex tank opening conditions.

[0142] The application of a seven-degree-of-freedom robotic arm enhances the motion flexibility of the end effector, and the combination of segmented planning strategy and admittance control further optimizes the smoothness and accuracy of the docking process.

[0143] The entire system requires no modification to the tank truck and can adapt to the actual working conditions of random parking posture and changing tank opening status of road transport tank trucks, effectively improving the robustness and reliability of autonomous loading arm docking and meeting the actual needs of industrial production.

[0144] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A self-positioning and docking control system for an end effector of an arm-mounted loading arm, characterized in that, The system comprises: A field station level global perception unit fixedly installed above the loading and unloading crane position, used for scanning the overall profile of the parked tank truck to obtain global pose data representing the vehicle parking posture; A crane pipe level local perception and execution unit integrated in the crane pipe end effector, used for imaging and three-dimensional measurement of the tank opening area to extract a plurality of natural feature points on the tank opening surface and their three-dimensional coordinates, and drive the end effector to move; A computing control unit in communication connection with the field station level global perception unit and the crane pipe level local perception and execution unit, configured to: Receive the global pose data and the three-dimensional coordinates of the natural feature points; Based on the three-dimensional coordinates of the natural feature points, calculate the local position and local posture of the tank opening; Fuse the global pose data and the local posture to generate a corrected estimation of the tank opening posture; Based on the corrected estimation and the local position, plan the spatial motion trajectory of the end effector; Control the crane pipe level local perception and execution unit to drive the end effector to move along the motion trajectory and achieve soft docking with the tank opening.

2. The swivel end effector autonomous positioning and docking control system of claim 1, wherein, The field station level global perception unit comprises a two-dimensional laser radar and a depth camera; The two-dimensional laser radar is used for horizontal scanning to calculate the horizontal yaw angle and lateral offset of the tank truck by fitting the profile line of the tank truck body side; The depth camera is used to obtain the depth image of the tank truck area and estimate the pitch angle of the vehicle parking plane.

3. The swivel end effector autonomous positioning and docking control system of claim 1, wherein, The crane pipe level local perception and execution unit comprises a vision sensor, a three-dimensional structured light sensor and a multi-degree-of-freedom mechanical arm; The vision sensor is used to collect images of the tank opening area; The three-dimensional structured light sensor is used to project light spots on the tank opening area and obtain three-dimensional point cloud data thereof; The vision sensor identifies natural feature points in the image and spatially maps them with the three-dimensional point cloud data to obtain the three-dimensional coordinates of each natural feature point; The multi-degree-of-freedom mechanical arm is used to carry the vision sensor and the three-dimensional structured light sensor and execute the motion trajectory.

4. The swivel end effector autonomous positioning and docking control system of claim 1, wherein, The computing control unit comprises a data fusion module configured to perform the following fusion calculation: wherein, represents the corrected tank opening plane normal vector, used to represent the final tank opening pose estimation, represents the local tank opening plane normal vector fitted by the set of natural feature points, represents the tank opening plane reference normal vector in the standard parking pose, represents the spatial rotation transformation matrix composed of the horizontal yaw angle and the pitch angle in the global pose data, is a fusion weight coefficient, whose value range is 0 to 1, used to adjust the correction strength of the global pose data on the local pose estimation.

5. The swivel end effector autonomous positioning and docking control system of claim 4, wherein, The fusion weight coefficient According to the dynamic adjustment of the confidence of the global pose data and the confidence of the local pose estimation, the confidence of the global pose data is evaluated by the fitting goodness of the scanning point cloud of the station-level global perception unit, and the confidence of the local pose estimation is evaluated by the number and distribution uniformity of the extracted natural feature points.

6. The swivel end effector autonomous positioning and docking control system of claim 3, wherein, The natural feature points include at least two of non-standard edge points of the tank opening flange, center points of bolt caps, corner points of hinges and corner points of nameplates; The vision sensor is a pre-trained lightweight convolutional neural network used to identify and classify the natural feature points.

7. The swivel end effector autonomous positioning and docking control system of claim 1, wherein, The computing control unit further comprises a motion planning module configured to: Based on the corrected estimation of the tank opening posture and position, generate the motion trajectory using a spatio-temporal joint planning algorithm; The motion trajectory includes a sequence of positions and a sequence of postures of the end effector in three-dimensional space, and the sequence of postures is planned to actively compensate for the vehicle skew represented by the global pose data.

8. The swivel end effector autonomous positioning and docking control system of claim 1, wherein, The crane pipe level local perception and execution unit further comprises a six-dimensional force sensor, and the computing control unit is further configured to: Switch to admittance control mode when the end effector approaches the tank opening to a preset distance; Based on the contact force and torque feedback of the six-dimensional force sensor, the position and attitude of the end effector are adjusted in real time to achieve compliant contact and insertion.

9. A method for controlling the autonomous positioning and docking of a swivel end effector, suitable for use in a system for controlling the autonomous positioning and docking of a swivel end effector according to any one of claims 1 to 8, characterized in that, The specific steps of the method are: Step one, through the fixedly installed field station level global perception device, the parked tank truck is scanned to obtain global pose data including horizontal yaw angle, lateral offset and pitch angle; Step two, through the local perception device mounted on the hose end effector, the tank opening area image is collected and its three-dimensional point cloud is obtained, the natural feature points in the image are identified and mapped to the three-dimensional point cloud, and the three-dimensional coordinates of each natural feature point are obtained; Step three, based on the three-dimensional coordinates of the natural feature points, the local position and local attitude of the tank opening are calculated; Step four, fuse the global pose data and the local attitude to generate a corrected estimate of the tank opening attitude; Step five, based on the corrected estimate and the local position, the spatial motion trajectory of the end effector is planned; Step six, control the end effector to move along the motion trajectory, and perform compliant control based on force feedback when approaching the tank opening to complete the docking.

10. The method of claim 9, wherein, The step four includes: Based on the three-dimensional coordinates of the natural feature points, the local plane normal vector of the tank opening is determined by a point set registration algorithm; Based on the horizontal yaw angle and the pitch angle in the global pose data, a spatial rotation transformation is determined; According to the confidence of the global pose data and the confidence of the local plane normal vector, a fusion weight coefficient is dynamically determined; Based on the local plane normal vector, the reference normal vector under the standard parking attitude, the spatial rotation transformation and the fusion weight coefficient, a corrected tank opening plane normal vector is determined by weighted fusion calculation.

Images