A rope-driven super-redundant robotic arm with joint posture self-perception and control method thereof

Abstract

The present invention belongs to the technical field of mechanical arms, and discloses a rope-driven super-redundant mechanical arm with self-sensing joint posture and a control method thereof, wherein the mechanical arm comprises a driving fixed module, a driving module, a visual module, a mechanical arm module and a reflective marking point group; multiple groups of driving modules are arranged along the circumference of the driving fixed module, the mechanical arm module is arranged at one end of the driving fixed module, each group of driving modules is connected to the mechanical arm module through a steel wire rope, and the reflective marking point group is arranged on the mechanical arm module; the visual module is arranged on the driving fixed module close to the mechanical arm module, the visual module is used to obtain the image of the reflective marking point group, and the visual module is also used to identify obstacle information. The present invention can feedback the posture information of the joint, so that the mechanical arm module does not need to rely on mechanical tooling for zero position calibration, the operation is convenient, and the cost is saved. The present invention realizes closed-loop control of posture, and improves the control accuracy compared with position closed-loop control and tension closed-loop control.

Description

Technical Field

[0001] The invention belongs to the technical field of mechanical arms, and in particular relates to a rope-driven super-redundant mechanical arm with self-sensing joint positions and postures and a control method thereof. Background Art

[0002] The rope-driven super-redundant manipulator is a new technology in the field of manipulators. The driving module and the manipulator module of the rope-driven super-redundant manipulator are separated. The movement of the manipulator is controlled by driving the wire rope. This method has great flexibility, reduces the size and weight of the manipulator, can adapt to complex working environments and unstructured tasks, and avoids the impact of the environment on the driving module. Compared with traditional manipulators, the number of joints is more, the redundancy is higher, and the number of degrees of freedom is much larger than the dimension of the task space, so that the manipulator can avoid obstacles more efficiently to adapt to various complex working environments and unpredictable working scenarios. Rope-driven super-redundant manipulators have been widely used in picking, assembly, medical and other scenarios.

[0003] The rope-driven super-redundant robot arm drives the robot arm to move by pulling the rope through the servo motor. Due to the elastic deformation of the rope, it is difficult to ensure the control accuracy by only using position closed-loop control.

[0004] For example, in patent document CN116038662A, a force sensor is used at the end of the wire rope to feedback the tension in the rope, and a force-position combined control method is proposed. For this type of robot arm, the wire rope and the robot arm are in direct contact and relative motion, and there is a large sliding friction between them. This part of the friction cannot be accurately calculated, and there are differences between the wire ropes. The elastic coefficient of each wire rope is different, and the friction between the wire rope and the robot arm is also different. Due to the above reasons, it is difficult to achieve precise control of the rope-driven super-redundant robot arm by using a force-position hybrid control method.

[0005] For example, patent document CN107363820A discloses a highly redundant flexible robotic arm device that can detect joint postures. It uniquely determines the posture of the current joint by detecting the relative rotation angle of two mutually perpendicular rotating shafts in the connecting hinge, and achieves precise control of the robotic arm through closed-loop feedback. This method can be used to calibrate the zero position of the robotic arm, but two sensors need to be installed at each joint, the structure is cumbersome, and multiple interfaces are required to receive feedback data, making the control complex.

[0006] In summary, the existing rope-driven super-redundant manipulator has the following problems:

[0007] 1. Using mechanical tooling to calibrate the zero position of the rope-driven super-redundant robot arm is costly and complex to operate.

[0008] 2. Since the wire rope has elastic deformation and the large sliding friction between the wire rope and the robotic arm cannot be accurately calculated, the accuracy of position closed-loop control and tension closed-loop control is poor. Summary of the invention

[0009] In view of the above problems, the present invention provides a rope-driven super-redundant manipulator with self-sensing joint posture and a control method thereof, which adopts the following technical solutions:

[0010] A rope-driven super-redundant manipulator with self-sensing joint posture, comprising a driving fixing module, a driving module, a vision module, a manipulator module and a reflective marking point group;

[0011] Among them, multiple groups of driving modules are arranged along the circumference of the driving fixed module, the robotic arm module is arranged at one end of the driving fixed module, each group of driving modules is connected to the robotic arm module through a wire rope, and the reflective marking point group is arranged on the robotic arm module; the visual module is arranged on the driving fixed module close to the robotic arm module, the visual module is used to obtain the image of the reflective marking point group, and the visual module is also used to identify obstacle information.

[0012] Furthermore, it also includes a slide and a support frame, the bottom of the support frame is slidably connected to the upper end of the slide, and the driving and fixing module is arranged on the top of the support frame.

[0013] Further, the driving fixing module includes a fixing plate, a front panel and a rear panel;

[0014] The fixing plate, the front panel and the rear panel are fixed on the top of the support frame in sequence and in parallel with each other at a set interval, and the fixing plate and the front panel are fixedly connected by a plurality of bolts.

