A control method for a five-axis serial manipulator of an explosive disposal robot

Abstract

The application discloses a control method for a five-axis serial mechanical arm of an explosive-handling robot. The application comprises the following steps: dividing the five-axis mechanism into three parts and respectively designing control algorithms; the first part is joint space operation of the first axis, the second part is defined as position and pitch parameter (x, z, alpha) operation in the XZ plane, and the third part is joint space operation of the fifth axis; the mechanical arm is directed to the target object through joint space operation of the first axis, the second part is operated in the plane space to approach the target object, then the fifth axis is operated in the joint space to adjust the grabbing posture, and the task operation is completed; the first part and the second part are used to operate the mechanical arm in the plane to reach a posture suitable for driving of the unmanned vehicle, and other task operations are performed on the target. The application combines forward and inverse kinematics modeling of joint space operation and plane operation space by designing the structure type and operation mode of the target object through the algorithm, effectively improves the operability of the mechanical arm mechanism, and reduces the operation difficulty.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robot control, and particularly relates to a control method for a five-axis serial mechanical arm of an explosive disposal robot. BACKGROUND

[0002] The explosive disposal robot refers to a platform capable of replacing explosive disposal personnel to implement investigation, transfer, disassembly and destruction on explosive devices and other dangerous articles. The platform mainly comprises a mobile chassis, a mechanical arm, a surveillance camera and a remote communication module and the like. The explosive disposal robot is generally developed by using various new materials and new technologies, and various key technologies are also concerned and valued by the academic and engineering circles. With the increasingly severe domestic and foreign situation today, the explosive disposal robot will develop more rapidly.

[0003] Defects and deficiencies of the prior art:

[0004] At present, the mechanical arm of the explosive disposal robot generally adopts a five-axis serial mechanical arm to perform operation on a target object, and has joint space and Cartesian space two operation modes. Since the five-axis serial mechanical arm is an under-actuated mechanical arm, it cannot realize grabbing at any position in space. If the mechanical arm adopting the standard D-H modeling is used to realize Cartesian space motion, the control precision of operation along the X, Y and Z axes will be affected due to the uncontrollable Z-axis posture, and the complexity of completing the task is increased.

[0005] In order to solve the control precision problem of the five-axis serial mechanical arm of the explosive disposal robot, and in the principle of easy implementation, high operability and good reliability, a control method for the five-axis serial mechanical arm of the explosive disposal robot is designed, so as to improve the operability and reliability of the explosive disposal robot, and further promote the development and application of the explosive disposal robot industry. SUMMARY

[0006] The purpose of the present application is to provide a control method for a five-axis serial mechanical arm of an explosive disposal robot, which combines joint space operation and inverse kinematics modeling of planar operation space by designing the structure type and operation mode of the target object through algorithm, and solves the problems of low operability, high operation difficulty and low precision of the existing mechanical arm.

[0007] To solve the above technical problems, the present application is realized by the following technical scheme:

[0008] The present application is a control method for a five-axis serial mechanical arm of an explosive disposal robot, comprising the following steps:

[0009] Step S1: divide the five-axis mechanism into three parts: the first part is the first axis, the second part includes the second axis, the third axis and the fourth axis, and the third part is the fifth axis, and control algorithm design is performed on each part respectively;

[0010] Step S2: define the action of the first part of the first axis as a joint space operation, define the action of the second part of the second axis, the third axis and the fourth axis as the operation of the position and pitch parameter (x, z, a) of the XZ plane, and define the third part of the fifth axis as a joint space operation;

[0011] Step S3: make the mechanical arm operation plane perpendicular to the target object by rotating the first axis, approach the target object by using the position control of the second part, then adjust the mechanical arm to face the target object, then rotate the fifth axis by an angle value, adjust the roll parameter angle of the predetermined operation, and realize the predetermined operation on the target object;

[0012] Step S4: after the predetermined operation, reasonably use the first part and the second part to operate the mechanical arm in the plane to the driving posture, drive to the destination, and realize other predetermined operations on the target.

