CN113084816B - New intelligent master-slave manipulator force feedback control method and system - Google Patents

Abstract

The invention discloses a novel intelligent master-slave manipulator force feedback control method and a system, wherein the method comprises the following steps: the method comprises the steps of obtaining the output torque of a driving motor of a driven arm, calculating the load of the driven arm, calculating the load feedback output torque of each driving motor of a driving arm required by the load of the driven arm according to a statics principle, and controlling the load feedback output voltage of each driving motor of the driving arm according to the load feedback output torque of each driving motor of the driving arm to realize the load feedback of the driven arm. The novel intelligent master-slave manipulator force feedback control method and system have the advantages that: real-time statics calculation is utilized, and the effect of load feedback of the driven arm without mechanical balance is achieved; and under the condition that no force sensor is arranged at the tail end of the driven arm, calculating the load mass through the torque of a motor of the driven arm.

Description

New intelligent master-slave manipulator force feedback control method and system

technical field

The invention belongs to the technical field of mechanical control of nuclear equipment, and in particular relates to a novel intelligent master-slave manipulator force feedback control method and system.

Background technique

In nuclear operation equipment, the manipulator slave arm needs to be used to operate radioactive objects at a long distance, and the personnel operate in the safe area through the manipulator penetration, and should be able to perceive the operating state of the manipulator slave arm at all times, that is, when the slave arm is unloaded, the personnel It is very easy to operate the active arm. When the driven arm is loaded, the personnel should feel the actual load, so that the personnel can truly feel the operating object.

In the traditional manipulator control method, the designer finds a fixed equilibrium pose through force analysis, and then uses a mechanical balance method to balance the gravitational moment during the swing of the manipulator. Various types of springs and mechanical mechanisms with good linearity achieve comprehensive torque balance. In practical application scenarios, due to machining errors and the clearance of gears on the transmission chain, the mechanical balance system inevitably generates deviations, and the serial manipulator structure will cause this deviation to accumulate joint by joint. In addition, since the mechanical balance mechanism cannot be modified after the design and manufacture is completed, if the balance pose needs to be changed, the mechanical mechanism needs to be redesigned, and the new balance pose is still fixed and cannot balance the gravitational moment of the robotic arm during the teleoperation process in real time. , so it is difficult to achieve the effect of easily operating the active arm, and the operator cannot accurately feel the feedback force of the slave arm. Because all kinds of sensors are easy to fail in the irradiation environment, the force sensor cannot be installed on the slave arm in the hot chamber. When the slave arm carries a load or collides with the environment, the control system cannot obtain the relationship between the slave arm and the outside world. Because of the interactive force information, it is difficult to reflect the load force or collision force of the slave arm on the master arm.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide a new type of intelligent master-slave manipulator force feedback control method and system for the above-mentioned deficiencies in the prior art. sense of control.

The technical solution adopted to solve the technical problem of the present invention is to provide a new type of intelligent master-slave manipulator force feedback control method. The manipulator includes a master arm and a slave arm connected to the master arm, and the master arm and the slave arm are in an isomorphic series connection. A type of articulated manipulator, including a mechanical transmission arm and a corresponding electronic control device, has multiple degrees of freedom, and each degree of freedom is provided with a corresponding drive motor and encoder, which is used to drive the mechanical transmission arm to move and monitor each movement posture. In addition, the active arm is provided with a torque sensor between the mechanical transmission arm and the servo motor to monitor the torque state of each degree of freedom. The mechanical transmission arm, the torque sensor (active arm end), the servo motor and the encoder are connected in series in sequence, and the torque sensor (active arm end), the servo motor and the encoder are placed in a non-radioactive environment. The method includes the following steps:

Obtain the output torque of the drive motor of the slave arm, calculate the load of the slave arm, and calculate the load feedback output torque of each drive motor of the master arm required by the load of the slave arm according to the statics principle. The load feedback output torque controls the load feedback output voltage of each drive motor of the master arm, so as to realize the load feedback of the slave arm.

Preferably, the new intelligent master-slave manipulator force feedback control method further includes the following steps:

