Accurate docking system and docking method for marine mechanical arm

Abstract

The invention discloses an accurate docking system and docking method for marine mechanical arms. The system comprises a marine mechanical arm, an area-array camera is arranged at a three-dimensional connector rotating joint, the three-dimensional connector rotating joint drives the camera to rotate, and a mechanical arm flange used for being in butt joint with a ship side header flange is arranged at the tail end of the mechanical arm. According to the method, the position of the first virtual flange serves as a rough butt joint end point, the four virtual flanges are arranged behind the first virtual flange in the direction of the central axis and the approximate axis of the ship side header flange to serve as transition positions of step-by-step butt joint, and multi-step control step-by-step butt joint in the precise butt joint process is achieved through step-by-step positioning butt joint. And the butt joint precision is improved.

Description

Technical Field

[0001] This invention belongs to the field of marine robotic arm docking technology, and in particular to a precise docking system and docking method for marine robotic arms. Background Technology

[0002] Currently, automatic docking between marine robotic arms based on vision positioning systems and shipside manifold flanges has been initially achieved. Operators can complete one-click docking of the robotic arm by operating a few simple buttons with a handheld remote control. However, automatic docking is not yet mature, with a relatively low success rate and inability to achieve precise docking. This is mainly due to the following shortcomings:

[0003] (1) The entire automatic docking process relies on one-step motion control from the starting point to the end point. Under this one-step motion control method, the robotic arm is prone to deviating from the preset motion trajectory, resulting in loss of control. In particular, when the robotic arm is close to the ship side manifold flange, position or angle orientation problems are likely to occur, leading to jamming or scraping of the ship side manifold flange.

[0004] (2) When docking is performed using a one-step motion control method, the area array camera is always in motion. Not only are the images captured noisy, but the positioning can only be performed once at a certain position, resulting in low positioning accuracy. This can cause deviations in docking guidance, leading to uncontrollable results and potential safety hazards. Summary of the Invention

[0005] The purpose of this invention is to address the problems existing in the prior art by proposing a precision docking system and method for marine robotic arms, so as to achieve multi-step control between the marine robotic arm and the manifold flange on the ship after rough docking, gradually decouple the movements of each joint, and improve docking accuracy.

[0006] The technical solution to achieve the purpose of this invention is as follows: On the one hand, a precision docking system for a marine robotic arm is provided. The system includes a marine robotic arm, which includes a base and multiple joints. The base and joints, as well as the joints themselves, are connected by rigid pipes. The multiple joints include at least a horizontal rotary joint, an inner arm rotary joint, an outer arm rotary joint, a three-dimensional joint rotary joint, and a self-weight joint. The horizontal rotary joint, inner arm rotary joint, and outer arm rotary joint are driven by hydraulic cylinders, and the three-dimensional joint rotary joint is driven by a hydraulic motor and is located at the end of the marine robotic arm. The self-weight joint uses its own weight to ensure that the flange at the end of the marine robotic arm always faces forward. A surface array camera is installed at the three-dimensional joint rotary joint, and the three-dimensional joint rotary joint drives the surface array camera to rotate. An end effector for docking with the manifold flange on the side of the ship is provided at the end of the marine robotic arm.

[0007] Furthermore, the end effector includes a robotic arm flange that docks with the ship-side manifold flange, and also includes a plurality of guide rods for centering arranged radially outward from the robotic arm flange.

[0008] On the other hand, a method for precise docking of a marine robotic arm is provided, the method comprising the following steps:

[0009] Step 1: Determine four virtual flanges between the end of the robotic arm and the ship-side manifold flange along the central axis of the ship-side manifold flange and its approximate axis. The position of the first virtual flange is the rough end point of the docking, and the positions of the other three virtual flanges and the ship-side manifold flange are the target positions for the step-by-step movement of the robotic arm.

[0010] Step 2: Control the end effector to move to the position of the first virtual flange via PLC, thus completing the rough docking;

[0011] Step 3: Keep the device stationary and take multiple photos of the ship-side manifold flange with the area array camera to achieve the positioning of the ship-side manifold flange by the area array camera.

[0012] Step 4: Establish a kinematic coordinate system based on the DH method;

[0013] Step 5: Use i to represent the sequence number and set i = 2;

