CN115157256B - Remote control robot system control method capable of automatically switching working modes - Google Patents

Abstract

The invention relates to a remote control robot system control method capable of automatically switching working modes, which comprises the following steps: s1, measuring the distance between the tail end of the driven side robot and a work target; s2, judging the motion stage of the driven side robot through a distance threshold value, and calculating a weighting coefficient of the current motion stage; s3, calculating the control quantity of the driven side robot in the current motion stage; s4, according to the control quantity of the driven side robot in the step S3, different control modes are adopted for the driven side robot. The distance sensor is used for detecting the distance between the driven side robot and the operation target and is used as a basis for judging the current motion stage of the driven side robot, and the calculated value of the mode switching function is used as a weighting coefficient of different control modes, so that the driven side robot can quickly approach the target, the accuracy requirement of short-distance operation is easily met, and the automatic switching of two working modes, namely a speed control mode and a position control mode, can be realized.

Description

Control method of remote-controlled robot system capable of automatically switching working modes

Technical Field

The invention belongs to the technical field of teleoperation robots, and in particular relates to a control method for a teleoperation robot system capable of automatically switching working modes.

Background Art

At present, most of the control methods of tele-controlled robot systems proposed at home and abroad adopt single position control or speed control. As the tele-controlled operation environment becomes more complex, the disadvantages of single control mode become increasingly prominent. Therefore, a comprehensive control strategy is needed to automatically adjust the control mode to meet higher tele-controlled operation requirements.

At present, speed control can make the slave robot in the telecontrol robot move quickly in space, but compared with position control, the use of speed control will reduce the position control accuracy of the slave robot. Therefore, in the existing research, the position control and speed control are combined together, which can not only realize the automatic switching between the position control and speed control of the slave side, but also ensure the control accuracy of close-range operations while shortening the operation time. However, the switching conditions in the existing research involve the speed of the active robot in the telecontrol robot and lack transition. It is difficult for the operator to control the operation speed during the operation process, resulting in frequent switching of the control mode, which affects the operation experience. Therefore, it is necessary to design a telecontrol robot system control method that can automatically switch the working mode, which is of great significance for avoiding the operator's frequent switching of control modes and enhancing the operation experience.

Summary of the invention

In view of the problems existing in the prior art, the present invention provides a remote-controlled robot system control method which can automatically switch working modes. A distance sensor is used to detect the distance between a slave-side robot and an operating target as a basis for judging the current motion stage of the slave-side robot. The calculated value of the mode switching function is then used as a weighting coefficient for different control modes. This allows the slave-side robot to quickly approach the target and easily meet the accuracy requirements of close-range operations. It can also realize automatic switching between the speed control mode and the position control mode, thereby improving work efficiency and quality.

The technical solution adopted by the present invention is a remote control robot system control method capable of automatically switching working modes, which comprises the following steps:

S1, measuring the distance between the end of the slave-side robot and the working target through a distance sensor;

S2. Determine the motion stage of the slave robot by the distance threshold, and calculate the weighted coefficient corresponding to the motion in the current stage, as follows:

S21, if the distance value d _m2 ≤d ≤d _max , the slave-side robot is in the first motion stage, then the weighted coefficient of the first motion stage of the slave-side robot is S(d)=1;

S22. If the distance value d _m1 ≤ d ≤ d _m2 , the slave-side robot is in the second motion stage, and the weighted coefficient of the second motion stage of the slave-side robot is:

Where S(d) is the mode switching function; d is the distance between the end of the slave robot and the work target measured by the distance sensor; d _m1 and d _m2 are the distance thresholds; d _max is the farthest operating distance of the slave robot;

S23, if the distance value is 0≤d≤d _m1 , the slave-side robot is in the third motion stage, then the weighted coefficient of the third motion stage of the slave-side robot is S(d)=0;

S3. According to the motion stage of the slave robot, the control amount of the slave robot corresponding to the current motion stage is calculated. The control amount expression is:

Where _Us is the control quantity of the slave robot; S is the weighting coefficient; _Fm is the force of the master robot; _Fs is the force of the slave robot; is the dimensionless value of the angular displacement of the active side robot; is the dimensionless value of the angular velocity of the slave robot;

S4. According to the change of the control amount in step S3, the slave-side robot corresponds to different control modes, as follows:

S41: if the control variable of the slave-side robot is a dimensionless deviation signal between the angular displacement of the active-side robot and the angular velocity of the slave-side robot, the slave-side robot is in a speed control mode;

S42: If the control amount of the slave-side robot is the weighted sum of the deviation signals of the force signal, angular displacement and angular velocity, the slave-side robot is in a transition stage from a speed control mode to a position control mode;

S43: If the control amount of the slave-side robot is the master-side robot force deviation signal, the slave-side robot is in a position control mode.

Furthermore, when the slave robot is in the position control mode, an additional force is applied to the end of the active robot, and the user at one side of the active robot can obtain feedback force from the motion process of the active robot and the slave robot in real time, and the feedback force expression is:

F _r =(1-S)[F _d +K(q _sd -q _s )];

Where _Fr is the feedback force, K is the gain coefficient, _Fd is the additional force, _qsd is the desired angular displacement of the slave robot, and _qs is the actual angular displacement of the slave robot.

Preferably, when the slave-side robot is in the speed control mode in step S41, the specific steps of the speed control method are:

S411, the operator pulls the operating handle on the active robot, measures the joint angle q _m of the active robot through the displacement sensor, and performs dimensionless processing to obtain the dimensionless value of the angular displacement of the active robot

S412, the dimensionless value of the angular displacement of the active side robot The dimensionless value of the angular velocity of the slave robot A comparison is made and the generated deviation signal is used as a driving signal of the slave-side robot, so that the angular velocity of the slave-side robot follows the joint angle of the corresponding active side, thereby realizing the control of the speed of the slave-side robot.

Preferably, when the slave-side robot is in the position control mode in step S43, the position control method comprises the following specific steps:

S431, the operator pulls the operating handle on the active-side robot, measures the operating force F _h applied to the operating handle through a force sensor, and obtains the applied force F _m of the active-side robot through amplification processing;

S432, comparing the force _Fm of the active-side robot with the force _Fs of the passive-side robot, and generating a deviation signal which can be adjusted by the passive-side controller to control the movement of the passive-side robot;

S433. Calculate the expected angular displacement _qsd of the slave-side robot according to the joint angle _qm of the active-side robot, and compare the expected angular displacement _{qsd of} the slave-side robot with the actual angular displacement _qs of the slave-side robot. Feedback the generated deviation signal to the active-side controller to realize position control of the slave-side robot.

Preferably, the distance threshold is:

d _m1 = 0.2d ₀ ;

d _m2 =0.3d ₀ ;

Where _d0 is the distance between the end of the slave robot and the work target at the initial moment.

The characteristics and beneficial effects of the present invention are:

1. The present invention provides a remote-controlled robot system control method that can automatically switch working modes. By calculating the control quantity of the slave-side robot corresponding to different motion stages, the automatic switching of the two working modes of speed control mode and position control mode can be realized. At the same time, a transition stage is added between the two control modes to avoid interference with the control system caused by direct mode switching, which affects the operator's experience.

2. The present invention provides a remote-controlled robot system control method that can automatically switch working modes. When the slave-side robot is far away from the working target, it is helpful to control the slave-side robot to quickly approach the working target. When the slave-side robot is close to or in contact with the working target, it is helpful to control the position of the slave-side robot to meet the positioning requirements and improve the working efficiency and quality.

3. The present invention provides a remote-controlled robot system control method that can automatically switch working modes. A slight additional force is introduced into the feedback force, which can help remind the operator that the end effector of the slave-side robot is close to the work target and the control mode is switched. The speed can be reduced to reduce the impact caused by the contact between the slave-side robot and the work target and reduce errors.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a flow chart of a remote robot system control method of the present invention;

FIG2 is a schematic diagram of a control mode switching diagram of a telecontrol robot system of the present invention;

FIG3 is a schematic diagram of a speed control mode and a position control mode of the present invention;

FIG4 is a schematic diagram of a control system according to Embodiment 1 of the present invention;

5a-5d are diagrams showing simulation results of Example 1 of the present invention.

DETAILED DESCRIPTION

In order to fully describe the technical content, structural features, objectives and effects of the present invention, the following will be described in detail with reference to the accompanying drawings.

The present invention provides a remote control robot system control method capable of automatically switching working modes, as shown in FIG1 and FIG2, which comprises the following steps:

S1, measuring the distance between the end of the slave robot and the work target through the distance sensor;