[0013] As a preferred technical solution, the connection between one end of the first axis and the base is provided with a first joint; the connection between the other end of the first axis and the second axis is provided with a second joint; the connection between the second axis and the third axis is provided with a third joint; the connection between the third axis and the fourth axis is provided with a fourth joint; and the connection between the fourth axis and the fifth axis is provided with a fifth joint.

[0014] As a preferred technical solution, the first part is a joint operation, which is used to rotate around the Z-axis of the base coordinate system, so that the actions of the second axis, the third axis and the fourth axis are always in the same plane; and the rotation plane is converted into a body by rotation, so as to realize the position operation in three-dimensional space.

[0015] As a preferred technical solution, the second part is a plane operation, which is used to realize the position displacement (X, Z) of the XZ plane and the rotation angle a of the plane around the Y-axis, and to separately model the second axis, the third axis and the fourth axis according to the standard D-H, so as to calculate the homogeneous transformation matrix between the joints based on the D-H parameters, and the specific formula is as follows:

[0016]

[0017] The homogeneous transformation matrix from the second joint to the fourth joint is T2 4 (θ2, θ3, θ4), and the position parameter and the pitch parameter (x, z, a) in the XZ plane are calculated through θ2, θ3 and θ4;

[0018] In the formula, θ2 represents the angle value of the second joint, θ3 represents the angle value of the third joint, θ3 represents the angle value of the third joint, θ4 represents the angle value of the fourth joint, x represents the projection distance of the XZ plane on the X-axis, z represents the projection distance of the XZ plane on the Z-axis, and a represents the rotation angle value around the Y-axis.

[0019] As a preferred technical scheme, the step-by-step calculation is performed on x, z and alpha respectively in the homogeneous transformation matrix, and then the joint angle values θ2', θ3' and θ4' are determined through inverse kinematics, which are used to control the action of each joint shaft motor, and the step-by-step operation of x, z and alpha is realized through the coordinated action of the joint motors.

[0020] Wherein, θ2' represents the desired angle value of the second joint, θ3' represents the desired angle value of the third joint, and θ3' represents the desired angle value of the third joint.

[0021] As a preferred technical scheme, the second part of the plane operation is combined with the rotation of the first part around the Z axis to realize the operation of the body space, that is, when a joint moves by an angle θ1, the base system rotates around the Z axis by the same angle θ1, which is equivalent to rotating the operation plane around the Z axis by an angle θ1.

[0022] As a preferred technical scheme, in the step S2, the third part is a joint operation, and the operation angle adjustment is realized through the adjustment of the fifth shaft based on the first part and the second part reaching the spatial target point.

[0023] The present application has the following beneficial effects:

[0024] The present application combines the forward and inverse kinematics modeling of the joint space operation and the plane operation space, effectively improves the operability of the mechanical arm mechanism and reduces the operation difficulty, and solves the problems of difficult algorithm design and low precision after implementation of the five-axis under-actuated mechanism operation algorithm.

[0025] Of course, any product implementing the present application does not necessarily need to achieve all the advantages described above. BRIEF DESCRIPTION OF DRAWINGS

[0026] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed for the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0027] Figure 1 A control method flowchart for a five-axis serial mechanical arm of an explosive-handling robot;

[0028] Figure 2 A coordinate system diagram of a mechanical arm on a mobile platform. DETAILED DESCRIPTION

[0029] With reference to the drawings of the embodiments of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described, obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the protection scope of the present application.

[0030] Please refer to Figure 1 As shown in the figure, the present application is a control method for a five-axis serial mechanical arm of an explosive disposal robot, comprising the following steps:

[0031] Step S1: divide the five-axis mechanism into three parts: the first part is the first axis (base axis), the second part includes the second axis, the third axis and the fourth axis, and the third part is the fifth axis, and control algorithm design is performed on each part respectively;

[0032] The base coordinate system of the mechanical arm on the moving platform is established as shown in the attached Figure 2 As shown in the figure, the X axis points to the front of the vehicle body, the Y axis points to the left side of the vehicle head, and the Z axis is perpendicular to the ground and upward according to the right-hand rule;

