Seaborne docking series-parallel robot with two-stage wave compensation

Abstract

The invention discloses a two-stage wave compensation seaborne butt joint series-parallel robot which comprises a base, a series mechanical arm, an end effector and a miniature six-degree-of-freedom parallel mechanism. The tandem type mechanical arm is installed on a deck base of the supply ship and serves as a first-stage wave compensation device, and the tandem type mechanical arm responds to the motion trend of the ship body and conducts large-range and low-frequency compensation motion. The miniature six-degree-of-freedom parallel mechanism is installed between a tail end flange and a tail end executor of the tandem type machine and serves as a secondary wave compensation device to conduct small-range and high-precision position and posture fine adjustment. The two-stage compensation works cooperatively through a central controller, and finally high-precision and stable tracking and docking of a docking interface of a supplied ship are achieved. The safety and the success rate of automatic supply operation on the sea under the severe sea condition are greatly improved.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of marine rescue, in particular to a two-stage wave compensation marine docking series-parallel robot. BACKGROUND

[0002] In the fields of military, marine scientific research, transportation and even marine rescue, the long-term deployment and continuous operation capability of marine equipment are crucial. The existing marine docking schemes mainly have the following shortcomings: a pure series mechanical arm scheme: a large series mechanical arm has a large workspace, but due to its series structure characteristics, the end rigidity is low, the motion accuracy decreases with the lengthening of the arm span, the response speed is slow, the high-frequency wave motion cannot be compensated, and docking failure or a large collision force is easily caused. A single-stage compensation scheme: whether force control or position control is used, the single-stage compensation cannot simultaneously meet the needs of large-range motion compensation and high-precision fine adjustment. When the wave height is large, the compensation effect is limited. SUMMARY

[0003] The application aims to provide a two-stage wave compensation marine docking series-parallel robot, and solve the automatic docking problem of an unmanned ship docking interface in complex sea conditions.

[0004] The two-stage wave compensation marine docking series-parallel robot comprises a base fixedly installed on a deck of a supply ship, a series mechanical arm having a root connected with the base and comprising a plurality of rotary joints and forming a multi-degree-of-freedom series mechanism, a micro six-degree-of-freedom parallel mechanism having a lower platform rigidly connected with an output flange at the end of the series mechanical arm through a connecting flange, an end effector installed on an upper platform of the micro six-degree-of-freedom parallel mechanism and used for docking an interface on a supplied ship, a measurement system comprising an inertial measurement unit (IMU) for measuring the motion of the supply ship, a binocular vision sensor for measuring the relative pose of the end effector and the docking interface, and a six-dimensional force sensor for measuring the contact force at the end of the series mechanical arm, and a central controller receiving the data of the measurement system and outputting control instructions to drive the series mechanical arm and the micro six-degree-of-freedom parallel mechanism to move cooperatively. The series mechanical arm is used as a first-stage wave compensation mechanism, adopts a force control mode, and compensates for large-range and low-frequency displacement and angle deviation caused by the motion of the ship body. The micro six-degree-of-freedom parallel mechanism is used as a second-stage wave compensation mechanism, adopts a position control mode, and compensates for small-range and high-frequency pose errors. The central controller coordinates the work of the two-stage compensation mechanisms and forms a master-slave cooperative relationship.

[0005] Further, the series mechanical arm calculates the expected motion trajectory of the end of the mechanical arm according to the contact force information fed back by the six-dimensional force sensor, and realizes the first-stage compensation for the motion of the ship body.

[0006] Further, the micro six-degree-of-freedom parallel mechanism performs high-frequency and high-precision pose fine adjustment according to the relative pose deviation of the end effector and the docking interface fed back by the binocular vision sensor, so as to eliminate the residual error after the first compensation.

[0007] Further, the control algorithm of the central controller adopts a master-slave control strategy, including: the serial manipulator for the first compensation as a master mechanism, responsible for macro positioning and motion; and the parallel mechanism for the second compensation as a slave mechanism, responsible for micro fine adjustment and dither elimination.

[0008] Further, the end effector is a docking device, the serial manipulator is attached to the surface of the supplied ship in the force control mode, a spiral trajectory planning is used to search with constant contact force, when the pose value of the end effector along the Z-axis of the docking interface coordinate system is detected to be less than a set threshold, it is determined that the interface is successfully found, and the constant contact force is maintained.