[0014] Step 6: Read the current angle values ​​of each joint, calculate the homogeneous transformation matrix of each joint, and then calculate the pose matrix of the robotic arm end coordinate system relative to the geodetic coordinate system.

[0015] Step 7: Acquire the pose matrix of the ship-side manifold flange relative to the coordinate system of the robotic arm end effector using the area array camera. ;

[0016] Step 8, based on the pose matrix of target position i Calculate the pose matrix of the target position i in the geodetic coordinate system. ;

[0017] Step 9: Obtain the angle difference of each joint when the end effector of the robotic arm reaches the target position i using the vector product method;

[0018] Step 10: Input the joint angle difference values ​​into the fuzzy PID control method to calculate the control quantity, and then control the end of the robotic arm to move from the current position to the target position i through the PLC according to the magnitude of the control quantity;

[0019] Step 11: Obtain the distance between the current end effector of the robotic arm and the target position i using an area scan camera;

[0020] If the distance between the current end of the robotic arm and the target position i is less than the error setting value, the robotic arm reaches the target position i and then proceeds to step 12; otherwise, steps 6 to 11 are repeated.

[0021] Step 12: Let i = i + 1. When i = 2 or i = 3, take the position of the end of the robotic arm as the current position and repeat steps 6 to 12.

[0022] When i=4, only drive the inner arm rotation joint and the outer arm rotation joint to make them move, take the position of the end of the robotic arm as the current position, and repeat steps 6 to 12.

[0023] When i=5, set only the outer arm rotation joint to be driven to move it, take the position of the end of the robotic arm as the current position, and repeat steps 6 to 12.

[0024] Otherwise, terminate the docking operation.

[0025] Further, in step 1, the position of the first virtual flange is the distance from the ship-side manifold flange plane L1 along the central axis of the ship-side manifold flange; the position of the second virtual flange is the distance from the ship-side manifold flange plane L2 along the central axis of the ship-side manifold flange; the position of the third virtual flange is the distance from the ship-side manifold flange plane L2 along the central axis of the ship-side manifold flange, but offset from the axis direction. Location, The angle of rotation of the horizontal rotary joint relative to its initial position. The parameter is adjustable; the position of the fourth virtual flange is the distance from the plane L3 of the ship side manifold flange along the central axis of the ship side manifold flange; where L1 is greater than L2, and L2 is greater than L3.

[0026] Furthermore, in step 4, the robotic arm model is simplified, and 0-link coordinate system, 1-link coordinate system, 2-link coordinate system, 3-link coordinate system, 4-link coordinate system, 5-link coordinate system, 6-link coordinate system and 7-link coordinate system are established in sequence. These are the coordinate systems corresponding to the base, horizontal rotary joint, inner arm rotary joint, outer arm rotary joint, self-weight joint, three-dimensional joint rotary joint, rigid tube between the three-dimensional joint rotary joint and the end effector, and the end effector, respectively.

[0027] Furthermore, in step 6, the formula for calculating the pose matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system is:

[0028]

[0029] In the formula, Let be the homogeneous transformation matrix between the 1-link coordinate system and the 0-link coordinate system. Let be the homogeneous transformation matrix between the two-link coordinate system and the one-link coordinate system. Let be the homogeneous transformation matrix between the 3-link coordinate system and the 2-link coordinate system. Let be the homogeneous transformation matrix between the 4-link coordinate system and the 3-link coordinate system. Let be the homogeneous transformation matrix between the 5-link coordinate system and the 4-link coordinate system. Let be the homogeneous transformation matrix between the 6-link coordinate system and the 5-link coordinate system. This is the homogeneous transformation matrix between the 7-link coordinate system and the 6-link coordinate system. This is the pose matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system; Let be the attitude matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system. This is the position vector of the robotic arm's end-effector coordinate system relative to the geodetic coordinate system.

[0030] Furthermore, in step 8, the pose matrix of the target position i with respect to the geodetic coordinate system... The calculation formula is:

[0031] .

[0032] Furthermore, step 9 specifically includes the following processes:

[0033] Step 9.1: Based on the relationship between the joint motion velocity and the end-effector motion velocity of the multi-link mechanism in the system, iteratively calculate the end-effector angular velocity and linear velocity of link j in its coordinate system. , The calculation formulas are as follows:

