Land LNG double-truck arm cooperative operation automatic docking system and docking method thereof

Abstract

The application discloses a kind of land LNG double loading arm collaborative operation automatic docking system and its docking method.System in each loading and unloading arm third joint hard pipe axis and fourth joint rotation axis are acute angle angle of inclusion.In the loading arm actuator and between loading arm flange, increase virtual rotary joint, so that loading arm flange can be virtually rotated around loading arm actuator axis, and further virtually as independent loading arm flange joint.In the process of solving inverse solution, virtual joint is introduced.After rough positioning I stage motion trajectory planning is completed, it is no longer re-planned;Precision positioning II stage is based on column camera and marker guide closed-loop control.The application realizes the automatic docking of double loading arm end effector and tank car flange, improves the automation level of loading arm, and further improves the utilization rate of reachable working space of loading arm end effector in the docking process, reduces the difficulty of target pose matrix setting, reduces trajectory planning time, improves docking accuracy.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of LNG loading arm butt joint, in particular to a land LNG double loading arm cooperative operation automatic butt joint system and a butt joint method thereof. BACKGROUND

[0002] Compared with traditional fuels such as coal and oil, LNG is a relatively clean energy, and its combustion produces relatively less carbon dioxide and other harmful substances. At present, LNG is used as the main energy in many scenarios, and is transported over long distances by LNG tank cars. When liquid natural gas needs to be loaded, in order to effectively avoid the collision of double arms and complete butt joint, it often needs to consume a lot of manpower, and the staff needs to be highly concentrated to avoid accidents, therefore, it is of great significance and application prospect to improve the automation level of double loading arms.

[0003] The Chinese invention patent with application number 202010925026.2 discloses a semi-automatic tank car loading and unloading arm and a control method thereof. The loading and unloading arm adopts a double-arm structure, and two loading and unloading arms can realize automatic and alternating expansion, butt joint preparation position, recovery and other movements. However, the adjacent joints of the loading and unloading arm are connected through a known parallel or vertical structure, which limits the utilization rate of the end flange operation space of the loading and unloading arm, and in the butt joint process, the end flange can only automatically move to the butt joint preparation position, and the butt joint process from the butt joint preparation position to the tank car flange needs to be realized by manual operation, and the automation level needs to be further improved.

[0004] The Chinese invention patent with application number 202010385980.7 discloses an intelligent loading and unloading system, which preliminarily coarsely positions the pose information of the target flange on the ship through a coarse positioning vision camera, further real-time finely positions the pose information of the target flange on the ship through a fine positioning vision camera, plans the movement trajectory of the loading and unloading arm through the vision acquisition target flange pose information, and calculates the required movement angle value of each joint of the loading and unloading arm through a known inverse kinematics algorithm. The intelligent loading and unloading system has good effect when the vision camera real-time feedback target flange pose information, but when the vision camera cannot capture the target flange pose information, it can only be open-loop controlled, and the butt joint accuracy is limited. In addition, the intelligent loading and unloading system has good inverse solution effect when the joint degrees of freedom are independent, but it is difficult to give the target pose matrix when the joint degrees of freedom are coupled. SUMMARY

[0005] The present application aims at the problems existing in the prior art, and provides a land LNG double loading arm cooperative operation automatic butt joint system and a butt joint method thereof.

[0006] The technical solution for achieving the object of the application is: on the one hand, a land LNG double-loading vehicle arm cooperative operation automatic docking system is provided, which comprises a support column, and a liquid-phase arm, a gas-phase arm, a column camera and a laser radar installed on the support column, wherein the liquid-phase arm and the gas-phase arm each comprise five joints, five hard pipes, a visual camera and a marker.

[0007] The land LNG double-loading vehicle arm cooperative operation automatic docking system further comprises: the third joint hard pipe axis of each loading and unloading arm and the fourth joint rotation axis form an acute angle; in the land LNG loading and unloading arm device, a flange is fixedly connected to the end of the loading and unloading arm, and a virtual wrist joint is added between the end of the land LNG loading and unloading arm and the flange, so that the flange can virtually rotate around the axis of the end of the loading and unloading arm.

[0008] On the other hand, a land LNG double-loading vehicle arm cooperative operation automatic docking method is provided, which comprises the following steps:

[0009] For the liquid-phase arm:

[0010] Step 1, establishing a land LNG double-loading vehicle arm coordinate system;

[0011] Step 2, planning a tank truck parking area;

[0012] Step 3, determining a coarse positioning I position;

[0013] Step 4, planning a motion trajectory of the liquid-phase arm flange in the coarse positioning I stage;

[0014] Step 5, discretizing the trajectory points and determining the pose matrix of each trajectory point;

[0015] Step 6, inversely solving the joint angles corresponding to each trajectory point;

[0016] Step 7, performing angle trajectory planning on each joint to obtain the joint angle of each joint at each time;

[0017] Step 8, controlling the liquid-phase arm flange to move along the planned trajectory from the starting point to the coarse positioning I position through the PLC;

[0018] Step 9, measuring the position of the tank truck liquid-phase flange through the laser radar and calculating a coarse positioning II position;

[0019] Step 10, planning a motion trajectory of the end flange in the coarse positioning II stage;

[0020] Step 11, controlling the liquid-phase arm flange to move to the coarse positioning II position along the planned trajectory through the PLC;

[0021] Step 12, acquiring the pose matrix of the tank truck liquid-phase flange relative to the liquid-phase arm flange coordinate system based on the liquid-phase camera.

[0022] Step 13, based on the column camera, the liquid phase marker, and the pose matrix of the tank car liquid phase flange relative to the liquid phase arm flange coordinate system, the pose matrix of the tank car liquid phase flange relative to the column camera coordinate system is obtained;

[0023] Step 14, read the current angle value of each joint of the liquid phase arm, and calculate the pose matrix of the tank car liquid phase flange relative to the fixed coordinate system;

[0024] Step 15, plan the fine positioning position;

[0025] Step 16, plan the motion trajectory of the liquid phase arm flange from the current position to the fine positioning position, and calculate the distance D1 of the liquid phase arm flange and the fine positioning position;

[0026] Step 17, if D1 is greater than the preset threshold, control the liquid phase arm flange to move a distance along the planned motion trajectory through PLC, and return to execute step 12;

[0027] Step 18, if D1 is less than or equal to the preset threshold, it indicates that the liquid phase arm flange has reached the fine positioning position, and the fine positioning I stage motion control is ended;

[0028] Step 19, based on the column camera, the liquid phase marker, and the pose matrix of the tank car liquid phase flange relative to the liquid phase arm flange coordinate system is calculated;

[0029] Step 20, read the current angle value of each joint of the liquid phase arm, and calculate the pose matrix of the tank car liquid phase flange relative to the fixed coordinate system;

[0030] Step 21, plan the motion trajectory of the liquid phase arm flange from the current position to the tank car liquid phase flange position, and calculate the distance D2 of the liquid phase arm flange and the tank car liquid phase flange;

[0031] Step 22, if D2 is greater than the preset threshold, control the liquid phase arm flange to move a distance along the planned motion trajectory through PLC, and return to step 19;