[0015] Furthermore, the driving module also includes a servo motor, a reducer, a coupling, a support block, a screw rod, a polished rod, a locking device, a slider, a guide wheel mounting block, a support fixing block, a force sensor and a guide wheel bracket;

[0016] The servo motor, the reducer, the coupling, the support block, the screw rod, and the support fixing block are connected in sequence. The servo motor is fixedly connected to the fixing plate after being connected to the reducer. The support block is fixedly connected to the front panel, and the support fixing block is fixedly connected to the rear panel.

[0017] One end of the screw rod is fixed to the front panel through the support block, the other end of the screw rod is fixed to the rear panel through the support fixing block, the two ends of the light rod are respectively fixed to the front panel and the rear panel, and the light rod is arranged parallel to the screw rod;

[0018] The slider is sleeved on the screw rod and the polished rod, the guide wheel mounting block is fixedly arranged on the rear panel, one side of the force sensor is connected to the guide wheel mounting block, and the other side of the force sensor is connected to the guide wheel bracket;

[0019] The robotic arm module is fixedly arranged on the rear panel, one end of the steel wire rope passes through the hole in the middle of the rear panel and is connected to the joint of the robotic arm module, the other end of the steel wire rope passes through the guide wheel bracket and the guide wheel mounting block in sequence, and is fixedly connected to the locking device, and the locking device is fixedly arranged on the slider.

[0020] Furthermore, one side of the force sensor is fixed on the guide wheel mounting block, the other side of the force sensor is connected to the guide wheel bracket, one end of the wire rope is fixed on the joint of the robotic arm module, and the other end of the wire rope passes through the first fixed pulley at the bottom of the guide wheel bracket, the second fixed pulley at the bottom of the guide wheel mounting block, and the third fixed pulley at the top of the guide wheel mounting block in sequence, and is fixedly connected to the locking device.

[0021] Further, the locking device includes a fixing stud, a clamping head and a clamping nut;

[0022] Among them, one end of the fixing stud is fixed to the slider through a thread, the clamping nut is threadedly connected to the other end of the fixing stud, the clamping head is arranged between the clamping nut and the fixing stud, the wire rope passes through the fixing stud, the clamping head and the clamping nut in sequence, and the wedge device of the clamping head clamps the wire rope.

[0023] Furthermore, the visual module includes a plurality of cameras arranged on the rear panel, the plurality of cameras are arranged in two groups, and the two groups of cameras are respectively distributed on two concentric circles.

[0024] Furthermore, the robotic arm module includes a plurality of joints connected in series, and two adjacent joints are connected by a connecting block.

[0025] Furthermore, each joint rotates relative to an adjacent joint around two rotation axes, one connecting block provides two degrees of freedom, and the number of degrees of freedom of the robotic arm module is twice the number of joints.

[0026] Further, the joint includes a mechanical arm body, and transition connection structures are provided at both ends of the mechanical arm body, and the transition connection structure includes an annular body, and the mechanical arm body and the annular body are interference fit, and two connecting parts are relatively provided on the annular body, and the connecting parts are provided with connecting holes;

[0027] The connection block is provided with a plurality of mounting holes along the circumferential direction, and the mounting holes of the connection block are fixed to the connecting holes of the transition connection structures of the two joints by pins.

[0028] Furthermore, the mechanical arm body adopts a carbon fiber tube.

[0029] Furthermore, the plurality of mounting holes include a first mounting hole, a second mounting hole, a third mounting hole and a fourth mounting hole, the first mounting hole and the third mounting hole are arranged opposite to each other, and the second mounting hole and the fourth mounting hole are arranged opposite to each other.

[0030] Furthermore, a plurality of through holes are circumferentially arranged on the annular body, the number of the through holes is equal to the number of steel wire ropes, the number of the through holes is three times the number of joints, each group of steel wire ropes includes three wire ropes, and one end of the three steel wire ropes is connected to the tail end joint.

[0031] Furthermore, the robotic arm body is provided with a plurality of joint holes, and a number of the joint holes are provided with reflective marking points, which are threadedly connected to the joint holes. The plurality of reflective marking points on a joint form a reflective marking point group, and different joints are distinguished by the different distances between the reflective marking points in each reflective marking point group.

[0032] The present invention also provides a control method for the rope-driven super-redundant mechanical arm with self-sensing joint posture, comprising the following steps:

[0033] The visual module sends the collected image of the reflective marking point group on the robotic arm module to the controller; wherein the visual module includes a plurality of cameras;

[0034] The controller determines the three-dimensional coordinate information of the reflective marking point group in the image relative to the camera coordinate system according to the reflective marking point group image;

[0035] The controller determines the rotation angle of each joint of the robot arm module relative to the previous joint in two directions according to the three-dimensional coordinate information of the reflective marking point group relative to the camera coordinate system;

[0036] The controller determines the position and posture of each joint of the robotic arm module according to the rotation angle of each joint of the robotic arm module relative to the previous joint in two directions.