[0009] The control method of the two-stage wave compensation offshore docking serial-parallel robot comprises the following steps: S1, initial positioning stage: the docking interface of the supplied ship is recognized and preliminarily positioned by a vision sensor, the central controller controls the motion of the serial manipulator based on the rough three-dimensional pose of the interface fed back by the vision, so that the end effector approaches a preliminary position in front of the target docking interface; S2, accurate search stage of the docking interface: the central controller detects the contact with the surface of the ship through a six-dimensional force sensor, switches to the force control mode, maintains constant contact force along the Z-axis of the docking interface coordinate system, controls the position along the X and Y axes, drives the end effector to move along the preset spiral search trajectory, and the interface is successfully found; S3, first compensation starting stage: the central controller drives the serial manipulator to perform compensation motion through a force control algorithm and inverse kinematics based on the end force data fed back by the six-dimensional force sensor and the motion data of the supplied ship measured by the IMU, so as to compensate for large-range and low-frequency ship motion; S4, second compensation starting stage: the central controller processes the relative pose deviation fed back by the vision sensor, drives the micro six-degree-of-freedom parallel mechanism to perform rapid fine adjustment, compensates for the residual high-frequency vibration and pose error, and dynamically adjusts the start of the second compensation according to the sea state level. S5, cooperative operation stage: under the cooperation of the serial manipulator and the parallel mechanism, the accurate and soft docking of the end effector and the docking interface is completed, and the connection is maintained until the supply operation is completed. S6, supply completion stage: after the supply is completed, the system is withdrawn.

[0010] The electronic device comprises a memory and a processor, the memory stores a computer program, and the processor implements the method when executing the program.

[0011] The application discloses a computer readable storage medium, which stores a computer program, and the program is executed by a processor to realize the method.

[0012] Beneficial effects: Compared with the prior art, the application has the following remarkable advantages: the application combines the advantages of large workspace of a serial mechanical arm and high precision and good rigidity of a six-degree-of-freedom parallel mechanism, realizes two-stage compensation of coarse and fine combination, and the compensation effect is far superior to that of a single-stage system, and the performance is more superior; the parallel mechanism is responsible for high-frequency response, effectively suppresses high-frequency vibration caused by waves, solves the problem of response delay of the serial mechanical arm, and the response speed is faster. Force control of the serial mechanical arm makes the first-stage compensation flexible, and even if accidental contact occurs, it is also low-impedance soft contact; the second-stage compensation ensures accurate alignment, greatly reduces the risk of damage to equipment caused by rough alignment, and the alignment is safer. The serial-parallel robot configuration can better adapt to different sea state levels, only needs first-stage compensation in low sea state, and two-stage cooperation in high sea state, and the system is more robust and adaptable. BRIEF DESCRIPTION OF DRAWINGS

[0013] Figure 1 is a structural diagram of the application; Figure 2 is a method flowchart of the application. DETAILED DESCRIPTION

[0014] The technical solutions of the application will be further described below with reference to the drawings.

[0015] As Figure 1As shown in the figure, the embodiment of the application provides a two-stage wave compensation offshore docking series-parallel robot, which comprises: a base 1 rigidly fixed to the deck of a supply ship by bolts. A series mechanical arm 2 is connected with the base 1 through a flange and the upper surface of the base 1, so that the series mechanical arm 2 is supported on the base 1. The series mechanical arm 2 is composed of six rotating joints connected in series, so as to realize large-range movement of the end in space. A binocular vision sensor 3 is installed on the side of the wrist of the series mechanical arm through a mounting bracket, and is used for observing and identifying the docking interface on the supplied ship in real time during docking. A six-dimensional force sensor 4 is installed on the end flange of the series mechanical arm 2, and one end of the six-dimensional force sensor 4 is connected with the end flange of the series mechanical arm 2, and the other end is used for connecting a subsequent mechanism, so as to accurately measure and feedback the three-dimensional force and three-dimensional torque information generated by the end of the series mechanical arm 2 when contacting with the environment. A micro six-degree-of-freedom parallel mechanism 5 is rigidly connected with the lower interface of the six-dimensional force sensor 4 through a connecting flange, so that the parallel mechanism 5 is suspended on the end of the series mechanical arm 2. An end effector 6 is installed on the upper platform of the micro six-degree-of-freedom parallel mechanism 5 through a special interface, and the center of the installation interface of the end effector 6 coincides with the motion center of the upper platform. An inertial measurement unit (IMU) 7 is installed on the supply ship close to the base 1, and is used for accurately measuring the six-degree-of-freedom rocking motion of the supply ship under the action of waves. A central controller 8 (located in a control cabin) adopts a high-performance industrial computer and is constructed on a robot operating system (ROS), which receives all data from the binocular vision sensor 3, the six-dimensional force sensor 4 and the IMU 7 in real time through a high-speed bus, performs real-time calculation through the internal integrated multi-sensor fusion algorithm and the hierarchical control law, and outputs coordinated control instructions to drive the joint servo motors of the series mechanical arm 2 and the electric supporting rods of the micro six-degree-of-freedom parallel mechanism 5, so as to realize cooperative operation of two-stage compensation. Embodiment

