Method and system for motion control and error compensation of offshore transfer docking device

Abstract

The application provides a motion control and error compensation method and system for a marine transfer docking device, which comprises the following steps: S1: moving a multi-legged robot to a target docking position above by a crane to preliminarily align each foot with a target docking point; S2: acquiring the coordinates of a foot bottom target following point by using a camera, and precisely making each foot bottom reach the following point directly above the target docking point by controlling the extension and retraction of three groups of telescopic cylinders; S3: lowering the robot by the crane, and driving the cylinders to extend and retract by a mobility control model when the feet touch the ground, so as to realize the coincidence of the feet and the target docking point and the landing buffer; S4: based on the fixed coordinates of a load following point in a ship coordinate system, controlling the extension and retraction of the cylinders to adjust the body pose, so as to accurately position the goods at the following point directly above the load docking point; and S5: controlling the extension and retraction of the cylinders to lower the body, and accurately delivering the goods to the load docking point.

Description

Technical Field

[0001] This invention relates to the field of ship systems and equipment control technology, specifically to a motion control and error compensation method and system for a marine transshipment docking device. Background Technology

[0002] Quadruped robots are characterized by their flexible movement and precise control. When used in conjunction with a crane, the quadruped robot is firmly fixed to the cargo. The crane lifts the quadruped robot and the cargo for transshipment. The quadruped robot then docks with the target vessel via its legs, adjusting the cargo's orientation to achieve docking. This method allows for precise docking of cargo in any position and orientation, offering good versatility. However, the marine environment is constantly changing, and transshipment docking systems are characterized by multi-source disturbances, large hysteresis, variable parameters, strong coupling, and nonlinear operation.

[0003] Several control methods exist for offshore transshipment and docking, but traditional controllers struggle to meet the required control accuracy. Existing advanced control methods, however, are largely dependent on system parameters, making them difficult to implement in practical engineering. Among similar invention patents, patent CN121578648A proposes a composite control method combining fuzzy adaptive PID and adaptive terminal sliding mode. This method uses a hybrid architecture of inverse kinematics feedforward and hierarchical terminal sliding mode feedback, employing an improved smoothing sign function to suppress sliding mode chattering, achieving high-precision control and fast response. However, this method suffers from high system complexity, numerous control parameters, and difficult tuning. Patent CN121721942A proposes an improved Smith predictive control method with an output delay observer. This method constructs an augmented state-space model and uses an adaptive observer to estimate the time-varying output delay online, combining inverse kinematics feedforward control to correct the terminal position, achieving high control accuracy. However, this method relies on model accuracy, and observer tuning is complex.

[0004] Therefore, there is a need to propose a robust controller that is not heavily dependent on models, can achieve high-precision control, and is easy to implement in engineering. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the purpose of this invention is to provide a motion control and error compensation method and system for a marine transshipment and docking device.

[0006] According to one aspect of the present invention, a motion control and error compensation method for a marine transshipment docking device is applied to a multi-legged robot. The multi-legged robot includes a body, legs, feet, a cargo docking device, and a camera. The upper surface of the body is provided with lifting points, and multiple legs are distributed around the outer perimeter of the lower surface of the body. Each leg includes three Hooke hinges, three telescopic electric cylinders, two ball joints, and an ankle joint. The three Hooke hinges are respectively fixed to the lower surface of the body. The bases of the three telescopic electric cylinders are respectively connected to the three Hooke hinges. The telescopic end of one telescopic electric cylinder is fixedly connected to the ankle joint, and the telescopic ends of the other two telescopic electric cylinders are respectively connected to the ankle joint through two ball joints. The feet are installed at the ends of the ankle joints. The cargo docking device is located in the middle of the lower surface of the body, and the camera is located on the side of the body. The method includes: Step S1: Using the crane and lifting points in coordination, control the multi-legged robot to move above the target docking position, so that each leg is initially aligned with the target docking point; Step S2: Obtain the coordinates of the target following point at the bottom of the foot in the camera coordinate system using the camera, and control the extension and retraction of the three telescopic electric cylinders to position the bottom of the foot of each outrigger at the coordinates of the target following point; wherein, the target following point is located directly above the target docking point; Step S3 involves lowering the multi-legged robot using a crane until the bottom of its legs lands at the target docking point. When the bottom of the legs contacts the area where the target docking point is located, the admittance control model drives three telescopic electric cylinders to extend and retract, thereby aligning the bottom of the legs with the target docking point and cushioning the landing of the bottom of the legs. Step S4: Based on the fixed coordinates of the load following point in the ship coordinate system, the fuselage position is adjusted by controlling the extension and retraction of three telescopic electric cylinders so that the cargo is located at the fixed coordinates of the load following point; wherein, the load following point is located directly above the load docking point; Step S5: Based on the fixed coordinates of the load docking point in the ship's coordinate system, the fuselage is lowered by controlling the extension and retraction of three telescopic electric cylinders, so that the cargo is positioned at the load docking point.