[0037] Further, the controller determines the three-dimensional coordinate information of the reflective marking point group in the image relative to the camera coordinate system according to the reflective marking point group image, including the following steps:

[0038] The robot arm module is in a horizontal posture, and obtains the three-dimensional coordinate information of each reflective marker point relative to the camera coordinate system according to the reflective marker point group image;

[0039] According to the three-dimensional coordinate information of each reflective marking point relative to the camera coordinate system, four reflective marking points that are closest to each other are combined into a reflective marking point group, and the reflective marking point group is taken as a whole to obtain the three-dimensional coordinate information of each reflective marking point group in the camera coordinate system;

[0040] When the robot arm module moves, the image at this time and the previous frames of images are used as input to determine the movement trend and track the position of the reflective marker points in real time to prevent misjudgment of joints.

[0041] Beneficial effects of the present invention:

[0042] 1. The present invention can feed back the position information of the joint, so that the robot arm module does not need to rely on mechanical tooling for zero position calibration, which is convenient to operate and saves costs.

[0043] 2. The present invention uses a visual module to feed back joint posture information to a controller in real time, thereby realizing closed-loop control of posture. Compared with position closed-loop control and tension closed-loop control, the control accuracy is improved.

[0044] 3. The visual module of the present invention can identify information about obstacles in the environment, so that the robotic arm module has a certain obstacle avoidance capability.

[0045] Other features and advantages of the present invention will be described in the following description, and partly become obvious from the description, or be understood by implementing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0047] Figure 1 The overall assembly diagram of a rope-driven super-redundant manipulator with self-sensing joint posture according to an embodiment of the present invention is shown;

[0048] Figure 2 A schematic diagram showing the structure of a single driving module according to an embodiment of the present invention is shown;

[0049] Figure 3 A schematic diagram of installing a guide wheel mounting block and a force sensor according to an embodiment of the present invention is shown;

[0050] Figure 4 A schematic structural diagram of a locking device according to an embodiment of the present invention is shown;

[0051] Figure 5 A schematic diagram showing the installation of multiple cameras of a vision module according to an embodiment of the present invention is shown;

[0052] Figure 6 A schematic structural diagram of a mechanical arm module according to an embodiment of the present invention is shown;

[0053] Figure 7 A schematic diagram of an explosion of connections between adjacent joints of a robotic arm module according to an embodiment of the present invention is shown;

[0054] Figure 8 A schematic diagram of three-dimensional coordinate information of a joint according to an embodiment of the present invention is shown;

[0055] Fig. 9 A schematic flow chart of a control method for a rope-driven hyper-redundant robotic arm with self-sensing joint posture according to an embodiment of the present invention is shown.

[0056] In the figure: 1, slide; 2, support frame; 3, drive fixing module; 4, drive module; 5, vision module; 6, robot arm module; 7, reflective marking point group; 31, fixing plate; 32, front panel; 33, rear panel; 41, servo motor; 42, reducer; 43, coupling; 44, support block; 45, lead screw; 46, light rod; 47, locking device; 48, slider; 49, guide wheel mounting block; 410, support fixing block; 411, force sensor; 412, guide wheel bracket; 413, wire rope; 414, first fixed pulley; 415, second fixed pulley; 416. third fixed pulley; 471. fixing stud; 472. clamping head; 473. clamping nut; 51. first camera; 52. second camera; 53. third camera; 54. fourth camera; 55. fifth camera; 56. sixth camera; 57. seventh camera; 61. joint; 62. connecting block; 611. robot arm body; 612. transition connection structure; 613. annular body; 614. connecting part; 615. mounting hole; 616. pin; 617. gasket; 618. cotter pin; 6111. joint hole; 6121. through hole; 71. reflective marking point. DETAILED DESCRIPTION

[0057] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0058] It should be noted that the terms "first", "second" etc. in the present application are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchanged in appropriate circumstances, so that the embodiments of the present application described here. In the present application, the orientation or positional relationship indicated by the terms "upper", "lower", "left", "right", "front", "back", "top", "bottom", "inside", "outside", "middle", "vertical", "horizontal", "lateral", "longitudinal" etc. are based on the orientation or positional relationship shown in the accompanying drawings.

[0059] The present invention provides a rope-driven super-redundant manipulator with self-sensing joint posture and a control method thereof, which can feedback the joint posture information, so that the manipulator does not need to rely on mechanical tooling for zero position calibration, is easy to operate, and saves costs. The present invention feeds back the joint posture information to the controller in real time through vision to achieve closed-loop control of the posture, which improves the control accuracy compared to position closed-loop control and tension closed-loop control.

[0060] like Figure 1 As shown, a rope-driven super-redundant robotic arm with self-sensing joint posture includes a slide 1, a support frame 2, a drive fixing module 3, a drive module 4, a vision module 5, a robotic arm module 6 and a reflective marking point group 7.

[0061] The bottom of the support frame 2 is slidably connected to the upper end of the slide 1 , the driving and fixing module 3 is arranged on the top of the support frame 2 , and a plurality of driving modules 4 are evenly distributed along the circumference of the driving and fixing module 3 .