[0034]

[0035] In the formula, , These are the angular velocity and linear velocity of link j+1 in its coordinate system, respectively; This refers to the change in joint angle connected to link j+1. Let be the rotation matrix between the j+1 link coordinate system and the j link coordinate system. unit column vector , Let be the position vector between the j+1 link coordinate system and the j link coordinate system;

[0036] Step 9.2, let The angular velocity and linear velocity at the end will represent the final velocity. , Write it in the form of joint angle vectors , It is a Jacobian matrix that contains angle information. for The column vector formed;

[0037] Step 9.3: By inverting the equation, the angle difference of each joint can be calculated based on the pose difference at a certain moment.

[0038] Furthermore, in step 11, the error setting value is 10mm.

[0039] Compared with the prior art, the significant advantages of this invention are:

[0040] (1) The present invention takes the position of the first virtual flange as the rough docking endpoint, and sets three virtual flanges along the central axis of the ship side manifold flange and its vicinity after the first virtual flange as the transition position for step docking. The step positioning docking realizes the multi-step control of the precise docking process and improves the docking accuracy.

[0041] (2) The present invention sets up a virtual flange along the central axis of the ship side manifold flange, so that the alignment between the robotic arm and the ship side manifold flange is achieved first in the step-by-step docking process. In the final docking, if the alignment is not performed first, the guide rod will touch the ship side manifold flange first. When the guide rod touches the ship side manifold flange first, the robotic arm will be unable to move, resulting in docking failure. The present application sets up a virtual flange along the central axis of the ship side manifold flange, thereby avoiding the situation where the guide rod touches the ship side manifold flange first by performing the alignment first, thus improving the success rate of docking.

[0042] (3) By setting a horizontal offset at a position close to the ship side manifold flange, the present invention enables the pre-action of the horizontal and three-dimensional joints, reducing the joint action of four joints to two joints. When the guide rod approaches the ship side manifold flange, if the joint action of four joints is still used, the guide rod will scrape or rub against the ship side manifold flange due to inconsistent starting speeds or mismatched running speeds between the joints, resulting in docking failure. The starting speed and running speed of the joints are largely constrained by the hydraulic drive system. The present invention sets a horizontal offset at a position close to the ship side manifold flange, and by pre-action of the two joints of the horizontal and three-dimensional joints, avoids the situation of joint action of four joints when the space for adjustment is small when the guide rod approaches or surrounds the ship side manifold flange, thus improving the stability of docking.

[0043] (4) In this invention, after the robotic arm reaches the first virtual flange and completes the rough docking, the area array camera takes multiple pictures of the ship-side manifold flange in a static state. The area array camera positions the manifold flange based on the multiple pictures taken at the same time. Since the robotic arm is in a static state, the area array camera continuously takes pictures of the pose of the ship-side manifold flange. Multiple spatial judgments are made, and the positioning data is filtered to achieve the positioning of the ship-side manifold flange by the area array camera. This avoids the noise impact on identification and positioning during dynamic acquisition and achieves more accurate positioning.

[0044] The present invention will now be described in further detail with reference to the accompanying drawings. Attached Figure Description

[0045] Figure 1 This is a schematic diagram of the structure of a shore-based ship robotic arm precision docking system in one embodiment.

[0046] Figure 2 This is a schematic diagram of the robotic arm flange and the ship side manifold flange in one embodiment.

[0047] Figure 3 A schematic diagram of the DH coordinate system of the robotic arm in one embodiment, wherein Figure 3 (a) in 3 is the front view, and (b) in 3 is the side view.

[0048] Figure 4 This is a flowchart illustrating a precise docking method for a marine robotic arm in one embodiment. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0050] It should be noted that if the embodiments of the present invention involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of the components in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0051] Furthermore, if the embodiments of this invention involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by this invention.

