CN103991077B - A shared control method for robot hand controllers based on force fusion - Google Patents

Abstract

The present invention proposes a shared control method for hand controllers based on force fusion, the steps of which include: firstly taking the target object as the guide, and realizing manipulating the hand controller to control the movement of the robot through the operator's own intelligent decision-making; Working scene image data and real-time reconstruction of the three-dimensional environment, and the target object as the guide to build a virtual guiding force that enables the robot to approach the target autonomously, using this virtual force to guide the hand controller to control the robot's movement, and combine the operator's manipulation force with the target object's The virtual guidance force is fused, and the movement of the hand controller and the robot is controlled through the fusion force, so that the machine intelligence and human intelligence can control the robot to approach the target object and complete the operation process. The invention can realize the effective combination of human intelligence and machine intelligence, and ensure safe and fast operation process.

Description

A shared control method for robot hand controllers based on force fusion

technical field

The invention belongs to a control method of a robot, in particular to a hand controller sharing control method based on force fusion.

Background technique

Robots have played an increasingly important role in the completion of tasks in extreme environments such as high temperature, high pressure, and strong radiation. Robots are developing in the direction of intelligence. However, due to the constraints of the development level of control, sensing and artificial intelligence, it is difficult to develop a fully autonomous robot in a changing environment in the short term. The control terminal, referred to as the hand controller, is a bridge for human-computer interaction between the operator and the robot, and must have high control efficiency and real-time performance. In the long run, the autonomous control of the robot's operation is mainly controlled in real time through the robot operation terminal.

However, in today's complex and high-risk environment, for the operator and the entire operation process, there are still many problems to be improved in the previous control method of the robot hand controller.

First of all, the success and efficiency of the completion of robot tasks depends too much on the skill level and proficiency of the operator. Since the hand controller differs greatly from the robot in terms of structure, form, and size, the operator often has to go through repeated pose adjustments during remote control, which will undoubtedly lead to low robot control efficiency. At the same time, in a state of high concentration of attention, long-term operation is likely to cause psychological tension and mental fatigue of the operator.

Secondly, due to the complexity of the operating environment and tasks, most of the current operating methods are difficult to meet the control requirements in actual operations. Because these methods cannot obtain the depth information of the environment, the operator mainly relies on two-dimensional images to infer the relative pose of the robot and the target, and further decide how to manipulate the hand controller, which will inevitably increase the workload of the operator. At the same time, due to the large amount of data in the image, there is inevitably a large time lag in the long-distance transmission, and misoperations caused by this occur frequently.

Furthermore, most of the existing control terminals adopt unilateral position servo or bilateral force feedback servo control strategies, which cannot provide comprehensive and predictable information feedback to operators. According to experience, when the operator uses the hand controller to control the robot, he is more concerned about the relative pose of the robot end, the task target and the obstacle, so as to control the robot to approach the target quickly and safely. Displacement and force sensors do not provide relevant information during this phase of operation.

Finally, combining the advantages of the operator's manual control and the robot's autonomous control, coordinate between the human and the autonomous control system to jointly control the robot, that is, shared control. Although shared control has been widely used in robot control occasions such as unpredictable environments and unreliable control signals caused by time delays, existing shared control methods generally have problems such as many sensing signals and complex control systems, and no matter the degree of freedom The shared control method of segmentation and fusion (human manual and robot autonomy control different degrees of freedom of the robot separately, rather than jointly controlling a certain degree of freedom), or the task decomposition control method based on vision sharing (human first manually manipulates the robot to the specified location, the robot will complete the task independently), the intelligence of the operator and the intelligence of the robot are still relatively independent in the control loop, and the degree of integration is not high, so it is difficult to adapt to the complex and changeable working environment.

Contents of the invention

Aiming at the problems in operability and safety in the field of robot manipulation mentioned above, the present invention proposes a shared control method for robot hand controllers based on force fusion.

The technical solution adopted in the present invention: the present invention studies the man-machine sharing control method of the fusion of the manipulating force on the robot hand controller and the virtual guiding force. Virtual guidance force has the functions of "prediction" and "guidance" in robot motion control, and the operator's manipulation force is an output form of human advanced decision-making. Using appropriate control methods, reasonably assigning the weights of manual participation of the operator and autonomous participation of the machine, and effectively combining the autonomous task planning of the robot with manual control, the integration of human intelligence and machine intelligence can be realized. Its specific method includes the following steps