[0032] Step 23, if D2 is less than or equal to the preset threshold, it indicates that the liquid phase arm flange has reached the tank car liquid phase flange position, and the fine positioning II stage motion control is ended;

[0033] Step 24, plan the recovery transition position;

[0034] Step 25, control the liquid phase arm flange to exit to the recovery transition position through PLC;

[0035] Step 26, plan the motion trajectory of the liquid phase arm flange from the recovery transition position to the starting point;

[0036] Step 27, control the liquid phase arm flange to move from the recovery transition position to the starting point through PLC;

[0037] For the gas phase arm, the process is the same as the above process.

[0038] The land LNG double-loading vehicle arm cooperative operation automatic docking method also includes the following contents:

[0039] (1) The virtual rotary joint is added between the liquid phase actuator and the liquid phase arm flange, so that the liquid phase arm flange can virtually rotate around the liquid phase actuator axis, and the liquid phase arm flange is virtually an independent liquid phase arm flange joint, and an independent liquid phase arm flange joint coordinate system is established, the Z axis of the liquid phase arm flange joint coordinate system is collinear with the liquid phase actuator axis (in the known method, the liquid phase arm flange coordinate system is generally integrated into the liquid phase fifth joint coordinate system, that is, only the coordinate system is established according to the actual motion joint).

[0040] (2) Since the position of the tank car is different each time, the position of the liquid phase flange detected by the laser radar is also different each time, and then the rough positioning position is also different each time (in the known method, the motion trajectory of the liquid phase arm flange from the starting point to the rough positioning position needs to be re-planned in the rough positioning motion control process each time). The rough positioning motion control process is divided into rough positioning I stage and rough positioning II stage, the rough positioning I position is determined according to the tank car parking area and the tank car flange identification area, and the motion trajectory of the liquid phase arm flange from the starting point to the rough positioning I position is planned, which is the rough positioning I stage motion trajectory. After the rough positioning I stage motion trajectory is planned, no matter how the tank car is parked in the tank car parking area, the rough positioning I stage motion trajectory remains unchanged and does not need to be re-planned. Then, the rough positioning II position is calculated according to the position of the liquid phase flange detected by the laser radar, and the motion trajectory from the rough positioning I position to the rough positioning II position II is planned, which is the rough positioning II stage motion trajectory.

[0041] (3) The inverse kinematics is solved by using Newton iteration method in this patent, and the Jacobian matrix needs to be constructed in the solving process. In this invention, not only the actual existing joint variables, but also the virtual joints that do not actually exist are added to the Jacobian matrix as an independent joint variable to solve the inverse solution (in the known method, only the actual existing joint variables are introduced into the Jacobian matrix to solve the inverse solution). The inverse solution of this invention contains actual joint angles and virtual joint angles, and the PLC controller only controls the actual joints to move along the joint space planning trajectory, and does not control the virtual joints (in the known method, the PLC controller controls all the calculated joints to move).

[0042] (4) The fine positioning motion control of the method of the present application is also divided into two stages, i.e., fine positioning I stage and fine positioning II stage, which is different from the known method (the fine positioning motion control of the known method is divided into two stages, i.e., fine positioning I stage and fine positioning II stage, wherein the fine positioning I stage is liquid-phase camera guided closed-loop control, and the fine positioning II stage is open-loop control without liquid-phase camera guidance), and the method of the present application is not only the fine positioning I stage which is liquid-phase camera guided closed-loop control, but also the fine positioning II stage which is closed-loop control based on column camera and liquid-phase marker guidance.

[0043] (5) The docking process of the loading arm flange and the tank car flange: taking the liquid-phase arm as an example, in the coarse positioning process, the liquid-phase arm flange moves from position ξ0 to coarse positioning I position ξ5, then the coarse positioning I position ξ5 moves to coarse positioning II position ξ1; in the fine positioning process, the liquid-phase arm flange moves from the coarse positioning II position ξ1 to the fine positioning position ξ2, at this time, the liquid-phase arm flange and the tank car liquid-phase flange are closer, and the liquid-phase arm flange axis and the tank car liquid-phase flange axis coincide; in the docking process, the liquid-phase arm flange translates forward along the tank car liquid-phase flange axis with the normal line unchanged, from the fine positioning position ξ2 to the liquid-phase arm flange position ξ3, at this time, the liquid-phase arm flange and the tank car liquid-phase flange are docked; in the recovery I stage, the liquid-phase arm flange exits along the tank car liquid-phase flange axis, from the position ξ3 to the recovery transition position ξ4, at this time, the liquid-phase arm flange axis and the tank car liquid-phase flange axis coincide, but the liquid-phase arm flange and the tank car liquid-phase flange have been separated; in the recovery II stage, the liquid-phase arm flange moves from the recovery transition position ξ4 to the starting position ξ0.

[0044] (6) The overall docking process of the land LNG double-loading arm cooperative operation automatic docking system is: initially, the liquid-phase arm and the gas-phase arm are parked on the support column; the liquid-phase arm is coarsely positioned; the gas-phase arm is coarsely positioned; the liquid-phase arm is finely positioned; the gas-phase arm is finely positioned; the gas-phase arm is recovered and finally parked on the support column; the liquid-phase arm is recovered and finally parked on the support column.

[0045] Compared with the prior art, the present application has the following obvious advantages:

[0046] (1) In the XOY plane of the fixed coordinate system, the loading arm flange and the tank truck flange generally complete butt joint in the third quadrant and the tank truck flange axis is approximately parallel to the X axis of the fixed coordinate system, the adjacent joints of the loading and unloading arm of the known method are connected through parallel or vertical structures, so that the connecting point of the fifth hard pipe and the fifth joint of the loading arm is migrated by a distance L to the positive direction of the X axis, and then the length of the fifth hard pipe needs to be compensated by the length of the fifth hard pipe with a length of L, resulting in the shortening of the effective length of the fifth hard pipe. Not only the length of the fifth hard pipe is wasted, but also the fifth hard pipe is easily interfered with the fourth joint and the fourth hard pipe due to the overlap of the fifth hard pipe and the fourth joint and the fourth hard pipe. The present application sets the included angle between the third hard pipe and the fourth joint axis to be an acute angle, so that the fourth hard pipe is unfolded outward, reducing the overlap of the fifth hard pipe and the fourth joint and the fourth hard pipe, avoiding interference, reducing the length waste of the fifth hard pipe, and at the same time, the connecting point of the fifth hard pipe and the fifth joint is migrated to the negative direction of the X axis, and the loading arm flange is also migrated to the negative direction of the X axis, thereby expanding the proportion of the reachable working space of the loading arm flange in the third quadrant and improving the utilization rate of the reachable working space of the loading arm flange.

[0047] (2) In the physical object of the land LNG loading and unloading arm device, the loading arm end is fixedly connected with the loading arm flange. Since the included angle between the third hard pipe and the fourth joint axis of the present application is an acute angle, the movement of the loading arm flange will also cause the rotation of the loading arm flange around its own central axis, that is, the movement degree of freedom and the rotation degree of freedom of the loading arm flange are coupled. The present application increases a virtual wrist joint between the end of the land LNG loading and unloading arm and the loading arm flange, so that the loading arm flange can virtually rotate around the axis of the end of the loading and unloading arm, and the rotation caused by the movement of the loading arm flange is offset by the virtual rotation of the joint of the loading arm flange, thereby realizing the virtual decoupling of the movement degree of freedom and the rotation degree of freedom of the loading arm flange.