[0052] In one embodiment, combined Figure 1 and Figure 2 A precision docking system for a marine robotic arm is provided. The system includes a marine robotic arm, which comprises a base 1 and multiple joints. The base and joints, as well as the joints themselves, are connected by rigid pipes. The multiple joints include at least a horizontal rotary joint 2, an inner arm rotary joint 3, an outer arm rotary joint 4, a three-dimensional joint rotary joint 5, and a self-weight joint 6. The horizontal rotary joint, inner arm rotary joint, and outer arm rotary joint are driven by hydraulic cylinders, while the three-dimensional joint rotary joint is driven by a hydraulic motor and is located at the end of the marine robotic arm. The self-weight joint uses its own weight to ensure that the flange 73 at the end of the marine robotic arm always faces forward. A surface array camera 9 is installed at the three-dimensional joint rotary joint 5, which drives the surface array camera 9 to rotate. An end effector 7 for docking with the manifold flange on the ship's side is provided at the end of the marine robotic arm.

[0053] Furthermore, the end effector 7 includes a robotic arm flange 71 that docks with the ship-side manifold flange, and also includes a plurality of guide rods 72 arranged radially outward along the robotic arm flange 81 for centering.

[0054] Specifically, the robotic arm includes a base 1, a column 11, a horizontal rotary joint 2, a first rigid tube 21, an inner arm rotary joint 3, an inner arm rigid tube 31, an outer arm rotary joint 4, an outer arm rigid tube 41, a self-weight joint 6, a second rigid tube 61, a three-dimensional joint rotary joint 5, a third rigid tube 51, and an end effector 7, connected in sequence. A surface scan camera 9 is installed at the three-dimensional joint rotary joint 5. The end effector 7 connects the flange 71 to the ship-side manifold flange 8 through the rotation of the robotic arm's joints.

[0055] In one embodiment, combined Figure 4 A method for precise docking of a marine robotic arm is provided, the method comprising the following steps:

[0056] Step 1: Determine four virtual flanges along the central axis and approximate axis of the ship side manifold flange 8 between the end of the robotic arm and the ship side manifold flange. The position of the first virtual flange is the rough end point of the docking, and the positions of the other three virtual flanges and the ship side manifold flange are the target positions for the step-by-step movement of the robotic arm.

[0057] Step 2: Control the end effector to move to the position of the first virtual flange using the PLC;

[0058] Step 3: While remaining stationary, the area array camera 9 continuously captures the pose of the ship-side manifold flange 8, performs multiple spatial judgments, and filters the positioning data to achieve the positioning of the ship-side manifold flange by the area array camera 9. Here, the first virtual flange position satisfies the requirement that the area array camera 9 can capture the ship-side manifold flange 8. During the shooting process of the camera at the first virtual flange position, the robotic arm remains stationary, and the area array camera 9 continuously captures images of the flange, enabling more accurate positioning.

[0059] Step 4: Establish a kinematic coordinate system based on the DH method;

[0060] Step 5: Use i to represent the sequence number and set i = 2;

[0061] Step 6: Read the current angle values ​​of each joint, calculate the homogeneous transformation matrix of each joint, and then calculate the pose matrix of the robotic arm end coordinate system relative to the geodetic coordinate system.

[0062] Step 7: Acquire the pose matrix of the ship-side manifold flange relative to the coordinate system of the robotic arm end effector using the area array camera. ;

[0063] Step 8, based on the pose matrix of target position i Calculate the pose matrix of the target position i in the geodetic coordinate system. ;

[0064] Step 9: Obtain the angle difference of each joint when the end effector of the robotic arm reaches the target position i using the vector product method;

[0065] Step 10: Input the joint angle difference values ​​into the fuzzy PID control method to calculate the control quantity, and then control the end of the robotic arm to move from the current position to the target position i through the PLC according to the magnitude of the control quantity;

[0066] Step 11: Obtain the distance between the current end effector of the robotic arm and the target position i using an area scan camera;

[0067] If the distance between the current end of the robotic arm and the target position i is less than the error setting value, the robotic arm reaches the target position i and then proceeds to step 12; otherwise, steps 6 to 11 are repeated.

[0068] Step 12: Let i = i + 1. When i = 2 or i = 3, take the position of the end of the robotic arm as the current position and repeat steps 6 to 12.

[0069] When i=4, only drive the inner arm rotation joint and the outer arm rotation joint to make them move, take the position of the end of the robotic arm as the current position, and repeat steps 6 to 12.

[0070] When i=5, set only the outer arm rotation joint to be driven to move it, take the position of the end of the robotic arm as the current position, and repeat steps 6 to 12.