Acquiring the attitude of the master arm at any time, the step of calculating the load of the slave arm further includes calculating the load of the slave arm in combination with acquiring the attitude of the master arm.

Preferably, the algorithm used to calculate the follower arm load is a load identification algorithm.

Preferably, the new intelligent master-slave manipulator force feedback control method further includes the following steps:

Obtain the attitude of the active arm at any time, calculate the gravity compensation output torque of the driving motor of the active arm required for gravity compensation in real time, and control the gravity compensation output of each driving motor of the active arm according to the gravity compensation output torque of the driving motor of the active arm. voltage to realize the gravity compensation of the active arm.

Preferably, the posture of the active arm at any time is the joint angle of each joint of the active arm at any time.

Preferably, the algorithm used to calculate the gravity compensation output torque of the driving motor of the active arm required for gravity compensation in real time is a gravity compensation algorithm.

Preferably, the novel intelligent master-slave manipulator force feedback control method further includes the following steps: calculating the force rendering compensation output of the driving motor of the active arm required for force rendering in real time according to the posture of the active arm in any time state Torque, according to the force rendering compensation output torque of the driving motor of the main arm, the force rendering compensation output voltage of each driving motor of the main arm is controlled to realize the force rendering compensation of the main arm.

Preferably, the algorithm used for the force rendering compensation of the output torque of the driving motor of the active arm required for the real-time calculation of the force rendering is a force rendering algorithm.

The present invention also provides a system for the above-mentioned novel intelligent master-slave manipulator force feedback control method, comprising:

Manipulators, including:

The active arm, the slave arm connected with the active arm, the active arm is connected with the industrial computer, and the slave arm is connected with the industrial computer;

Industrial computer, including:

The motor torque acquisition unit is used to acquire the output torque of the drive motor of the slave arm and send it to the calculation unit of the upper computer.

The drive unit is configured to control the load feedback output voltage of each drive motor of the master arm according to the received load feedback output torque of each drive motor of the master arm, so as to realize the load feedback of the slave arm;

Host computer, including:

The calculation unit is used to calculate the load of the slave arm according to the output torque of the drive motor of the slave arm received, and according to the statics principle, calculate the load feedback output torque of each drive motor of the master arm required by the load of the slave arm, and send To the drive unit of the industrial computer.

Preferably, the industrial computer further includes:

The attitude acquisition unit is used to acquire the attitude of the active arm at any time and send it to the computing unit of the host computer;

The computing unit of the host computer is also used to calculate the gravity compensation output torque of the driving motor of the driving arm required for gravity compensation in real time according to the received attitude of the driving arm in any time state, and send it to the driving unit of the industrial computer;

The driving unit of the industrial computer is also used for controlling the gravity compensation output voltage of each driving motor of the driving arm according to the received gravity compensation output torque of the driving motor of the driving arm, so as to realize the gravity compensation of the driving arm.

Preferably, the computing unit of the host computer is further configured to calculate the force rendering compensation output torque of the driving motor of the active arm required for force rendering in real time according to the received posture of the active arm in any time state, and send it to the industrial computer. Drive unit;

The driving unit of the industrial computer is also used to control the force rendering compensation output voltage of each driving motor of the main arm according to the received force rendering compensation output torque of the driving motor of the main arm, so as to realize the force rendering compensation of the main arm.

Preferably, the calculation unit of the host computer is further configured to calculate the load of the slave arm according to the received output torque of the drive motor of the slave arm and the attitude of the master arm at any time.

The beneficial effects of the novel intelligent master-slave manipulator force feedback control method and system of the present invention are that: by using real-time statics solution, the effect of load feedback of the slave arm without mechanical balance is realized; in the case where no force sensor is installed at the end of the slave arm Next, the load mass is calculated by the torque of the slave arm motor.

Description of drawings

1 is a flowchart of a new intelligent master-slave manipulator force feedback control method in Embodiment 1 of the present invention;

2 is a schematic structural diagram of a system used by the new intelligent master-slave manipulator force feedback control method in Embodiment 1 of the present invention;

