Synchronous control method for underwater contact detection robot

Abstract

The invention provides a synchronous control method for an underwater contact detection robot, and the method comprises the steps: constructing a first kinematical equation based on the multi-dimensional speed and pose of the robot, constructing a second kinematical equation based on the joint parameters of the mechanical arm and a homogeneous transformation matrix between the adjacent joints, and constructing a synchronous control system for the underwater contact detection robot. Fusing the first kinematic equation and the second kinematic equation to form an overall motion model; constructing a third-order time polynomial based on path boundary conditions of the end effector, and determining an end track of the end effector; and an inverse kinematics solving function containing a minimum norm item and a null space item is constructed through the overall Jacobian matrix, the inverse kinematics solving function is solved, and the moving speed and the joint angle of the robot at any moment are determined. According to the method, the robot and the mechanical arm can be synchronously controlled, and multi-target motion planning of joint limit obstacle avoidance and stable robot posture is considered while the precision of the end effector is ensured.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and specifically to a synchronous control method for an underwater contact detection robot. Background Technology

[0002] In marine environments, underwater contact inspection tasks (such as pipeline inspection, ship inspection, and seabed structure maintenance) place higher demands on the operational capabilities of underwater robots. Traditional underwater robots mostly complete tasks through remote control or semi-autonomous methods, but in complex or dynamic environments, the motion planning of a single entity is no longer sufficient to meet the requirements of high precision and strong stability.

[0003] Compared to standalone carriers or robotic arms, collaborative systems can simultaneously plan the motion trajectories of underwater robots and robotic arms, eliminating the isolated movement of either robot or robotic arm, thus shortening detection time and improving underwater detection efficiency.

[0004] Therefore, a collaborative motion planning method that can integrate multi-degree-of-freedom robotic arms and underwater robots is needed to improve the intelligence and automation level of underwater operations. Summary of the Invention

[0005] In view of this, the problem to be solved by the present invention is to provide a synchronous control method for an underwater contact detection robot, which can synchronously control the robot and the robotic arm, while ensuring the detection accuracy of the end effector, taking into account the multi-objective motion planning of joint limit obstacle avoidance and robot posture stability, and effectively improving the intelligence and automation level of underwater operations.