[0016] As Figure 2 shown, the embodiment of the application also provides a control method of a two-stage wave compensation offshore docking series-parallel robot, which comprises the following steps: S1, an initial positioning stage, the binocular vision sensor 3 identifies the docking interface on the supplied ship, the central controller 8 inversely solves the target angles of the joints of the series mechanical arm 2 based on the rough three-dimensional pose of the interface fed back by vision, and controls the movement of the series mechanical arm 2 by using a path planning algorithm based on Cartesian space, so that the end effector 6 smoothly approaches to a prepared position in front of the target docking interface.

[0017] S2, the docking interface accurate search stage, the series mechanical arm 2 adopts force control mode, the central controller 8 feeds back six-dimensional force data of the series mechanical arm 2 end through the six-dimensional force sensor 4, when detecting the contact with the ship surface, the central controller switches to force control mode, specifically, force tracking is implemented along the docking interface coordinate system Z axis, a constant contact force 2N is maintained;Along the docking interface coordinate system X and Y axis, position control is carried out, the end effector 6 is driven to move along the preset helix search trajectory, and the search is carried out on the ship surface with constant contact force, until the pose value of the end effector 6 along the docking interface coordinate system Z axis is less than the set threshold value, then it is determined that the interface is successfully found, then the series mechanical arm 2 continues to maintain the constant contact force through force control, and prepares for subsequent continuous docking and supply operation.

[0018] S3, the first compensation start stage, the IMU 7 monitors the supply ship (series mechanical arm 2 base) motion data in real time, the central controller 8 adjusts the end contact force through force control algorithm according to the series mechanical arm 2 end force data fed back by the six-dimensional force sensor 4, so as to guarantee the flexible operation and ensure the flexibility of the contact with the interface, and avoid generating excessive impact force;The central controller 8 receives the six-degree-of-freedom oscillation motion data of the supply ship (series mechanical arm 2 base) measured by the IMU 7 installed on the base in real time, converts the disturbance motion of the base into compensation instructions for each joint angle of the series mechanical arm 2 through the kinematics model between the base and the end, and adopts inverse kinematics, drives the series mechanical arm 2 to perform compensation motion, so that the end effector 6 follows the overall motion trend of the docking interface, and the end roughly tracks the target, completes large-range displacement compensation, and the displacement compensation range is ± (5-50) cm, and the angle compensation range is ± (5-30) °. In this stage, the micro six-degree-of-freedom parallel mechanism 5 keeps neutral configuration.

[0019] S4, the second compensation start stage, the central controller 8 processes the relative pose deviation of the end effector 6 and the docking interface fed back by the vision sensor 3, solves the mapping of the deviation in the micro six-degree-of-freedom parallel mechanism 5 spatial coordinate system, and adopts PID control algorithm to quickly drive each electric supporting rod of the micro six-degree-of-freedom parallel mechanism 5 to perform telescopic motion for fast fine adjustment, compensate the residual high-frequency vibration and pose error of the series mechanical arm 2, the displacement compensation range is ± 5 cm, and the angle compensation range is ± 5 °, and finally realizes docking;The central controller 8 judges the current sea state level according to the IMU 7 data, and continuously enables the second compensation in high sea state, and can suspend the compensation in low sea state or when the docking is stable to save energy consumption.

[0020] S5, the cooperative operation stage, during the whole docking and supply process, the two-stage compensation works continuously. The series mechanical arm 2 is responsible for large-range movement and positioning, and the six-degree-of-freedom parallel mechanism 5 is responsible for the final fine adjustment and stability, and the two cooperate to ensure that the docking interface always maintains reliable connection in waves.

[0021] S6, the replenishment is completed, after the replenishment is completed, the process is reversed, and the system is retrieved.

[0022] The application combines the large working space of the serial mechanical arm and the high accuracy of the six-degree-of-freedom parallel mechanism, realizes two-stage compensation, and the compensation effect is far superior to that of a single-stage system, and the performance is more superior; the parallel mechanism is responsible for high-frequency response, effectively suppresses the high-frequency vibration caused by waves, solves the problem of response delay of the serial mechanical arm, and the response speed is faster. The force control of the serial mechanical arm makes the first-stage compensation have flexibility, even if accidental contact occurs, it is also a low-impedance soft contact; the second-stage compensation ensures accurate alignment, greatly reduces the risk of rough docking damaging the equipment, and the docking is safer. The serial-parallel robot configuration can better adapt to different sea state levels, only one-stage compensation is needed in low sea state, and two-stage cooperation is needed in high sea state, and the system is more robust and adaptable.