[0062] The mechanical arm module 6 is arranged at one end of the driving fixed module 3. Figure 2 As shown, each group of driving modules 4 is connected to the robotic arm module 6 through a steel wire rope 413, and the reflective marking point group 7 is set on the robotic arm module 6; the visual module 5 is set on the driving fixed module 3 close to the robotic arm module 6, and the visual module 5 is used to obtain the image of the reflective marking point group 7. The visual module 5 is also used to identify obstacle information in the environment, so that the robotic arm module 6 has a certain obstacle avoidance capability.

[0063] like Figure 1 As shown, for example, the driving fixing module 3 includes a fixing plate 31, a front panel 32 and a rear panel 33, wherein the fixing plate 31, the front panel 32 and the rear panel 33 are fixed in sequence and parallel to each other at a set interval on the top of the support frame 2, and the holes on the fixing plate 31, the front panel 32 and the rear panel 33 correspond to each other, forming a support for the driving module 4; the fixing plate 31 and the front panel 32 are fixedly connected by multiple bolts, and multiple groups of driving modules 4 are arranged in a circle and fixed between the fixing plate 31, the front panel 32 and the rear panel 33.

[0064] like Figure 2As shown, for example, the drive module 4 also includes a servo motor 41, a reducer 42, a coupling 43, a support block 44, a screw 45, a light rod 46, a locking device 47, a slider 48, a guide wheel mounting block 49, a support fixing block 410, a force sensor 411 and a guide wheel bracket 412.

[0065] Among them, the servo motor 41, the reducer 42, the coupling 43, the support block 44, the screw 45, and the support fixing block 410 are connected in sequence, wherein the servo motor 41 is fixedly connected to the fixing plate 31 after being connected to the reducer 42, the support block 44 is fixedly connected to the front panel 32, and the support fixing block 410 is fixedly connected to the rear panel 33.

[0066] The screw rod 45 is fixed between the front panel 32 and the rear panel 33 through the support support block 44 and the support fixing block 410. One end of the screw rod 45 is fixed to the front panel 32 through the support support block 44, and the other end of the screw rod 45 is fixed to the rear panel 33 through the support fixing block 410. Both ends of the light rod 46 are fixed to the front panel 32 and the rear panel 33 by bolts, respectively. The light rod 46 is arranged parallel to the screw rod 45, and the rear panel 33 is fixedly connected to the front panel 32 through the light rod 46.

[0067] The slider 48 is mounted on the lead screw 45 and the polished rod 46. The slider 48 converts the rotational motion of the lead screw 45 driven by the servo motor 41 into a linear motion. The guide wheel mounting block 49 is fixedly arranged on the rear panel 33. One side of the force sensor 411 is connected to the guide wheel mounting block 49, and the other side of the force sensor 411 is connected to the guide wheel bracket 412.

[0068] For example, the robot arm module 6 is fixedly set on the rear panel 33, one end of the wire rope 413 passes through the hole in the middle of the rear panel 33 and is connected to the joint 61 of the robot arm module 6, and the other end of the wire rope 413 passes through the guide wheel bracket 412 and the guide wheel mounting block 49 in turn to change the direction of the wire rope 413, and then is fixedly connected to the locking device 47, and the locking device 47 is fixedly set on the slider 48.

[0069] Under the action of the servo motor 41, the slider 48 is driven to move, the slider 48 is fixedly connected to the locking device 47, the locking device 47 moves together with the slider 48, the length of the wire rope 413 changes, and the robot arm module 6 is driven to move.

[0070] like Figure 3As shown, the guide wheel mounting block 49, the guide wheel bracket 412 and the force sensor 411 together constitute a force measuring device, one side of the force sensor 411 is fixed to the guide wheel mounting block 49 by threads, and the other side of the force sensor 411 is connected to the guide wheel bracket 412 by threads, one end of the wire rope 413 is fixed to the outside of the through hole 6121 on the transition connection structure 612 of the robot module 6, and the other end of the wire rope 413 passes through the first fixed pulley 414 at the bottom of the guide wheel bracket 412, the second fixed pulley 415 at the bottom of the guide wheel mounting block 49, and the third fixed pulley 416 at the top of the guide wheel mounting block 49 in sequence, and is fixedly connected to the locking device 47. In this way, the force sensor 411 can measure the tension in the wire rope 413.

[0071] like Figure 4 As shown, for example, the locking device 47 includes a fixing stud 471, a clamping head 472 and a clamping nut 473, one end of the fixing stud 471 is fixed to the slider 48 by a thread, the other end of the fixing stud 471 is installed in the clamping head 472, the clamping nut 473 is threadedly connected to the other end of the fixing stud 471, the clamping head 472 is arranged between the clamping nut 473 and the fixing stud 471, the wire rope 413 passes through the fixing stud 471, the clamping head 472 and the clamping nut 473 in sequence, and the wedge device of the clamping head 472 clamps the wire rope 413.