[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows: A synchronous control method for an underwater contact detection robot, the robot including a robotic arm whose end effector contacts the device to be detected, includes: constructing an overall motion model; constructing a first kinematic equation representing the robot's motion based on the robot's velocity and pose in several dimensions; constructing a second kinematic equation representing the motion of the robotic arm's end effector based on the joint parameters of the robotic arm and the homogeneous transformation matrix between adjacent joints; and fusing the first and second kinematic equations to form an overall motion model containing an overall Jacobian matrix between the robot and the robotic arm. Generate the end-effector trajectory. Based on the starting position, starting velocity, end position, and end velocity of the end effector's path, construct a third-order time polynomial. By solving the third-order time polynomial, determine the robot's desired position and desired velocity at each moment to form the end-effector trajectory. The overall control commands are determined, and an inverse kinematics solution function containing minimum norm terms and null space terms is constructed using the overall Jacobian matrix. The inverse kinematics solution function is then solved based on the desired position and desired velocity at a certain moment in the end trajectory. The robot's moving speed and joint angles at that moment are then determined to form the robot's overall control commands.

[0007] Furthermore, the first kinematic equation is: , in, This is the linear velocity rotation matrix for transforming the position vector from the inertial coordinate system to the vehicle coordinate system. This is the angular velocity rotation matrix for transforming the attitude vector from the inertial coordinate system to the vehicle coordinate system. This is the linear velocity vector in the carrier coordinate system, containing the velocities along the x, y, and z axes. Let be the angular velocity vector in the carrier coordinate system, containing rotations about the x-axis, y-axis, and z-axis. Let be the displacement vector in the inertial coordinate system, containing displacements along the x-axis, y-axis, and z-axis. This is the attitude vector in the inertial coordinate system, containing roll, pitch, and yaw angles. It is the pose vector in the inertial coordinate system.

[0008] Furthermore, the second kinematic equation is: , , in, This represents the homogeneous transformation matrix between the coordinates corresponding to joint i-1 and the coordinates corresponding to joint i. This represents the rotation matrix from coordinate system i to coordinate system i+1. This indicates the position of the origin of coordinate system i-1 within coordinate system i. This represents the velocity vectors of each joint in the carrier coordinate system. The Jacobian matrix represents the linear velocity component of the robotic arm. The Jacobian matrix representing the angular velocity component of the robotic arm. These are the rotational angular velocity and linear velocity of joint 1, joint 2... joint n, respectively.

[0009] Furthermore, the overall motion model is as follows: , = , in, Let the overall Jacobian matrix be the matrix between the robot and the robotic arm. , where is the overall velocity vector of the robot in the carrier coordinate system. Indicates the end effector of the robotic arm In the inertial coordinate system The lower position, This indicates the position of the end effector of the robotic arm in the carrier coordinate system. This represents the robot's angular velocity transformation matrix. This represents the linear velocity transformation matrix of the robot.

[0010] Furthermore, the inverse kinematics solution function is: , in, The overall Jacobian matrix between the robot and the robotic arm The false rebellion, For the desired speed of the end effector, For positive definite matrix gain, This represents the difference between the current position of the end effector in the inertial coordinate system and the desired position of the end effector. Let I be the weight of the null space term of the pseudo-inverse of the global Jacobian matrix, and let I be the identity matrix. The solution is in null space; False reversal The formula is: , , in, Let the overall Jacobian matrix between the robot and the robotic arm be . The weight matrix, For the control weights of velocity in each degree of freedom of the robot, (1- ) represents the angular velocity control weights for each joint of the robotic arm, where u is the dimension of the velocity vector and n is the number of degrees of freedom of the robotic arm's joints. Further, the control weights... The formula for calculation is: , , in, This represents the outside of the robot's boundary constraint space. denoted by , represents the external space of the robotic arm's workspace, and 'e' represents the positional error between the end effector and the target object in the inertial coordinate system.

[0011] Furthermore, the zero-space term includes the angle constraint functions of each joint of the robotic arm. And robot posture constraint function The angle constraint function is solved by gradient projection method. and attitude constraint function To obtain the null space solution .

[0012] Furthermore, the angle constraint function for: , Where i represents the i-th joint. These represent the maximum and minimum limit angles of the joint, respectively.

[0013] Furthermore, the attitude constraint function for: , in, This is the roll angle. The pitch angle, Yaw angle These represent positive definite constants, which are constant values ​​set for the coupling effects of different attitudes.

[0014] The beneficial effects of this invention are: By fusing the motion models of the robot and the robotic arm to construct an overall motion model, the overall Jacobian matrix of the robot is determined. Based on the overall Jacobian, an inverse kinematics solution function is constructed, and the gradient projection method is used for redundancy decomposition. Complex detection tasks are processed in layers according to priority, which avoids joint limits and maintains robot posture stability while strictly ensuring end-effector accuracy. The coordinated motion of the robot and the robotic arm effectively solves the kinematic redundancy problem between the robot and the robotic arm while ensuring the end-effector accuracy, thus improving the reliability, safety and adaptability of the system operation.

[0015] Trajectory planning using cubic time polynomials ensures the smoothness and continuity of the end effector's motion, reduces impact, and is suitable for precision underwater operations. Attached Figure Description

[0016] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a structural diagram of a detection robot according to the present invention; Figure 2 This is a flowchart of a synchronization control method for an underwater contact detection robot according to the present invention; Figure 3 It is a function The curve graph. Detailed Implementation

[0017] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the specification of this invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed terms.

[0019] This invention provides a synchronization control method for an underwater contact detection robot, such as... Figure 1 As shown, the inspection robot includes a robotic arm whose end effector contacts the surface of the device under inspection. When the robot is working, it moves along the device under inspection and controls the end effector of the robotic arm to contact the surface of the device, completing the underwater contact inspection task.

[0020] like Figure 2 As shown, the synchronous control method includes constructing an overall motion model, constructing a first kinematic equation representing the robot's motion based on the robot's velocity and pose in several dimensions, constructing a second kinematic equation representing the motion of the robot's end effector based on the joint parameters of the robotic arm and the homogeneous transformation matrix between adjacent joints, and fusing the first and second kinematic equations to form an overall motion model containing the overall Jacobian matrix between the robot and the robotic arm.

[0021] The robot's motion includes its three-degree-of-freedom position information, three-degree-of-freedom attitude information, three-degree-of-freedom linear velocity information, and three-degree-of-freedom rotation angle information. Based on the above information, the first kinematic equations are constructed, including: Establish the robot's 6-dimensional pose vector in the inertial coordinate system. : , , , in, This is the robot's position vector in the inertial coordinate system. For displacement along the x-axis, For displacement along the y-axis, Displacement along the z-axis; For transpose, This is the robot's attitude vector in the inertial coordinate system. This is the roll angle. The pitch angle, This is the yaw angle.

[0022] Establish the 6-dimensional velocity vector of the robot in its carrier coordinate system. : , , , in, It is the linear velocity vector. The velocity along the x-axis, The velocity along the y-axis, The velocity along the z-axis, It is the angular velocity vector. Let be the angular velocity of rotation about the x-axis. Let be the angular velocity of rotation about the y-axis. Let be the angular velocity of rotation about the z-axis. This is the velocity vector.

[0023] The rotation matrix J between the inertial coordinate system and the carrier coordinate system is determined based on the actual installation positions of the carrier sensors and inertial sensors inside the robot. The first kinematic equation is then constructed based on the rotation matrix J. , in, It is a 3×3 zero matrix. This is the linear velocity rotation matrix for transforming the position vector from the inertial coordinate system to the vehicle coordinate system. This is the angular velocity rotation matrix for transforming the attitude vector from the inertial coordinate system to the vehicle coordinate system.

[0024] Joint parameters include joint rotation angle, link offset, link length, and link torsion angle. Constructing the second kinematic equation involves: building a homogeneous transformation matrix T for adjacent joints based on the standard DH parameter method. The link coordinate system of the standard DH parameter method is fixed to the end of each link, facilitating direct determination of the end effector's pose.

[0025] The sequence of parameter changes in the coordinate systems of any two adjacent links of the robotic arm is: θ represents the joint rotation angle, and d represents the offset of the link. Indicates the length of the link. This indicates the torsion angle of the connecting rod.

[0026] Therefore, the homogeneous transformation matrix between adjacent joints can be expressed as: , in, This represents the homogeneous transformation matrix between the coordinates corresponding to joint i-1 and the coordinates corresponding to joint i, where z represents the z-axis and x represents the x-axis. This represents the rotation angle of joint i. This represents the link offset corresponding to joint i. This indicates the length of the link corresponding to joint i-1. This represents the torsion angle of joint i-1.

[0027] The homogeneous transformation matrix between coordinates i-1 and i. Specifically: , Where s represents sin calculation and c represents cos calculation. This represents the rotation matrix from coordinate system i to coordinate system i+1. This indicates the position of the origin of coordinate system i-1 within coordinate system i.

[0028] By using the joint parameters of each joint within the robotic arm and the homogeneous transformation matrix between adjacent joints, a second kinematic equation is constructed to represent the velocity of the end effector in the robotic arm's base coordinate system.

[0029] The second kinematic equations involve solving the Jacobian matrix of the detection arm using vector calculus: , in, This refers to the velocity vectors of each joint in the carrier coordinate system. The Jacobian matrix represents the linear velocity component of the robotic arm. The Jacobian matrix representing the angular velocity component of the robotic arm. These are the rotational angular velocities and linear velocities of joints 1, 2, ..., n, respectively (q can be obtained by calculating the homogeneous transformation matrix).

[0030] The first and second kinematic equations are combined to form a holistic motion model: , = , in, Let the overall Jacobian matrix be the matrix between the robot and the robotic arm. , which is the second velocity vector in the carrier coordinate system. Indicates the end effector of the robotic arm In the inertial coordinate system The lower position, This indicates the position of the end effector of the robotic arm in the carrier coordinate system. This represents the robot's angular velocity transformation matrix. This represents the linear velocity transformation matrix of the robot.

[0031] The end effector trajectory is generated by constructing a third-order time polynomial based on the starting position, starting velocity, end position, and end velocity of the end effector's path. The desired position and velocity of the robot at each time step are determined by solving the third-order time polynomial to form the end effector trajectory.

[0032] The formula for trajectory planning using a third-order time polynomial is: , The differentiated velocity planning formula is as follows: , in, Let t be the parameter, and s(t) be the distance traveled at time t. This represents the speed of movement at time t.

[0033] By constructing a third-order time polynomial that satisfies the trajectory boundary conditions (the robot's starting position, starting velocity, end position, and end velocity), a smooth and continuous end trajectory is generated in the task space. The end trajectory includes the desired position and desired velocity at several consecutive moments.

[0034] The process of solving parameters based on trajectory boundary conditions is as follows: , Solving for: , Where T is the duration of the robot's movement trajectory (set manually based on experience). The starting and ending positions of the trajectory (set according to the actual trajectory of the robot). These are the movement speeds at the start and end positions of the trajectory, respectively. One embodiment of this application is: based on the robot's usage scenario, the default settings... All are 0.

[0035] The overall control commands are determined, and an inverse kinematics solution function containing minimum norm terms and null space terms is constructed using the overall Jacobian matrix. The inverse kinematics solution function is then solved based on the desired position and desired velocity at a certain moment in the end trajectory. The robot's moving speed and joint angles at that moment are then determined to form the robot's overall control commands.

[0036] The inverse kinematics solution function is: , in, The overall Jacobian matrix between the robot and the robotic arm The false rebellion, For the desired speed of the end effector, For positive definite matrix gain, This is the difference between the current position of the end effector in the inertial coordinate system and the desired position of the end effector. It is used to avoid velocity drift during numerical integration of the velocity vector. Let I be the weight of the null term of the pseudo-inverse of the global Jacobian matrix. Solving for the term in the null space does not affect the position tracking accuracy of the end effector; it only adjusts the redundancy of degrees of freedom by adjusting the degrees of freedom within the robot and robotic arm. I is the identity matrix. It is a null space solution.

[0037] The null term is used to control the self-motion within the robot and robotic arm, without affecting the motion accuracy of the end effector. The inverse kinematics solution function decomposes the overall control of the robot into minimum norm terms and null terms, ensuring the positional accuracy of the end effector while redundantly decomposing the joint motions within the robot.

[0038] False reversal The formula is: , , in, Let the overall Jacobian matrix between the robot and the robotic arm be . The weight matrix, For the control weights of velocity in each degree of freedom of the robot, (1- ) represents the angular velocity control weights of each joint of the robotic arm, u is the dimension of the velocity vector, and n is the number of degrees of freedom of the robotic arm's joints.

[0039] Through pseudo-inverse Add weight matrix Adjust the minimum norm solution and null space solution The cumulative ratio of speed control and joint control is adjusted by the weight matrix. Inside Based on ensuring that the position and speed of the end effector meet the requirements, different priorities are assigned to the robot's extreme operating conditions to control the robot to finally complete the detection task under coordinated motion.

[0040] Based on the workspace of the robotic arm, a boundary space Gc for the overall operation of the robot is drawn. In one embodiment of this application, the boundary space Gc is the robot's maximum observable range. The boundary space Gc also includes the workspace Ga that the robotic arm can reach. The boundary space Gc and the workspace Ga serve as the boundary for the simultaneous movement of the robot and the robotic arm.

[0041] like Figure 3 As shown, control weights The formula for calculation is: , In the formula, It is a fifth-order smooth polynomial. Denotes the outer boundary of the boundary constraint space Gc. Let Ga represent the outer boundary of the robotic arm's workspace, and e represent the positional error between the end effector and the target object in the inertial coordinate system. Since the boundary constraint space includes the robotic arm's workspace, i.e. , when e> That is, when the end effector is outside the boundary space Gc, =1 only allows the robot body to move, which can effectively plan the speed of both.

[0042] The null term includes the angle constraint functions of each joint of the robotic arm. And robot posture constraint function The angle constraint function is solved by gradient projection method. and attitude constraint function To obtain the null space solution .

[0043] One embodiment of this application is as follows: assuming the number of arm joints being detected is 3, and the angular limit angle of the i-th joint is... Let be the maximum limit angle and the minimum limit angle, respectively. Then the joint angle constraint function is: , Where i represents the i-th joint The constraint functions that include the robot's pitch, roll, and yaw angles are: .

[0044] These represent positive definite constants, which are constant values ​​set according to the coupling effect of the angular velocity motion of each joint of the robot on different posture angles.

[0045] Based on the above constraint functions, redundancy decomposition is performed using the gradient projection method to obtain the solution in the null space: , in, This represents the solutions for the angles of each joint of the robotic arm and the robot's posture within the null space.

[0046] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention shall still fall within the scope of the technical solution of the present invention.

Claims

1. A synchronous control method for an underwater contact inspection robot, the robot comprising a robotic arm whose end effector contacts the device to be inspected, characterized in that, include, Construct an overall motion model. Based on the robot's velocity and pose in several dimensions, construct the first kinematic equation to represent the robot's motion. Based on the joint parameters of the robotic arm and the homogeneous transformation matrix between adjacent joints, construct the second kinematic equation to represent the motion of the robotic arm's end effector. Combine the first and second kinematic equations to form an overall motion model containing the overall Jacobian matrix between the robot and the robotic arm. Generate the end-effector trajectory. Based on the starting position, starting velocity, end position, and end velocity of the end effector's path, construct a third-order time polynomial. By solving the third-order time polynomial, determine the robot's desired position and desired velocity at each moment to form the end-effector trajectory. The overall control commands are determined, and an inverse kinematics solution function containing minimum norm terms and null space terms is constructed using the overall Jacobian matrix. The inverse kinematics solution function is then solved based on the desired position and desired velocity at a certain moment in the end trajectory. The robot's moving speed and joint angles at that moment are then determined to form the robot's overall control commands.

2. The synchronization control method for an underwater contact detection robot according to claim 1, characterized in that, The first kinematic equation is: , in, This is the linear velocity rotation matrix for transforming the position vector from the inertial coordinate system to the vehicle coordinate system. This is the angular velocity rotation matrix for transforming the attitude vector from the inertial coordinate system to the vehicle coordinate system. This is the linear velocity vector in the carrier coordinate system, containing the velocities along the x, y, and z axes. Let be the angular velocity vector in the carrier coordinate system, containing rotations about the x-axis, y-axis, and z-axis. Let be the displacement vector in the inertial coordinate system, containing displacements along the x-axis, y-axis, and z-axis. This is the attitude vector in the inertial coordinate system, containing roll, pitch, and yaw angles. It is the pose vector in the inertial coordinate system.

3. The synchronization control method for an underwater contact detection robot according to claim 1, characterized in that, The second kinematic equation is: ， ， in, This represents the homogeneous transformation matrix between the coordinates corresponding to joint i-1 and the coordinates corresponding to joint i. This represents the rotation matrix from coordinate system i to coordinate system i+1. This indicates the position of the origin of coordinate system i-1 within coordinate system i. This represents the velocity vectors of each joint in the carrier coordinate system. The Jacobian matrix represents the linear velocity component of the robotic arm. The Jacobian matrix representing the angular velocity component of the robotic arm. These are the rotational angular velocity and linear velocity of joint 1, joint 2... joint n, respectively.

4. The synchronization control method for an underwater contact detection robot according to claim 1, characterized in that, The overall motion model is as follows: ， = ， in, For the overall Jacobian matrix of the robot and robotic arm, Let this be the robot's overall velocity vector in the carrier coordinate system. Indicates the end effector of the robotic arm In the inertial coordinate system The lower position, This indicates the position of the end effector of the robotic arm in the carrier coordinate system. This represents the robot's angular velocity transformation matrix. This represents the linear velocity transformation matrix of the robot.

5. The synchronization control method for an underwater contact detection robot according to claim 1, characterized in that, The inverse kinematics solution function is: , in, The overall Jacobian matrix between the robot and the robotic arm The false rebellion, For the desired speed of the end effector, For positive definite matrix gain, This represents the difference between the current position of the end effector in the inertial coordinate system and the desired position of the end effector. Let I be the weight of the null space term of the pseudo-inverse of the global Jacobian matrix, and let I be the identity matrix. The solution is in null space; False reversal The formula is: ， ， in, Let the overall Jacobian matrix between the robot and the robotic arm be . The weight matrix, For the control weights of velocity in each degree of freedom of the robot, (1- ) represents the angular velocity control weights of each joint of the robotic arm, u is the dimension of the robot's velocity vector, and n is the number of degrees of freedom of the robotic arm's joints.

6. The synchronization control method for an underwater contact detection robot according to claim 5, characterized in that, The control weight The formula for calculation is: ， ， in, This represents the outside of the robot's boundary constraint space. denoted by , represents the external space of the robotic arm's workspace, and 'e' represents the positional error between the end effector and the target object in the inertial coordinate system.

7. The synchronization control method for an underwater contact detection robot according to claim 1, characterized in that, The null term includes the angle constraint functions of each joint of the robotic arm. And robot posture constraint function The angle constraint function is solved by gradient projection method. and attitude constraint function To obtain the null space solution .

8. The synchronization control method for an underwater contact detection robot according to claim 7, characterized in that, The angle constraint function for: ， Where i represents the i-th joint. These represent the maximum and minimum limit angles of the joint, respectively.

9. The synchronization control method for an underwater contact detection robot according to claim 7, characterized in that, The attitude constraint function for: ， in, This is the roll angle. The pitch angle, Yaw angle These represent positive definite constants.

Images