[0007] Preferably, in steps S2, S3, S4, and S5, the extension and retraction control of the three telescopic electric cylinders is achieved through the following method: Obtain the target telescopic length of the three telescopic electric cylinders ; Motion planning is performed based on the current length of the electric cylinder, where the length L of the telescopic electric cylinder satisfies the following relationship:

[0008] in, This is the current length of the joint; This is the initial time. The moment when the electric cylinder reaches the target extension / retraction length; For one Time is 0. A fifth-degree polynomial function with time interval 1 is used to ensure that the position, velocity, and acceleration of the electric cylinder remain continuous and do not change abruptly during the movement of the cylinder.

[0009] Preferably, in step S2, the target telescopic length of the three telescopic electric cylinders is obtained in the following way: The target length of the telescopic electric cylinder when the foot is located at the target following point is calculated using a leg inverse kinematics model. Among them, the inverse kinematics model of the legs:

[0010]

[0011] in, The Jacobian matrix represents the inverse kinematics of the robot's legs; This indicates the coordinates of the target following point in the fuselage coordinate system; H represents the coordinates of the target docking point in the fuselage coordinate system, and H represents the height difference between the target following point and the target docking point. Preferably, in step S3, the target telescopic length of the three telescopic electric cylinders is obtained in the following way: The deviation between the actual and desired positions of the foot's sole is obtained using an admittance control model; the admittance control model is as follows:

[0012] In the formula, This is the quality matrix; Here is the damping matrix; Here is the stiffness matrix; The impact force when the bottom of the foot comes into contact with the area where the target docks; Set the admittance value; , indicating the robot's desired position With actual location Deviation between; for The first derivative; for The second derivative; Based on the deviation, the coordinates of the target's desired position are obtained, i.e.:

[0013] The target length of the telescopic electric cylinder when the bottom of the foot is at the desired target position is determined by using the inverse kinematics model of the leg.

[0014] Preferably, in step S3, from the moment the bottom of the foot begins to make contact with the area where the target docking point is located until the docking with the target docking point is completed, the threshold of admittance control is gradually increased, so that the force on the robot's foot gradually increases, and the robot's gravity is gradually transferred from the wire rope to the foot; when the tension of the crane wire rope is detected to be 0, the crane is released to complete the docking with the target docking point.

[0015] Preferably, in steps S4 and S5, the target length of the telescopic electric cylinder is obtained through manual teaching.

[0016] According to another aspect of the present invention, a motion control and error compensation system for a marine transshipment docking device is applied to a multi-legged robot. The multi-legged robot includes a body, legs, feet, a cargo docking device, and a camera. The upper surface of the body is provided with lifting points, and multiple legs are distributed around the outer perimeter of the lower surface of the body. Each leg includes three Hooke hinges, three telescopic electric cylinders, two ball joints, and an ankle joint. The three Hooke hinges are respectively fixed to the lower surface of the body. The bases of the three telescopic electric cylinders are respectively connected to the three Hooke hinges. The telescopic end of one telescopic electric cylinder is fixedly connected to the ankle joint, and the telescopic ends of the other two telescopic electric cylinders are respectively connected to the ankle joint through two ball joints. The feet are installed at the ends of the ankle joints. The cargo docking device is located in the middle of the lower surface of the body, and the camera is located on the side of the body. The system includes: Module M1: Through the cooperation of the crane and lifting points, the multi-legged robot is controlled to move above the target docking position, so that each leg is initially aligned with the target docking point; Module M2: It acquires the coordinates of the target following point at the bottom of the foot in the body coordinate system through the camera, and controls the extension and retraction of three telescopic electric cylinders to position the bottom of the foot of each leg at the coordinates of the target following point; wherein, the target following point is located directly above the target docking point; Module M3 lowers the multi-legged robot using a crane until the bottom of its legs lands at the target docking point. When the bottom of the leg contacts the area where the target docking point is located, the admittance control model drives three telescopic electric cylinders to extend and retract, so that the bottom of the leg coincides with the target docking point and the bottom of the leg is cushioned upon landing. Module M4: Based on the fixed coordinates of the load following point in the ship's coordinate system, the fuselage position is adjusted by controlling the extension and retraction of three telescopic electric cylinders so that the cargo is located at the fixed coordinates of the load following point; wherein, the load following point is located directly above the load docking point; Module M5: Based on the fixed coordinates of the load docking point in the ship's coordinate system, the fuselage is lowered by controlling the extension and retraction of three telescopic electric cylinders, so that the cargo is positioned at the load docking point.