[0033] Step S2: define the action of the first part of the first axis as joint space operation, define the action of the second part of the second axis, the third axis and the fourth axis as XZ plane position and pitch parameter (x, z, a) operation, and define the third part of the fifth axis as joint space operation;

[0034] The third part is joint operation, based on the first part and the second part to reach the spatial target point, through the adjustment of the fifth axis, the operation angle adjustment is realized;

[0035] Step S3: rotate the first axis to make the mechanical arm operation plane perpendicular to the target object, use the position control of the second part to approach the target object, then adjust the mechanical arm to face the target operation object, and then rotate the angle value of the fifth axis to adjust the roll parameter angle of the predetermined operation, to realize the predetermined operation on the target object;

[0036] Step S4: after the predetermined operation, reasonably use the first part and the second part to operate the mechanical arm in the plane to the driving posture, drive to the destination, and realize other predetermined operations on the target.

[0037] The connection between one end of the first axis and the base is provided with the first joint; the connection between the other end of the first axis and the second axis is provided with the second joint; the connection between the second axis and the third axis is provided with the third joint; the connection between the third axis and the fourth axis is provided with the fourth joint; and the connection between the fourth axis and the fifth axis is provided with the fifth joint.

[0038] The first part is joint operation, which is used for rotating around the Z-axis of the base coordinate system, so that the actions of the second axis, the third axis and the fourth axis are always in the same plane; the rotation plane is converted into a body to realize the position operation in three-dimensional space.

[0039] The second part is plane operation, which is used for realizing the position displacement (X, Z) in the XZ plane and the rotation angle a around the Y-axis, and separately modeling the second axis, the third axis and the fourth axis according to the standard D-H, and calculating the homogeneous transformation matrix between the joints based on the D-H parameters, and the specific formula is as follows:

[0040]

[0041] The homogeneous transformation matrix between the second joint and the fourth joint is T2 4 (θ2, θ3, θ4), and the position parameters and the pitch parameters (x, z, a) in the XZ plane are calculated through θ2, θ3 and θ4;

[0042] In the formula, θ2 represents the angle value of the second joint, θ3 represents the angle value of the third joint, θ3 represents the angle value of the third joint, θ4 represents the angle value of the fourth joint, x represents the projection distance of the XZ plane on the X-axis, z represents the projection distance of the XZ plane on the Z-axis, and a represents the rotation angle value around the Y-axis.

[0043] The step calculation is performed on x, z and a in the homogeneous transformation matrix respectively, and then the joint angle values θ2', θ3' and θ4' are determined through inverse kinematics, which are used to control the actions of the motor of each joint axis, and the step operation of x, z and a is realized through the coordinated action of the motors of the joints;

[0044] In the formula, θ2' represents the desired angle value of the second joint, θ3' represents the desired angle value of the third joint, and θ3' represents the desired angle value of the third joint.

[0045] The second part of the plane operation is combined with the rotation around the Z-axis of the first part to realize the operation in the body space, that is, after the joint action θ1 angle, the base system rotates around the Z-axis by the same angle θ1, which is equivalent to rotating the operation plane around the Z-axis by the angle θ1.

[0046] The second part contains three axes, which is a three-degree-of-freedom mechanism, and can realize the XZ plane position control and the control around the Y-axis.

[0047] The third part is defined as joint operation, which realizes the adjustment of the end grasping posture, and based on the first part and the second part reaching the space target point, the adjustment of the operation angle can be realized through the fifth axis adjustment.

[0048] The above three parts of operation are reasonably combined to realize the predetermined operation (grasping, throwing and other operations) on the target object.

[0049] One specific application of the embodiment is:

[0050] Step 1: The five-axis mechanism is divided into three parts: the first part (only containing the first joint), the second part (containing the second, third, and fourth joints), and the third part (only containing the fifth joint) for control algorithm design respectively.

[0051] Step 2: The action of the first joint in the first part is defined as joint space operation, the action of the second, third, and fourth joints in the second part is defined as XZ plane position and pitch parameter (x, z, a) operation, and the fifth axis in the third part is defined as joint space operation.