[0048] (3) In the coordinate system established by the known method, there is a coupling relationship between the position term and the rotation term when setting the target pose matrix. When the value of the position term is given, the value of the rotation term needs to be calculated according to the coupling relationship. Since it is difficult to derive the coupling relationship, it is difficult to set the target pose matrix. In the coordinate system established by the present application, the rotation caused by the movement of the liquid phase arm flange will be offset by the virtual rotation of the joint coordinate system of the liquid phase arm flange, thereby realizing the virtual decoupling of the movement degree of freedom and the rotation degree of freedom of the liquid phase arm flange, and the position term and the rotation term can not satisfy the coupling relationship, thereby reducing the difficulty of setting the target pose matrix.

[0049] (4) The known method needs to re-plan the whole trajectory of the liquid phase arm flange from the starting point to the rough positioning position in each rough positioning motion control process, and the trajectory planning time is relatively long. In the method, the rough positioning I stage motion trajectory is not re-planned in each rough positioning process, but the liquid phase arm flange is directly controlled to move from the starting point to the rough positioning I position, and then the rough positioning II stage motion trajectory is updated according to the specific position of the tank truck flange. Compared with the known method, the trajectory planning time of the whole rough positioning motion control can be effectively reduced in each rough positioning process since the rough positioning I stage motion trajectory planning process is omitted.

[0050] (5) In the process of solving the inverse solution, the virtual joint is introduced, and in the process of controlling the loading arm motion through the PLC, the virtual joint is ignored. This way makes the inverse solution easier to solve.

[0051] (6) The fine positioning II stage of the known method is open-loop control without liquid phase camera guidance. Since the open-loop control has limited accuracy, the docking accuracy is limited. In the method, not only the fine positioning I stage is closed-loop control with liquid phase camera guidance, but also the fine positioning II stage is closed-loop control based on the column camera and liquid phase marker guidance, so as to ensure that the final docking process is always guided by the camera closed loop, and improve the docking accuracy.

[0052] The application will be described in further detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0053] Figure 1 is a structure schematic diagram of the automatic docking system of the land LNG double loading arms.

[0054] Figure 2 is an angle diagram of the third hard tube and the fourth joint axis of the gas phase arm and the liquid phase arm. Figure 2 (a) and (b) in the figure are angle diagrams of the third hard tube and the fourth joint axis of the gas phase arm and the liquid phase arm, respectively.

[0055] Figure 3 is a schematic diagram of the liquid phase arm flange and the tank truck liquid phase flange.

[0056] Figure 4 is an angle diagram of the liquid phase three hard tube and the liquid phase four joint axis.

[0057] Figure 5 is an angle diagram of the liquid phase three hard tube and the liquid phase four joint axis of the known method.

[0058] Figure 6 is a schematic diagram of the liquid phase actuator position comparison of the application and the known method.

[0059] Figure 7is a flow chart of the automatic docking method of the land LNG double-loading arm cooperative operation of the application.

[0060] Figure 8 is a schematic diagram of the D-H coordinate system of the liquid-phase loading arm of the known method.

[0061] Figure 9 is a schematic diagram of the D-H coordinate system of the liquid-phase loading arm of the application.

[0062] Figure 10 is a schematic diagram of the tank parking area and the flange motion trajectory planning of the liquid-phase arm.

[0063] Figure 11 is a schematic diagram of the docking process of the flange of the loading arm and the flange of the tank.

[0064] Figure 12 is a schematic diagram of the reachable working space of the flange of the loading and unloading arm of the application and the known method in the X-Y plane.

[0065] Figure 13 is the reachable working space of the flange of the loading and unloading arm of the application and the plumb line inverse solution verification diagram, wherein Figure 13 (a) in the figure is the planned plumb line, Figure 13 (b) in the figure is the joint angle after inverse solution, Figure 13 (c) in the figure is the actual trajectory, Figure 13 (d) in the figure is the comparison between the planned inverse solution trajectory and the actual inverse solution trajectory. DETAILED DESCRIPTION

[0066] In order to make the purpose, technical scheme and advantages of the application clearer and more apparent, the application will be further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the application and do not limit the application.

[0067] It should be noted that if the application examples involve directional indications (such as up, down, left, right, front, back, etc.), the directional indications are only used to explain the relative position relationship, movement condition, etc. between the components in a certain posture (as shown in the drawings), and if the certain posture changes, the directional indications will also change accordingly.

[0068] In one embodiment, as shown in Figure 1 , Figure 2 The application is a land LNG double-loading arm cooperative operation automatic docking system, which comprises a support column 1, a liquid-phase arm 2, a gas-phase arm 3, a column camera 75 and a laser radar 76 installed on the support column 1, wherein the liquid-phase arm 2 and the gas-phase arm 3 each comprise five joints, five hard tubes, a visual camera and a marker.

[0069] The liquid phase arm 2 comprises a liquid phase I joint 21, a liquid phase I hard tube 22, a liquid phase II joint 25, a liquid phase II hard tube 26, a liquid phase III joint 27, a liquid phase III hard tube 23, a liquid phase IV joint 24, a liquid phase IV hard tube 210, a liquid phase V joint 28, a liquid phase V hard tube 29, and a liquid phase executor 4, wherein the liquid phase V hard tube 29 is provided with a liquid phase camera 73 and a liquid phase marker 74; the gas phase arm 3 comprises a gas phase I joint 31, a gas phase I hard tube 32, a gas phase II joint 35, a gas phase II hard tube 36, a gas phase III joint 37, a gas phase III hard tube 33, a gas phase IV joint 34, a gas phase IV hard tube 310, a gas phase V joint 38, a gas phase V hard tube 39, and a gas phase executor 5, wherein the gas phase V hard tube 39 is provided with a gas phase camera 71 and a gas phase marker 72.

[0070] In the prior art, adjacent joints of the loading and unloading arm are connected in parallel or vertically, in the present application, the third joint hard tube axis of each loading and unloading arm and the fourth joint rotation axis form an acute angle, in the actual land LNG loading and unloading arm device, a flange is fixedly connected to the end of the loading and unloading arm, and a virtual wrist joint is added between the end of the loading and unloading arm and the flange, so that the flange can virtually rotate around the axis of the end of the loading and unloading arm.

[0071] As shown in Figure 2 , in the gas phase arm 3, the angle between the gas phase III hard tube 33 and the axis of the gas phase IV joint 34 is an acute angle, and in the present embodiment, the angle is 45 degrees. In the liquid phase arm 2, the angle between the liquid phase III hard tube 23 and the axis of the liquid phase IV joint 24 is an acute angle, and in the present embodiment, the angle is 45 degrees.

