Continuum robot with stiffness adjustment function and load sensing control method

Abstract

A continuum robot with stiffness adjustment function and a load sensing control method, comprising a robot body with stiffness adjustment function, a motion sensing module, a force sensing module, a motion detection method, a force detection method, and a motion and stiffness control method. The robot body comprises a base, a joint, a steel wire rope, and a linear motor; the motion sensing module comprises a monocular camera and an encoder; the force sensing module comprises a tactile sensor, a tension sensor, and a six-dimensional force sensor; the motion detection method realizes multiple binocular recognition of the position and distance of joints and ends through an encoder and multiple monocular splicing; the force detection method detects the contact force position through a tactile sensor, the contact force magnitude through a tension sensor, and the force and torque of the end through a six-dimensional force sensor; the motion and stiffness control method is based on information detection and uses a PID closed loop to control the motion speed and joint stiffness magnitude, thereby achieving smooth, safe, high-precision position and force control.

Description

Technical Field

[0001] The present invention belongs to the intersecting field of robotics, control science, computer science, and sensor technology, and more specifically, relates to a continuum robot with a stiffness adjustment function and a load sensing control method. Background Art

[0002] Industrial robotic arms composed of rigid links have the advantages of large load and precise movement, but they have few degrees of freedom and insufficient flexibility. In the face of complex working environments, they will inevitably experience rigid collisions, which poses a safety hazard. The bionic continuum robot imitates flexible biological organs such as elephant trunks and octopus tentacles. It is composed of multiple joints and flexible materials. Its main form of movement is the expansion and contraction and bending deformation of the elastic structure. It has good adaptability, flexibility, obstacle avoidance and environmental adaptability. It can bend and deform according to the environment in complex and narrow environments to avoid collisions with complex operating environments. It has great application value in rescue, medical treatment, deep cavity detection and other application scenarios where traditional industrial robotic arms are difficult to work. However, the load capacity of the continuum robot is limited, and it is difficult for the end to bear a relatively large load like a rigid robotic arm. The main reason is that the stiffness of the joints is relatively low. Therefore, researchers have proposed solutions for adjusting joint stiffness, such as the "robot stiffness controllable joint and stiffness control method thereof" disclosed in patent publication number: CN107718040A, which uses the thermal effect of electric current to change the shape of the shape memory alloy metal sheet, change the distance between the outer elastic skeleton and the inner elastic skeleton of the stiffness structure part, and realize the change of the wall thickness of the variable stiffness structure part, so that the stiffness controllable joint can switch between the rigid working state and the flexible working state. However, there is a temperature change time in the control process, which leads to the problem of slow response speed of stiffness change; Patent publication number: CN202622798U discloses a "magnetorheological continuum robot manipulator", which can adjust the magnetic field strength of the magnetorheological fluid in the hose by changing the current of the coil, thereby controlling the rheological properties of the magnetorheological fluid and realizing the conversion between liquid and solid, which plays a role in adjusting the stiffness and damping of the entire manipulator, but in actual applications, there are problems such as complex structure, poor stability, slow response and heat interference of the magnetic circuit. Summary of the invention

[0003] In view of the problem that it is difficult to balance stiffness and speed in the stiffness adjustment of the above-mentioned continuum robot, researchers need to develop new technologies to realize active perception and control methods of loads, and to realize intelligent control of matching stiffness according to the size of external loads, thereby achieving precise, high-speed and stable motion and operation control. The present invention proposes a continuum robot with stiffness adjustment function and a load perception control method, including a robot body with stiffness adjustment function, a motion perception module, a force perception module, a motion detection method, a force detection method, and a motion and stiffness control method.

[0004] To achieve the above object, the technical solution adopted by the present invention is:

[0005] The present invention provides a continuum robot with a stiffness adjustment function, comprising a robot body, a motion sensing module and a force sensing module, wherein the robot body comprises a base, a joint, a steel wire rope and a linear motor, wherein the linear motor comprises N 1 N 1 is a positive integer greater than or equal to 3, the linear motor is installed in the base, and the joint includes N 2 N 2 is a positive integer greater than or equal to 3, the joints are hingedly mounted in series on the top of the base to form a robot arm, each of the joints has a segment and a stiffness adjustment device, the wire rope includes N 1 Root, one end of which is connected to N 1 The linear motors are respectively connected and fixed, and the other ends are fixed on one of the joints at the end;