3 is a flowchart of a new intelligent master-slave manipulator force feedback control method in Embodiment 2 of the present invention;

4 is a schematic structural diagram of a system used in the new intelligent master-slave manipulator force feedback control method in Embodiment 2 of the present invention;

FIG. 5 is a schematic diagram of the composition of the active arm/slave arm of the present invention.

In the picture: 1-active arm; 2-slave arm; 3-industrial computer; 4-host computer.

Detailed ways

In order to make those skilled in the art better understand the technical solutions of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

Embodiments of the present patent are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present patent, but should not be construed as a limitation on the present patent.

Example 1

As shown in FIG. 1, this embodiment provides a new type of intelligent master-slave manipulator force feedback control method. The manipulator includes a master arm 1 and a slave arm 2 connected to the master arm 1. The master arm 1 and the slave arm 2 are the same A series-type articulated manipulator with a mechanical structure, including a mechanical transmission arm and a corresponding electronic control device, has multiple degrees of freedom, and each degree of freedom is provided with a corresponding drive motor and encoder, which is used to drive the mechanical transmission arm to move and monitor each sports posture. In addition, the active arm 1 is provided with a torque sensor between the mechanical transmission arm and the servo motor for monitoring the torque state of each degree of freedom. The mechanical transmission arm, the torque sensor (1 end of the active arm), the servo motor and the encoder are connected in series in sequence, and the torque sensor (1 end of the active arm), the servo motor and the encoder are placed in a non-radioactive environment.

The method includes the following steps:

S101 acquires the output torque of the drive motor of the slave arm 2 .

S102 calculates the load of the slave arm 2, and calculates the load feedback output torque of each drive motor of the master arm 1 required for the load of the slave arm 2 according to the statics principle.

S103 controls the load feedback output voltage of each drive motor of the master arm 1 according to the load feedback output torque of each drive motor of the master arm 1 , so as to realize the load feedback of the slave arm 2 .

As shown in FIG. 2 , this embodiment also provides a system used in a new type of intelligent master-slave manipulator force feedback control method, including:

Manipulators, including:

Active arm 1, slave arm 2 connected with active arm 1, active arm 1 is connected with industrial computer 3, and slave arm 2 is connected with industrial computer 3;

Industrial computer 3, including:

The motor torque acquisition unit is used to acquire the output torque of the drive motor of the slave arm 2 and send it to the calculation unit of the upper computer 4,

The drive unit is used to control the load feedback output voltage of each drive motor of the master arm 1 according to the received load feedback output torque of each drive motor of the master arm 1, so as to realize the load feedback of the slave arm 2;

Host computer 4, including:

The calculation unit is used to calculate the load of the slave arm 2 according to the output torque of the drive motor of the slave arm 2 received, and according to the principle of statics, calculate the load feedback of each drive motor of the master arm 1 required for the load of the slave arm 2 The output torque is sent to the drive unit of the industrial computer 3.

The beneficial effects of the new intelligent master-slave manipulator force feedback control method and system of the present embodiment are: by using real-time statics solution, the effect of load feedback of the slave arm 2 without mechanical balance is realized; the end of the slave arm 2 has no installation force In the case of a sensor, the load mass is calculated from the motor torque of the slave arm 2.

Example 2

As shown in FIG. 3, this embodiment provides a new type of intelligent master-slave manipulator force feedback control method. The manipulator includes a master arm 1 and a slave arm 2 connected to the master arm 1. The master arm 1 and the slave arm 2 are the same A series-type articulated manipulator with a mechanical structure, including a mechanical transmission arm and a corresponding electronic control device, has multiple degrees of freedom, and each degree of freedom is provided with a corresponding drive motor and encoder, which is used to drive the mechanical transmission arm to move and monitor each sports posture. In addition, the active arm 1 is provided with a torque sensor between the mechanical transmission arm and the servo motor for monitoring the torque state of each degree of freedom. The mechanical transmission arm, the torque sensor (1 end of the active arm), the servo motor and the encoder are connected in series in sequence, and the torque sensor (1 end of the active arm), the servo motor and the encoder are placed in a non-radioactive environment. The method includes the following steps:

S201 obtains the output torque of the drive motor of the slave arm 2, obtains the attitude of the master arm 1 in any time state, calculates the load of the slave arm 2, and calculates the load of the slave arm 2 according to the statics principle. The load feedback output torque of the drive motor is used to control the load feedback output voltage of each drive motor of the master arm 1 according to the load feedback output torque of each drive motor of the master arm 1, so as to realize the load feedback of the slave arm 2.

S202 calculates the gravity compensation output torque of the driving motor of the driving arm 1 required for gravity compensation in real time according to the posture of the driving arm 1 in any time state, and controls each drive of the driving arm 1 according to the gravity compensation output torque of the driving motor of the driving arm 1 The gravity of the motor compensates the output voltage to realize the gravity compensation of the active arm 1 .

S203 calculates the force rendering compensation output torque of the driving motor of the driving arm 1 required for force rendering in real time according to the posture of the driving arm 1 in any time state, and controls the power rendering compensation output torque of the driving arm 1 according to the force rendering compensation output torque of the driving motor of the driving arm 1 The force rendering compensation output voltage of each drive motor realizes the force rendering compensation of the active arm 1 .

The novel intelligent master-slave manipulator force feedback control method in this embodiment is a control method for the friendly operation force sense of the master-slave manipulator based on statics solution.

Preferably, the algorithm used to calculate the load of the follower arm 2 is a load identification algorithm.

Preferably, the posture of the active arm 1 in any time state is the joint angle of each joint of the active arm 1 in any time state.

Preferably, the algorithm used to calculate the gravity compensation output torque of the drive motor of the active arm 1 required for gravity compensation in real time is a gravity compensation algorithm.

Preferably, the algorithm used for force rendering to compensate the output torque of the driving motor of the active arm 1 required for real-time calculation of force rendering is a force rendering algorithm.

As shown in FIG. 4 , this embodiment provides a system for a new type of intelligent master-slave manipulator force feedback control method, including:

Manipulators, including:

Active arm 1, slave arm 2 connected with active arm 1, active arm 1 is connected with industrial computer 3, and slave arm 2 is connected with industrial computer 3;

Industrial computer 3, including:

The motor torque acquisition unit is used to acquire the output torque of the drive motor of the slave arm 2 and send it to the calculation unit of the upper computer 4,

The drive unit is used to control the load feedback output voltage of each drive motor of the master arm 1 according to the received load feedback output torque of each drive motor of the master arm 1, so as to realize the load feedback of the slave arm 2;

Host computer 4, including:

The calculation unit is used to calculate the load of the slave arm 2 according to the output torque of the drive motor of the slave arm 2 received, and according to the principle of statics, calculate the load feedback of each drive motor of the master arm 1 required for the load of the slave arm 2 The output torque is sent to the drive unit of the industrial computer 3.

Preferably, the industrial computer 3 further includes:

The attitude acquisition unit is used to acquire the attitude of the active arm 1 at any time, and send it to the calculation unit of the host computer 4;

The calculation unit of the host computer 4 is also used to calculate the gravity compensation output torque of the driving motor of the driving arm 1 required for gravity compensation in real time according to the received attitude of the driving arm 1 in any time state, and send it to the driving force of the industrial computer 3 unit;

The driving unit of the industrial computer 3 is also used to control the gravity compensation output voltage of each driving motor of the driving arm 1 according to the received gravity compensation output torque of the driving motor of the driving arm 1 , so as to realize the gravity compensation of the driving arm 1 .

As shown in FIG. 5 , a schematic diagram of the master arm 1/slave arm 2 degrees of freedom in this embodiment is used for a new type of intelligent master-slave manipulator force feedback control method.

Preferably, the calculation unit of the host computer 4 is also used to obtain the output torque of the driving motor of the active arm 1 in real time according to the received attitude of the active arm 1 at any time, and to calibrate the empty space of the active arm 1 through the industrial computer 3 During the load, the torque value of the drive motor obtains the compensation torque required for force rendering, and sends it to the drive unit of the industrial computer 3;