[0072] As shown in Figure 3 , the liquid phase executor 4 is connected to the liquid phase flange 41 of the liquid phase arm 2 and the liquid phase flange 5 of the tank car through the rotation of the joints of the liquid phase arm 2.

[0073] As shown in Figure 4 , the liquid phase arm 2 is a series open-chain mechanical arm, and the three-dimensional model of the liquid phase arm 2 is simplified, the joints of the liquid phase arm are replaced by pie columns, and the hard tubes of the liquid phase arm are replaced by cylindrical links, so as to facilitate the research on the actual liquid phase arm 2. In the present embodiment, the angle between the axis of the liquid phase III hard tube 23 and the axis of the liquid phase IV joint 24 is an acute angle, and preferably, the angle is 45 degrees.

[0074] As shown in Figure 4 and Figure 5 , in the prior art, the angle between the liquid phase III hard tube 23 and the axis of the liquid phase IV joint 24 is generally a right angle, i.e. 90 degrees. In the present application, the angle between the liquid phase III hard tube 23 and the axis of the liquid phase IV joint 24 is an acute angle, and preferably, the angle is 45 degrees.

[0075] As shown in Figure 4 , Figure 5 , Figure 6As shown, in the XOY plane of the fixed coordinate system, the liquid phase flange of the tank truck is generally located in the third quadrant, and the axis of the liquid phase flange of the tank truck is approximately parallel to the X-axis of the fixed coordinate system. In the known method, the rotation axes of the liquid phase III rigid pipe 23 and the liquid phase IV joint 24 form a 90-degree angle, causing the connection point of the liquid phase V rigid pipe 29 and the liquid phase V joint 28 to shift a distance L in the positive X-axis direction. Consequently, a rod length of L is required to compensate for this, resulting in a shorter effective length of the liquid phase V rigid pipe 29. This not only wastes the rod length of the liquid phase V rigid pipe 29, but also easily causes interference between the liquid phase V rigid pipe 29 and the liquid phase IV joint 24 and the liquid phase IV rigid pipe 210 because the liquid phase V rigid pipe 29 overlaps with them. In this invention, the angle between the axes of the liquid phase III rigid tube 23 and the liquid phase IV joint 24 is an acute angle, specifically 45 degrees in this embodiment. This allows the liquid phase IV rigid tube 210 to extend outwards, reducing the overlap between the liquid phase V rigid tube 29, the liquid phase IV joint 24, and the liquid phase IV rigid tube 210, thus avoiding interference and minimizing wasted length of the liquid phase V rigid tube 29. Simultaneously, the connection point between the liquid phase V rigid tube 29 and the liquid phase V joint 28 is shifted towards the negative X-axis, causing the liquid phase arm flange 41 to also shift towards the negative X-axis, expanding the proportion of the working space accessible to the liquid phase arm flange 41 in the third quadrant. Since the liquid phase arm flange 41 and the tank truck liquid phase flange typically dock in the third quadrant, this invention improves the utilization rate of the working space accessible to the liquid phase arm flange 41.

[0076] In one embodiment, an automated docking method for collaborative operation of dual LNG loading arms on land is provided, such as... Figure 7 As shown, it includes the following steps:

[0077] Step 1: Establish the coordinate system for the onshore LNG dual-loading arm. Taking the liquid phase arm as an example, the loading arm coordinate system is established based on the DH method. Figure 3 It is known that a liquid phase actuator 4 is fixedly connected to the end of the liquid phase V rigid tube 29 of the present invention, and a liquid phase arm flange 41 is fixedly connected to the liquid phase actuator 4. In known methods, considering that the liquid phase actuator 4 and the liquid phase arm flange 41 are fixedly connected to the liquid phase V rigid tube 29, and the pose of the liquid phase arm flange 41 is consistent with the pose of the liquid phase actuator 4 and the pose of the end of the liquid phase V rigid tube 29, the coordinate system of the liquid phase arm flange 41 is generally integrated into the coordinate system of the liquid phase V joint 28, that is, the coordinate system is established only according to the actual moving joint, such as... Figure 8 As shown. This invention adds a virtual rotary joint between the liquid phase actuator 4 and the liquid phase arm flange 41, allowing the liquid phase arm flange 41 to virtually rotate around the axis of the liquid phase actuator 4. Furthermore, the liquid phase arm flange 41 is virtually treated as an independent liquid phase arm flange joint, and an independent liquid phase arm flange joint coordinate system is established. The Z-axis of the liquid phase arm flange joint coordinate system is collinear with the axis of the liquid phase actuator 4. The coordinate system established by this invention is shown in the figure. Figure 9 As shown, Figure 9The difference between the coordinate system established by the present application and the coordinate system established by the known method is shown in the middle dashed box.

[0078] Step 2: Plan the tank car parking area. According to the reachable working space of the liquid phase arm flange 41, the size of the tank car 81 outer frame, and the position of the tank car liquid phase flange 813 on the tank car, plan the tank car parking area 82, as shown in the figure. In the tank car parking area 82, the tank car 81 can be parked in any position in the area with any attitude, but the tank car 81 body cannot exceed the area boundary. Figure 10

[0079] Next, the present application carries out the coarse positioning motion control of the liquid phase arm flange 41. The coarse positioning motion control process of the known method is as follows: first, detect the position of the tank car liquid phase flange 813 using the laser radar 76, then calculate the coarse positioning position according to the position of the tank car liquid phase flange 813 and the empirical formula, plan the motion trajectory of the liquid phase arm flange 41 from the starting point to the coarse positioning position, and finally control the liquid phase arm flange 41 to move along the trajectory. Since the position of the tank car 81 parked each time is different, the position of the tank car liquid phase flange 813 detected by the laser radar 41 each time is also different, and the coarse positioning position each time is also different. In the coarse positioning motion control process of the known method each time, the motion trajectory of the liquid phase arm flange 41 from the starting point to the coarse positioning position needs to be re-planned as a whole, and the trajectory planning time is relatively long. The present application divides the coarse positioning motion control process into two stages: coarse positioning I stage and coarse positioning II stage. First, determine the coarse positioning I position according to the tank car parking area 82 and the tank car flange recognition area, and plan the motion trajectory of the liquid phase arm flange 41 from the starting point to the coarse positioning I position. This trajectory is the coarse positioning I stage motion trajectory. After the coarse positioning I stage motion trajectory is planned, no matter what position the tank car 81 is parked in the tank car parking area 82 with what attitude, the coarse positioning I stage motion trajectory remains unchanged and does not need to be re-planned. Then, calculate the coarse positioning II position according to the position of the tank car liquid phase flange 813 detected by the laser radar 76 and the empirical formula, and plan the motion trajectory from the coarse positioning I position to the coarse positioning II position II. This trajectory is the coarse positioning II stage motion trajectory. In the coarse positioning process of the present application method each time, the coarse positioning I stage motion trajectory does not need to be re-planned, but the liquid phase arm flange 41 is directly controlled to move from the starting point to the coarse positioning I position, and then the coarse positioning II stage motion trajectory is only updated according to the specific position of the tank car flange each time. Compared with the known method, in the coarse positioning process each time, the coarse positioning I stage motion trajectory planning process is omitted, so the overall trajectory planning time of the coarse positioning motion control can be effectively reduced.