[0071] Otherwise, terminate the docking operation.

[0072] Preferably, in some embodiments, in step 1, the position of the first virtual flange is the position at a distance L1 from the plane of the ship-side manifold flange along the central axis of the ship-side manifold flange; the position of the second virtual flange is the position at a distance L2 from the plane of the ship-side manifold flange along the central axis of the ship-side manifold flange; and the position of the third virtual flange is the position at a distance L2 from the plane of the ship-side manifold flange along the central axis of the ship-side manifold flange, but offset from the axis direction. Location, The angle of rotation of the horizontal rotary joint relative to its initial position. The parameter is adjustable; the position of the fourth virtual flange is the distance from the plane L3 of the ship side manifold flange along the central axis of the ship side manifold flange; where L1 is greater than L2, and L2 is greater than L3.

[0073] Preferably, the pose matrix of the first virtual flange is: L1 is 600mm; the pose matrix of the second virtual flange is L2 is 420mm; the pose matrix of the third virtual flange is ,generally The pose matrix of the fourth virtual flange is: L3 is 250mm; the pose matrix of the ship's side manifold flange is... .

[0074] In this invention, the position of the first virtual flange is taken as the rough docking endpoint. Three virtual flanges are set along the central axis of the ship side manifold flange and in its vicinity after the first virtual flange as transition positions for step-by-step docking. The step-by-step positioning docking realizes multi-step control of the precise docking process and improves docking accuracy.

[0075] Furthermore, by setting up a virtual flange along the central axis of the ship's side manifold flange, alignment between the robotic arm and the ship's side manifold flange is achieved first during the step-by-step docking process. During the final docking, without prior alignment, the guide rod may touch the ship's side manifold flange first, causing the robotic arm to become immobile and resulting in docking failure. This invention, by setting up a virtual flange along the central axis of the ship's side manifold flange, avoids this situation by performing alignment first, thereby improving the docking success rate.

[0076] Furthermore, in one embodiment, in step 4, the robotic arm model is simplified as follows: Figure 3 As shown, the following coordinate systems are established sequentially: 0-link coordinate system, 1-link coordinate system, 2-link coordinate system, 3-link coordinate system, 4-link coordinate system, 5-link coordinate system, 6-link coordinate system, and 7-link coordinate system. These correspond to the coordinate systems of the base, horizontal rotary joint, inner arm rotary joint, outer arm rotary joint, self-weight joint, three-dimensional joint rotary joint, rigid tube between the three-dimensional joint rotary joint and the end effector, and the end effector, respectively.

[0077] Furthermore, in one embodiment, in step 6, the formula for calculating the pose matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system is:

[0078]

[0079] In the formula, Let be the homogeneous transformation matrix between the 1-link coordinate system and the 0-link coordinate system. Let be the homogeneous transformation matrix between the two-link coordinate system and the one-link coordinate system. Let be the homogeneous transformation matrix between the 3-link coordinate system and the 2-link coordinate system. Let be the homogeneous transformation matrix between the 4-link coordinate system and the 3-link coordinate system. Let be the homogeneous transformation matrix between the 5-link coordinate system and the 4-link coordinate system. Let be the homogeneous transformation matrix between the 6-link coordinate system and the 5-link coordinate system. This is the homogeneous transformation matrix between the 7-link coordinate system and the 6-link coordinate system. This is the pose matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system; Let be the attitude matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system. This is the position vector of the robotic arm's end-effector coordinate system relative to the geodetic coordinate system.

[0080] Furthermore, in one embodiment, the pose matrix of the target position i with respect to the geodetic coordinate system in step 8... The calculation formula is:

[0081] .

[0082] Furthermore, in one embodiment, step 9 specifically includes the following process:

[0083] Step 9.1: Based on the relationship between the joint motion velocity and the end-effector motion velocity of the multi-link mechanism in the system, iteratively calculate the end-effector angular velocity and linear velocity of link j in its coordinate system. , The calculation formulas are as follows:

[0084]

[0085] In the formula, , These are the angular velocity and linear velocity of link j+1 in its coordinate system, respectively; This refers to the change in joint angle connected to link j+1. Let be the rotation matrix between the j+1 link coordinate system and the j link coordinate system. unit column vector , Let be the position vector between the j+1 link coordinate system and the j link coordinate system;