[0017] Preferably, in modules M2, M3, M4, and M5, the extension and retraction control of the three telescopic electric cylinders is achieved through the following method: Obtain the target telescopic length of the three telescopic electric cylinders ; Motion planning is performed based on the current length of the electric cylinder, where the length L of the telescopic electric cylinder satisfies the following relationship:

[0018] in, This is the current length of the joint; This is the initial time. The moment when the electric cylinder reaches the target extension / retraction length; For one Time is 0. A fifth-degree polynomial function with time interval 1 is used to ensure that the position, velocity, and acceleration of the electric cylinder remain continuous and do not change abruptly during the movement of the cylinder.

[0019] Preferably, in module M2, the target telescopic length of the three telescopic electric cylinders is obtained in the following way: The target length of the telescopic electric cylinder when the foot is located at the target following point is calculated using a leg inverse kinematics model. Among them, the inverse kinematics model of the legs:

[0020]

[0021] in, The Jacobian matrix represents the inverse kinematics of the robot's legs; This indicates the coordinates of the target following point in the fuselage coordinate system; H represents the coordinates of the target docking point in the fuselage coordinate system, and H represents the height difference between the target following point and the target docking point. Preferably, in module M3, the target telescopic length of the three telescopic electric cylinders is obtained in the following way: The deviation between the actual and desired positions of the foot's sole is obtained using an admittance control model; the admittance control model is as follows:

[0022] In the formula, This is the quality matrix; Here is the damping matrix; Here is the stiffness matrix; The impact force when the bottom of the foot comes into contact with the area where the target docks; Set the admittance value; , indicating the robot's desired position With actual location Deviation between; for The first derivative; for The second derivative; Based on the deviation, the coordinates of the target's desired position are obtained, i.e.:

[0023] The target length of the telescopic electric cylinder when the bottom of the foot is at the desired target position is determined by using the inverse kinematics model of the leg.

[0024] Compared with the prior art, the present invention has the following beneficial effects: This invention achieves compliant buffering and deviation compensation during robot descent through admittance control model and leg inverse kinematics model. This control method has excellent model-free characteristics, achieving compliant adaptation and precise docking without the need for an accurate system dynamics model. This characteristic also makes it easier to apply in the engineering field. Attached Figure Description

[0025] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 A flowchart illustrating the motion control and error compensation method for a marine transshipment and docking device; Figure 2 This is a schematic diagram of the quadruped robot during the transshipment and docking process in this embodiment; Figure 3 This is a schematic diagram of the quadruped robot in this embodiment; Figure 4 A schematic diagram of the outriggers; Figure 5 A schematic diagram showing the connection point between the foot and the target. Figure 6 A schematic diagram of the docking point between the goods and the load; In the diagram: 1. Supply ship; 2. Crane; 3. Quadruped robot; 31. Lifting point; 32. Fuselage; 33. Camera; 34. Outrigger; 341. First Hooke hinge; 342. Second Hooke hinge; 343. Third Hooke hinge; 344. First telescopic electric cylinder; 345. Second telescopic electric cylinder; 346. Third telescopic electric cylinder; 347. Second ball joint; 348. Third ball joint; 349. Ankle joint; 35. Foot; 36. Cargo docking device; 4. Steel wire rope; 5. Cargo; 6. Target docking point; 7. Target ship; 8. Load docking point. Detailed Implementation

[0026] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0027] A motion control and error compensation method for a marine transshipment and docking device, applied to quadruped robots, such as... Figure 2-6 As shown, the quadruped robot 3 includes a body 32, legs 34, feet 35, a cargo docking device 36, and a camera 33. The upper surface of the body 32 has a lifting point 31, and multiple legs 34 are distributed around the outer perimeter of the lower surface of the body 32. Each leg 34 includes a first Hooke hinge 341, a second Hooke hinge 342, a third Hooke hinge 343, a first telescopic electric cylinder 344, a second telescopic electric cylinder 345, a third telescopic electric cylinder 346, a second ball joint 347, a third ball joint 348, and an ankle joint 349. The first Hooke hinge 341, the second Hooke hinge 342, and the third Hooke hinge 343 are fixed to the lower end of the body 32. On the surface, the bases of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345, and the third telescopic electric cylinder 346 are respectively connected to the first Hooke hinge 341, the second Hooke hinge 342, and the third Hooke hinge 343. The telescopic end of the first telescopic electric cylinder 344 is fixedly connected to the ankle joint 349. The telescopic ends of the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346 are respectively connected to the ankle joint 349 through the second ball hinge 347 and the third ball hinge 348. The foot 35 is installed at the end of the ankle joint 349. The cargo 5 docking device 36 is located in the middle of the lower end face of the body 32. The camera 33 is located on the side of the body 32.