[0080] ​Step 3: Determine the coarse positioning I position 86. Outside the tank truck parking area 82, multiple liquid phase camera 73 scans the tank truck liquid phase flange 813 profile experiment, according to the liquid phase camera 73 scanning experiment results, determine the tank truck liquid phase flange 813 profile identification area 88, as shown in Figure 10 , the boundary point of the tank truck liquid phase flange 813 profile identification area 88 close to the side of the loading arm is taken as the coarse positioning I position 83, as shown in Figure 10 .

[0081] Step 4: Plan the motion trajectory 84 of the liquid phase arm flange 41 in the coarse positioning I stage. Take the liquid phase arm flange 41 parking position 83 as the starting point, and take the coarse positioning I position 86 as the terminal point, plan the motion trajectory of the liquid phase arm flange 41, which is outside the tank truck parking area 82 to avoid collision between the loading arm and the tank truck outer frame, and the planning result is as shown in Figure 10 .

[0082] Step 5: Discretize the trajectory points and determine the pose matrix of each trajectory point. Discretize the motion trajectory 84 into several trajectory points, and determine the pose matrix of each point as shown in formula (1):

[0083]

[0084] The angle between the liquid phase III hard tube 23 and the liquid phase IV joint 24 axis of the present application is an acute angle, so that in the process of controlling the movement of the liquid phase arm flange 41, the rotation of the liquid phase arm flange 41 will also be caused, resulting in the coupling relationship between the movement freedom and the rotation freedom of the liquid phase arm flange 41. In Figure 8 The coordinate system established by the known method, the position items p x , p y , p z and the rotation items r 11 , r 21 , r 31 , r 12 , r 22 , r 32 , r 13 , r 23 , r 33 in the target pose matrix shown in formula (1) exist a coupling relationship, when the values of the position items p x , p y , p z are given, the rotation items r 11 , r 21 , r 31 , r 12 , r 22 , r 32 , r 13 , r 23 , r 33The value of the target pose matrix is difficult to set up, and if the corresponding relationship between the position term and the rotation term does not satisfy the coupling relationship, no effective solution will be obtained in the subsequent inverse solution operation, so in the coordinate system established based on the known method as shown in Figure 8 , the virtual rotary joint is added between the liquid phase actuator 4 and the liquid phase arm flange 41, so that the liquid phase arm flange 41 can virtually rotate around the axis of the liquid phase actuator 4, and the liquid phase arm flange 41 is virtually independent of the liquid phase arm flange joint, and an independent liquid phase arm flange joint coordinate system is established, the Z axis of the liquid phase arm flange joint coordinate system is collinear with the axis of the liquid phase actuator 4, and the coordinate system as shown in Figure 9 is established. In the coordinate system as shown in Figure 9 , the rotation caused by the movement of the liquid phase arm flange 41 will be offset by the virtual rotation of the liquid phase arm flange joint coordinate system, thereby realizing the virtual decoupling of the movement degree of freedom and the rotation degree of freedom of the liquid phase arm flange 41, which is specifically manifested in that when the target pose matrix shown in formula (1) is set, if the position terms p x , p y , p z and the rotation terms r 11 , r 21 , r 31 , r 12 , r 22 , r 32 , r 13 , r 23 , r 33 do not satisfy the coupling relationship, effective solutions can be obtained in the subsequent inverse solution operation, thereby reducing the difficulty of setting up the target pose matrix.

[0085] Step 6: Inverse solution of each trajectory point corresponding to each joint angle. The present application uses Newton iteration method to solve inverse kinematics, and in the solving process, it is necessary to construct the Jacobian matrix. The known method only introduces the actual existing joint variables, such as liquid phase I joint to liquid phase V joint, into the Jacobian matrix to solve the inverse solution, and in the present application, not only the actual existing joint variables, such as liquid phase I joint to liquid phase V joint, are introduced into the Jacobian matrix, but also the virtual joint, such as the liquid phase arm flange joint, is added to the Jacobian matrix as an independent joint variable to solve the inverse solution together. The specific process is as follows:

[0086] (1) The target pose matrix of the trajectory point is known , the values of the six joint angles θ1, θ2, … θ5, θ6 when reaching this pose are solved, and a nonlinear equation group is constructed: F(θ1, θ2, …, θ5, θ6) = (f1, f2, …, f n ) T ;

[0087] (2) The homogeneous transformation matrix of the final liquid phase arm flange coordinate system {6} relative to the fixed coordinate system {0} is known A homogeneous transformation matrix can also be obtained for each round of iteration i is the number of iterations, i = 0, 1, 2…. Subtract all elements in the two matrices one by one to get a system of equations As shown in equation (2).

[0088]

[0089] (3) The inverse Jacobian matrix of this system of equations can be determined from equation (2) as equation (3),

[0090]

[0091] (4) From equation θ i+1 = θ i -J i (θ i ) -1 F(θ i ) iteratively solve each joint angle.

[0092] In step 6, the formulas involved in the inverse solution process of the known method are only θ1: θ5, and the formulas involved in the inverse solution process of the patent method of the invention contain θ1: θ6.

[0093] Step 7: Trajectory planning is performed for each joint to obtain the joint angle of each joint at each time. Take liquid phase I joint 21 as an example. Based on the cubic spline interpolation algorithm, the joint angle values of liquid phase I joint 21 at various trajectory points are fitted into a trajectory, making the trajectory of liquid phase I joint 21 smooth and stable. Repeat this process for liquid phase II joint 25 to liquid phase V joint 28 and the liquid phase arm flange joint to complete the trajectory planning of liquid phase II joint 25 to liquid phase V joint 28 and the liquid phase arm flange joint. Based on the joint space motion trajectory of liquid phase I joint 21 to liquid phase V joint 28 and the liquid phase arm flange joint, update the motion trajectory 84 of the liquid phase arm flange 41 in the coarse positioning I stage. After the update is completed, no matter what attitude the tank car is parked in the tank car parking area 82 domain, the motion trajectory 84 remains unchanged and does not need to be re-planned.

[0094] Step 8: send the joint space planning trajectory of liquid phase I joint 21 to liquid phase V joint 28 to the PLC, and control the liquid phase arm flange 41 to move along the motion trajectory 84 from the starting point 83 to the coarse positioning I position 86 by the PLC. The difference between the present application and the known method is that in the known method, the PLC controller controls all the calculated joints to move; the inverse solution of the present application contains actual joint angles and virtual joint angles, and the PLC controller only controls the actual joints to move along the joint space planning trajectory, and does not control the virtual joints. Since the tank car liquid phase flange and the liquid phase arm flange are both circular, the rotation of the liquid phase arm flange along the axis does not affect the docking result, so the virtual joint is not controlled, which does not affect the docking result.

[0095] Step 9: measure the position of the tank car liquid phase flange 813 by laser radar, and calculate the coarse positioning II position 89 based on the position of the tank car liquid phase flange 813 and the empirical formula, as shown in Figure 10 , to ensure that the liquid phase camera 73 can detect the pose information of the tank car liquid phase flange 813 before the liquid phase arm flange 41 moves to the coarse positioning II position 89.