Claims

1. A two-stage wave-compensated marine docking serial-parallel robot, characterized in that, include: The base is fixedly installed on the deck of the supply ship; the tandem robotic arm, with its root connected to the base, has multiple rotary joints, forming a multi-degree-of-freedom tandem mechanism; the lower platform of the miniature six-degree-of-freedom parallel mechanism is rigidly connected to the end output flange of the tandem robotic arm via a connecting flange; the end effector is installed on the upper platform of the miniature six-degree-of-freedom parallel mechanism for docking with the interface on the supply ship; the measurement system includes an inertial measurement unit (IMU) for measuring the motion of the supply ship, a binocular vision sensor for measuring the relative pose of the end effector and the docking interface, and a six-dimensional force sensor for measuring the contact force at the end of the tandem robotic arm; the central controller receives data from the measurement system and outputs control commands to drive the tandem robotic arm and the miniature six-degree-of-freedom parallel mechanism to move in coordination; the tandem robotic arm, as the first-level wave compensation mechanism, adopts a force control mode to compensate for large-range, low-frequency displacement and angular deviations caused by the ship's motion; the miniature six-degree-of-freedom parallel mechanism, as the second-level wave compensation mechanism, adopts a position control mode to compensate for small-range, high-frequency pose errors; the central controller coordinates the work of the two-level compensation mechanisms, forming a master-slave cooperative relationship.

2. The two-stage wave-compensated marine docking serial-parallel robot according to claim 1, characterized in that, The serial robotic arm calculates the desired end-effector trajectory based on the contact force information fed back by the six-dimensional force sensor, thereby achieving first-level compensation for the ship's motion.

3. The two-stage wave-compensated marine docking serial-parallel robot according to claim 1, characterized in that, The miniature six-degree-of-freedom parallel mechanism performs high-frequency, high-precision pose fine-tuning based on the relative pose deviation between the end effector and the docking interface fed back by the binocular vision sensor, in order to eliminate the residual error after the first-level compensation.

4. The two-stage wave-compensated marine docking serial-parallel robot according to claim 1, characterized in that, The central controller's control algorithm employs a master-slave control strategy, which includes: a serial robotic arm with primary compensation acts as the master mechanism, responsible for macroscopic positioning and motion; a parallel mechanism with secondary compensation acts as the slave mechanism, responsible for microscopic fine-tuning and de-jittering.

5. A two-stage wave-compensated marine docking serial-parallel robot according to claim 1, characterized in that, The end effector is a docking device. The serial robotic arm fits against the surface of the replenished vessel in force control mode and uses a spiral trajectory planning to search with a constant contact force. When the pose value of the end effector along the Z-axis of the docking interface coordinate system is less than the set threshold, it is determined that the interface has been successfully found and the constant contact force is maintained.

6. A control method for a marine docking series-parallel robot based on two-stage wave compensation according to any one of claims 1, characterized in that, Includes the following steps: S1. Initial positioning stage: The docking interface of the replenished ship is identified and initially located by visual sensors. The central controller uses path planning algorithm to control the movement of the serial robotic arm based on the rough three-dimensional pose of the interface based on visual feedback, so that the end effector approaches the preparatory position in front of the target docking interface. S2, Precise Search Stage of Docking Interface: The central controller detects the contact with the ship's surface through a six-dimensional force sensor, switches to force control mode, maintains a constant contact force along the Z-axis of the docking interface coordinate system, performs position control along the X and Y axes, and drives the end effector to move along a preset spiral search trajectory until the interface is successfully found. S3, First-level compensation start-up phase: The central controller, based on the end force data fed back by the six-dimensional force sensor and the supply ship motion data measured by the IMU, drives the serial robotic arm to perform compensation motion through force control algorithm and inverse kinematics solution to compensate for large-range, low-frequency hull motion. S4, Secondary Compensation Activation Phase: The central controller processes the relative pose deviation fed back by the visual sensor, drives the micro six-degree-of-freedom parallel mechanism to perform rapid fine-tuning, compensates for residual high-frequency vibration and pose error, and dynamically adjusts the activation of secondary compensation according to the sea state level. S5. Collaborative Operation Phase: Under the collaborative action of the serial robotic arm and the parallel mechanism, the end effector and the docking interface are precisely and smoothly docked, and the connection is maintained until the resupply operation is completed. S6, Supply Completion Phase: After the supply is completed, the system is recalled.

7. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the program, it implements the method as described in claim 6.

8. A computer-readable storage medium storing a computer program, characterized in that, When the program is executed by the processor, it implements the method as described in claim 6.

Images