[0028] like Figure 1 As shown, the method includes: Step S1: Using the crane 2 and lifting point 31, the quadruped robot 3 and cargo 5 are transferred from the supply ship 1 to above the target docking position, so that each leg 35 is initially aligned with the target docking point 6, and the target docking position is located on the target ship 7.

[0029] It is understandable that the crane 2 works with the lifting point 31 through the steel wire rope 4. The crane 2 adopts the position control mode, first vertically lifting the robot to a certain height, and then horizontally transporting it to a certain height above the docking base. This process does not require the robot to be exactly above the docking point; it is sufficient to observe it roughly above the docking point.

[0030] Step S2: The camera 33 acquires the coordinates of the target following point at the bottom of the foot 35 in the coordinate system of the body 32. The first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346 are controlled to extend and retract so that the bottom of the foot 35 of each leg 34 is located at the coordinates of the target following point; wherein, the target following point is located directly above the target docking point 6. In step S3, the quadruped robot 3 is lowered by the crane 2 until the bottom of the foot 35 falls to the target docking point 6. When the bottom of the foot 35 contacts the area where the target docking point 6 is located, the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346 are driven to extend and retract through the admittance control model, so as to realize the overlap of the bottom of the foot 35 with the target docking point 6 and the landing cushioning of the bottom of the foot 35. Step S4: Based on the fixed coordinates of the load following point in the ship coordinate system, the hull 32 is adjusted by controlling the extension and retraction of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346 so that the cargo 5 is located at the fixed coordinates of the load following point; wherein, the load following point is located directly above the load docking point 8. Step S5: Based on the fixed coordinates of the load docking point 8 in the ship coordinate system, the fuselage 32 is lowered by controlling the extension and retraction of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346, so that the cargo 5 is at the load docking point 8.

[0031] In this embodiment, the extension and retraction control of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345, and the third telescopic electric cylinder 346 in steps S2, S3, S4, and S5 is achieved in the following manner: Acquire the first telescopic electric cylinder 344, the second telescopic electric cylinder 345, and the third telescopic electric cylinder 346. In this embodiment, in steps S2, S3, S4, and S5, the extension and retraction control of the first telescopic electric cylinder, the second telescopic electric cylinder, and the third telescopic electric cylinder is achieved in the following manner: The quadruped robot 3 includes a body 32, legs 34, feet 35, a cargo docking device 36, and a camera 33. The upper surface of the body 32 is provided with a lifting point 31, and multiple legs 34 are distributed around the outer perimeter of the lower surface of the body 32. Each leg 34 includes a first Hooke hinge 341, a second Hooke hinge 342, a third Hooke hinge 343, a first telescopic electric cylinder 344, a second telescopic electric cylinder 345, a third telescopic electric cylinder 346, a second ball joint 347, a third ball joint 348, and an ankle joint 349. The first Hooke hinge 341, the second Hooke hinge 342, and the third Hooke hinge 343 are fixed to the lower surface of the body 32. The bases of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345, and the third telescopic electric cylinder 346 are respectively connected to the first Hooke hinge 341, the second Hooke hinge 342, and the third Hooke hinge 343. The telescopic end of the first telescopic electric cylinder 344 is fixedly connected to the ankle joint 349. The telescopic ends of the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346 are respectively connected to the ankle joint 349 through the second ball hinge 347 and the third ball hinge 348. The foot 35 is installed at the end of the ankle joint 349. The cargo 5 docking device 36 is located in the middle of the lower end face of the body 32. The camera 33 is located on the side of the body 32.

[0032] The methods include: Step S1: Using the crane 2 and lifting point 31, the quadruped robot 3 and cargo 5 are transferred from the supply ship 1 to above the target docking position, so that each leg 35 is initially aligned with the target docking point 6, and the target docking position is located on the target ship 7.

[0033] It is understandable that the crane 2 works with the lifting point 31 through the steel wire rope 4. The crane 2 adopts the position control mode, first vertically lifting the robot to a certain height, and then horizontally transporting it to a certain height above the docking base. This process does not require the robot to be exactly above the docking point; it is sufficient to observe it roughly above the docking point.