[0006] The motion sensing module includes a monocular camera and an encoder. 3 The monocular cameras are fixed on the top of the base, N 3 is an even number and a positive integer greater than or equal to 2, and is distributed in a 90-degree circular array. The encoder includes N 1 , respectively installed inside the corresponding linear motor to detect the displacement of the linear motor;

[0007] The sensing module includes an array tactile sensor, a tension sensor and a six-dimensional force sensor. The array tactile sensor is fixed on the outside of the robot arm. The tension sensor includes N 1 The tension sensors are respectively fixed at the corresponding connection points between the linear motor output shaft and the steel wire rope, and the six-dimensional force sensor is fixed on the end joint of the robot mechanical arm.

[0008] The present invention provides a load sensing control method for a continuum robot with a stiffness adjustment function, including a motion detection method and a motion and stiffness control method:

[0009] The motion detection method detects the displacement of the linear motor by the encoder to obtain the corresponding extension and contraction amount of the wire rope, and obtains the position estimation value (x) of the end of the robot mechanical arm by constant curvature model conversion. 1 ,y 1 ,z 1), all joints are identified by the corresponding monocular camera, the bending direction of the robot arm is determined according to the imaging of the corresponding joint on the corresponding monocular camera, and the two adjacent monocular cameras A with the most imaging of the robot arm are i and A i+1 The binocular positioning subsystem is composed of i = 1, 2, 3, and each joint and six-dimensional force sensor are positioned to obtain the deformation information of the robot and the estimated position value of the end (x 2 ,y 2 ,z 2 ), and finally use (x, y, z) as the final position detection value of the end of the robot arm:

[0010] x=μx 1 +(1-μ)x 2

[0011] y=μy 1 +(1-μ)y 2

[0012] z=μz 1 +(1-μ)z 2

[0013] Where μ is the weight coefficient;

[0014] The force detection method detects the position (r, h, λ) of the contact force F between the outer side of the robot arm and the obstacle through an array tactile sensor, and 1 The tension sensor detects the tension N of the wire rope to obtain the magnitude of the contact force F = NR / r, and the force and torque of the joint at the end are detected by the six-dimensional force sensor;

[0015] The specific steps of the motion and stiffness control method are as follows:

[0016] S1: Given the initial and target end pose data;

[0017] S2: Based on the constant curvature model, establish the base coordinate system O p , curved coordinate system O pb and the end coordinate system O e , maps the given end position from the operation space (x, y, z) representing the end position to the joint space (θ, φ, L) representing its posture, and according to the geometric relationship:

[0018]

[0019] Where (x p ,y p ,z p) is the coordinate of the end joint center in the coordinate system of the base, a is the bending angle, b is the rotation angle, and L is the total length of the robot arm;

[0020] Then mapped to the drive space {ΔL k}, based on the fact that the wire rope between the two joints is a straight line, the coordinates of the wire rope on each joint are deduced from the top of the base through the bending angle, rotation angle and the total length of the robot arm, and then the total length of the wire rope is obtained:

[0021]

[0022]

[0023] L 0 =L / n,a i+1 =a i +a / n,a 1 =a / n

[0024]

[0025] Where R k1 is the rotation transformation matrix of the kth wire rope, R i2 is the bending transformation matrix of the i-th joint, P ki1 is the coordinate vector of the kth wire rope in the i-th joint coordinate system in the end coordinate system, P i1 is the translation transformation vector of the i-th joint, n is the number of joints, r i is the radius of the ith joint, a i is the bending angle of the i-th joint, b k is the rotation angle of the kth wire rope, L 0 is the midpoint length between each joint, L i is the length of the kth wire rope between the i-1th joint and the i-th joint, L k is the length of the kth wire rope, (x ki ,y ki ,z ki ) is the coordinate of the kth wire rope at the i-th joint;

[0026] By obtaining the total length of each wire rope, the linear motor displacement required to achieve a given target terminal posture can be solved, and the linear motor displacement required for the actual terminal posture can also be obtained;

[0027] S3: Control the linear motor to perform corresponding telescopic movement;

[0028] S4: The encoder measures the displacement of the linear motor and calculates the actual displacement of the linear motor;