[0052] Step 3: Rotate the first axis to make the robot arm operation plane perpendicular to the target object, then use the second part position control to approach the target object, adjust the pitch parameter angle of the predetermined operation (grasping), and then rotate the fifth axis angle value to adjust the roll parameter angle of the predetermined operation (grasping), to achieve the predetermined operation on the target object.

[0053] Step 4: After the predetermined operation (grasping), use the first part and the second part to operate the robot arm in the plane to the driving pose, drive to the destination, and achieve other predetermined operations (throwing, etc.) on the target.

[0054] This embodiment divides the control of the five-axis serial robot arm into three parts and implements control respectively: the first part is the first axis (base axis), the second part contains the second, third, and fourth axes, and the third part is the fifth axis. The robot arm establishes a base coordinate system on the moving platform as shown in Figure 2 The X axis points to the front of the vehicle body, the Y axis points to the left side of the vehicle head, and the Z axis is perpendicular to the ground and points upward.

[0055] From the position relationship of each axis of the five-axis serial robot arm, it can be known that the first axis can realize rotation around the Z axis of the base coordinate system, the actions of the second, third, and fourth axes are always in a plane, and the five-axis can realize adjustment of the operation angle.

[0056] Based on the above characteristics, and considering the poor joint space operability and the problem of under-actuation of Cartesian operation, an algorithm model for the bomb disposal robot containing joint operation and plane position operation is designed as follows:

[0057] The first part is defined as joint operation, which realizes rotation around the Z axis of the base coordinate system. By rotating the plane, it can be converted into a body, thereby realizing position operation in three-dimensional space.

[0058] The second part is defined as plane operation, which realizes position displacement (X, Z) and plane rotation angle a around the Y axis in the XZ plane. The second, third, and fourth axes are separately modeled by standard D-H, and based on the D-H parameters, the homogeneous transformation matrix between the joints can be calculated, which is like:

[0059]

[0060] Based on the homogeneous transformation matrix of each joint, the homogeneous transformation matrix between the 2nd joint and the 4th joint can be calculated Then the position parameter and the pitch parameter (x, z, a) in the XZ plane can be calculated by θ2, θ3, θ4.

[0061] Wherein:

[0062] θ2 represents the angle value of the 2nd joint, θ3 represents the angle value of the 3rd joint, θ3 represents the angle value of the 3rd joint, θ4 represents the angle value of the 4th joint, x represents the projection distance of the XZ plane on the X axis, z represents the projection distance of the XZ plane on the Z axis, and a represents the rotation angle value around the Y axis.

[0063] Based on the step-by-step calculation of x, z, a in the homogeneous transformation matrix, the desired angle values θ2', θ3', θ4' of each joint can be determined by inverse kinematics, for controlling the action of each joint axis motor, and the step-by-step operation of x, z, a is realized by the coordinated action of each joint motor.

[0064] Wherein: θ2' represents the desired angle value of the 2nd joint, θ3' represents the desired angle value of the 3rd joint, and θ3' represents the desired angle value of the 3rd joint.

[0065] Combining the second part of the plane operation with the first part of the rotation around the Z axis can realize the operation of the body space: when a joint moves by an angle θ1, the base system rotates around the Z axis by the same angle θ1, which is equivalent to rotating the operation plane around the Z axis by an angle θ1.

[0066] Wherein, the second part contains three axes, which is a three-degree-of-freedom mechanism, and can realize XZ plane position control and rotation around the Y axis.

[0067] The third part is defined as joint operation, which realizes the adjustment of the end gripping posture, and based on the arrival of the space target point by the first part and the second part, the operation angle adjustment can be realized by adjusting the fifth axis.

[0068] The above three parts of operation are reasonably combined to realize the predetermined operation (grasping, throwing, etc.) of the target object.

[0069] It is worth noting that in the above system embodiment, each unit included is only divided according to the functional logic, but is not limited to the above division, as long as the corresponding function can be realized; in addition, the specific name of each functional unit is only for the convenience of mutual differentiation, and does not limit the protection scope of the present application.