[0034] Step S2: As Figure 5 As shown, the camera 33 acquires the coordinates of the target following point at the bottom of the foot 35 in the coordinate system of the body 32. By controlling the extension and retraction of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346, the bottom of the foot 35 of each leg 34 is positioned at the coordinates of the target following point; wherein, the target following point is located directly above the target docking point 6. In step S3, the quadruped robot 3 is lowered by the crane 2 until the bottom of the foot 35 falls to the target docking point 6. When the bottom of the foot 35 contacts the area where the target docking point 6 is located, the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346 are driven to extend and retract through the admittance control model, so as to realize the overlap of the bottom of the foot 35 with the target docking point 6 and the landing cushioning of the bottom of the foot 35. Step S4: As Figure 6 As shown, based on the fixed coordinates of the load following point in the ship coordinate system, the posture of the fuselage 32 is adjusted by controlling the extension and retraction of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346, so that the cargo 5 is located at the fixed coordinates of the load following point; wherein, the load following point is located directly above the load docking point 8. Step S5: As Figure 6 As shown, based on the fixed coordinates of the load docking point 8 in the ship coordinate system, the fuselage 32 is lowered by controlling the extension and retraction of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345 and the third telescopic electric cylinder 346, so that the cargo 5 is at the load docking point 8.

[0035] In this embodiment, the extension and retraction control of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345, and the third telescopic electric cylinder 346 in steps S2, S3, S4, and S5 is achieved in the following manner: Obtain the target telescopic length of the first telescopic electric cylinder 344, the second telescopic electric cylinder 345, and the third telescopic electric cylinder 346. ; Motion planning is performed on the electric cylinder based on its current length, where the cylinder length L satisfies the following relationship:

[0036] in, This is the current length of the joint; This is the initial time. The moment when the electric cylinder reaches the target extension / retraction length; For one Time is 0. A fifth-degree polynomial function with time interval 1 is used to ensure that the position, velocity, and acceleration of the electric cylinder remain continuous and do not change abruptly during the movement of the cylinder.

[0037] In this embodiment, in step S2, the target extension / retraction length of the three electric cylinders is obtained in the following way: The target length of the telescopic electric cylinder when the foot is located at the target following point is calculated using a leg inverse kinematics model. The inverse kinematics model for the legs is as follows:

[0038]

[0039] in, The Jacobian matrix represents the inverse kinematics of the robot's legs; This indicates the coordinates of the target following point in the fuselage coordinate system; H represents the coordinates of the target docking point in the fuselage coordinate system; H is the height difference between the target following point and the target docking point. In this embodiment, in step S3, the target extension / retraction length of the three electric cylinders is obtained in the following way: The deviation between the actual and desired positions of the foot's sole is obtained using an admittance control model; the admittance control model is as follows:

[0040] In the formula, This is the quality matrix; Here is the damping matrix; Here is the stiffness matrix; The impact force when the bottom of the foot comes into contact with the area where the target docks; Set the admittance value; , indicating the robot's desired position With actual location Deviation between; for The first derivative; for The second derivative; Based on the deviation, the coordinates of the target's desired position are obtained, i.e.:

[0041] The target length of the telescopic electric cylinder when the bottom of the foot is at the desired target position is determined by using the inverse kinematics model of the leg.

[0042] Based on the above scheme, since the target ship is in motion due to the influence of waves, the robot's feet will collide with the docking point during the descent. The collision force F at the foot is detected by a foot force sensor and compared with the admittance setpoint. By comparison, when the collision force exceeds the set value, the excess contact force is converted into a compensating displacement of the robot's foot through admittance control, thereby achieving a compliant buffering effect. By superimposing this compensating displacement onto the current position of the robot's foot, the desired position of the robot's foot at the next moment can be obtained. Then, the desired positions of the three telescopic electric cylinders at the next moment can be obtained through inverse kinematics calculation of the legs, and these positions are used as the target positions to control the movement of the electric cylinders.

[0043] Meanwhile, considering the measurement error of the vision sensor, this invention designs the foot docking point as a flared structure, that is, a flared structure that is wider at the top and narrower at the bottom. In this case, it is only necessary to ensure that the foot is always within the flared opening, without needing to align it with the center of the docking point. When a positional error exists, the foot can slide along the inclined surface of the flared opening into the center of the docking point, effectively reducing the control difficulty. Simultaneously, admittance control is used to compensate for the positional error. When the foot contacts the side of the flared opening, it experiences a significant lateral force. Based on admittance control, this lateral force is converted into lateral displacement compensation to further correct the positional error, ensuring smooth docking of the robot's foot with the docking point.