a. Operate the terminal through the hand controller, take the target object as the guide, and realize the manual manipulation force signal to control the movement of the robot through the operator's own intelligent decision-making;

b. Obtain the image data of the operation scene through stereo vision technology and reconstruct the three-dimensional environment in real time, and construct a virtual guiding force (including attraction, repulsion, and image servo force) that guides the robot to approach the target object based on the target object, so as to realize the virtual guiding force Local autonomous control of robots;

c. Fuse the operator's manual control force described in step a with the virtual guidance force of the target object described in step b, and control the movement of the hand controller and robot through the fusion force to realize the joint action of machine intelligence and human intelligence. Control the robot to approach the target object and complete the operation process.

The step a uses the hand controller to operate the terminal, is guided by the target object, and realizes the manual manipulation force signal to control the movement of the robot through the operator's own intelligent decision-making, and is realized in the following specific ways:

1. Build a robot motion control platform based on force closed loop;

2. The computer collects the manipulative force signal and transmits the data to prepare for the system to control the movement of the hand controller;

3. Based on the mass-damping system control strategy, according to the dynamic model formula of the hand controller control system

$\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; \&CenterDot;</span>{\stackrel{<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; \&CenterDot;</span>{\stackrel{<span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; )</span>$

The motion of the hand controller is controlled in real time. In the formula, M and B are the inertia matrix and damping matrix of the hand controller respectively, J is the Jacobian matrix of the hand controller, F _h is the manipulation force applied by the operator, and F _e is the contact force between the robot and the environment, and q _D is the expected motion pose of the hand controller;

4. Transmit the motion position and posture signals of the hand controller to the motion joints of the robot, so that the robot motion is synchronized with the motion of the hand controller, guided by the target object, and realize the human manipulation force signal to control the robot motion through the operator's own intelligent decision-making .

The step b obtains the image data of the operation scene through stereo vision technology and reconstructs the scene environment in real time, and constructs a virtual guiding force that guides the robot to approach the target object guided by the target object, and uses this virtual force to guide the hand controller to control the movement of the robot, as follows Way to achieve:

1. Obtain the image data of the operation scene through stereo vision technology and reconstruct the 3D environment in real time, and calculate the expected pose of the robot under the guidance of the target object. The method is as follows:

1) Obtain and calculate the spatial coordinates of each pixel in the background image in real time by the environment camera, and then generate the elevation data model of the background environment (obstacles);

2) According to the color and texture features of each pixel in the image acquired by the hand camera, the job object area is extracted, and then the job object image features are constructed;

3) According to the results of image matching and camera calibration, calculate the spatial coordinates of each pixel in the work object area;

4) Construct the bounding box model of the operation object, and use the axis direction of the OBB bounding box of the operation object obtained by visual calculation as the expected pose direction to calculate the expected pose of the robot end;

2. Calculate the virtual attraction, repulsion, and image servo force of the operation object and obstacles on the end of the robot through the acquired image data of the operation scene and the reconstructed 3D environment:

5) Taking the relative amount of the current pose of the robot end and the expected pose as a parameter, based on the obtained characteristics of the work object and the spatial coordinates of each pixel, according to the force relationship of the object in the gravitational field

F _ζ = -α(γ-γ _* )

Calculate the virtual gravitational force F _ζ of the work object suffered by the robot, where α is the gravitational coefficient, γ and γ* are the current pose and expected pose vector of the end of the robot, respectively;