S2. Determine the motion stage of the slave robot by the distance threshold and calculate the weighted coefficient corresponding to the motion in the current stage, as follows:

S21. If the distance value d _m2 ≤ d ≤ d _max , the slave-side robot is in the first motion stage, then the weighted coefficient of the first motion stage of the slave-side robot is S(d) = 1;

S22. If the distance value d _m1 ≤ d ≤ _{d m2} , the slave-side robot is in the second motion stage, and the weighted coefficient of the slave-side robot in the second motion stage is:

Where S(d) is the mode switching function; d is the distance between the end of the slave robot and the work target measured by the distance sensor; d _m1 and d _m2 are the distance thresholds; d _max is the farthest operating distance of the slave robot;

S23. If the distance value is 0≤d≤d _m1 , the slave-side robot is in the third motion stage, and the weighted coefficient of the slave-side robot in the third motion stage is S(d)=0.

S3. According to the motion stage of the slave robot, the control amount of the slave robot corresponding to the current motion stage is calculated. The control amount expression is:

Where _Us is the control quantity of the slave robot; S is the weighting coefficient; _Fm is the force of the master robot; _Fs is the force of the slave robot; is the dimensionless value of the angular displacement of the active side robot; is the dimensionless value of the angular velocity of the slave robot;

S4. According to the change of the control amount in step S3, the slave-side robot corresponds to different control modes, as follows:

S41, if the control variable of the slave-side robot is a dimensionless deviation signal between the angular displacement of the active-side robot and the angular velocity of the slave-side robot, the slave-side robot is in a speed control mode;

S42: If the control amount of the slave-side robot is the weighted sum of the deviation signals of the force signal, angular displacement and angular velocity, the slave-side robot is in a transition stage from a speed control mode to a position control mode;

S43: If the control quantity of the slave-side robot is the master and the slave-side robot has a force deviation signal, the slave-side robot is in position control mode.

In a preferred embodiment, the distance threshold is:

d _m1 = 0.2d ₀ ;

d _m2 =0.3d ₀ ;

Where _d0 is the distance between the end of the slave robot and the work target at the initial moment.

As shown in Figure 3, when the slave robot is in position control mode, an additional force is applied to the end of the active robot, and the user on the active robot side can obtain feedback force from the motion process of the active robot and the slave robot in real time. The feedback force is composed of the introduction of a slight additional force and the corresponding joint angular displacement deviation of the master and slave sides. During the operation, the operator can be reminded to switch the control mode and the slave robot approaches the operation target, while the operator can feel the interference between the slave robot and the environment. The feedback force expression is:

F _r =(1-S)[F _d +K(q _sd -q _s )];

Where _Fr is the feedback force, K is the gain coefficient, _Fd is the additional force, _qsd is the desired angular displacement of the slave robot, and _qs is the actual angular displacement of the slave robot.

As shown in FIG3 , when the slave-side robot is in the speed control mode in step S41 , the specific steps of the speed control method are as follows:

S411, the operator pulls the operating handle 1 on the active side robot, measures the joint angle q _m of the active side robot through the displacement sensor, and performs dimensionless processing to obtain the dimensionless value of the angular displacement of the active side robot

S412, the dimensionless value of the angular displacement of the active side robot The dimensionless value of the angular velocity of the slave robot A comparison is made and the generated deviation signal is used as a driving signal for the slave robot, so that the angular velocity of the slave robot follows the joint angle of the corresponding active side, thereby realizing the control of the speed of the slave robot.

As shown in FIG3 , when the slave-side robot is in the position control mode in step S43 , the specific steps of the position control method are as follows:

S431, the operator pulls the operating handle 1 on the active-side robot, measures the operating force F _h applied to the operating handle 1 through the force sensor, and obtains the applied force F _m of the active-side robot by amplification processing;

S432, comparing the force _Fm of the active-side robot with the force _Fs of the passive-side robot, and the generated deviation signal can be adjusted and controlled by the passive-side controller to control the movement of the passive-side robot.

S433. Calculate the expected angular displacement _qsd of the slave-side robot according to the joint angle _qm of the active-side robot, and compare the expected angular displacement _{qsd of} the slave-side robot with the actual angular displacement _qs of the slave-side robot. Feedback the generated deviation signal to the active-side controller to realize position control of the slave-side robot.

The specific operation steps of the present invention are as follows:

In one embodiment, as shown in FIG. 4 , the structures of the master and slave sides are similar, and the remote-controlled robot system mainly includes: an operating handle 1, a force sensor 2, a drive motor 3, a straight shaft 4, a crankshaft 5, an angle sensor 6, a first A/D sensor 7, a second A/D sensor 8, an operating target 9, a distance sensor 10, a first D/A sensor 11, a second D/A sensor 13, a control unit 12 and an amplifier circuit 14.

The process of automatically switching working modes and the corresponding relationship between control mode, distance and weighting coefficient are shown in Figures 1 and 2. When the slave robot is operated by telecontrol, it is generally approached from a distance to the work target, and the description is made according to this process. The distance sensor 10 detects the distance d between the end of the slave robot and the work target 9 in real time. When d _m2 ≤ d ≤ d _max , the angle sensors 6 on the master and slave sides detect the corresponding joint angles q _m (q _m1 q _m2 ) and q _s (q _s1 q _s2 ), which are processed by the first A/D sensor 7 and the second A/D sensor 8 and input to the control unit 12. The force sensor 2 of the active robot detects the operating force F _h , which is processed by the amplifier circuit 14 to obtain the active side force F _m , and then processed by the first A/D sensor 7 to input to the control unit 12. The slave side force sensor 2 detects the slave side force F _s , which is processed by the second A/D sensor 8 and input to the control unit 12. The control unit 12 calculates the difference between the dimensionless active side angular displacement of the active side and the dimensionless angular velocity of the driven side, and processes and outputs it through the first D/A sensor 11 as the control amount U _s (U _s1 U _s2 ) of the drive motor 3 of the driven side robot. At the same time, the difference is processed and output through the second D/A sensor 13 as the control amount U _m (U _m1 U _m2 ) of the drive motor 3 of the active side robot, ensuring that the speed of the driven side follows the position of the active side to achieve speed control.

When d _m1 ≤d ≤ _{d m2} , the input of the control unit 12 remains unchanged, and the value of the weighting coefficient S is calculated. The control unit 12 calculates the difference between the dimensionless angular displacement of the active side and the dimensionless angular velocity of the driven side, and simultaneously calculates the difference between the active side force and the driven side force, and multiplies them by the weighting coefficients S and (1-S) respectively, and then adds the sum, which is processed and output by the first D/A sensor 11 as the control amount of the drive motor 3 of the driven side robot. At the same time, the control unit 12 calculates the expected joint angle of the driven side according to the joint angle of the active side, subtracts it from the joint angle actually detected by the driven side, and processes and outputs it by the second D/A sensor 13 as the control amount of the drive motor 3 of the active side robot.

When 0≤d≤d _m1 , the input of the control unit 12 remains unchanged. The control unit 12 calculates the difference between the active side force and the passive side force as the control amount of the drive motor 3 of the passive robot. At the same time, the control unit 12 calculates the expected joint angle of the passive side according to the active side joint angle, subtracts it from the joint angle actually detected by the passive side, and processes the output through the second D/A converter 13 as the control amount of the active side motor 3, so that the displacement of the passive side follows the displacement of the active side to achieve position control. The master and slave sides use a mapping ratio of 1:3 for position tracking (the corresponding magnification ratio can be formulated according to needs).

As shown in Figure 5a, with single position control, the master and slave position tracking accuracy is high, which is very suitable for close-range or contact operations. As shown in Figure 5b, with single speed control, it can be seen from the displacement curves of the master and slave sides that the slave side responds and moves quickly; by comparing the normalized values of the master and slave sides, the position following effect is not ideal, which is very suitable for free movement of the slave side.

As shown in Figure 5c and Figure 5d, before 11.789s, the S value is 1, and the slave robot adopts the speed control mode; between 11.789s and 13.958s, the slave robot is in the transition stage from the speed control mode to the position control mode; after 13.958s, the slave robot adopts the position control mode. By comparing the master-slave displacement curve, it can be seen that the response and movement speed of the slave side is relatively fast. Secondly, by observing the normalized values of the master and slave displacements, it can be seen that switching from the speed control mode to the position control mode can improve the response speed of the slave side while meeting the position tracking accuracy during close-range or contact operations.

The embodiments described above are only descriptions of the preferred implementation modes of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims (5)