[0044] In this embodiment, in step S3, from the moment the bottom of the foot begins to make contact with the area where the target docking point is located until the docking with the target docking point is completed, the threshold of admittance control is gradually increased, so that the force on the robot's foot gradually increases, and the robot's gravity is gradually transferred from the wire rope to the foot; when the tension of the crane wire rope is detected to be 0, the crane is released to complete the docking with the target docking point.

[0045] Understandably, as the admittance control threshold... As the crane's cable tension gradually increases, the load-bearing capacity of the outriggers also increases, leading to a corresponding increase in the equivalent stiffness of the legs. This allows the robot's weight to be gradually transferred from the steel cable to its feet. When the tension in the crane's steel cable is detected to be zero, it indicates that the robot's weight is now entirely borne by its legs. Releasing the crane at this point will not affect the robot, and the robot completes its docking with the target point.

[0046] In this embodiment, in steps S4 and S5, the target length of the telescopic electric cylinder is obtained through manual teaching.

[0047] It should be noted that the Jacobian matrix of the inverse kinematics model of the leg... Obtained through the following methods: The single-leg structure can be described as a 1UP-2UPS configuration, where U represents the Hooke joint, P represents the sliding joint (i.e., the telescopic electric cylinder), and S represents the ball joint. In other words, each leg contains one UP chain and two UPS chains. The elongation of all P-joints in the three chains is combined and represented in matrix form, along with the position of the foot's bottom in the fuselage coordinate system. Then we can obtain the Jacobian matrix of the inverse kinematics of the leg:

[0048] In the formula, , and These represent the target extension amounts of the first, second, and third telescopic electric cylinders, respectively; x, y, and z represent the components of the bottom of the foot on the x, y, and z axes in the fuselage coordinate system, respectively.

[0049] It should be noted that the fuselage coordinate system is defined with the U-joint of the UP branch (i.e., the first Hooke hinge) as the origin, the extension direction of the P-joint (i.e., the first telescopic electric cylinder) as the x-axis, and the two rotational axes of the U-joint as the y-axis and z-axis, respectively.

[0050] Based on the above scheme, the elongation of each P pair in the three branches can be calculated as follows: Following the approach of solving the inverse kinematics of the leg, based on the position of the foot's base in the fuselage coordinate system... to obtain The two corners of the U-type branch , and the elongation of P-p Based on these three variables, the pose matrix of the ankle joint relative to the fuselage can be calculated. Based on this pose matrix This allows us to determine the position of the S-joint on the ankle joint in the body coordinate system, and further determine the elongations of the other two UPS branches. and .

[0051] Therefore, the relationship between the two corners of the UP branch and the ankle pose matrix should be established first. Specifically, looking along branch one (UP branch), we can see that the ankle first extends along the x-direction. Then rotate along the z-axis The angle, finally rotated along the y-axis. From this perspective, if we consider:

[0052] In the formula, These represent the abbreviations of the corresponding trigonometric functions.

[0053] Based on the above sequence of motion, the pose matrix can be easily obtained. for:

[0054] Because the end position is The location, therefore targeting Secondary position equations:

[0055] in, This indicates the position of the sole of the foot in the ankle coordinate system; , and Let x, y, and z be the components of the foot's base on the x, y, and z axes in the ankle joint coordinate system. The ankle joint coordinate system is defined with the fixed point between the UP branch and the ankle joint (i.e., the connection point between the telescopic end of the first telescopic cylinder and the ankle joint) as the origin, and its direction is the same as that of the fuselage coordinate system.

[0056] It can be solved :

[0057] extract Second line of the secondary position equation:

[0058] By combining and merging, we can obtain:

[0059] Using the auxiliary angle formula, we can obtain:

[0060]

[0061]

[0062] In the formula, This represents the phase offset.

[0063] Will Treat it as an unknown, and Treating them as known numbers, we can obtain:

[0064]

[0065] because or The numerical computation performance of the function is not as good as Function, therefore usable Solving with functions We can obtain:

[0066] At this point, we can find... The two rotations of the U-joint and the extension of the P-joint are used to obtain the pose matrix of the ankle relative to the body coordinate system. .

[0067] Using the pose matrix The positions of the second and third ball joints on the ankle joint in the fuselage coordinate system can be determined separately:

[0068]

[0069] in, and This indicates the positions of the second and third ball joints in the fuselage coordinate system. and This indicates the positions of the second and third ball joints in the ankle joint coordinate system.

[0070] Furthermore, the extension amounts of the two P-type components (i.e., the second and third telescopic electric cylinders) can be calculated. and As shown in the following formula:

[0071]

[0072] in, and These represent the positions of the second and third Hooke hinges in the fuselage coordinate system, respectively. , and This can be determined during the mechanism design phase.

[0073] The present invention also provides a motion control and error compensation system for a marine transshipment and docking device. The motion control and error compensation system for the marine transshipment and docking device can be implemented by executing the process steps of the motion control and error compensation method for the marine transshipment and docking device. That is, those skilled in the art can understand the motion control and error compensation method for the marine transshipment and docking device as a preferred embodiment of the motion control and error compensation system for the marine transshipment and docking device.

[0074] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0075] In the description of this application, it should be understood that the terms "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this application.

[0076] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A motion control and error compensation method for a marine transshipment and docking device, applied to a multi-legged robot, characterized in that, The multi-legged robot includes a body, legs, feet, a cargo docking device, and a camera. The upper surface of the body has lifting points, and multiple legs are distributed around the outer perimeter of the lower surface of the body. Each leg includes three Hooke hinges, three telescopic electric cylinders, two ball joints, and an ankle joint. The three Hooke hinges are fixed to the lower surface of the body. The bases of the three telescopic electric cylinders are connected to the three Hooke hinges. The telescopic end of one telescopic electric cylinder is fixedly connected to the ankle joint, and the telescopic ends of the other two telescopic electric cylinders are connected to the ankle joint via two ball joints. The feet are mounted at the ends of the ankle joints. The cargo docking device is located in the middle of the lower surface of the body, and the camera is located on the side of the body. The method includes: Step S1: Using the crane and lifting points in coordination, control the multi-legged robot to move above the target docking position, so that each leg is initially aligned with the target docking point; Step S2: Obtain the coordinates of the target following point at the bottom of the foot in the body coordinate system using the camera, and control the extension and retraction of three telescopic electric cylinders to position the bottom of the foot of each leg at the coordinates of the target following point; wherein, the target following point is located directly above the target docking point; Step S3 involves lowering the multi-legged robot using a crane until the bottom of its legs lands at the target docking point. When the bottom of the legs contacts the area where the target docking point is located, the admittance control model drives three telescopic electric cylinders to extend and retract, thereby aligning the bottom of the legs with the target docking point and cushioning the landing of the bottom of the legs. Step S4: Based on the fixed coordinates of the load following point in the ship coordinate system, the fuselage position is adjusted by controlling the extension and retraction of three telescopic electric cylinders so that the cargo is located at the fixed coordinates of the load following point; wherein, the load following point is located directly above the load docking point; Step S5: Based on the fixed coordinates of the load docking point in the ship's coordinate system, the fuselage is lowered by controlling the extension and retraction of three telescopic electric cylinders, so that the cargo is positioned at the load docking point.

2. The method according to claim 1, characterized in that, In steps S2, S3, S4, and S5, the extension and retraction control of the three telescopic electric cylinders is achieved through the following methods: Obtain the target telescopic length of the three telescopic electric cylinders ; Motion planning is performed based on the current length of the electric cylinder, where the length L of the telescopic electric cylinder satisfies the following relationship: in, This is the current length of the joint; This is the initial time. The moment when the electric cylinder reaches the target extension / retraction length; For one Time is 0. A fifth-degree polynomial function with time interval 1 is used to ensure that the position, velocity, and acceleration of the electric cylinder remain continuous and do not change abruptly during the movement of the cylinder.

3. The method according to claim 2, characterized in that, In step S2, the target telescopic length of the three telescopic electric cylinders is obtained in the following way: The target length of the telescopic electric cylinder when the foot is located at the target following point is calculated using a leg inverse kinematics model. Among them, the inverse kinematics model of the legs: in, The Jacobian matrix represents the inverse kinematics of the robot's legs; This indicates the coordinates of the target following point in the fuselage coordinate system; H represents the coordinates of the target docking point in the fuselage coordinate system; H represents the height difference between the target following point and the target docking point.

4. The method according to claim 3, characterized in that, In step S3, the target telescopic length of the three telescopic electric cylinders is obtained in the following way: The deviation between the actual and desired positions of the foot's sole is obtained using an admittance control model; the admittance control model is as follows: In the formula, This is the quality matrix; Here is the damping matrix; Here is the stiffness matrix; The impact force when the bottom of the foot comes into contact with the area where the target docks; Set the admittance value; , indicating the robot's desired position With actual location Deviation between; for The first derivative; for The second derivative; Based on the deviation, the coordinates of the target's desired position are obtained, i.e.: The target length of the telescopic electric cylinder when the bottom of the foot is at the desired target position is determined by using the inverse kinematics model of the leg.