[0029] S5: The corresponding monocular camera identifies whether the end joint position is in the picture and within a certain frame, so as to determine the end direction. The corresponding adjacent monocular cameras form a binocular camera. Because each monocular camera is in a fixed position, the monocular camera focal length f and binocular center distance D are obtained after binocular calibration before use.

[0030] Take the optical center as the origin of the camera coordinate system and establish the camera coordinate system O c , the xOy plane of the camera coordinate system coincides with the xOy plane of the base coordinate system, and the axis Z c It is parallel to the z-axis of the parallel base coordinate system, coincides with the optical axis of the camera, and its direction is consistent with the field of view of the camera;

[0031] Establish a transition coordinate system, that is, the image physical coordinate system O l and O r , the coordinates of the point P in the space in the physical coordinate system of the image in the left and right images are P l (u l ,v l ) and P r (u r ,v r ), the point coordinate depth is obtained from the similar triangle, that is, the z-axis coordinate of point P in the base coordinate system:

[0032]

[0033] The coordinates P of point P in the left image pixel coordinate system l (u l ,v l ) and the reprojection matrix Q, we get the 3D coordinates of point P in the left camera coordinate system, and then through the rotation and translation matrix R, we get the 3D coordinates of point P in the base coordinate system:

[0034]

[0035] Among them, (c lx ,c ly ) is the origin o l Coordinates in the physical coordinate system of the left image, c rx is the origin o r In the physical coordinate system of the right image, u 2 Coordinates in the axis direction;

[0036] Then, the homogeneous transformation matrix is ​​obtained through the position of the camera installed on the base, and the three-dimensional coordinates of point P in the base coordinate system are obtained; the coordinates of the joint center are obtained from the three-point coordinates of the end joint and the three-point circle formula, and substituted into the formula in S2 to obtain the displacement of the linear motor;

[0037] S6: The actual linear motor displacement and the end position of the robot arm are obtained by fusing the linear motor displacement measured by the encoder and the linear motor displacement measured by the binocular camera;

[0038] S7: The array tactile sensor detects the position of the contact force, the tension sensor detects the tension of the wire rope and calculates the size of the contact force, and the six-dimensional force sensor detects the force and torque of the robot arm end joint in the contact operation process with the environment;

[0039] S8: according to the position and magnitude of the contact force, solving the control current of the stiffness adjustment device of the joint, and controlling the stiffness adjustment device of each joint;

[0040] S9: Determine whether the actual linear motor displacement reaches the target linear motor displacement, if not, enter S3; if yes, the motion control ends.

[0041] Compared with the prior art, the present invention has the following beneficial effects:

[0042] The continuum robot with stiffness adjustment function and the load sensing control method of the present invention are based on the continuum robot with a stiffness adjustment device, and use tactile sensors and tension sensors to sense external force. Because the greater the stiffness, the greater the control current required, the greater the energy consumption, and the longer the required heating time, the slower the system response speed, so the present invention can actively sense and control the stiffness adjustment device to adjust the appropriate stiffness at an appropriate speed; multiple monocular cameras installed on the robot body are combined with motor encoders, and the monocular cameras are combined into multi-cameras through position judgment, which has the advantages of compact structure and high measurement accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 The continuum robot of the present invention is a stereoscopic view of the overall structure Figure 1 ;

[0044] Figure 2 The continuum robot of the present invention is a stereoscopic view of the overall structure Figure 2 ;

[0045] Figure 3 It is a schematic diagram of the joint mechanism composition of the present invention;

[0046] Figure 4 This is a schematic diagram of the installation and viewing angle of the monocular camera of the present invention;

[0047] Figure 5 is a schematic diagram of a force detection system of the present invention;

[0048] Figure 6 The curved coordinate system of the present invention

[0049] Figure 7Schematic diagram of the binocular camera coordinate system and depth detection of the present invention;

[0050] Figure 8 It is a schematic diagram of the force detection method of the present invention;

[0051] Fig. 9 The figure is a flow chart of the motion and stiffness control method of the present invention.

[0052] Reference numerals:

[0053] 1. Robot body; 1-1. Base; 1-2. Joints; 1-2-1. Segments; 1-2-2. Stiffness adjustment device; 1-3. Steel wire rope; 1-4. Linear motor; 1-5. Robot arm; 2. Motion sensing module; 2-1. Monocular camera; 2-2. Encoder; 3. Force sensing module; 3-1. Array tactile sensor; 3-2. Tension sensor; 3-3. Six-dimensional force sensor; 4. Obstacle. DETAILED DESCRIPTION