[0070] In addition, those skilled in the art can understand that all or part of the steps in the above-mentioned method of each embodiment can be completed by a program instructing relevant hardware, and the corresponding program can be stored in a computer readable storage medium.

[0071] The preferred embodiments of the application disclosed above are only used to help explain the application. The preferred embodiments do not describe all of the details of the application, and the application is not limited to the specific embodiments described. Obviously, many modifications and variations can be made according to the content of the specification. The specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the application, so that those skilled in the art can well understand and utilize the application. The application is limited only by the claims and their full scope and equivalents.

Claims

1. A control method for a five-axis serial robotic arm of an explosive ordnance disposal robot, characterized in that, Includes the following steps: Step S1: Divide the five-axis mechanism into three parts: the first part is axis 1, the second part includes axis 2, axis 3, and axis 4, and the third part is axis 5, and design control algorithms for each part separately; Step S2: Define the motion of the first axis in the first part as a joint space operation; define the motion of the second, third, and fourth axes in the second part as the position and pitch parameters (x,z,α) in the XZ plane; and define the fifth axis in the third part as a joint space operation. Step S3: Rotate the first axis to make the operating plane of the robotic arm perpendicular to the target object, use the second part of the position control to move closer to the target object, then adjust the robotic arm to face the target object, and then rotate the fifth axis angle value to adjust the roll parameter angle of the predetermined operation to achieve the predetermined operation on the target object; Step S4: After the scheduled operation, use the first and second part of the plane to operate the robotic arm to the driving posture, drive to the destination, and perform other scheduled operations on the target; The first part is a joint operation, used to rotate around the Z-axis of the base coordinate system, and converts the plane of rotation into a volume to realize position operation in three-dimensional space; The second part involves planar operations, used to realize the positional displacement (X,Z) in the XZ plane and the rotation angle α of the plane around the Y-axis. Standard DH modeling is performed separately for the second, third, and fourth axes. Based on the DH parameters, the homogeneous transformation matrix between joints can be calculated, as shown in the following formula: The homogeneous transformation matrix from joint 2 to joint 4 is: The position and pitch parameters (x, z, α) in XZ are calculated using θ2, θ3, and θ4. In the formula, θ2 represents the angle value of the second joint, θ3 represents the angle value of the third joint, θ4 represents the angle value of the fourth joint, x represents the projection distance of the XZ plane on the X-axis, z represents the projection distance of the XZ plane on the Z-axis, and α represents the rotation angle value around the Y-axis. The third part is joint operation, which is based on the first and second parts to reach the spatial target point, and the operation angle is adjusted by adjusting the fifth axis.

2. The control method for a five-axis serial robotic arm of an explosive ordnance disposal robot according to claim 1, characterized in that, The connection between one end of the first shaft and the base is designated as the first joint; the connection between the other end of the first shaft and the second shaft is designated as the second joint; the connection between the second shaft and the third shaft is designated as the third joint; the connection between the third shaft and the fourth shaft is designated as the fourth joint; and the connection between the fourth shaft and the fifth shaft is designated as the fifth joint.

3. The control method for a five-axis serial robotic arm of an explosive ordnance disposal robot according to claim 1, characterized in that, The homogeneous transformation matrix performs step calculations on x, z, and α respectively, and then determines the joint angle values ​​θ2′, θ3′, and θ4′ through inverse kinematics, which are used to control the movement of each joint axis motor. The joint motors work together to perform step operations on x, z, and α. Where θ2′ represents the desired angle value of the second joint, and θ3′ represents the desired angle value of the third joint.

4. The control method for a five-axis serial robotic arm of a bomb disposal robot according to claim 3, characterized in that, The second part of planar operation is combined with the first part of rotation around the Z-axis to realize volume space operation. That is, when a joint moves by an angle θ1, the base system rotates around the Z-axis by the same angle θ1, which is equivalent to the operation plane rotating around the Z-axis by an angle θ1.

Images