5. The method according to claim 4, characterized in that, In step S3, from the moment the bottom of the foot begins to make contact with the target docking point until the docking is completed, the threshold of admittance control is gradually increased, so that the force on the robot's foot gradually increases, and the robot's gravity is gradually transferred from the wire rope to the foot. When the tension of the crane wire rope is detected to be 0, the crane is released to complete the docking with the target docking point.

6. The method according to claim 1, characterized in that, In steps S4 and S5, the target length of the telescopic electric cylinder is obtained through manual teaching.

7. A motion control and error compensation system for a marine transshipment and docking device, applied to a multi-legged robot, characterized in that, The multi-legged robot includes a body, legs, feet, a cargo docking device, and a camera. The upper surface of the body has lifting points, and multiple legs are distributed around the outer perimeter of the lower surface of the body. Each leg includes three Hooke hinges, three telescopic electric cylinders, two ball joints, and an ankle joint. The three Hooke hinges are fixed to the lower surface of the body. The bases of the three telescopic electric cylinders are connected to the three Hooke hinges. The telescopic end of one telescopic electric cylinder is fixedly connected to the ankle joint, and the telescopic ends of the other two telescopic electric cylinders are connected to the ankle joint via two ball joints. The feet are mounted at the ends of the ankle joints. The cargo docking device is located in the middle of the lower surface of the body, and the camera is located on the side of the body. The system includes: Module M1: Through the cooperation of the crane and lifting points, the multi-legged robot is controlled to move above the target docking position, so that each leg is initially aligned with the target docking point; Module M2: The camera acquires the coordinates of the target following point at the bottom of the foot in the body coordinate system, and controls the extension and retraction of three telescopic electric cylinders to position the bottom of the foot of each leg at the coordinates of the target following point; wherein, the target following point is located directly above the target docking point; Module M3 lowers the multi-legged robot using a crane until the bottom of its legs lands at the target docking point. When the bottom of the leg contacts the area where the target docking point is located, the admittance control model drives three telescopic electric cylinders to extend and retract, so that the bottom of the leg coincides with the target docking point and the bottom of the leg is cushioned upon landing. Module M4: Based on the fixed coordinates of the load following point in the ship's coordinate system, the fuselage position is adjusted by controlling the extension and retraction of three telescopic electric cylinders so that the cargo is located at the fixed coordinates of the load following point; wherein, the load following point is located directly above the load docking point; Module M5: Based on the fixed coordinates of the load docking point in the ship's coordinate system, the fuselage is lowered by controlling the extension and retraction of three telescopic electric cylinders, so that the cargo is positioned at the load docking point.

8. The system according to claim 7, characterized in that, In modules M2, M3, M4, and M5, the extension and retraction control of the three telescopic electric cylinders is achieved through the following methods: Obtain the target telescopic length of the three telescopic electric cylinders ; Motion planning is performed based on the current length of the electric cylinder, where the length L of the telescopic electric cylinder satisfies the following relationship: in, This is the current length of the joint; This is the initial time. The moment when the electric cylinder reaches the target extension / retraction length; For one Time is 0. A fifth-degree polynomial function with time interval 1 is used to ensure that the position, velocity, and acceleration of the electric cylinder remain continuous and do not change abruptly during the movement of the cylinder.

9. The system according to claim 8, characterized in that, In module M2, the target telescopic lengths of the three telescopic electric cylinders are obtained in the following way: The target length of the telescopic electric cylinder when the foot is located at the target following point is calculated using a leg inverse kinematics model. Among them, the inverse kinematics model of the legs: in, The Jacobian matrix represents the inverse kinematics of the robot's legs; This indicates the coordinates of the target following point in the fuselage coordinate system; H represents the coordinates of the target docking point in the fuselage coordinate system; H represents the height difference between the target following point and the target docking point.

10. The system according to claim 9, characterized in that, In module M3, the target telescopic lengths of the three telescopic electric cylinders are obtained in the following way: The deviation between the actual and desired positions of the foot's sole is obtained using an admittance control model; the admittance control model is as follows: In the formula, This is the quality matrix; Here is the damping matrix; Here is the stiffness matrix; The impact force when the bottom of the foot comes into contact with the area where the target docks; Set the admittance value; , indicating the robot's desired position With actual location Deviation between; for The first derivative; for The second derivative; Based on the deviation, the coordinates of the target's desired position are obtained, i.e.: The target length of the telescopic electric cylinder when the bottom of the foot is at the desired target position is determined by using the inverse kinematics model of the leg.

Images