The driving unit of the industrial computer 3 is also used to control the force rendering compensation output voltage of each driving motor of the driving arm 1 according to the received force rendering compensation output torque of the driving motor of the driving arm 1, so as to realize the force rendering compensation of the driving arm 1.

Preferably, the calculation unit of the host computer 4 is further configured to calculate the load of the slave arm 2 according to the received output torque of the drive motor of the slave arm 2 combined with the attitude of the master arm 1 at any time.

The calculation unit is specifically the actuator of the master-slave manipulator force feedback control algorithm, obtains the corresponding degrees of freedom attitude according to the encoders of the active arm 1, and calculates the gravity compensation output torque of the driving motor of the active arm 1 required for gravity compensation in real time through the space matrix transformation. ; Calculate the load size by calibrating the drive motor torque value of the slave arm 2 with no load through the industrial computer 3, and send these data to the industrial computer 3 using the TCP/IP communication protocol.

The industrial computer 3 receives the torque information sent from the host computer 4 and controls the master arm 1; monitors the joint angle of the master arm 1 and the torque value of the drive motor of the slave arm 2 in real time, and sends it to the host computer 4 using the TCP/IP communication protocol .

The master arm 1 and the slave arm 2 include two isomorphic manipulators, a servo motor, a torque sensor and an encoder connected to the manipulator for driving the manipulator, which can be used to complete teleoperation tasks (nuclear environment teleoperation machinery). The arm is an industrial robotic arm for special applications, and there is no mature commercial product on the market at present).

The new intelligent master-slave manipulator force feedback control system in this embodiment compensates the gravitational moment of the master arm 1 in real time during the teleoperation process based on the statics solution, identifies the weight of the load when the slave arm 2 is loaded, and The load force of the slave arm 2 is reflected on the master arm 1 .

The system used in the new intelligent master-slave manipulator force feedback control method in this embodiment to perform force feedback specifically refers to: the industrial computer 3 monitors the posture of the active arm 1 at any time, that is, the values of the encoders of each degree of freedom, and the relationship with the active arm. 1. The output torque of the drive motor of the slave arm 2 with the same posture, and send these data to the host computer 4; The mass of the object (may be zero); based on the principle of statics and combined with the attitude of the active arm 1, calculate the output torque of each driving motor of the active arm 1 required for gravity compensation and load force rendering; the upper computer 4 will calculate the calculated active arm The output torque value of the drive motor of 1 is returned to the industrial computer 3, and the industrial computer 3 controls the output voltage of each drive motor of the master arm 1 through the driver, and finally realizes that at any time and any attitude, and the slave arm 2 has no force sensor, the active The gravity compensation and load force rendering effect of arm 1 avoids the deviation caused by mechanical balance and improves the efficiency and comfort of master-slave teleoperation.

Compared with the prior art, the beneficial effects of the new intelligent master-slave manipulator force feedback control method and system of the present embodiment are:

(1) Real-time statics solution is used to realize the effect of gravity compensation and load force rendering of active arm 1 without mechanical balance;

(2) To solve the gravity compensation of the active arm 1 by controlling the gravity compensation algorithm, cancel the mechanical gravity compensation mechanism, greatly simplify the structure of the manipulator, and make the manipulator more lightweight;

(3) In the case where no force sensor is installed at the end of the slave arm 2, the load mass of the slave arm 2 is identified by the output torque of the drive motor of the slave arm 2 and the attitude of the master arm 1, and the rendering effect of the load force of the master arm 1 is completed. ;

(4) The full balance of the arm's gravity is realized through the gravity compensation algorithm, and the use of the load force rendering method can provide the operator with an accurate and real feedback force effect.

It can be understood that the above embodiments are only exemplary embodiments adopted to illustrate the principle of the present invention, but the present invention is not limited thereto. For those skilled in the art, without departing from the spirit and essence of the present invention, various modifications and improvements can be made, and these modifications and improvements are also regarded as the protection scope of the present invention.

Claims (8)