6) Taking the relative amount of the current pose of the robot end and the expected pose as a parameter, combined with the background environment (or obstacle) elevation information measured by the environmental camera, to solve the distance between the end of the robot and the background environment along its velocity direction, according to the repulsion field The force relation formula of the object in

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; )</span>\stackrel{<span\; class="notranslate">\; \&Right\; Arrow;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; ></span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right.$

Calculate the virtual repulsion F _γ of the obstacle suffered by the robot, where k is the scaling factor, ρ is the distance between the end of the robot and the background environment or obstacles along its velocity direction, and ρ ₀ is the set distance between the end of the robot and the obstacle or environment safe distance between is the unit vector of the robot’s terminal motion velocity;

7) In order to keep the work object in the image always near the center of the camera field of view, the image-based virtual servo force is constructed using the image feature point coordinates of the work object as parameters.

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; S</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; m</span>}{\mathrm{<span\; class="notranslate">\; L</span>}}^{\text{<span class="notranslate"> +</span>}}{\stackrel{<span\; class="notranslate">\; \&Right\; Arrow;</span>}{<span\; class="notranslate">\; \&dtri;</span>}}_{\mathrm{<span\; class="notranslate">\; S</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}\mathrm{<span\; class="notranslate">\; V</span>}$

Among them, V is the image-based repulsion field potential energy function, is the transposition of the gradient vector of the potential function V at s, s = (u, v) is the feature point of the current image, s ^* = (u ^* , v ^* ) is the expected feature point of the image, M is the movement speed of the hand camera and The Jacobian matrix between the robot end pose changes, L ⁺ = (L ^T L) ^-1 L ^T is the image Jacobian generalized inverse matrix between the motion speed of the image feature point of the work object and the motion speed of the hand camera;

3. Synthesize the virtual attraction, repulsion, and image servo force of the target object and obstacles obtained in the above method 2 on the end of the robot, and map them to the motion joints of the robot hand controller:

8) Synthesize the gravitational force of the work object, the repulsive force of obstacles, and the image servo force into

$\mathrm{<span\; class="notranslate">\; f</span>}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&alpha;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&zeta;</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&beta;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; f</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\\ \mathrm{<span\; class="notranslate">\; m</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\end{array}\right]$

In the formula, α, β, γ are the influence coefficients of different repulsive forces, ^H F _T , ^H M _T are force and moment vectors, respectively;

9) Map the virtual guiding force in the synthesized robot workspace to each joint of the hand controller, and calculate the virtual guiding force/torque τ _q in the joint coordinate space of the hand controller as:

${\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>{\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; R</span>}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; f</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\\ \mathrm{<span\; class="notranslate">\; R</span>}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; m</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\end{array}\right]$

In the formula, is the rotation matrix of the coordinate system of the robot end effector relative to the coordinate system of the hand controller, and J is the Jacobian matrix of the hand controller.

4. Construct the control model of the hand controller with the mass-damping system, and apply the virtual torque obtained in the above step 3 to the "predictive" control of the hand controller's pose:

10) Each joint of the hand controller will produce the expected motion trend q _D under the action of the virtual force/torque τ _q , the joints of the hand controller are simplified as a mass-damping system, according to the dynamic model formula of the hand controller control system

$\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}$

Design the position controller for the hand control.

11) Use the position prediction of the hand controller to control the robot to generate corresponding follow-up, and realize the robot movement controlled by the hand controller under the guidance of the virtual force guided by the target object.

The fusion of the operator's manipulation force described in step c and the virtual guidance force of the target object is realized in the following manner:

1) With a reasonable weight coefficient, the virtual force/torque τ _q at each joint of the hand controller, the manipulation force F _h , and the contact force F _e between the robot and the environment are jointly applied to the mass-damping control system model of the hand controller , the hand controller will produce the desired movement trend q _D , according to the formula of the dynamic model of the hand controller control system

$\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; )</span>$

Design the position controller for the hand control. k ₁ and k ₂ are the weight coefficients of the autonomous control of the hand controller and the manual control of the operator, respectively.

2) Guided by virtual force for path planning, the robot can quickly approach the target object in the initial stage. In the final control stage, the operator's control force on the hand controller is the decisive role to realize the real-time adjustment of the robot's pose, and finally achieve the goal of the robot. Shared control under the joint action of intelligence and human intelligence.

Compared with the prior art, the present invention has the following advantages and technical effects:

1. Propose a new man-machine shared control method that integrates the visual cognition results of the environment with the operator's manipulation force in the form of virtual force, so as to realize the organic integration of robot autonomous task planning and manual control;

2. Reduce the requirements for the proficiency of the operator, and realize the navigation of the robot to the manual operation to a certain extent;

3. On the basis of automatic navigation approaching the target object, the operator can be assisted by manual adjustment to ensure a safer and more accurate operation process;