[0096] Step 10: plan the motion trajectory 87 of the end flange 41 in the coarse positioning stage II. Take the coarse positioning I position 86 as the starting point and the coarse positioning II position 89 as the end point to plan the motion trajectory of the end flange 41, and the planning result is shown in Figure 10 .

[0097] Step 11: control the liquid phase arm flange 41 to move along the motion trajectory 87 by the PLC, and in the process of movement, the liquid phase camera 73 detects the pose information of the tank car liquid phase flange 813 in real time, if the liquid phase camera 73 cannot detect the pose information of the tank car liquid phase flange 813, then control the liquid phase arm flange 41 to continue moving along the motion trajectory 87, if the liquid phase camera 73 detects the pose information of the tank car liquid phase flange 813, then the liquid phase arm flange 41 stops moving, and the current position is taken as the coarse positioning II position.

[0098] The steps 3 to 11 of the present application complete the coarse positioning motion control of the liquid phase arm flange 41.

[0099] Next, the application carries out fine positioning control of the liquid phase arm flange 41. The fine positioning motion control of the known method is divided into two stages: fine positioning I stage and fine positioning II stage. The fine positioning I stage is closed-loop control guided by the liquid phase camera 73, and the fine positioning II stage is open-loop control without the guidance of the liquid phase camera 73. Since the open-loop control has limited accuracy, the docking accuracy is limited. The fine positioning motion control of the method of the application is also divided into two stages: fine positioning I stage and fine positioning II stage. Unlike the known method, the fine positioning I stage of the method of the application is not only closed-loop control guided by the liquid phase camera 73, but also fine positioning II stage is closed-loop control guided based on the column camera 75 and the liquid phase marker 74. In order to ensure that the final docking process is always guided by the camera closed loop, the docking accuracy is improved.

[0100] Next, the application carries out fine positioning I stage motion control of the liquid phase arm flange 41.

[0101] Step 12: Based on the liquid phase camera 73, the pose matrix of the tank car liquid phase flange 813 relative to the liquid phase arm flange coordinate system is collected

[0102] Step 13: Based on the column camera 75, the pose matrix of the liquid phase marker 74 relative to the column camera 75 coordinate system is collected Based on the measurement, the pose matrix of the liquid phase arm flange 41 relative to the liquid phase marker 74 coordinate system is obtained Combined with the pose matrix of the tank car liquid phase flange 813 relative to the liquid phase arm flange coordinate system collected by the liquid phase camera 73 The pose matrix of the tank car liquid phase flange 813 relative to the column camera 75 coordinate system is calculated As shown in formula (4).

[0103]

[0104] Step 14: Read the current angle values of each joint of the liquid phase arm 2, and calculate the homogeneous transformation matrix of each joint Further calculate the pose matrix of the liquid phase arm flange 41 coordinate system relative to the fixed coordinate system As shown in formula (5), the pose matrix of the tank car liquid phase flange 813 relative to the fixed coordinate system is calculated As shown in formula (6).

[0105]

[0106] Step 15: According to the pose matrix of the tank car liquid phase flange 813 relative to the fixed coordinate system The precise positioning position 811 is planned. This precise positioning position 89 is on the axis of the liquid phase flange 813 of the tank truck, and the distance from it to the center of the liquid phase flange 813 of the tank truck is D0. D0 is determined according to the size of the guide rod on the liquid phase actuator 4.

[0107] Step 16: Plan the motion trajectory 810 of the liquid phase arm flange 41 from the current position to the precise positioning position 811, and calculate the distance D1 between the liquid phase arm flange 41 and the precise positioning position 811 along the motion trajectory 810.

[0108] Step 17: If D1>D throne D throne To set a smaller threshold, indicating that the liquid phase arm flange 41 has not yet reached the precise positioning position 811, a floor (D1 / D) is uniformly inserted between the start and end points on the trajectory 810. thrtwo ) discrete points, where floor represents rounding down, D thrtwo This represents the distance the liquid phase arm flange 41 moves in each step. The inserted discrete point, together with the starting point and the ending point, constitutes the trajectory point. The pose matrix of the second trajectory point is determined, and the joint angle corresponding to the pose matrix is ​​solved inversely. The liquid phase arm flange 41 is controlled by the PLC to move along the trajectory 810 from the current trajectory point to the second trajectory point, and then returns to step 12.

[0109] Step 18: If D1≤D throne This indicates that the liquid phase arm flange 41 has reached the precise positioning position 811, and the motion control of the precise positioning stage I has ended.

[0110] Next, this invention performs motion control for the second stage of precise positioning of the liquid phase arm flange 41. Due to limitations in the structure size of the liquid phase arm and the installation position of the liquid phase camera 73, the liquid phase camera 73 struggles to consistently detect the position and orientation information of the tank truck's liquid phase flange 813 during the second stage of precise positioning. In known methods, open-loop control docking without liquid phase camera guidance is used in the second stage of precise positioning. However, due to the limited accuracy of open-loop control, the docking accuracy is limited. This invention, in the second stage of precise positioning, uses the column camera 75 and the liquid phase marker 74 to acquire the position and orientation information of the tank truck's liquid phase flange 813 relative to the coordinate system of the liquid phase arm flange 41 in real time. This ensures that the docking process is always guided by a closed-loop camera, improving the docking accuracy.

[0111] Step 19: Acquire the pose matrix of the liquid marker 74 relative to the coordinate system of the pillar camera 75 based on the pillar camera 75. Calculate the pose matrix of the tank truck liquid phase flange 813 relative to the liquid phase arm flange 41 coordinate system. As shown in formula (7).

[0112]

[0113] Step 20: read the current angle values of each joint of the liquid phase arm 2, and calculate the homogeneous transformation matrix of each joint Further calculate the pose matrix of the liquid phase arm flange 41 coordinate system relative to the fixed coordinate system As shown in formula (5), calculate the pose matrix of the tank car liquid phase flange 813 relative to the fixed coordinate system As shown in formula (6).

[0114] Step 21: plan the motion trajectory 815 of the liquid phase arm flange from the current position to the position of the tank car flange 813, and calculate the distance D2 between the liquid phase arm flange 41 and the tank car flange 813 along the motion trajectory 815.

[0115] Step 22: if D2>D throne , it means that the tank car flange 41 has not reached the position of the tank car liquid phase flange 813, then evenly insert floor(D2 / D thrtwo ) discrete points between the start point and the end point on the trajectory 815, and the inserted discrete points together with the start point and the end point constitute the trajectory points, determine the pose matrix of the second trajectory point, inverse solve the joint angle corresponding to the pose matrix, control the liquid phase arm flange 41 to move from the current trajectory point to the second trajectory point along the trajectory 815 through PLC, and return to step 19.

[0116] Step 23: if D2≤D throne , it means that the liquid phase arm flange 41 has reached the position of the tank car liquid phase flange 813, and the motion control of the fine positioning II stage is ended.

[0117] Next, the present application carries out the recovery motion control of the liquid phase arm flange 41.