1. a novel intelligent master-slave manipulator force feedback control method, the manipulator comprises a master arm, a slave arm connected with the master arm, it is characterized in that, described method may further comprise the steps: Obtain the output torque of the drive motor of the slave arm, calculate the load of the slave arm, and calculate the load feedback output torque of each drive motor of the master arm required by the load of the slave arm according to the statics principle. The load feedback output torque controls the load feedback output voltage of each drive motor of the master arm to realize the load feedback of the slave arm; Obtain the attitude of the main arm at any time, calculate the gravity compensation output torque of the driving motor of the main arm required for gravity compensation in real time, and control the gravity compensation output voltage of each driving motor of the main arm according to the gravity compensation output torque of the driving motor of the main arm , to realize the gravity compensation of the main arm; According to the posture of the main arm at any time, the force rendering compensation output torque of the driving motor of the main arm required for real-time calculation of force rendering is calculated, and the force rendering of each driving motor of the main arm is controlled according to the force rendering compensation output torque of the driving motor of the main arm. Compensate the output voltage to realize the force rendering compensation of the active arm. 2. The novel intelligent master-slave manipulator force feedback control method according to claim 1, further comprising the following steps: Acquiring the attitude of the active arm at any time, the step of calculating the load of the slave arm further includes calculating the load of the slave arm in combination with the acquired attitude of the active arm. 3. The novel intelligent master-slave manipulator force feedback control method according to claim 1 or 2, wherein the algorithm used for calculating the load of the slave arm is a load identification algorithm. 4 . The novel intelligent master-slave manipulator force feedback control method according to claim 1 , wherein the posture of the active arm at any time is the joint angle of each joint of the active arm at any time. 5 . 5 . The new intelligent master-slave manipulator force feedback control method according to claim 1 , wherein the algorithm used to calculate the gravity compensation output torque of the driving motor of the active arm required for gravity compensation in real time is a gravity compensation algorithm. 6 . 6 . The novel intelligent master-slave manipulator force feedback control method according to claim 1 , wherein the force rendering algorithm is used for the force rendering and compensation output torque of the driving motor of the active arm required for real-time calculation of force rendering. 7 . 7. A system used in the novel intelligent master-slave manipulator force feedback control method according to any one of claims 1 to 6, characterized in that, comprising: Manipulators, including: The active arm, the slave arm connected with the active arm, the active arm is connected with the industrial computer, and the slave arm is connected with the industrial computer; Industrial computer, including: The motor torque acquisition unit is used to acquire the output torque of the drive motor of the slave arm and send it to the calculation unit of the upper computer. The drive unit is configured to control the load feedback output voltage of each drive motor of the master arm according to the received load feedback output torque of each drive motor of the master arm, so as to realize the load feedback of the slave arm; Industrial computer, also including: The attitude acquisition unit is used to acquire the attitude of the active arm at any time and send it to the computing unit of the upper computer; The computing unit of the host computer is also used to calculate the gravity compensation output torque of the driving motor of the driving arm required for gravity compensation in real time according to the received attitude of the driving arm at any time, and send it to the driving unit of the industrial computer; The driving unit of the industrial computer is also used to control the gravity compensation output voltage of each driving motor of the driving arm according to the received gravity compensation output torque of the driving motor of the driving arm, so as to realize the gravity compensation of the driving arm; Host computer, including: The calculation unit is used to calculate the load of the slave arm according to the output torque of the drive motor of the slave arm received, and according to the statics principle, calculate the load feedback output torque of each drive motor of the master arm required by the load of the slave arm, and send To the drive unit of the industrial computer; The computing unit of the host computer is also used to calculate the force rendering compensation output torque of the driving motor of the driving arm required for force rendering in real time according to the received attitude of the driving arm at any time, and send it to the driving unit of the industrial computer; The driving unit of the industrial computer is also used to control the force rendering compensation output voltage of each driving motor of the main arm according to the received force rendering compensation output torque of the driving motor of the main arm, so as to realize the force rendering compensation of the main arm. 8. The system used for the new intelligent master-slave manipulator force feedback control method according to claim 7, characterized in that, The calculation unit of the host computer is also used to calculate the load of the slave arm according to the received output torque of the drive motor of the slave arm and the attitude of the master arm at any time.

Images