4. Predicting possible collisions between the robot and the environment can effectively ensure the safety of the robot operating system.

Description of drawings

Fig. 1 is a composition diagram of the method control system of the present invention.

Detailed ways

The present invention will be further described in detail below in conjunction with the embodiments and accompanying drawings, but the embodiment of the present invention is not limited to this embodiment.

Referring to Fig. 1, a hand controller sharing control method based on force fusion includes the following steps:

a. Use the hand controller to operate the terminal, take the target object as the guide, and realize the manual manipulation force signal to control the movement of the robot through the operator's own intelligent decision-making, and realize it in the following specific ways:

1. Build a robot motion control platform based on force closed loop;

2. The computer collects the manipulative force signal and transmits the data to prepare for the system to control the movement of the hand controller;

3. Based on the mass-damping system control strategy, according to the dynamic model formula of the hand controller control system

$\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; )</span>$

The motion of the hand controller is controlled in real time. In the formula, M and B are the inertia matrix and damping matrix of the hand controller respectively, J is the Jacobian matrix of the hand controller, F _h is the manipulation force applied by the operator, and F _e is the contact force between the robot and the environment, and q _D is the expected motion pose of the hand controller;

4. Transmit the motion position and posture signals of the hand controller to the motion joints of the robot, so that the robot motion is synchronized with the motion of the hand controller, guided by the target object, and realize the human manipulation force signal to control the robot motion through the operator's own intelligent decision-making .

b. Obtain the image data of the operation scene through stereo vision technology and reconstruct the three-dimensional environment in real time, and construct a virtual guiding force (including attraction, repulsion, and image servo force) that guides the robot to approach the target object guided by the target object, and guide the robot with this virtual guiding force The hand controller controls the movement of the robot;

Implemented as follows:

1. Obtain the image data of the operation scene through stereo vision technology and reconstruct the 3D environment in real time, and calculate the expected pose of the robot under the guidance of the target object. The method is as follows:

1) Obtain and calculate the spatial coordinates of each pixel in the background image in real time by the environment camera, and then generate the elevation data model of the background environment (obstacles);

2) According to the color and texture features of each pixel in the image acquired by the hand camera, the job object area is extracted, and then the job object image features are constructed;

3) According to the results of image matching and camera calibration, calculate the spatial coordinates of each pixel in the work object area;

4) Construct the bounding box model of the operation object, and use the axis direction of the OBB bounding box of the operation object obtained by visual calculation as the expected pose direction to calculate the expected pose of the robot end;

2. Calculate the virtual attraction, repulsion, and image servo force of the operation object and obstacles on the end of the robot through the acquired image data of the operation scene and the reconstructed 3D environment:

5) Taking the relative amount of the current pose of the robot end and the expected pose as a parameter, based on the obtained characteristics of the work object and the spatial coordinates of each pixel, according to the force relationship of the object in the gravitational field

F _ζ = -α(γ-γ _* )

Calculate the virtual gravitational force F _ζ of the work object suffered by the robot, where α is the gravitational coefficient, γ and γ* are the current pose and expected pose vector of the end of the robot, respectively;

6) Taking the relative amount of the current pose of the robot end and the expected pose as a parameter, combined with the background environment (or obstacle) elevation information measured by the environmental camera, to solve the distance between the end of the robot and the background environment along its velocity direction, according to the repulsion field The force relation formula of the object in

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; )</span>\stackrel{<span\; class="notranslate">\; \&Right\; Arrow;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; ></span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right.$

Calculate the virtual repulsion F _γ of the obstacle suffered by the robot, where k is the scaling factor, ρ is the distance between the end of the robot and the background environment or obstacles along its velocity direction, and ρ ₀ is the set distance between the end of the robot and the obstacle or environment safe distance between is the unit vector of the robot’s terminal motion velocity;

7) In order to keep the work object in the image always near the center of the camera field of view, the image-based virtual servo force is constructed using the image feature point coordinates of the work object as parameters.

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; S</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; m</span>}{\mathrm{<span\; class="notranslate">\; L</span>}}^{\text{<span class="notranslate"> +</span>}}{\stackrel{<span\; class="notranslate">\; \&Right\; Arrow;</span>}{<span\; class="notranslate">\; \&dtri;</span>}}_{\mathrm{<span\; class="notranslate">\; S</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}\mathrm{<span\; class="notranslate">\; V</span>}$

Among them, V is the image-based repulsion field potential energy function, is the transposition of the gradient vector of the potential function V at s, s = (u, v) is the feature point of the current image, s ^* = (u ^* , v ^* ) is the expected feature point of the image, M is the movement speed of the hand camera and The Jacobian matrix between the robot end pose changes, L ⁺ = (L ^T L) ^-1 L ^T is the image Jacobian generalized inverse matrix between the motion speed of the image feature point of the work object and the motion speed of the hand camera;

3. Synthesize the virtual attractive force, repulsive force, and image servo force of the target object and obstacles obtained in the above method 2 on the end of the robot according to their respective influences, and map them to the motion joints of the robot hand controller:

8) Synthesize the gravitational force of the work object, the repulsive force of obstacles, and the image servo force into