[0072] like Figure 5 As shown, for example, the visual module 5 includes a plurality of cameras arranged on the rear panel 33, and the plurality of cameras are arranged in two groups, and the two groups of cameras are respectively distributed on two concentric circles. For example, 7 cameras are arranged, and the 7 cameras are distributed on two concentric circles. Four cameras are arranged on the inner circle, including a first camera 51, a second camera 52, a third camera 53, and a fourth camera 54, which are respectively 60°, 120°, 240°, and 300° to the x-axis; three cameras are arranged on the outer circle, including a fifth camera 55, a sixth camera 56, and a seventh camera 57, which are respectively 0°, 90°, and 180° to the x-axis.

[0073] like Figure 6 As shown, for example, the robot arm module 6 includes a plurality of joints 61 connected in series, and two adjacent joints 61 are connected via a connecting block 62 .

[0074] like Figure 7 As shown, the joint 61 includes a mechanical arm body 611. For example, the mechanical arm body 611 adopts a carbon fiber tube. Transition connection structures 612 are provided at both ends of the mechanical arm body 611. The transition connection structure 612 includes an annular body 613. The mechanical arm body 611 and the annular body 613 are interference fit. Two connecting parts 614 are relatively arranged on the annular body 613, and connecting holes are provided on the connecting parts 614.

[0075] A plurality of mounting holes 615 are circumferentially arranged on the connecting block 62. For example, the plurality of mounting holes 615 include a first mounting hole 615, a second mounting hole 615, a third mounting hole 615 and a fourth mounting hole 615. The first mounting hole 615 and the third mounting hole 615 are arranged opposite to each other, the second mounting hole 615 and the fourth mounting hole 615 are arranged opposite to each other, and a first straight line between a center point of the first mounting hole 615 and a center point of the third mounting hole 615 is perpendicular to a second straight line between a center point of the second mounting hole 615 and a center point of the fourth mounting hole 615.

[0076] The mounting hole 615 of the connecting block 62 is fixed to the connecting holes of the transition connection structure 612 of the two joints 61 by means of a pin 616, a gasket 617 and a cotter pin 618. Each joint 61 rotates around two rotation axes relative to the adjacent joint 61. Figure 8 As shown, one connecting block 62 provides two degrees of freedom, and the number of degrees of freedom of the robot arm module 6 is twice the number of joints 61 .

[0077] The annular body 613 of the transition connection structure 612 is provided with a plurality of through holes 6121 along the circumferential direction for the steel wires 413 to pass through, and the number of through holes 6121 is equal to the number of steel wires 413, which is three times the number of joints 61. Each group of steel wires 413 includes three wires, which are 120 degrees apart from each other, and one end of the three steel wires 413 is connected to the tail end joint 61.

[0078] The steel wire rope 413 passes through the through hole 6121 on the annular body 613, so that the joint 61 can rotate due to the change in the length of the steel wire rope 413, thereby controlling the movement of the robotic arm. The number of steel wire ropes 413 between each joint 61 gradually decreases from the head end to the tail end of the robotic arm module 6. Three steel wire ropes 413 are reduced through one joint 61, and the number of steel wire ropes 413 is three times the number of joints 61.

[0079] For example, a plurality of joint holes 6111 are provided on the robot arm body 611, and reflective marking points 71 are provided on some of the joint holes 6111. The reflective marking points 71 are threadedly connected to the joint holes 6111. The plurality of reflective marking points 71 on a joint 61 form a reflective marking point group 7. The reflective marking points 71 in each reflective marking point group 7 have different distances from each other to distinguish different joints 61.

[0080] For example, nine joint holes 6111 are provided on the robot arm body 611, each reflective marking point group 7 includes four reflective marking points 71, a reflective marking point 71 is placed in the middle position of each joint 61, and the remaining three reflective marking points 71 are placed in the remaining eight joint holes 6111. There are 56 placement methods in total, and a method equal to the number of joints 61 in the robot arm module 6 is randomly selected. Different joints 61 are distinguished by the different distances between the reflective marking points 71 in each reflective marking point group 7.

[0081] like Fig. 9 As shown, the present invention also provides a control method for the above-mentioned rope-driven super-redundant manipulator with self-sensing joint posture, comprising the following steps:

[0082] S0. Establish a camera coordinate system at the center point of the joint 61 at the front end of the robot arm module 6, and place the robot arm module 6 in a horizontal posture.

[0083] S1, the visual module 5 sends the collected image of the reflective marking point group 7 on the robotic arm module 6 to the controller, as follows:

[0084] The images of the reflective marking point group 7 on the tail end joint 61 of the robot arm module 6 are collected by the four cameras of the inner circle, and the images of the reflective marking point group 7 on the head end joint 61 of the robot arm module 6 are collected by the three cameras of the outer circle.

[0085] The seven cameras of the inner and outer circles ensure that the field of view can completely cover the robot module 6. When the robot module 6 is in any posture, it is ensured that one reflective marking point group 7 can be captured by at least two cameras.