[0118] Step 24: plan the recovery transition position. According to the pose matrix of the tank car liquid phase flange 813 relative to the fixed coordinate system plan the recovery transition position 814, which is on the axis of the tank car liquid phase flange 813 and has a distance D3 to the center of the tank car liquid phase flange 813, D3 being determined according to the size of the guide rod on the liquid phase actuator 4.

[0119] Step 25: control the liquid phase arm flange 41 to exit to the recovery transition position 814 through PLC.

[0120] Step 26: plan the motion trajectory 85 of the liquid phase arm flange 41 from the recovery transition position 814 to the start point 83. The trajectory is outside the tank car parking area 82 to avoid collision between the loading arm and the outer frame of the tank car, and the planning result is as shown in Figure 10 .

[0121] Step 27: control the loading arm flange to move from the recovery transition position 814 to the start point 83 along the trajectory 85 through PLC.

[0122] The docking process is now complete.

[0123] like Figure 11 As shown, taking the liquid phase arm as an example, the docking process between the liquid phase arm flange and the tank truck liquid phase flange is illustrated: During the coarse positioning process, the liquid phase arm flange moves from position ξ0 to coarse positioning I position ξ5, and then moves from coarse positioning I position ξ5 to coarse positioning II position ξ1; During the fine positioning process, the liquid phase arm flange moves from coarse positioning II position ξ1 to fine positioning position ξ2. At this time, the liquid phase arm flange and the tank truck liquid phase flange are closer, and the axis of the liquid phase arm flange coincides with the axis of the tank truck liquid phase flange; During the docking process, the liquid phase arm flange remains unchanged under the condition that the normal line remains unchanged. Under these conditions, the liquid arm flange moves forward along the axis of the tank truck's liquid phase flange, from the precise positioning position ξ2 to the liquid phase arm flange position ξ3. At this point, the liquid phase arm flange and the tank truck's liquid phase flange are successfully docked. In recovery stage I, the liquid phase arm flange retracts along the axis of the tank truck's liquid phase flange, moving from position ξ3 to the recovery transition position ξ4. At this point, the axis of the liquid phase arm flange coincides with the axis of the tank truck's liquid phase flange, but the liquid phase arm flange and the tank truck's liquid phase flange have separated. In recovery stage II, the liquid phase arm flange moves from the recovery transition position ξ4 to the starting position ξ0.

[0124] The overall docking process of the onshore LNG dual-loading arm collaborative automatic docking system is as follows: Initially, the liquid phase arm and the gas phase arm are docked on the support column; the liquid phase arm is coarsely positioned; the gas phase arm is coarsely positioned; the liquid phase arm is finely positioned; the gas phase arm is finely positioned; the gas phase arm is retracted and finally docked on the support column; the liquid phase arm is retracted and finally docked on the support column.

[0125] As a specific example, the invention is further illustrated in one embodiment.

[0126] like Figure 9 As shown in Table 1, taking the liquid phase arm of the present invention as an example, a loading arm coordinate system is established and the joint parameters are instantiated.

[0127] Table 1. DH Parameter Table for Instantiated Liquid Phase Arm of this Patent

[0128]

[0129]

[0130] Based on Table 1, the homogeneous transformation matrices for each joint are calculated as follows:

[0131]

[0132] Considering that the tank truck flange is generally located in the third quadrant, and the tank truck flange axis is approximately parallel to the fixed coordinate system X axis, in the embodiment, the posture of the liquid phase arm flange is set as the liquid phase arm flange coordinate system Z axis being always parallel to the fixed coordinate system X axis, and under this condition, the reachable working space range of the liquid phase arm flange is obtained as shown in Figure 12 Compared with the known method, more parts of the reachable working space of the liquid phase arm flange are in the third quadrant, which can be used for docking with the tank truck flange, and the utilization rate of the end effector reachable space is improved.

[0133] As shown in (a) of Figure 13 , taking the inverse solution of any plumb line trajectory as an example, the coordinates of the trajectory starting point in the fixed coordinate system are (-1000, -2500, 300), and the coordinates of the trajectory ending point in the fixed coordinate system are (-1000, -2500, 1100), using the method of the present application, the inverse solution of the liquid phase arm flange motion is solved, and the inverse solution result is shown in (b) of Figure 13 , wherein theta6 is the virtual rotation angle of the virtual joint, the inverse solution is input into the forward solution model to obtain the model output actual trajectory as shown in (c) of Figure 13 , the planned trajectory in (a) of Figure 13 is unified with the model output actual trajectory in (c) of Figure 13 in a figure to obtain (d) of Figure 13 , by comparing the planned trajectory and the model output actual trajectory, it can be seen that the planned trajectory is consistent with the model output actual trajectory, which indicates that the inverse solution method of the present application is correct. In addition, theta6 is the virtual rotation angle of the virtual joint, and the inverse value of the virtual joint angle is the angle value of the liquid phase arm flange rotating around its own axis.

[0134] In summary, the present application realizes automatic docking of the double-tank truck arm end effector and the tank truck flange, improves the automation level of the truck loading arm, and further improves the utilization rate of the reachable working space of the truck loading arm end effector during the docking process, reduces the difficulty of setting the target pose matrix, reduces the trajectory planning time, and improves the docking accuracy.