$\mathrm{<span\; class="notranslate">\; f</span>}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&alpha;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&zeta;</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&beta;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; f</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\\ \mathrm{<span\; class="notranslate">\; m</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\end{array}\right]$

In the formula, α, β, γ are the influence coefficients of different repulsive forces, ^H F _T , ^H M _T are force and moment vectors, respectively;

9) Map the virtual guiding force in the synthesized robot workspace to each joint of the hand controller, and calculate the virtual guiding force/torque τ _q in the joint coordinate space of the hand controller as:

${\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>{\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; R</span>}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\mathrm{<span\; class="notranslate">\; f</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\\ \mathrm{<span\; class="notranslate">\; R</span>}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; \&CenterDot;</span>\mathrm{<span\; class="notranslate">\; m</span>}_{\mathrm{<span\; class="notranslate">\; T</span>}}^{\mathrm{<span\; class="notranslate">\; h</span>}}\end{array}\right]$

In the formula, is the rotation matrix of the coordinate system of the robot end effector relative to the coordinate system of the hand controller, and J is the Jacobian matrix of the hand controller.

4. Construct the control model of the hand controller with the mass-damping system, and apply the virtual torque obtained in the above step 3 to the "predictive" control of the hand controller's pose:

10) Each joint of the hand controller will produce the expected motion trend q _D under the action of the virtual force/torque τ _q , the joints of the hand controller are simplified as a mass-damping system, according to the dynamic model formula of the hand controller control system

$\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; \&Center\; Dot;</span>{\stackrel{<span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}$

Design the position controller for the hand control.

11) Use the position prediction of the hand controller to control the robot to generate corresponding follow-up, and realize the robot movement controlled by the hand controller under the guidance of the virtual force guided by the target object.

c. Fuse the operator's manipulation force described in step a with the virtual guidance force of the target object described in step b, and control the motion of the hand controller and robot through the fusion force to realize the control under the joint action of machine intelligence and human intelligence The robot approaches the target object and completes the operation process; it is realized by the following methods:

1) With a reasonable weight coefficient, the virtual force/torque τ _q at each joint of the hand controller, the manipulation force F _h , and the contact force F _e between the robot and the environment are jointly applied to the mass-damping control system model of the hand controller , the hand controller will produce the desired movement trend q _D , according to the formula of the dynamic model of the hand controller control system