[0086] S2. The controller determines the three-dimensional coordinate information of the reflective marking point group 7 in the reflective marking point group 7 image relative to the camera coordinate system according to the reflective marking point group 7 image, as follows:

[0087] S21, placing the robot arm module 6 in a horizontal state, and obtaining the three-dimensional coordinate information of each reflective marking point 71 relative to the camera coordinate system according to the image of the reflective marking point group 7.

[0088] S22. According to the three-dimensional coordinate information of each reflective marking point 71 relative to the camera coordinate system, when the robot arm is in a horizontal posture, the four reflective marking points 71 that are closest to each other are combined into a reflective marking point group 7, and the reflective marking point group 7 is taken as a whole to obtain the three-dimensional coordinate information of each reflective marking point group 7 in the camera coordinate system.

[0089] S23, when the robot arm module 6 moves, the current image and the previous frames of images are taken as input, the movement trend is determined, and the position of the reflective marking point 71 is tracked in real time to prevent the joint 61 from being misjudged.

[0090] By simultaneously and synchronously acquiring images with multiple cameras, according to the principle of binocular imaging technology, the pixel difference of the images of corresponding points of the same reflective marking point group 7 by different cameras is obtained, the depth information of the reflective marking point group 7 is obtained, and then the three-dimensional coordinate information of the reflective marking point group 7 is obtained.

[0091] The same number of reflective marking points 71 are fixed on each joint 61 to form a reflective marking point group 7 . The reflective marking points 71 in each reflective marking point group 7 have different distances from each other, forming a variety of forms for distinguishing different joints 61 .

[0092] In this step, the collected image of the reflective marking point group 7 is analyzed to identify the reflective marking point 71, and then the three-dimensional coordinate information of each reflective marking point 71 relative to the camera coordinate system is obtained. The camera coordinate system is a Cartesian coordinate system with the center of the end of the robot arm as the origin, set to coordinate system A.

[0093] The four reflective marking points 71 that are closest to each other are grouped into a reflective marking point group 7, and the three-dimensional coordinate information of each reflective marking point group 7 relative to the camera coordinate system is obtained. A Cartesian coordinate system is established with the center of the sphere of the central reflective marking point 71 in each reflective marking point group 7 as the coordinate origin, and is set as coordinate system B1-B8. The coordinates of the reflective marking point 71 in each reflective marking point group 7 coordinate system are known, and the homogeneous transformation matrix between the coordinate system of each reflective marking point group 7 and the camera coordinate system can be solved from the corresponding points of the four reflective marking points 71 in space.

[0094] For example, let's use one reflective marking point group 7 as an example, and the other reflective marking point groups 7 are the same. Assuming the coordinates of the four reflective marking points 71 are (A1, B1), (A2, B2), (A3, B3), (A4, B4), the solution formula can be expressed as:

[0095] Assume that in coordinate system A, the three-dimensional coordinates of the four points are (x A1 ,y A1 ,z A1 ), (x A2 ,y A2 ,z A2 ), (x A3 ,y A3 ,z A3 ), (x A4 ,y A4 ,z A4 ).

[0096] Assume that in coordinate system B, the three-dimensional coordinates of the four points are (x B1 ,y B1 ,z B1 ), (x B2 ,y B2 ,z B2 ), (x B3 ,y B3 ,z B3 ), (x B4 ,y B4 ,z B4 ).

[0097] The homogeneous transformation matrix form is as follows:

[0098]

[0099] Here, R is a 3x3 rotation matrix and t is a 3x1 translation vector.

[0100] Use the least squares method to solve the rotation matrix R as follows:

[0101]

[0102] R=Vt T ·U T

[0103] Among them, svd is the singular value decomposition, P A and P B are four points in coordinate systems A and B expressed in homogeneous coordinates, and · represents matrix multiplication.

[0104] The translation vector t is set to the difference between the first point in coordinate system B and the corresponding point in coordinate system A, and the homogeneous transformation matrix T is obtained as follows:

[0105] t=P B1 -R.P A1

[0106] According to the correspondence between the preset arrangement of the reflective marking point group 7 and the joint 61 of the robot module 6, the corresponding robot serial number is obtained. The three-dimensional coordinate information calculation of the tail end joint 61 of the robot module 6 can refer to Figure 8 The position of the center of the sphere of the central reflective marking point 71 of the reflective marking point group 7 relative to the position of the joint hole 6111 on the robot body 611 is known, and the position of the joint hole 6111 on the robot body 611 relative to the position of the tail joint 61 of the robot body 611 is also known. A coordinate system is established based on the intersection of the axis hole line of the tail joint 61 of the robot module 6 and the center line of the robot module 6 as the coordinate origin, set as the coordinate system C1-C8, and determine the homogeneous transformation matrix T1 corresponding to the transformation of the reflective marking point group 7 coordinate system and the robot module 6 tail joint 61 coordinate system.