[0054] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0055] Example: Refer to the attached Figure 1 , Attachment Figure 2 , Attachment Figure 3 , Attachment Figure 4 The continuum robot with stiffness adjustment function includes a robot body 1, a motion sensing module 2 and a force sensing module 3. The robot body includes a base 1-1, a joint 1-2, a steel wire 1-3 and a linear motor 1-4. The linear motor 1-4 includes 9 pieces, which are installed in the base 1-1. The joints 1-2 include 40 pieces, which are hinged in series and installed on the top of the base 1-1 to form a robot arm 1-5. Each of the joints 1-2 has a segment 1-2-1 and a stiffness adjustment device 1-2-2. The steel wire includes 9 pieces, one end of which is respectively connected and fixed to the 9 linear motors 1-4, and the other end is fixed to one of the joints 1-2 at the end.

[0056] Refer to the attached Figure 1 , Attachment Figure 2 , Attachment Figure 3 , Attachment Figure 4 The motion sensing module 2 includes a monocular camera 2-1 and an encoder 2-2. The four monocular cameras 2-1 are fixed on the top of the base 1-1 and are distributed in a 90-degree circular array. The encoder 2-2 includes nine units, which are respectively installed inside the nine linear motors 1-4 to detect the displacement of the linear motors 1-4.

[0057] Refer to the attached Figure 1 , Attachment Figure 2 , Attachment Figure 3 , Attachment Figure 5 The force sensing module 3 includes an array tactile sensor 3-1, a tension sensor 3-2 and a six-dimensional force sensor 3-3. The array tactile sensor 3-1 is fixed on the outside of the robot arm 1-5. The nine tension sensors 3-2 are respectively fixed on the connection between the output shafts of the nine linear motors 1-4 and the nine steel ropes. The six-dimensional force sensor 3-3 is fixed on the 1-5 end joints of the robot arm.

[0058] Refer to the attached Figure 1 , Attachment Figure 2 , Attachment Figure 4 , Attachment Figure 6 , Attachment Figure 7 The motion detection method detects the displacement of the linear motor 1-4 by the encoder 2-2, obtains the expansion and contraction of the nine steel ropes 1-3, and obtains the position estimation value (x) of the end of the robot mechanical arm 1-5 by converting the constant curvature model. 1 ,y 1 ,z 1 ), the 40 joints 1-2 are identified by the four monocular cameras 2-1, the bending direction of the robot arm 1-5 is determined according to the imaging of the 40 joints 1-2 on the four monocular cameras 2-1, and the two adjacent monocular cameras A with the most imaging of the robot arm 1-5 are used to identify the 40 joints 1-2. i and A i+1 The binocular positioning subsystem is composed of i=1, 2, 3, and then the 40 joints 1-2 and the six-dimensional force sensor 3-3 are positioned, so as to obtain the deformation information of the robot and the estimated position value of the end (x 2 ,y 2 ,z 2 ), and finally use (x, y, z) as the final position detection value of the end of the robot arm:

[0059] x=μx 1 +(1-μ)x 2 y=μy 1 +(1-μ)y 2 z=μz 1 +(1-μ)z 2

[0060] μ is a weight coefficient, obtained based on experimental tests, such as 0.5.

[0061] Refer to the attached Figure 1 , Attachment Figure 2 , Attachment Figure 5 , Attachment Figure 8The force detection method detects the position (r, h, λ) of the contact force F between the outer side of the robot arm 1-5 and the obstacle 4 through the array tactile sensor 3-1, and detects the tension N of the nine steel ropes 1-3 through the tension sensor 3-2 to obtain the size of the contact force F, F=NR / r; and detects the force and torque generated by the robot end in the process of operating the object through the six-dimensional force sensor 3-3.

[0062] See attached Figure 1 , Attachment Figure 2 , Attachment Figure 4 , Attachment Figure 5 , Attachment Figure 6 , Attachment Figure 7 , Attachment Figure 8 , Attachment Fig. 9 , the specific steps of the motion and stiffness control method are as follows:

[0063] S1: Given the initial and target end pose data;