$\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; \&CenterDot;</span>{\stackrel{<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; \&CenterDot;</span>{\stackrel{<span\; class="notranslate">\; .</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; D.</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; k</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; h</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; )</span>$

Design the position controller for the hand control. k ₁ and k ₂ are the weight coefficients of the autonomous control of the hand controller and the manual control of the operator, respectively.

2) Guided by virtual force for path planning, the robot can quickly approach the target object in the initial stage. In the final control stage, the operator's control force on the hand controller is the decisive role to realize the real-time adjustment of the robot's pose, and finally achieve the goal of the robot. Shared control under the joint action of intelligence and human intelligence.

The above-mentioned embodiment is a preferred implementation mode of the present invention, but the implementation mode of the present invention is not limited by the above-mentioned embodiment, except any changes, modifications, substitutions and combinations made without departing from the spirit and principle of the present invention , simplification, all should be equivalent replacement methods, and are all included in the protection scope of the present invention.

Claims (3)

1. A hand controller sharing control method based on force fusion, is characterized in that, comprises the following steps: a. Use the hand controller to operate the terminal, take the target object as the guide, and realize the manual manipulation force signal to control the movement of the robot through the operator's own intelligent decision-making, and realize it in the following specific ways: 1. Build a robot motion control platform based on force closed loop; 2. The computer collects the manipulative force signal and transmits the data to prepare for the system to control the movement of the hand controller; 3. Based on the mass-damping system control strategy, according to the dynamic model formula of the hand controller control system The motion of the hand controller is controlled in real time. In the formula, M and B are the inertia matrix and damping matrix of the hand controller respectively, J is the Jacobian matrix of the hand controller, F _h is the manipulation force applied by the operator, and F _e is the contact force between the robot and the environment, and q _D is the expected motion pose of the hand controller; 4. Transmit the motion position and posture signals of the hand controller to the motion joints of the robot, so that the robot motion is synchronized with the motion of the hand controller, guided by the target object, and realize the human manipulation force signal to control the robot motion through the operator's own intelligent decision-making ; b. Obtain the image data of the operation scene through stereo vision technology and reconstruct the three-dimensional environment in real time, and construct a virtual guiding force that guides the robot to approach the target object guided by the target object, so as to realize the local autonomous control of the robot through the virtual guiding force; c. Fuse the operator's manual control force described in step a with the virtual guidance force of the target object described in step b, and control the movement of the hand controller and robot through the fusion force to realize the joint action of machine intelligence and human intelligence. Control the robot to approach the target object and complete the operation process. 2. The hand controller sharing control method based on force fusion according to claim 1, characterized in that, in the step b, the image data of the operation scene is obtained through stereo vision technology and the three-dimensional environment is reconstructed in real time, and the construction is guided by the target object Guide the robot to approach the virtual guiding force of the target object, and use this virtual force to guide the hand controller to control the robot's movement, as follows: 1. Obtain the image data of the operation scene through stereo vision technology and reconstruct the 3D environment in real time, and calculate the expected pose of the robot under the guidance of the target object. The method is as follows: 1) Obtain and calculate the spatial coordinates of each pixel in the background image in real time by the environment camera, and then generate the elevation data model of the background environment; 2) According to the color and texture features of each pixel in the image acquired by the hand camera, the job object area is extracted, and then the job object image features are constructed; 3) According to the results of image matching and camera calibration, calculate the spatial coordinates of each pixel in the work object area; 4) Construct the bounding box model of the operation object, and use the axis direction of the OBB bounding box of the operation object obtained by visual calculation as the expected pose direction to calculate the expected pose of the robot end; 2. Calculate the virtual attraction, repulsion, and image servo force of the operation object and obstacles on the end of the robot through the acquired image data of the operation scene and the reconstructed 3D environment: 1) Taking the relative amount of the current pose and the expected pose of the robot end as a parameter, based on the obtained characteristics of the work object and the spatial coordinates of each pixel, according to the force relationship formula of the object in the gravitational field F _ζ = -α(γ-γ _* ) Calculate the virtual gravitational force F _ζ of the work object suffered by the robot, where α is the gravitational coefficient, γ and γ* are the current pose and expected pose vector of the end of the robot, respectively; 2) Taking the relative amount of the current pose of the robot end and the expected pose as a parameter, combined with the background environment elevation information measured by the environmental camera, to solve the distance between the end of the robot and the background environment along its velocity direction, according to the force of the object in the repulsive force field relational formula ${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; )</span>\stackrel{<span\; class="notranslate">\; \&Right\; Arrow;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; ,</span>& \mathrm{<span\; class="notranslate">\; \&rho;</span>}<span\; class="notranslate">\; ></span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right.$ Calculate the virtual repulsion F _γ of the obstacle suffered by the robot, where k is the scaling factor, ρ is the distance between the end of the robot and the background environment or obstacles along its velocity direction, and ρ ₀ is the set distance between the end of the robot and the obstacle or environment safe distance between is the unit vector of the robot’s terminal motion velocity; 3) In order to keep the work object in the image always near the center of the camera field of view, the image-based virtual servo force is constructed using the image feature point coordinates of the work object as parameters. ${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; S</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; ML</span>}}^{<span\; class="notranslate">\; +</span>}{\stackrel{<span\; class="notranslate">\; \&Right\; Arrow;</span>}{<span\; class="notranslate">\; \&dtri;</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}\mathrm{<span\; class="notranslate">\; V</span>}$ Among them, V is the image-based repulsion field potential energy function, is the transposition of the gradient vector of the potential function V at s, s = (u, v) is the feature point of the current image, s ^* = (u ^* , v ^* ) is the expected feature point of the image, M is the movement speed of the hand camera and The Jacobian matrix between the robot end pose changes, L ⁺ = (L ^T L) ^-1 L ^T is the image Jacobian generalized inverse matrix between the motion speed of the image feature point of the work object and the motion speed of the hand camera; 3. Synthesize the virtual attraction, repulsion, and image servo force of the target object and obstacles obtained in the above method 2 on the end of the robot, and map them to the motion joints of the robot hand controller: 1) Synthesize the gravitational force of the work object, the repulsive force of obstacles, and the image servo force into ${}^{\mathrm{<span\; class="notranslate">\; h</span>}}\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&alpha;F</span>}}_{\mathrm{<span\; class="notranslate">\; \&zeta;</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&beta;F</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&gamma;F</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{}^{\mathrm{<span\; class="notranslate">\; h</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; T</span>}}\\ {}^{\mathrm{<span\; class="notranslate">\; h</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; T</span>}}\end{array}\right]$ In the formula, α, β, γ are the influence coefficients of different repulsive forces, ^H F _T , ^H M _T are force and moment vectors, respectively; 2) Map the virtual guiding force in the synthesized robot workspace to each joint of the hand controller, and calculate the virtual guiding force/torque τ _q in the joint coordinate space of the hand controller as: ${\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>{\left[\begin{array}{cccc}{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; \&tau;</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; J</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}\left[\begin{array}{c}{}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}\mathrm{<span\; class="notranslate">\; R</span>}{<span\; class="notranslate">\; \&CenterDot;</span>}^{\mathrm{<span\; class="notranslate">\; h</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; I</span>}}\\ {}_{\mathrm{<span\; class="notranslate">\; h</span>}}^{\mathrm{<span\; class="notranslate">\; m</span>}}\mathrm{<span\; class="notranslate">\; R</span>}{<span\; class="notranslate">\; \&CenterDot;</span>}^{\mathrm{<span\; class="notranslate">\; h</span>}}{\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; T</span>}}\end{array}\right]$ In the formula, is the rotation matrix of the coordinate system of the robot end effector relative to the coordinate system of the hand controller, and J is the Jacobian matrix of the hand controller; 4. Construct the control model of the hand controller with the mass-damping system, and apply the virtual torque obtained in the above step 3 to the "predictive" control of the hand controller's pose: 1) Under the action of the virtual force/torque τ _q , each joint of the hand controller will produce the expected motion trend q _D , the joints of the hand controller are simplified as a mass-damping system, according to the formula of the dynamic model of the hand controller control system Design the position controller of the hand controller; 2) The position of the hand controller is used to predict and control the robot to generate corresponding follow-up, so as to realize the movement of the robot controlled by the hand controller under the guidance of the virtual force guided by the target object. 3. The hand controller sharing control method based on force fusion according to claim 1, wherein the fusion of the operator's manipulation force described in step c and the virtual guiding force of the target object is realized in the following way: 1) With a reasonable weight coefficient, the virtual force/torque τ _q at each joint of the hand controller, the manipulation force F _h , and the contact force F _e between the robot and the environment are jointly applied to the mass-damping control system model of the hand controller , the hand controller will produce the desired movement trend q _D , according to the formula of the dynamic model of the hand controller control system Design the position controller of the hand controller; k ₁ and k ₂ are the weight coefficients of the hand controller's autonomous control and the operator's manual control respectively; 2) Guided by virtual force for path planning, the robot can quickly approach the target object in the initial stage. In the final control stage, the operator's control force on the hand controller is the decisive role to realize the real-time adjustment of the robot's pose, and finally achieve the goal of the robot. Shared control under the joint action of intelligence and human intelligence.