[0107] The three-dimensional coordinate information of the reflective marking point group 7 relative to the camera coordinate system is used to calculate the three-dimensional coordinate information of the tail joint 61 of the robot module 6 relative to the camera coordinate system. The solution formula can be expressed as:

[0108]

[0109] S3. The controller determines the rotation angles of each joint 61 of the robot arm module 6 in two directions relative to the previous joint 61 according to the three-dimensional coordinate information of the reflective marking point group 7 relative to the camera coordinate system.

[0110] According to the three-dimensional coordinate information of the end joint 61 of the robot module 6 in the camera coordinate system, the end joint 61 of the robot module 6 is recorded as the first joint 61, and so on, the head joint 61 of the robot module 6 is the eighth joint 61. Assume that the coordinates of the center point between the first joint 61 and the second joint 61 are (x AC1 ,y AC1 ,z AC1 ), the coordinate of the center point between the 0th joint 61 and the 1st joint 61 is (x AC0 ,y AC0 ,z AC0 ), the rotation angles θ1 and θ2 of the first joint 61 of the robot module 6 in two vertical directions The solution formula can be expressed as:

[0111] Δx=x A1 -x A0

[0112] Δy=y A1 -y A0

[0113] Δz=z A1 -z A0

[0114]

[0115]

[0116] Assume that the coordinates of the center point between the second joint 61 and the third joint 61 are (x A2 ,y A2 ,z A2 ), the rotation angles θ1 and θ2 of the first joint 61 of the robot module 6 in two vertical directions have been obtained. Therefore, the homogeneous transformation matrix between the camera coordinate system A and the end coordinate system C1 of the first joint 61 of the robot module 6 can be obtained: 0 A1, according to the second transformation matrix, the point (x AC2 ,y AC2 ,z AC2 ) in the coordinate system (x c2 ,y c2 ,z c2 ), the solution formula can be expressed as:

[0117]

[0118] PC2 = 0 A1×P2

[0119] Therefore, the rotation angles θ2 and θ3 of the second joint 61 in two perpendicular directions are Similar to the first joint 61, the solution formula can be expressed as:

[0120] Δx=x C2

[0121] Δy=y C2

[0122] Δz=z C2

[0123]

[0124]

[0125] By the above method, the secondary transformation matrix between all joint 61 coordinate systems and the rotation angle θ of the joint 61 of the robot module 6 in two vertical directions can be obtained. i and

[0126] S4. The controller determines the position and posture of each joint 61 of the robot arm module 6 according to the rotation angle of each joint 61 of the robot arm module 6 relative to the previous joint 61 in two directions.

[0127] The present invention feeds back the posture information of the joint 61 to the controller in real time through the visual module 5, thereby realizing closed-loop control of the posture, and improving the control accuracy compared with position closed-loop control and tension closed-loop control.

[0128] Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent substitutions for some of the technical features therein; and these modifications or substitutions do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A control method for a rope-driven hyper-redundant manipulator with self-sensing joint posture. It is characterized in that The following steps are involved: The visual module sends the collected image of the reflective marking point group on the robotic arm module to the controller; wherein the visual module includes a plurality of cameras; The controller determines the three-dimensional coordinate information of the reflective marker point group in the image relative to the camera coordinate system according to the reflective marker point group image, including the following steps: placing the robot arm module in a horizontal state, and obtaining the three-dimensional coordinate information of each reflective marker point relative to the camera coordinate system according to the reflective marker point group image; combining the four reflective marker points closest to each other into a reflective marker point group according to the three-dimensional coordinate information of each reflective marker point relative to the camera coordinate system, and taking the reflective marker point group as a whole to obtain the three-dimensional coordinate information of each reflective marker point group in the camera coordinate system; when the robot arm module moves, taking the current image and the previous frames of images as input, judging the movement trend, and tracking the position of the reflective marker points in real time to prevent misjudgment of joints; The controller determines the rotation angle of each joint of the robot arm module relative to the previous joint in two directions according to the three-dimensional coordinate information of the reflective marking point group relative to the camera coordinate system; The controller determines the position and posture of each joint of the robotic arm module according to the rotation angle of each joint of the robotic arm module relative to the previous joint in two directions.

2. A rope-driven hyper-redundant manipulator with self-sensing joint position and posture. It is characterized in that Used to execute the control method of claim 1, comprising a driving and fixing module, a driving module, a vision module, a mechanical arm module and a reflective marking point group; Among them, multiple groups of driving modules are arranged along the circumference of the driving fixed module, the robotic arm module is arranged at one end of the driving fixed module, each group of driving modules is connected to the robotic arm module through a wire rope, and the reflective marking point group is arranged on the robotic arm module; the visual module is arranged on the driving fixed module close to the robotic arm module, the visual module is used to obtain the image of the reflective marking point group, and the visual module is also used to identify obstacle information.

3. The rope-driven super-redundant manipulator with self-sensing joint posture according to claim 2, It is characterized in that It also includes a slide and a support frame, the bottom of the support frame is slidably connected to the upper end of the slide, and the driving and fixing module is arranged on the top of the support frame.