[0135] The basic principles, main features and advantages of the present application are shown and described above. It should be understood by those skilled in the art that the present application is not limited by the above examples, and the above examples and descriptions in the specification are only to illustrate the principles of the present application. Any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. An automated docking method for collaborative operation of dual-arm LNG loading trucks on land, characterized in that, An automatic docking system is set up, which includes a support column (1), and a liquid phase arm (2), a gas phase arm (3), a column camera (75) and a lidar (76) installed on the support column (1). The liquid phase arm (2) and the gas phase arm (3) each include five joints, five rigid tubes, a vision camera and a marker. The liquid phase arm (2) includes, in sequence, a liquid phase I joint (21), a liquid phase I rigid tube (22), a liquid phase II joint (25), a liquid phase II rigid tube (26), a liquid phase III joint (27), a liquid phase III rigid tube (23), a liquid phase IV joint (24), a liquid phase IV rigid tube (210), a liquid phase V joint (28), a liquid phase V rigid tube (29), and a liquid phase actuator (4). A liquid phase camera (73) and a liquid phase marker (74) are installed on the liquid phase V rigid tube (29). The axis of the III rigid tube in each loading arm forms an acute angle with the rotation axis of the IV joint. A flange is fixedly connected to the end of the loading arm. The automatic docking method includes a coarse positioning stage, a fine positioning stage, a recovery stage I, and a recovery stage II. The coarse positioning stage is divided into coarse positioning stage I and coarse positioning stage II. During the coarse positioning process of the liquid phase arm, the liquid phase arm flange moves from the starting position ξ0 to the coarse positioning I position ξ5, and then moves from the coarse positioning I position ξ5 to the coarse positioning II position ξ1. Specifically, the following steps are included: Step 1, establish the coordinate system of the land-based LNG dual loading arm: add a virtual rotary joint between the liquid phase actuator and the liquid phase arm flange, further virtualize the liquid phase arm flange as an independent liquid phase arm flange joint, and establish an independent liquid phase arm flange joint coordinate system. The Z-axis of the liquid phase arm flange joint coordinate system is collinear with the axis of the liquid phase actuator. Step 2: Plan the tanker truck parking area; Step 3, determine coarse positioning I position ξ5; outside the tank truck parking area, determine the tank truck liquid phase flange contour recognition area using a liquid phase camera, and take the boundary point of the tank truck liquid phase flange contour recognition area near the loading arm as coarse positioning I position ξ5. Step 4, plan the motion trajectory of the liquid phase arm flange in the coarse positioning stage I (84); Step 5: Discretize the trajectory points and determine the pose matrix for each trajectory point; Step 6, solve the joint angles corresponding to each trajectory point in reverse: use Newton's iteration method to solve the inverse kinematics. In the solution process, it is necessary to construct the Jacobian matrix and add the liquid phase arm flange joint as an independent joint variable to the Jacobian matrix to solve the inverse together. Step 7: Perform angle trajectory planning for each joint to obtain the joint angle at each moment, and then obtain the motion trajectory of each joint, thereby updating the motion trajectory of the liquid phase arm flange coarse positioning stage I (84). Step 8: Control the liquid arm flange to move from the starting point to the coarse positioning I position ξ5 along the updated liquid arm flange coarse positioning I stage motion trajectory (84) via PLC; PLC only controls the actual joints and does not control the virtual joints. Step 9: Measure the position of the liquid phase flange of the tank truck using lidar and calculate the coarse positioning II position; Step 10, plan the motion trajectory of the liquid phase arm flange coarse positioning stage II (87); Step 11: Control the liquid phase arm flange to move along the movement trajectory (87) of the coarse positioning II stage of the liquid phase arm flange to the coarse positioning II position ξ1 by PLC: During the movement, the liquid phase camera detects the position and orientation information of the liquid phase flange of the tank truck in real time. When the position and orientation information of the liquid phase flange of the tank truck is detected, the liquid phase arm flange stops moving and takes the current position as the coarse positioning II position ξ1.

2. The automatic docking method for collaborative operation of dual-loading arms of LNG trucks on land according to claim 1, characterized in that, The gas phase arm (3) includes a gas phase I joint (31), a gas phase I rigid tube (32), a gas phase II joint (35), a gas phase II rigid tube (36), a gas phase III joint (37), a gas phase III rigid tube (33), a gas phase IV joint (34), a gas phase IV rigid tube (310), a gas phase V joint (38), a gas phase V rigid tube (39), and a gas phase actuator (5) connected in sequence. A gas phase camera (71) and a gas phase marker (72) are provided on the gas phase V rigid tube (39). During the fine positioning process of the liquid arm flange, the liquid arm flange moves from the coarse positioning II position ξ1 to the fine positioning position ξ2. At this time, the axis of the liquid arm flange coincides with the axis of the tank truck liquid flange. During the docking process, the liquid arm flange moves forward along the axis of the tank truck liquid flange while the normal remains unchanged, moving from the fine positioning position ξ2 to the tank truck liquid flange position ξ3. At this time, the docking of the liquid arm flange and the tank truck liquid flange is completed. During the recovery phase I, the liquid arm flange retracts along the tank truck liquid flange axis and moves from position ξ3 to the recovery transition position ξ4. At this time, the liquid arm flange axis coincides with the tank truck liquid flange axis, but the liquid arm flange and the tank truck liquid flange have separated. During the recovery phase II, the liquid phase arm flange moves from the recovery transition position ξ4 to the starting position ξ0.

3. The automatic docking method for collaborative operation of dual-loading arms of LNG on land according to claim 2, characterized in that, The docking method further includes the following steps: Step 12: Acquire the pose matrix of the tank truck's liquid phase flange relative to the joint coordinate system of the liquid phase arm flange based on the liquid phase camera; Step 13: Based on the pose matrix of the liquid phase marker relative to the coordinate system of the column camera, the pose matrix of the liquid phase arm flange relative to the coordinate system of the liquid phase marker is obtained by measurement. Combined with the pose matrix of the tank truck liquid phase flange relative to the joint coordinate system of the liquid phase arm flange acquired by the liquid phase camera, the pose matrix of the tank truck liquid phase flange relative to the coordinate system of the column camera is calculated. Step 14: Read the current angle values ​​of each joint of the liquid phase arm and calculate the pose matrix of the tank truck liquid phase flange relative to the fixed coordinate system. Step 15, plan the precise positioning position ξ2; Step 16: Plan the motion trajectory (810) of the liquid arm flange from the current position to the precise positioning position ξ2, and calculate the distance D1 between the liquid arm flange and the precise positioning position ξ2 along the motion trajectory (810). Step 17: If D1 is greater than the preset threshold, control the liquid phase arm flange to move a distance along the planned motion trajectory (810) through the PLC, and return to execute step 12; Step 18: If D1 is less than or equal to the preset threshold, it indicates that the liquid phase arm flange has reached the fine positioning position ξ2, and the motion control of the fine positioning stage I ends. Step 19: Based on the column camera and liquid phase markers, calculate the pose matrix of the tank truck liquid phase flange relative to the joint coordinate system of the liquid phase arm flange. Step 20: Read the current angle values ​​of each joint of the liquid phase arm and calculate the pose matrix of the tank truck liquid phase flange relative to the fixed coordinate system; Step 21: Plan the motion trajectory (815) of the liquid arm flange from the current position to the liquid flange position ξ3 of the tank truck, and calculate the distance D2 between the liquid arm flange and the liquid flange of the tank truck along the motion trajectory (815). Step 22: If D2 is greater than the preset threshold, the liquid phase arm flange is controlled by PLC to move a distance along the planned motion trajectory (815) and return to step 19. Step 23: If D2 is less than or equal to the preset threshold, it indicates that the liquid phase arm flange has reached the liquid phase flange position ξ3 of the tank truck, and the motion control of the fine positioning II stage ends. Step 24, plan the recycling transition location ξ4; Step 25: Control the liquid phase arm flange to retract to the recovery transition position ξ4 via PLC. Step 26, plan the motion trajectory of the liquid phase arm flange from the recovery transition position ξ4 to the starting point (85); Step 27: Control the liquid phase arm flange to move from the recovery transition position ξ4 to the starting point via PLC; For the gas phase arm, the process is the same as described above.

4. The automatic docking method for collaborative operation of dual-loading arms of LNG on land according to claim 3, characterized in that, If D1 is greater than the preset threshold in step 17, then floor(D1 / D) is uniformly inserted between the start and end points of the planned motion trajectory (810). thrtwo ) discrete points; where D thrtwo This indicates the distance the liquid phase arm flange moves in each step, with floor indicating rounding down.

5. The automatic docking method for collaborative operation of dual-loading arms of LNG on land according to claim 3, characterized in that, In step 22, if D2 is greater than the preset threshold, then floor(D2 / D) is uniformly inserted between the start and end points of the planned motion trajectory (810). thrtwo ) discrete points; where D thrtwo This indicates the distance the liquid phase arm flange moves in each step, with floor indicating rounding down.

Images