[0064] S2: Based on the constant curvature model, establish the coordinate system O of the base 1-1 p , curved coordinate system O pb and the end coordinate system O e , mapping the given end position from the operation space (x, y, z) representing the end position to the joint space (θ, φ, L) representing its posture, according to the geometric relationship:

[0065]

[0066] Where (x p ,y p ,z p ) is the center coordinate of the end joint in the coordinate system of the base 1-1, a is the bending angle, b is the rotation angle, and L is the total length of the robot arm 1-5.

[0067] Then mapped to the drive space {ΔL k}, since the wire rope 1-3 between the two joints is a straight line, the coordinates of the wire rope on each joint can be recursively deduced from the top of the base 1-1 through the bending angle, rotation angle and the total length of the robot arm 1-5, and then the total length of the wire rope 1-3 can be obtained:

[0068]

[0069]

[0070] L 0 =L / n,a i+1 =a i +a / n,a 1 =a / n

[0071]

[0072] Where R k1 is the rotation transformation matrix of the kth wire rope 1-3, R i2 is the bending transformation matrix of the i-th joint 1-2, P ki1 is the coordinate vector of the kth wire rope 1-3 in the i-th joint coordinate system in the end coordinate system, P i1 is the translation transformation vector of the i-th joint 1-2, n is the number of joints, r i is the radius of the i-th joint, a i is the bending angle of the i-th joint, b k is the rotation angle of the kth wire rope 1-3, L 0 is the midpoint length between each joint, L i is the length of the kth wire rope 1-3 between the i-1th joint and the i-th joint, L k is the length of the kth wire rope 1-3, (x ki ,y ki ,z ki ) is the coordinate of the kth wire rope 1-3 at the i-th joint.

[0073] By obtaining the total length of each wire rope 1-3, the displacement of the linear motor 1-4 required to achieve a given target terminal posture can be solved, and the displacement of the linear motor 1-4 required for the actual terminal posture can also be obtained;

[0074] S3: Control the linear motors 1-4 to perform corresponding telescopic movements;

[0075] S4: Encoder 2-2 measures the displacement of linear motor 1-4 and calculates the actual displacement of linear motor 1-4;

[0076] S5: Multiple monocular cameras 2-1 identify whether the positions of 40 joints 1-2 are in the picture and within a certain frame, so as to determine the end direction. The corresponding adjacent monocular cameras 2-1 form a binocular camera. Because the position of each monocular camera is fixed, the focal length f of the monocular camera 2-1 and the binocular center distance D are obtained after binocular calibration before use.

[0077] Take the optical center as the origin of the camera coordinate system and establish the camera coordinate system O c , the xOy plane of the camera coordinate system coincides with the xOy plane of the base coordinate system, and the axis Z c It is parallel to the z-axis of the parallel base coordinate system, coincides with the optical axis of the camera, and its direction is consistent with the direction of the camera's field of view. In order to more conveniently establish a relationship between the camera coordinate system and the image coordinate system, a transition coordinate system needs to be established, namely the image physical coordinate system O l and O r, the coordinates of the point P in the space in the physical coordinate system of the image in the left and right images are P l (u l ,v l ) and P r (u r ,v r ), the point coordinate depth is obtained from the similar triangle, that is, the z-axis coordinate of point P in the base coordinate system:

[0078]

[0079] The coordinates P of point P in the left image pixel coordinate system l (u l ,v l ) and the reprojection matrix Q, we can get the 3D coordinates of point P in the left camera coordinate system, and then through the rotation and translation matrix R, we can get the 3D coordinates of point P in the base coordinate system:

[0080]

[0081] Among them, (c lx ,c ly ) is the origin o l Coordinates in the physical coordinate system of the left image, c rx is the origin o r In the physical coordinate system of the right image, u 2 Axis direction coordinates.

[0082] Then, the homogeneous transformation matrix can be obtained by the position of the camera installed on the base, and the three-dimensional coordinates of point P in the base coordinate system can be obtained. The coordinates of the joint center can be obtained by the three-point coordinates of the end joint and the three-point circle formula. Substituting into the formula in S2 can obtain the displacement of the linear motor 1-4;

[0083] S6: The displacement of the linear motor 1-4 measured by the encoder 2-2 and the displacement of the linear motor 1-4 measured by the binocular camera are fused to obtain the actual displacement of the linear motor 1-4 and the end position of the robot arm 1-5;