[0086] Step 9.2, let The angular velocity and linear velocity at the end will represent the final velocity. , Write it in the form of joint angle vectors , It is a Jacobian matrix that contains angle information. for The column vector formed;

[0087] Step 9.3: By inverting the equation, the angle difference of each joint can be calculated based on the pose difference at a certain moment.

[0088] Furthermore, in one embodiment, in step 11, the error setting value is 10mm.

[0089] This invention reduces the combined action of four joints to two joints by setting a horizontal offset at a position close to the ship's side manifold flange, enabling pre-action of the horizontal and three-dimensional joints. When the guide rod approaches the ship's side manifold flange, if the combined action of four joints is still used, the inconsistent starting speeds or mismatched running speeds of each joint will cause deviation from the straight running trajectory, resulting in the guide rod scraping or rubbing against the ship's side manifold flange, leading to docking failure. Furthermore, the starting and running speeds of the joints are largely constrained by the hydraulic drive system. This invention, by setting a horizontal offset at a position close to the ship's side manifold flange and pre-actioning the horizontal and three-dimensional joints, avoids the situation where the adjustment space is limited when the guide rod approaches or surrounds the ship's side manifold flange, thus improving docking stability.

[0090] The embodiments described above are merely examples of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application.

Claims

1. A precision docking system for a marine robotic arm, the system comprising a marine robotic arm, the marine robotic arm including a base and multiple joints, wherein the base and joints, and the joints themselves, are connected by rigid tubing; the multiple joints include at least a horizontal rotary joint, an inner arm rotary joint, an outer arm rotary joint, a three-dimensional joint rotary joint, and a self-weight joint; wherein, The three joints—horizontal rotary joint, inner arm rotary joint, and outer arm rotary joint—are driven by hydraulic cylinders, while the three-dimensional joint rotary joint is driven by a hydraulic motor and is located at the end of the marine robotic arm. The self-weight joint relies on its own weight to ensure that the flange at the end of the marine robotic arm always faces forward. The feature is that a surface array camera is installed at the three-dimensional joint rotary joint, and the three-dimensional joint rotary joint drives the surface array camera to rotate. The end of the marine robotic arm is equipped with an end effector for docking with the manifold flange on the side of the ship.

2. The marine robotic arm precision docking system according to claim 1, characterized in that, The end effector includes a robotic arm flange that docks with the ship's side manifold flange, and also includes a plurality of guide rods for centering arranged radially outward from the robotic arm flange.

3. A method for precise docking of a marine robotic arm based on the system described in any one of claims 1 to 2, characterized in that, The method includes the following steps: Step 1: Determine four virtual flanges between the end of the robotic arm and the ship-side manifold flange along the central axis of the ship-side manifold flange and its approximate axis. The position of the first virtual flange is the rough end point of the docking, and the positions of the other three virtual flanges and the ship-side manifold flange are the target positions for the step-by-step movement of the robotic arm. Step 2: Control the end effector to move to the position of the first virtual flange using the PLC; Step 3: Keep the device stationary and take multiple photos of the ship-side manifold flange with the area array camera to achieve the positioning of the ship-side manifold flange by the area array camera. Step 4: Establish a kinematic coordinate system based on the DH method; Step 5: Use i to represent the sequence number and set i = 2; Step 6: Read the current angle values ​​of each joint, calculate the homogeneous transformation matrix of each joint, and then calculate the pose matrix of the robotic arm end coordinate system relative to the geodetic coordinate system. Step 7: Acquire the pose matrix of the ship-side manifold flange relative to the coordinate system of the robotic arm end effector using the area array camera. ; Step 8, based on the pose matrix of target position i Calculate the pose matrix of the target position i in the geodetic coordinate system. ; Step 9: Obtain the angle difference of each joint when the end effector of the robotic arm reaches the target position i using the vector product method; Step 10: Input the joint angle difference values ​​into the fuzzy PID control method to calculate the control quantity, and then control the end of the robotic arm to move from the current position to the target position i through the PLC according to the magnitude of the control quantity; Step 11: Obtain the distance between the current end effector of the robotic arm and the target position i using an area scan camera; If the distance between the current end of the robotic arm and the target position i is less than the error setting value, the robotic arm reaches the target position i and then proceeds to step 12; otherwise, steps 6 to 11 are repeated. Step 12: Let i = i + 1. When i = 2 or i = 3, take the position of the end of the robotic arm as the current position and repeat steps 6 to 12. When i=4, only drive the inner arm rotation joint and the outer arm rotation joint to make them move, take the position of the end of the robotic arm as the current position, and repeat steps 6 to 12. When i=5, set only the outer arm rotation joint to be driven to move it, take the position of the end of the robotic arm as the current position, and repeat steps 6 to 12. Otherwise, terminate the docking operation.