1. A remote control robot system control method capable of automatically switching working modes, characterized in that it comprises the following steps: S1, measuring the distance between the end of the slave robot and the work target through the distance sensor; S2. Determine the motion stage of the slave robot by the distance threshold, and calculate the weighted coefficient corresponding to the motion in the current stage, as follows: S21, if the distance value d _m2 ≤d ≤d _max , the slave-side robot is in the first motion stage, then the weighted coefficient of the first motion stage of the slave-side robot is S(d)=1; S22. If the distance value d _m1 ≤ d ≤ d _m2 , the slave-side robot is in the second motion stage, and the weighted coefficient of the second motion stage of the slave-side robot is: Where S(d) is the mode switching function; d is the distance between the end of the slave robot and the work target measured by the distance sensor; d _m1 and d _m2 are the distance thresholds; d _max is the farthest operating distance of the slave robot; S23, if the distance value is 0≤d≤d _m1 , the slave-side robot is in the third motion stage, then the weighted coefficient of the third motion stage of the slave-side robot is S(d)=0; S3. According to the motion stage of the slave robot, the control amount of the slave robot corresponding to the current motion stage is calculated. The control amount expression is: Where _Us is the control quantity of the slave robot; S is the weighting coefficient; _Fm is the force of the master robot; _Fs is the force of the slave robot; is the dimensionless value of the angular displacement of the active side robot; is the dimensionless value of the angular velocity of the slave robot; S4. According to the change of the control amount in step S3, the slave-side robot corresponds to different control modes, as follows: S41: if the control variable of the slave-side robot is a dimensionless deviation signal between the angular displacement of the active-side robot and the angular velocity of the slave-side robot, the slave-side robot is in a speed control mode; S42: If the control amount of the slave-side robot is the weighted sum of the deviation signals of the force signal, angular displacement and angular velocity, the slave-side robot is in a transition stage from a speed control mode to a position control mode; S43: If the control amount of the slave-side robot is the master-side robot force deviation signal, the slave-side robot is in a position control mode. 2. The control method of a teleoperated robot system capable of automatically switching working modes according to claim 1 is characterized in that when the slave-side robot is in the position control mode, an additional force is applied to the end of the active-side robot, and the user at one side of the active-side robot can obtain feedback force from the motion process of the active-side robot and the slave-side robot in real time, and the feedback force expression is: F _r =(1-S)[F _d +K(q _sd -q _s )]; Where _Fr is the feedback force, K is the gain coefficient, _Fd is the additional force, _qsd is the desired angular displacement of the slave robot, and _qs is the actual angular displacement of the slave robot. 3. The control method of a teleoperated robot system capable of automatically switching working modes according to claim 1, characterized in that when the slave robot is in the speed control mode in step S41, the specific steps of the speed control method are: S411, the operator pulls the operating handle on the active robot, measures the joint angle q _m of the active robot through the displacement sensor, and performs dimensionless processing to obtain the dimensionless value of the angular displacement of the active robot S412, the dimensionless value of the angular displacement of the active side robot The dimensionless value of the angular velocity of the slave robot A comparison is made and the generated deviation signal is used as a driving signal of the slave-side robot, so that the angular velocity of the slave-side robot follows the joint angle of the corresponding active side, thereby realizing the control of the speed of the slave-side robot. 4. The control method of a teleoperated robot system capable of automatically switching working modes according to claim 1 is characterized in that when the slave robot is in the position control mode in step S43, the specific steps of the position control method are as follows: S431, the operator pulls the operating handle on the active-side robot, measures the operating force F _h applied to the operating handle through a force sensor, and obtains the applied force F _m of the active-side robot through amplification processing; S432, comparing the force _Fm of the active-side robot with the force _Fs of the passive-side robot, and generating a deviation signal which can be adjusted by the passive-side controller to control the movement of the passive-side robot; S433. Calculate the expected angular displacement _qsd of the slave-side robot according to the joint angle _qm of the active-side robot, and compare the expected angular displacement _{qsd of} the slave-side robot with the actual angular displacement _qs of the slave-side robot. Feedback the generated deviation signal to the active-side controller to realize position control of the slave-side robot. 5. The control method of a remote-controlled robot system capable of automatically switching working modes according to claim 1, wherein the distance threshold is: d _m1 = 0.2d ₀ ; d _m2 =0.3d ₀ ; Where _d0 is the distance between the end of the slave robot and the work target at the initial moment.