[0084] S7: The array tactile sensor 3-1 detects the position of the contact force, the tension sensor 3-2 detects the tension of the wire rope 1-3 and calculates the size of the contact force, and the six-dimensional force sensor 3-3 detects the force and torque of the contact operation process between the end joint of the robot arm 1-5 and the environment;

[0085] S8: according to the position and magnitude of the contact force, the control current of the stiffness adjustment device 1-2-2 of the joint is calculated, and the stiffness adjustment device of each joint is controlled;

[0086] S9: Determine whether the actual displacement of the linear motor 1-4 reaches the target linear motor displacement, if not, enter S3; if yes, the motion control ends.

[0087] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention shall fall within the scope of protection required by the present invention.

Claims

1. A load sensing control method for a continuum robot with a stiffness adjustment function, the continuum robot with a stiffness adjustment function comprising a robot body (1), a motion sensing module (2) and a force sensing module (3), the robot body (1) comprising a base (1-1), a joint (1-2), a steel wire rope (1-3) and a linear motor (1-4), the linear motor (1-4) comprising N1, N1 being a positive integer greater than or equal to 3, the linear motor (1-4) being mounted on the base (1-1 ), the joints (1-2) include N2, N2 is a positive integer greater than or equal to 3, the joints (1-2) are hingedly mounted in series on the top of the base (1-1) to form a robot arm (1-5), each of the joints (1-2) has a segment (1-2-1) and a stiffness adjustment device (1-2-2), the steel wire rope (1-3) includes N1, one end of which is respectively connected and fixed to N1 linear motors (1-4), and the other end is fixed to one of the joints (1-2) at the end; The motion sensing module (2) comprises a monocular camera (2-1) and an encoder (2-2), wherein N3 monocular cameras (2-1) are fixed on the top of the base (1-1), N3 is an even number and a positive integer greater than or equal to 2, and are distributed in a 90-degree circular array, and the encoder (2-2) comprises N1, which are respectively installed inside the corresponding linear motor (1-4) to detect the displacement of the linear motor (1-4); The sensing module (3) comprises an array tactile sensor (3-1), a tension sensor (3-2) and a six-dimensional force sensor (3-3), wherein the array tactile sensor (3-1) is fixed on the outside of the robot mechanical arm (1-5), the tension sensor (3-2) comprises N1, the tension sensor (3-2) is respectively fixed on the connection between the output shaft of the corresponding linear motor (1-4) and the wire rope (1-3), and the six-dimensional force sensor (3-3) is fixed on the end joint of the robot mechanical arm (1-5); The invention comprises a motion detection method and a motion and stiffness control method, characterized in that: The motion detection method detects the displacement of the linear motor (1-4) through the encoder (2-2) to obtain the expansion and contraction amount of the corresponding steel wire rope (1-3), obtains the position estimation value (x1, y1, z1) of the end of the robot mechanical arm (1-5) through constant curvature model conversion, identifies all joints (1-2) through the corresponding monocular camera (2-1), determines the bending direction of the robot mechanical arm (1-5) according to the imaging of the corresponding joint (1-2) on the corresponding monocular camera (2-1), and places the two adjacent monocular cameras A with the most imaging of the robot mechanical arm (1-5) on the robot mechanical arm (1-5). i and A i+1 A binocular positioning subsystem is formed, i = 1, 2, 3, and each of the joints (1-2) and the six-dimensional force sensor (3-3) is positioned to obtain the deformation information of the robot and the estimated position value (x2, y2, z2) of the end, and finally (x, y, z) is used as the final position detection value of the end of the robot manipulator: x=μx1+(1-μ)x2 y=μy1+(1-μ)y2 z=μz1+(1-μ)z2 Where μ is the weight coefficient; The force detection method detects the position (r, h, λ) of the contact force F between the outside of the robot mechanical arm (1-5) and the obstacle (4) through an array tactile sensor (3-1), detects the tension N of the wire rope (1-3) through N1 tension sensors (3-2) to obtain the size F=NR / r of the contact force F, and detects the force and torque of the joint (1-2) at the end through the six-dimensional force sensor (3-3); The specific steps of the motion and stiffness control method are as follows: S1: Given the initial and target end pose data; S2: Based on the constant curvature model, establish the base coordinate system O p , curved coordinate system O pb and the end coordinate system O e , maps the given end position from the operation space (x, y, z) representing the end position to the joint space (θ, φ, L) representing its posture, and according to the geometric relationship: Where (x p ,y p ,z p ) is the coordinate of the end joint center in the coordinate system of the base, a is the bending angle, b is the rotation angle, and L is the total length of the robot arm; Then mapped to the drive space {ΔL k }, based on the fact that the wire rope (1-3) between the two joints (1-2) is a straight line, the coordinates of the wire rope (1-3) on each joint (1-2) are deduced from the top of the base (1-1) through the bending angle, rotation angle and the total length of the robot arm, and then the total length of the wire rope is obtained: L0=L / n,a i+1 =a i +a / n,a1=a / n Where R k1 is the rotation transformation matrix of the kth wire rope, R i2 is the bending transformation matrix of the i-th joint, P ki1 is the coordinate vector of the kth wire rope in the i-th joint coordinate system in the end coordinate system, P i2 is the translation transformation vector of the i-th joint, n is the number of joints, r i is the radius of the i-th joint, a i is the bending angle of the i-th joint, b k is the rotation angle of the kth wire rope, L0 is the midpoint length between each joint, L ki is the length of the kth wire rope between the i-1th joint and the i-th joint, L k is the length of the kth wire rope, (x ki ,y ki ,z ki ) is the coordinate of the kth wire rope at the i-th joint; By obtaining the total length of each wire rope, the displacement of the linear motor (1-4) required to achieve a given target end position can be calculated. Similarly, the displacement of the linear motor (1-4) required for the actual end position is obtained; S3: Control the linear motor (1-4) to perform corresponding telescopic movement; S4: The encoder measures the displacement of the linear motor (1-4) and calculates the actual displacement of the linear motor (1-4); S5: The corresponding monocular camera (2-1) identifies whether the position of the end joint (1-2) is in the picture and within a certain frame, thereby determining the end direction. The corresponding adjacent monocular cameras (2-1) form a binocular camera, because each monocular camera (2-1) The position is fixed, and the focal length f of the monocular camera and the binocular center distance D are obtained after binocular calibration before use; Take the optical center as the origin of the camera coordinate system and establish the camera coordinate system O c , xOy of the camera coordinate system and the base coordinate system Planes coincide, axis Z c It is parallel to the z-axis of the parallel base coordinate system, coincides with the optical axis of the camera, and its direction is consistent with the field of view of the camera; Establish a transition coordinate system, that is, the image physical coordinate system O l and O r , the coordinates of the point P in the space in the physical coordinate system of the image in the left and right images are P l (u l ,v l ) and P r (u r ,v r ), From the similar triangles, we can get the coordinate depth of point P, which is the z-axis coordinate of point P in the base coordinate system: The coordinates P of point P in the left image pixel coordinate system l (u l ,v l ) and the reprojection matrix Q, we get the 3D coordinates of point P in the left camera coordinate system, and then through the rotation and translation matrix R, we get the 3D coordinates of point P in the base coordinate system: Among them, (c lx ,c ly ) is the origin o l Coordinates in the physical coordinate system of the left image, c rx is the origin o r The coordinates of the u2 axis in the physical coordinate system of the right image; Then, the homogeneous transformation matrix is ​​obtained by the position of the camera installed on the base, and the three-dimensional coordinates of point P in the base coordinate system are obtained; The coordinates of the joint center are obtained from the coordinates of the three points of the end joint and the formula for drawing a circle with three points. Substitute them into the formula in S2 to obtain the displacement of the linear motor. S6: According to the linear motor displacement measured by the encoder (2-2) and the linear motor displacement measured by the binocular camera, the actual linear motor displacement and the end position of the robot arm are obtained by fusing; S7: The array tactile sensor (3-1) detects the position of the contact force, the tension sensor (3-2) detects the tension of the wire rope (1-3) and calculates the size of the contact force, and the six-dimensional force sensor (3-3) detects the force and torque of the contact operation process between the end joint of the robot manipulator (1-5) and the environment; S8: according to the position and magnitude of the contact force, the control current of the stiffness adjustment device (1-2-2) of the joint (1-2) is calculated, and the stiffness adjustment device (1-2-2) of each joint (1-2) is controlled; S9: Determine whether the actual displacement of the linear motor (1-4) reaches the target displacement of the linear motor (1-4), if not, enter S3; if yes, the motion control ends.

Images