4. The precise docking method for marine robotic arms according to claim 3, characterized in that, In step 1, the position of the first virtual flange is the distance from the ship-side manifold flange plane L1 along the central axis of the ship-side manifold flange; the position of the second virtual flange is the distance from the ship-side manifold flange plane L2 along the central axis of the ship-side manifold flange; the position of the third virtual flange is the distance from the ship-side manifold flange plane L2 along the central axis of the ship-side manifold flange, but offset from the central axis. Location, The angle of rotation of the horizontal rotary joint relative to its initial position. The parameter is adjustable; the position of the fourth virtual flange is the distance from the plane L3 of the ship side manifold flange along the central axis of the ship side manifold flange; where L1 is greater than L2, and L2 is greater than L3.

5. The precise docking method for marine robotic arms according to claim 3, characterized in that, In step 4, the robotic arm model is simplified, and the following coordinate systems are established sequentially: 0-link coordinate system, 1-link coordinate system, 2-link coordinate system, 3-link coordinate system, 4-link coordinate system, 5-link coordinate system, 6-link coordinate system, and 7-link coordinate system. These correspond to the coordinate systems of the base, horizontal rotary joint, inner arm rotary joint, outer arm rotary joint, self-weight joint, 3D joint rotary joint, rigid tube between the 3D joint rotary joint and the end effector, and the end effector, respectively.

6. The precise docking method for marine robotic arms according to claim 5, characterized in that, In step 6, the formula for calculating the pose matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system is: ； In the formula, Let be the homogeneous transformation matrix between the 1-link coordinate system and the 0-link coordinate system. Let be the homogeneous transformation matrix between the two-link coordinate system and the one-link coordinate system. Let be the homogeneous transformation matrix between the 3-link coordinate system and the 2-link coordinate system. Let be the homogeneous transformation matrix between the 4-link coordinate system and the 3-link coordinate system. Let be the homogeneous transformation matrix between the 5-link coordinate system and the 4-link coordinate system. Let be the homogeneous transformation matrix between the 6-link coordinate system and the 5-link coordinate system. This is the homogeneous transformation matrix between the 7-link coordinate system and the 6-link coordinate system. This is the pose matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system; Let be the attitude matrix of the robotic arm's end effector coordinate system relative to the geodetic coordinate system. This is the position vector of the robotic arm's end-effector coordinate system relative to the geodetic coordinate system.

7. The precise docking method for marine robotic arms according to claim 3, characterized in that, The pose matrix of target position i in the geodetic coordinate system in step 8 The calculation formula is: 。 8. The precise docking method for marine robotic arms according to claim 3, characterized in that, Step 9 includes the following specific steps: Step 9.1: Based on the relationship between the joint motion velocity and the end-effector motion velocity of the multi-link mechanism in the system, iteratively calculate the end-effector angular velocity and linear velocity of link j in its coordinate system. , The calculation formulas are as follows: ； In the formula, , These are the angular velocity and linear velocity of link j+1 in its coordinate system, respectively; This refers to the change in joint angle connected to link j+1. Let be the rotation matrix between the j+1 link coordinate system and the j link coordinate system. unit column vector , Let be the position vector between the j+1 link coordinate system and the j link coordinate system; Step 9.2, let The angular velocity and linear velocity at the end will represent the final velocity. , Write it in the form of joint angle vectors , It is a Jacobian matrix that contains angle information. for The column vector formed; Step 9.3: By inverting the equation, the angle difference of each joint can be calculated based on the pose difference at a certain moment.

9. The precise docking method for marine robotic arms according to claim 3, characterized in that, In step 11, the error setting value is 10mm.

Images