4. The rope-driven super-redundant manipulator with joint posture self-sensing according to claim 3, It is characterized in that The driving fixing module comprises a fixing plate, a front panel and a rear panel; The fixing plate, the front panel and the rear panel are fixed on the top of the support frame in sequence and in parallel with each other at a set interval, and the fixing plate and the front panel are fixedly connected by a plurality of bolts.

5. The rope-driven super-redundant manipulator with joint posture self-sensing according to claim 4, It is characterized in that The driving module also includes a servo motor, a reducer, a coupling, a support block, a screw rod, a polished rod, a locking device, a slider, a guide wheel mounting block, a support fixing block, a force sensor and a guide wheel bracket; The servo motor, the reducer, the coupling, the support block, the screw rod, and the support fixing block are connected in sequence. The servo motor is fixedly connected to the fixing plate after being connected to the reducer. The support block is fixedly connected to the front panel, and the support fixing block is fixedly connected to the rear panel. One end of the screw rod is fixed to the front panel through the support block, the other end of the screw rod is fixed to the rear panel through the support fixing block, the two ends of the light rod are respectively fixed to the front panel and the rear panel, and the light rod is arranged parallel to the screw rod; The slider is sleeved on the screw rod and the polished rod, the guide wheel mounting block is fixedly arranged on the rear panel, one side of the force sensor is connected to the guide wheel mounting block, and the other side of the force sensor is connected to the guide wheel bracket; The robotic arm module is fixedly arranged on the rear panel, one end of the steel wire rope passes through the hole in the middle of the rear panel and is connected to the joint of the robotic arm module, the other end of the steel wire rope passes through the guide wheel bracket and the guide wheel mounting block in sequence, and is fixedly connected to the locking device, and the locking device is fixedly arranged on the slider.

6. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 5, It is characterized in that One side of the force sensor is fixed on the guide wheel mounting block, the other side of the force sensor is connected to the guide wheel bracket, one end of the wire rope is fixed to the joint of the robotic arm module, and the other end of the wire rope passes through the first fixed pulley at the bottom of the guide wheel bracket, the second fixed pulley at the bottom of the guide wheel mounting block, and the third fixed pulley at the top of the guide wheel mounting block in sequence, and is fixedly connected to the locking device.

7. The rope-driven super-redundant manipulator with joint posture self-sensing according to claim 5, It is characterized in that The locking device comprises a fixing stud, a clamping head and a clamping nut; Among them, one end of the fixing stud is fixed to the slider through a thread, the clamping nut is threadedly connected to the other end of the fixing stud, the clamping head is arranged between the clamping nut and the fixing stud, the wire rope passes through the fixing stud, the clamping head and the clamping nut in sequence, and the wedge device of the clamping head clamps the wire rope.

8. A rope-driven super-redundant manipulator with self-sensing joint posture according to any one of claims 4 to 7, It is characterized in that The visual module includes a plurality of cameras arranged on a rear panel, wherein the plurality of cameras are arranged in two groups, and the two groups of cameras are respectively distributed on two concentric circles.

9. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 2, It is characterized in that The robot arm module includes a plurality of joints connected in series, and two adjacent joints are connected by a connecting block.

10. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 9, It is characterized in that Each joint rotates relative to the adjacent joints around two rotation axes, one connecting block provides two degrees of freedom, and the number of degrees of freedom of the robotic arm module is twice the number of joints.

11. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 9, It is characterized in that The joint comprises a mechanical arm body, and transition connection structures are provided at both ends of the mechanical arm body. The transition connection structure comprises an annular body, and the mechanical arm body and the annular body are interference fit. Two connecting parts are relatively provided on the annular body, and connecting holes are provided on the connecting parts. The connection block is provided with a plurality of mounting holes along the circumferential direction, and the mounting holes of the connection block are fixed to the connecting holes of the transition connection structures of the two joints by pins.

12. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 11, It is characterized in that The mechanical arm body is made of carbon fiber tube.

13. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 11, It is characterized in that The plurality of mounting holes include a first mounting hole, a second mounting hole, a third mounting hole and a fourth mounting hole. The first mounting hole and the third mounting hole are arranged opposite to each other, and the second mounting hole and the fourth mounting hole are arranged opposite to each other.

14. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 11, It is characterized in that The annular body is provided with a plurality of through holes along the circumferential direction, the number of the through holes is equal to the number of steel wire ropes, and the number of the through holes is three times the number of joints. Each group of steel wire ropes includes three wire ropes, and one end of the three steel wire ropes is connected to the tail end joint.

15. The rope-driven super-redundant manipulator with joint posture self-awareness according to claim 11, It is characterized in that The robotic arm body is also provided with a plurality of joint holes, and a number of the joint holes are provided with reflective marking points, which are threadedly connected to the joint holes. The plurality of reflective marking points on a joint form a reflective marking point group, and different joints are distinguished by the different distances between the reflective marking points in each reflective marking point group.

Images