Rope-driven super-redundant snakelike mechanical arm joint control method, medium and equipment
By establishing a D-H coordinate system and building a rope force controller, the problem of rope force control of super redundant serpentine robotic arm in rope drive is solved, and the comprehensive control of joint angle and rope force is realized, and the control accuracy and system response rate are improved.
Patent Information
- Application Number
- CN202410978895.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-22
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2044-07-22
AI Technical Summary
The prior art lacks effective control of the rope force of the ultra-redundant serpentine robot arm of the rope drive, resulting in the impact of control accuracy and system stability.
By establishing a D-H coordinate system, calculating the homogeneous transformation matrix, building a rope force controller, including the rope length control part and the rope force control part, using a parallel PID controller and a dead-zone PID controller, comprehensively considering the joint angle and rope force. A rope force control method for robotic arms was designed.
The precise control of the joint angle of the ultra-redundant robotic arm of rope drive and the interval control of rope force is realized, which shortens the control time and improves the system's response rate and stability.
Smart Images

Figure CN118927237A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of rope-driven super-redundant serpentine mechanical arms, and in particular to a rope-driven super-redundant serpentine mechanical arm joint control method, medium and equipment. Background Art
[0002] Nowadays, a new round of scientific and technological revolution and industrial transformation is accelerating. The new generation of information technology, biotechnology, new energy, new materials, etc. are deeply integrated with robotics technology. The robotics industry has ushered in an upgrade. Robotic arms have been widely used in manufacturing, health care, agriculture, aerospace and other fields, greatly improving production efficiency and operation accuracy. However, in some narrow and confined spaces (such as aircraft engines, maintenance of nuclear power plant equipment, etc.), traditional industrial articulated rigid robotic arms cannot enter the interior to perform operations due to size limitations. In contrast, the rope-driven super-redundant serpentine robotic arm can complete a series of precision operations such as drilling, sealing, testing, and assembly in a narrow space with its higher flexibility and freedom, as well as a lighter design. Its multi-degree-of-freedom and deformable characteristics enable it to bypass complex obstacles and reach areas that traditional robotic arms cannot reach, solving the pain points in the above operations. With the continuous development of technology, the application prospects of the rope-driven super-redundant serpentine robotic arm are becoming more and more broad, which can not only improve the safety and efficiency of operations, but also expand the application potential of robotics technology in more complex environments. This marks another major breakthrough in robotics technology, which will play a positive role in promoting the intelligent and automated development of various industries.
[0003] The control accuracy (positioning and trajectory tracking accuracy) of the end effector of the rope-driven super-redundant serpentine manipulator is the core indicator for measuring operational capabilities. For traditional rigid manipulators, their control methods have been developed relatively maturely, and can be mainly divided into three types of control methods: model-free, kinematic model-based, and dynamic model-based. Among them, the model-free control method is mainly based on artificial intelligence and data-driven methods to design the controller. Since it does not need to rely on the model, the complex modeling process is omitted. However, since the model-free control method requires a large amount of data for training, the control effect of the serpentine manipulator with a complex mechanical structure is not ideal; the control based on the dynamic model takes into account factors such as inertia, friction, and external force, can provide high-precision control effects, and can cope with complex load changes and external disturbances, improving the robustness and stability of the system. However, this type of method requires a detailed understanding of the physical characteristics of the system in order to establish an accurate dynamic model, and the model establishment and parameter identification are relatively complex. In addition, the control effect of this type of method is highly dependent on the accuracy of the model, and the model error will significantly affect the control performance. Kinematics-based control only needs to consider position and velocity, does not need to deal with the dynamic characteristics of the system, has a small amount of calculation, and is also easy to establish through geometric relationships. It is more applicable to lightweight rope-driven snake-like robotic arms.
[0004] However, the structure of the rope-driven super-redundant serpentine manipulator is complex, and the control method of the traditional manipulator cannot be simply transplanted directly to the rope-driven super-redundant serpentine manipulator. The rope-driven super-redundant serpentine manipulator has three transmission levels: "motor-rope", "rope-joint", and "joint-end". Each level contains complex disturbances that affect the control accuracy. For example, the "motor-rope" link has friction between the slider and the ball screw, fluctuations in the motor torque, etc.; the "rope-joint" link has pulley friction, rope hole friction, etc.; the "joint-end" link has joint friction, external contact force disturbances, etc., which are difficult to control. At the same time, current scholars have not given enough consideration to the control of rope force, which is a very important factor. If the rope force is too large, the rope may be broken. If the rope force is small, the rope will be loose, and the stiffness of the manipulator system will be poor. Summary of the invention
[0005] The purpose of the present invention is to solve the problem that the prior art lacks effective control of the rope force of a rope-driven super-redundant serpentine manipulator, and propose a rope-driven super-redundant serpentine manipulator joint control method, comprising the following steps:
[0006] S1, establish a rope-driven super-redundant serpentine manipulator DH coordinate system, and calculate the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system;
[0007] S2. Based on the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system, a mapping relationship between the joint angle values of the rope-driven super-redundant serpentine manipulator and the length value of the rope in the driving space is constructed;
[0008] S3. According to the mapping relationship between the joint angle value of the rope-driven super-redundant serpentine manipulator and the rope length value of the driving space, a rope force controller of the rope-driven manipulator is constructed. The controller includes a rope length control part and a rope force control part. The input of the rope force control part is the difference between the desired rope force and the actual rope force fed back by the manipulator, and the output is the desired speed of the rope force control by the driving motor; the input of the rope length control part is the difference between the desired joint angle and the actual joint angle fed back by the manipulator, and the output is the speed required for the cable length to be controlled by the driving motor; the sum of the desired speed of the rope force control by the driving motor and the speed required for the cable length to be controlled by the driving motor is the rope speed required by the manipulator;
[0009] S4. Control the movement of the robot arm according to the rope force controller of the rope-driven robot arm.
[0010] Furthermore, the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system is:
[0011]
[0012] Among them, two coordinate systems are established for each joint, and there are 2n coordinate systems for n joints. i-1 T i represents the pose matrix of the i-th coordinate system relative to the i-1-th coordinate system, q i Represents Z i-1 Axis from X i-1 Axis to X i The rotation angle of the axis, a i Indicates along X i Axis from Z i-1 Axis to Z i The distance between the axes, α i Indicates around X i Axis from Z i-1 Axis to Z i The rotation angle of the axis, d i Indicates along Z i-1 Axis from X i-1 Axis to X i Axis distance, Z i-1 represents the z-axis of the i-1th coordinate system, X i-1 represents the x-axis of the i-1th coordinate system, X i represents the x-axis of the i-th coordinate system, Z i Represents the z-axis of the i-th coordinate system.
[0013] Furthermore, the length of the rope between the upper and lower wire reels of a single joint of the robotic arm is:
[0014]
[0015] Among them, l1 represents the length of a rope between the upper and lower conductor drums, l B i-1 Indicates the lower wire tray wire hole B i-1 In the coordinate system {O l}, l B i Indicates the upper wire drum rope guide hole B i In the coordinate system {O l}, r is the distance from the upper wire drum rope guide hole to the upper wire drum center or the distance from the lower wire drum rope guide hole to the lower wire drum center, α is the joint around the coordinate system {O 2i} 2i The desired angle of rotation of the axis, β is the angle of rotation of the joint around the coordinate system {O 2i}y 2i The desired angle of rotation of the axis, coordinate system {O 2i} Origin O 2i to l and O u The distance between the upper and lower guide plates is d, where d is the distance between the upper and lower guide plates when they are parallel. 2i} 2i The y axis is the axis of one of the rotation axes of the universal joint. 2i The axis is the axis of the other rotation axis of the universal joint, z 2i The axes are established according to the right-hand rule, and the coordinate system {O l}The following wire drum center O l is the origin, the coordinate system {O l} l Axis parallel to the rotation axis x 2i , the vertical lower wire coil is z upward l Axis, y l The axis is established according to the right-hand rule, O l Indicates the center of the lower guide plate, O u represents the center of the upper guide plate, γ represents B i and O l The connection line with x l The angle between the axes in degrees;
[0016] Replacing γ with γ+2π / 3 and γ-2π / 3 gives the lengths of the other two ropes at the same joint.
[0017] Furthermore, the input angles after joint decoupling are used to solve the rope length, and the input angle of the i-th joint after decoupling is the sum of the input angles of the previous i joints.
[0018] Furthermore, the rope length control part includes two parallel PID controllers, and the expression is:
[0019]
[0020] e p1 =α-α r
[0021] e p2 =β-β r ,
[0022] in, is the expected velocity of the two degrees of freedom of a single joint, K p1 , K p2 are the proportional coefficients of the two PID controllers, K i1 , K i2 are the integral coefficients of the two PID controllers, K d1 , K d2 are the differential coefficients of the two PID controllers, α r is the actual angle of α fed back by the robot angle sensor, β r is the actual angle of β fed back by the robot arm angle sensor.
[0023] Further, the cable speed required to drive the universal joint of the robot arm is calculated based on the desired joint speed in:
[0024]
[0025] in:
[0026] T1=rcosβ r cosγ+dsinβ r / 2-rcosγ
[0027] T2=rcosα r sinγ+rcosγsinα r sinβ r -(1+cosβ r )dsinα r / 2-rsinγ
[0028] T3=rsinα r sinγ-rcosγcosα r sinβ r +(1+cosβ r )dcosα r / 2
[0029] They represent the expected speeds of the three cables at the same joint, and γ is replaced by and Get and
[0030] Based on the cable speed required to drive the robot's universal joint, we can find the speed required to drive the motor to control the cable length:
[0031]
[0032] in, It indicates the motor speed required to drive the rope to make relevant movements, n indicates the motor speed ratio, S indicates the ball screw lead, State the rope speed.
[0033] Furthermore, the rope force control part includes a PID controller with a dead zone. The input of the PID controller with a dead zone is the difference between the desired rope force and the actual rope force fed back by the manipulator, and the output is the desired speed of the driving motor for rope force control:
[0034]
[0035] e f =F d -F r ,
[0036] err0=F d -F l ,
[0037] Where u(t) is the expected speed of the driving motor to control the rope force, err0 represents the dead zone range, K p3 is the proportional coefficient of the PID controller with dead zone, K i3 K is the integral coefficient of the PID controller with dead zone. d3 is the differential coefficient of the PID controller with dead zone, F d is the expected rope force, F r is the actual rope force, F l Represents half the length of the expected rope force range.
[0038] The present invention also proposes a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the above-mentioned rope-driven super-redundant serpentine robot arm joint control method is implemented.
[0039] The present invention also proposes an electronic device, comprising a processor and a memory, wherein the processor and the memory are interconnected, wherein the memory is used to store a computer program, the computer program comprises computer-readable instructions, and the processor is configured to call the computer-readable instructions to execute the above-mentioned rope-driven super-redundant serpentine robot arm joint control method.
[0040] The beneficial effects brought by the technical solution provided by the present invention are:
[0041] The present invention combines the control of the angle of the rope-driven super-redundant serpentine manipulator and the control of the rope force, and constructs a mapping relationship between the joint angle value and the length of the driving rope for the "motor-rope" and "rope-joint" links, and constructs a mapping relationship from the joint speed to the rope speed. Taking the angle and force into consideration, a parallel PID control method is designed to perform closed-loop control of the joint angle, and the stability of the entire closed-loop system is ensured. Therefore, the method of this article can not only ensure the precise control of the joint angle of the rope-driven super-redundant manipulator and the interval control of the rope force, but also shorten the control time and improve the response rate of the system. It solves the problem that the prior art lacks effective control of the rope force of the rope-driven super-redundant serpentine manipulator. BRIEF DESCRIPTION OF THE DRAWINGS
[0042] Figure 1 is a flow chart of a rope-driven super-redundant serpentine manipulator joint control method according to an embodiment of the present invention;
[0043] Figure 2 Schematic diagram of the DH coordinate system of the rope-driven super-redundant serpentine manipulator constructed in an embodiment of the present invention;
[0044] Figure 3 Schematic diagram of a single-joint model of a rope-driven super-redundant mechanical arm according to an embodiment of the present invention;
[0045] Figure 4 is a block diagram of a controller of a rope-driven super-redundant serpentine manipulator according to an embodiment of the present invention;
[0046] Figure 5 is a block diagram of an electronic device in an exemplary embodiment of an embodiment of the present invention;
[0047] Figure 6 is the controller-free motion error of the rope-driven super-redundant serpentine manipulator according to an embodiment of the present invention, Figure 6 (a) is the expected angle and actual angle of motion without controller. Figure 6 (b) is the angle error of motion without controller;
[0048] Figure 7 It is a comparison of the motion errors of the rope-driven super-redundant serpentine manipulator using an angle PID controller and a parallel PID controller according to an embodiment of the present invention;
[0049] Figure 8 This is a comparison of the force control effects of the angle PID controller used in the rope-driven super-redundant serpentine manipulator of the embodiment of the present invention and the parallel PID controller of the present invention. Figure 8 (a) is the rope force of the angle PID controller, Figure 8 (b) is the rope force of parallel PID control;
[0050] Fig. 9 It is the tracking trajectory of the parallel PID controller of the embodiment of the present invention. DETAILED DESCRIPTION
[0051] To make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described below with reference to the accompanying drawings.
[0052] The flowchart of the rope-driven super-redundant serpentine robot arm joint control method according to the embodiment of the present invention is as follows: Figure 1 , specifically including the following steps:
[0053] S1. Establish a rope-driven super-redundant serpentine manipulator DH coordinate system and calculate the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system.
[0054] The schematic diagram of the DH coordinate system of the rope-driven super-redundant serpentine manipulator constructed in the embodiment of the present invention is from the reference Figure 2 The rope-driven super-redundant manipulator of the present invention has a total of n joints, and the universal joint structure in each joint has two rotational degrees of freedom. Two coordinate systems are established for each joint, and a total of 2n coordinate systems are established. The origin of the two coordinate systems of each joint is the center of the universal joint, the x-axis coincides with the common perpendicular line of the axis from joint i to joint i+1, the z-axis coincides with the rotation axes of the two rotational degrees of freedom, and the y-axis is established according to the right-hand rule. The 2n coordinate systems are sorted, and the two coordinate systems of the i-th joint are ranked 2i-1 and 2i, and the coordinate systems of the same rotational degrees of freedom of different joints are ranked as odd numbers or even numbers.
[0055] S2. Based on the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system, a mapping relationship between the joint angle values of the rope-driven super-redundant serpentine manipulator and the length value of the rope in the driving space is constructed.
[0056] Calculate the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system:
[0057]
[0058] Among them, the 2n coordinate systems established based on S1, i-1 T i represents the pose matrix of the i-th coordinate system relative to the i-1-th coordinate system, q i Represents Z i-1 Axis from X i-1 Axis to X i The rotation angle of the axis, a i Indicates along X i Axis from Z i-1 Axis to Z i The distance between the axes, α i Indicates around X iAxis from Z i-1 Axis to Z i The rotation angle of the axis, d i Indicates along Z i-1 Axis from X i-1 Axis to X i Axis distance, Z i-1 represents the z-axis of the i-1th coordinate system, X i-1 represents the x-axis of the i-1th coordinate system, X i represents the x-axis of the i-th coordinate system, Z i Represents the z-axis of the i-th coordinate system.
[0059] Based on the relationship of the homogeneous transformation matrix of the rope-driven super-redundant snake-like manipulator, the relationship between the coordinate system pose of each joint and the end coordinate system can be expressed as:
[0060] 0 T i = 0 T1 1 T2L i-1 T i
[0061] in, 0 T i represents the position and posture of the i-th coordinate system in the base coordinate system {0}, 0 T1 represents the pose of the first coordinate system in the base coordinate system {0}. 1 T2 represents the position of the second coordinate system in the first coordinate system. i-1 T i It represents the position and posture of the i-th coordinate system in the i-1-th coordinate system. The base coordinate system {0} takes the center of the lower wire disk of the first joint as its origin. When the rotation angle of the two rotation axes of the first joint is 0 degrees, the z-axis of the base coordinate system {0} is parallel to the z-axis of the first joint coordinate system, and the y-axis of the base coordinate system {0} is parallel to the y-axis of the first joint coordinate system. The x-axis of the base coordinate system {0} is established according to the right-hand rule.
[0062] The rope-driven super-redundant snake-like robot arm is composed of a series of connecting rods in series, and the movement of each joint is synchronously driven by multiple ropes. Therefore, it is necessary to construct a mapping relationship between the joint angle value and the rope length value in the driving space.
[0063] The single joint model of the rope-driven super redundant manipulator according to the embodiment of the present invention is referenced Figure 3 The robot arm joint includes two upper and lower wire reels and a universal joint structure. Figure 3 A i-1 , B i-1 , C i-1 They represent the three rope guide holes passing through the lower wire drum, A i , B i , Ci They represent the three guide wire holes passing through the upper wire drum. The universal joint structure of each joint contains two degrees of freedom, with the universal joint center O 2i Establish a coordinate system for the origin {O 2i}, coordinate system {O 2i} 2i The y axis is the axis of one of the rotation axes of the universal joint. 2i The axis is the axis of the other rotation axis of the universal joint, z 2i The axis is established according to the right-hand rule. The center of the following wire coil is O l As the origin, establish the coordinate system {O l}, coordinate system {O l} l Axis parallel to the rotation axis x 2i , the vertical lower wire coil is z upward l Axis, y l The axis is established according to the right-hand rule. The center of the above wire coil is the origin O u ,y u Axis parallel to the rotation axis y 2i , the vertical upper wire coil is z u Axis, x u The axis is established according to the right-hand rule, and the coordinate system {O u}. Around the coordinate system {O 2i} 2i Axis and y 2i The axis rotates, the angles are α and β respectively, when the angles are both 0, the upper and lower wire reels are parallel, the distance is d. According to the motion characteristics of the universal joint structure, O 2i to l and O u The distances are equal, both are d, so the transformation can be obtained as follows:
[0064] (1) Coordinate system {O l} around x l The axis rotates α, and then along z l The axis moves upward by d / 2 to obtain the coordinate system {O 2i}.
[0065] (2) Coordinate system {O 2i} around y 2i The axis rotates by β and then along z 2i The axis moves upward by d / 2 to obtain the coordinate system {O u}.
[0066] Get the homogeneous transformation matrix:
[0067]
[0068] Where T represents the homogeneous transformation matrix, Rot(x,α) represents the rotation α around the x-axis, It means moving upward along the z-axis by d / 2, and Rot(y,β) means rotating around the y-axis by β.
[0069] In the lower conductor disc coordinate system, define O l B i-1 With x l The angle between the axes is γ, then the coordinate system of the upper wire drum is u B i With x u The angle between the axes is also γ, and the distance from the guide hole to the center of the circle is r, so we can get B i Position in the upper wire drum coordinate system:
[0070]
[0071] Among them, uB i Indicates B i In the upper conductor disc coordinate system {O u} in the position.
[0072] The length of the rope between the upper and lower conductor drums is:
[0073]
[0074] Where l1 represents the length of the rope between the upper and lower conductor drums, l B i-1 Indicates the lower wire tray wire hole B i-1 In the coordinate system {O l}, l B i Indicates the upper wire drum rope guide hole B i In the coordinate system {O l}, r is the distance from the upper wire drum rope guide hole to the upper wire drum center or the distance from the lower wire drum rope guide hole to the lower wire drum center, α is the joint around the coordinate system {O 2i} 2i The desired angle of rotation of the axis, β is the angle of rotation of the joint around the coordinate system {O 2i}y 2i The desired angle of rotation of the axis, coordinate system {O 2i} Origin O 2i to l and O u The distance between the upper and lower guide plates of the i-th joint is d, where d is the distance between the upper and lower guide plates of the i-th joint when they are parallel. l represents the center of the lower guide plate of the ith joint, O u represents the center of the upper guide plate of the i-th joint, and O in the coordinate system of the lower guide plate l B l-1 With axis x lThe angle between the upper conductor disc and the u B l With x u The angle between them is also γ.
[0075] Replacing γ with γ+2π / 3 and γ-2π / 3 gives the lengths of the other two ropes at the same joint.
[0076] When solving the rope length, the input angle after joint decoupling is used to solve the rope length. The input angle of the i-th joint after decoupling is the sum of the input angles of the previous i joints.
[0077] When solving the first joint, the input angles are α1 and β1, and the actual input angle values after decoupling are also α1 and β1; when solving the second joint, the input angles are α2 and β2, and the actual input angle values after decoupling are α1+α2 and β1+β2; when solving the third joint, the input angles are α3 and β3, and the actual input angle values after decoupling are α1+α2+α3 and β1+β2+β3, and so on, to calculate all the rope length values.
[0078] S3. According to the mapping relationship between the joint angle value of the rope-driven super-redundant serpentine manipulator and the length value of the driving space rope, the rope force controller of the rope-driven manipulator is constructed. Figure 4 The controller includes a rope length control part and a rope force control part. The rope force control part inputs the difference between the desired rope force and the actual rope force fed back by the robot arm, and outputs the desired speed of the drive motor for controlling the rope force. The rope length control part inputs the difference between the desired joint angle and the actual joint angle fed back by the robot arm, and outputs the speed required for the drive motor to control the cable length. The sum of the desired speed of the drive motor for controlling the rope force and the speed required for the drive motor to control the cable length is the rope speed required by the robot arm.
[0079] The rope length control part includes two parallel PID controllers. The input of the rope length control part is the difference between the desired joint angle and the actual joint angle fed back by the robot arm. Here, the desired joint angle includes two parts: α and β. α is the angle of rotation of the joint around the coordinate system {O 2i} 2i The desired angle of rotation of the axis, β is the angle of rotation of the joint around the coordinate system {O 2i}y 2i The desired angle of rotation of the shaft.
[0080] The actual joint angle α of the angle α and the α fed back by the robot angle sensor r The difference is the input of the first PID controller, the actual joint angle β and the β fed back by the robot angle sensor r The difference is the input of the second PID controller, and the output of the two PID controllers is the desired joint velocity:
[0081]
[0082] e p1 =α-α r
[0083] e p2 =β-β r ,
[0084] in, is the expected velocity of the two degrees of freedom of a single joint, K p1 , K p2 are the proportional coefficients of the two PID controllers, K i1 , K i2 are the integral coefficients of the two PID controllers, K d1 , K d2 are the differential coefficients of the two PID controllers, α r is the actual angle of α fed back by the robot angle sensor, β r is the actual angle of β fed back by the robot arm angle sensor.
[0085] Calculate the cable speed required to drive the robot's universal joint based on the desired joint speed in:
[0086]
[0087] in:
[0088] T1=rcosβ r cosγ+dsinβ r / 2-rcosγ
[0089] T2=rcosα r sinγ+rcosγsinα r sinβ r -(1+cosβ r )dsinα r / 2-rsinγ
[0090] T3=rsinα r sinγ-rcosγcosα r sinβ r +(1+cosβ r )dcosα r / 2
[0091] They represent the expected speeds of the three cables at the same joint, and γ is replaced by and Get and θ r =[α r ,β r ] T .
[0092] Based on the cable speed required to drive the robot's universal joint, we can find the speed required to drive the motor to control the cable length:
[0093]
[0094] in, It indicates the motor speed required to drive the rope to make relevant movements, n indicates the motor speed ratio, S indicates the ball screw lead, State the rope speed.
[0095] The rope force control part includes a PID controller with dead zone. The input of the PID controller with dead zone is the difference between the desired rope force and the actual rope force fed back by the manipulator. The output is the desired speed of the drive motor for rope force control:
[0096]
[0097] e f =F d -F r ,
[0098] err0=F d -F l ,
[0099] Where u(t) is the expected speed of the driving motor to control the rope force, err0 represents the dead zone range, K p3 is the proportional coefficient of the PID controller with dead zone, K i3 K is the integral coefficient of the PID controller with dead zone. d3 is the differential coefficient of the PID controller with dead zone, F d is the expected rope force, F r is the actual rope force, F l Represents half the length of the expected rope force range.
[0100] The final required rope speed is
[0101] S4. Control the movement of the robot arm according to the rope force controller of the rope-driven robot arm.
[0102] In an exemplary embodiment, a computer-readable storage medium is included, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the above-mentioned rope-driven super-redundant serpentine robot arm joint control method is implemented.
[0103] See also Figure 5 In an exemplary embodiment, an electronic device is also included, including at least one processor, at least one memory, and at least one communication bus.
[0104] Wherein, a computer program is stored in the memory, and the computer program includes computer-readable instructions. The processor calls the computer-readable instructions stored in the memory through the communication bus to execute the above-mentioned rope-driven super-redundant serpentine robot arm joint control method.
[0105] In order to verify the effectiveness of the controller designed by the present invention, motion comparisons are carried out without a controller, using an angle PID controller, and using a parallel PID controller. Finally, the parallel PID controller is used to track the reference signal to verify the position tracking performance of the parallel controller designed by the present invention.
[0106] Figure 6 is the controller-free motion error of the rope-driven super-redundant serpentine manipulator according to an embodiment of the present invention, Figure 6 (a) is the expected angle and actual angle of motion without controller. Figure 6 (b) is the angle error of the motion without controller. The results show that under direct angle control, the angle error is large and the arrival speed is slow.
[0107] Figure 7 This is a comparison of the motion errors of the rope-driven super-redundant serpentine manipulator using an angle PID controller and a parallel PID controller. The results show that the angle accuracy of the angle PID and parallel PID control is high.
[0108] Figure 8 This is a comparison of the force control effects of the angle PID controller used in the rope-driven super-redundant serpentine manipulator of the embodiment of the present invention and the parallel PID controller of the present invention. Figure 8 (a) is the rope force of the angle PID controller, Figure 8 (b) shows the rope force under parallel PID control. Under angle PID, the rope force becomes smaller, and the rope becomes loose after multiple movements. However, the parallel PID control controls the force within the expected range to ensure that the rope is taut.
[0109] Fig. 9 It is the tracking trajectory of the parallel PID controller of the embodiment of the present invention. As can be seen from the figure, the tracking signals of the joint angles α and β can accurately track the reference signal in a very short time, verifying that the parallel PID controller of the design scheme has good position tracking performance.
[0110] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A rope-driven super-redundant serpentine robot arm joint control method, characterized in that: The following steps are involved: S1, establish a rope-driven super-redundant serpentine manipulator DH coordinate system, and calculate the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system; S2. Based on the homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system, a mapping relationship between the joint angle values of the rope-driven super-redundant serpentine manipulator and the length value of the rope in the driving space is constructed; S3. According to the mapping relationship between the joint angle value of the rope-driven super-redundant serpentine manipulator and the rope length value of the driving space, a rope force controller of the rope-driven manipulator is constructed. The controller includes a rope length control part and a rope force control part. The input of the rope force control part is the difference between the desired rope force and the actual rope force fed back by the manipulator, and the output is the desired speed of the rope force control by the driving motor; the input of the rope length control part is the difference between the desired joint angle and the actual joint angle fed back by the manipulator, and the output is the speed required for the cable length to be controlled by the driving motor; the sum of the desired speed of the rope force control by the driving motor and the speed required for the cable length to be controlled by the driving motor is the rope speed required by the manipulator; S4. Control the movement of the robot arm according to the rope force controller of the rope-driven robot arm.
2. A rope-driven super-redundant serpentine robot arm joint control method according to claim 1, characterized in that: The homogeneous transformation matrix between adjacent coordinate systems of the DH coordinate system is: Among them, two coordinate systems are established for each joint, and there are 2n coordinate systems for n joints. i-1 T i represents the pose matrix of the i-th coordinate system relative to the i-1-th coordinate system, q i Represents Z i-1 Axis from X i-1 Axis to X i The rotation angle of the axis, a i Indicates along X i Axis from Z i-1 Axis to Z i The distance between the axes, α i Indicates around X i Axis from Z i-1 Axis to Z i The rotation angle of the axis, d i Indicates along Z i-1 Axis from X i-1 Axis to X i Axis distance, Z i-1 represents the z-axis of the i-1th coordinate system, X i-1 represents the x-axis of the i-1th coordinate system, X i represents the x-axis of the i-th coordinate system, Z i Represents the z-axis of the i-th coordinate system.
3. A rope-driven super-redundant serpentine robot arm joint control method according to claim 2, characterized in that: The length of the rope between the upper and lower wire reels of a single joint of the robotic arm is: Among them, l1 represents the length of a rope between the upper and lower conductor drums, l B i-1 Indicates the lower wire tray wire hole B i-1 In the coordinate system {O l }, l B i Indicates the upper wire drum rope guide hole B i In the coordinate system {O l }, r is the distance from the upper wire drum rope guide hole to the upper wire drum center or the distance from the lower wire drum rope guide hole to the lower wire drum center, α is the joint around the coordinate system {O 2i } 2i The desired angle of rotation of the axis, β is the angle of rotation of the joint around the coordinate system {O 2i }y 2i The desired angle of rotation of the axis, coordinate system {O 2i } Origin O 2i to l and O u The distance between the upper and lower guide plates is d, where d is the distance between the upper and lower guide plates when they are parallel. 2i } 2i The y axis is the axis of one of the rotation axes of the universal joint. 2i The axis is the axis of the other rotation axis of the universal joint, z 2i The axes are established according to the right-hand rule, and the coordinate system {O l }The following wire drum center O l is the origin, the coordinate system {O l } l Axis parallel to the rotation axis x 2i , the vertical lower wire coil is z upward l Axis, y l The axis is established according to the right-hand rule, O l Indicates the center of the lower guide plate, O u represents the center of the upper guide plate, γ represents B i and O l The connection line with x l The angle between the axes in degrees; Replacing γ with γ+2π / 3 and γ-2π / 3 gives the lengths of the other two ropes at the same joint.
4. A rope-driven super-redundant serpentine robot arm joint control method according to claim 3, characterized in that: The rope length is solved using the input angles after joint decoupling. The input angle of the i-th joint after decoupling is the sum of the input angles of the previous i joints.
5. A rope-driven super-redundant serpentine robot arm joint control method according to claim 4, characterized in that: The rope length control part includes two parallel PID controllers, and the expression is: e p1 =α-α r e p2 =β-β r , in, are the expected velocities of the two degrees of freedom of a single joint, K p2 are the proportional coefficients of the two PID controllers, K i1 , K i2 are the integral coefficients of the two PID controllers, K d1 , K d2 are the differential coefficients of the two PID controllers, α r is the actual angle of α fed back by the robot angle sensor, β r is the actual angle of β fed back by the robot arm angle sensor.
6. A rope-driven super-redundant serpentine robot arm joint control method according to claim 5, characterized in that: Calculate the cable speed required to drive the robot's universal joint based on the desired joint speed in: in: T1=rcosβ r cosγ+dsinβ r / 2-rcosγ T2=rcosα r sinγ+rcosγsinα r sinβ r -(1+cosβ r )dsinα r / 2-rsinγ T3=rsinα r sinγ-rcosγcosα r sinβ r +(1+cosβ r )dcosα r / 2, They represent the expected speeds of the three cables at the same joint, and γ is replaced by and Get and Based on the cable speed required to drive the robot's universal joint, we can find the speed required to drive the motor to control the cable length: in, It indicates the motor speed required to drive the rope to make relevant movements, n indicates the motor speed ratio, S indicates the ball screw lead, State the rope speed.
7. A rope-driven super-redundant serpentine robot arm joint control method according to claim 6, characterized in that: The rope force control part includes a PID controller with dead zone. The input of the PID controller with dead zone is the difference between the desired rope force and the actual rope force fed back by the manipulator. The output is the desired speed of the drive motor for rope force control: e f =F d -F r , err0=F d -F l , Where u(t) is the expected speed of the driving motor to control the rope force, err0 represents the dead zone range, K p3 is the proportional coefficient of the PID controller with dead zone, K i3 is the integral coefficient of the PID controller with dead zone, K d3 is the differential coefficient of the PID controller with dead zone, F d is the expected rope force, F r is the actual rope force, F l Represents half the length of the expected rope force range.
8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.
9. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the processor and the memory are interconnected, wherein the memory is used to store a computer program, the computer program comprises computer-readable instructions, and the processor is configured to call the computer-readable instructions to execute the method according to any one of claims 1 to 7.
Citation Information
Patent Citations
Control method of cable-driven series joint type S-shaped mechanical arm
CN107263477A
Snake-shaped robot path following planning method
CN108994836A
Method for determining rope length change of rope-driven hyper-redundant degree-of-freedom robot
CN109760051A
Speed-level kinematics modeling method for rope-driven flexible mechanical arm
CN112936273A
Motion data processing method for rope-driven snakelike mechanical arm with rope hole gap
CN112975925A
Cited By
Force and position type hybrid control method for rope cluster driven large-span space manipulator
CN119328783A
