CN110825076B - Semi-autonomous control method for mobile robot formation navigation based on line of sight and force feedback - Google Patents

Abstract

The invention discloses a mobile robot formation navigation semi-autonomous control method based on line of sight and force feedback; the system consists of three parts: a master end, a slave end and a communication link. The master side contains an operator, an eye tracker, a hand control and a control computer. The slave side includes multiple mobile robot systems, cameras and work environments. The master and slave communicate wirelessly through WiFi. The multi-mobile robot system from the end has semi-autonomous control capability, and uses virtual rigid body algorithm to realize automatic obstacle avoidance and formation maintenance of multi-mobile robots. The master end uses the eye tracker to capture the operator's line of sight signal and converts it into the formation switching command of the slave end, and combines the three degrees of freedom of the end controller of the hand controller to perform remote intervention on the slave end. The control method combines force feedback and gaze tracking, and applies gaze tracking to the control of multiple mobile robots, which reduces the cognitive load of the operator and improves the efficiency and stability of the teleoperation control system.

Description

Semi-autonomous control method for mobile robot formation navigation based on line of sight and force feedback

technical field

The invention relates to a semi-autonomous control method for formation navigation of mobile robots based on line of sight and force feedback.

Background technique

Multi-robot systems have the advantages of good redundancy, robustness, and scalability. They have been widely studied in recent years and are used in large-scale reconnaissance detection, security inspections, and search and rescue tasks. However, these environments are often complex and changeable, and it is difficult to implement a fully autonomous control method for multiple mobile robots. Therefore, a more feasible and effective method is to combine with teleoperation technology in those places that are difficult for humans to approach or that will cause harm to humans. Perform tasks in complex environments.

In order to cope with complex and changeable environments, multi-robots need to switch formations when performing tasks. In the existing technology, two methods are mainly used to realize the formation switching: one is to use the position of the terminal HIP of the human-computer interaction device to give different target formation commands; The environment automatically realizes the switching of formations. For the first method, since the human-computer interaction equipment also needs to control the target speed of the slave robot, and the conversion of position information into target formation commands is not intuitive enough, the operator needs to be highly alert at all times. This method increases the cognitive load of the operator, which is inefficient and prone to fatigue. For the second method, it is difficult to implement the algorithm, and the system is prone to instability.

Contents of the invention

The purpose of the present invention is to introduce line-of-sight tracking into the main-end control loop of multiple mobile robots, combined with the feedback of force information, so that multiple mobile robots can accurately and timely complete formation while avoiding obstacles according to different environments during formation navigation. The shape switching reaches the target position smoothly.

The technical solution adopted by the present invention is: a semi-autonomous control method for mobile robot formation navigation based on line of sight and force feedback, which is composed of a master end, a slave end and a communication link; the master end includes an operator, a visual tracking device, and a force feedback man-machine interface equipment and control computer; the slave end includes multiple mobile robots, cameras and working environment; the communication link adopts WiFi or other wireless communication methods;

The operator at the master terminal interacts with the visual tracking device and the force feedback human-machine interface device, and sends the control command to the multi-mobile robot system at the slave-end through the communication link; the multi-mobile robot system includes a multi-robot system composed of n mobile robots. mobile platform system;

The vision tracking device is used to capture the visual signal of the operator, and convert this information into the target formation control instruction of the multi-mobile robot at the slave end; the multi-mobile system at the slave end forms an expected formation according to the received instruction.

The force feedback human-machine interface device has output feedback with three degrees of freedom, and is used to feed back the state of the multi-robot system from the slave end to the master end in the form of force signals;

The control computer uses a visual interface to pre-define the target formation of the multi-mobile platform at the slave end, and displays it on the interface in the form of a picture, and uses a visual tracking device to capture the operator's eye movement information, and according to the graphic that the operator is looking at , to form a corresponding click event; the slave robot makes a corresponding formation switch.

The camera is used to feed back the status of the multi-robots and the slave-end environment to the master-end in the form of video or pictures.

According to the graphics that the operator is looking at, the multi-robot system uses a virtual rigid body algorithm for formation control, and the multi-robot system at the slave end is regarded as a whole, in which each robot automatically realizes formation maintenance and automatic obstacle avoidance, through this In this way, the operator can focus on the overall control of the slave robot group without worrying about whether a single robot will encounter obstacles.

The pose between each robot and the adjacent robot is constant and will not change with the movement of the virtual rigid body. When the virtual rigid body perceives an obstacle or receives an instruction to switch formation, the vector in the virtual rigid body The relative pose between them will change accordingly and reappear as another rigid body; in this way, formation maintenance and automatic obstacle avoidance can be realized automatically.

The further improvement of the present invention is that: the force feedback human-machine interface device has output feedback with three degrees of freedom, wherein the x direction and z direction reflect the distance and angle information between the robot and the obstacle, and the output quantity in the y direction reflects the entire Formation size for multiple mobile platform systems.

For the force feedback man-machine interface equipment of control multi-robot system, utilize these two degrees of freedom of x and z of its end effector to control respectively the angular velocity and the linear velocity of the virtual rigid body in the multi-robot system from the end; Multi-robot system needs in the present invention Eventually reach the designated location, and avoid all obstacles as much as possible during the entire movement. Although the virtual rigid body algorithm adopted can realize automatic obstacle avoidance, since the algorithm has the same deadlock problem as the artificial potential field method, force feedback is used to assist obstacle avoidance.

Therefore, the output x direction of the force feedback human-machine interface device corresponds to the angular velocity information and the distance and angle information between the robot and the obstacle, and the z direction corresponds to the linear velocity information and the distance and angle information between the robot and the obstacle. The feedback force and the robot The linear velocity is related to the angular velocity, so that the robot can keep approaching and finally reach the target position.

A further improvement of the present invention is that: the force feedback man-machine interface device adopts a hand controller.

A further improvement of the present invention is that: the vision tracking device adopts an eye tracker.

Adopting the technical solution of the present invention will have the following advantages and beneficial effects:

(1) The solution of the present invention realizes the formation switching of the slave robot system by capturing the line-of-sight signal of the operator, and at the same time utilizes the remaining degrees of freedom of the end controller of the hand controller to control the size of the formation and increase the flexibility of the predefined formation It reduces the operator's cognitive load, avoids the difficulty of realizing local autonomy, and improves the efficiency and stability of the teleoperation control system.

(2) In the present invention, the multi-mobile platform at the slave end adopts a virtual rigid body algorithm, and the multi-robot system at the slave end is regarded as a whole, wherein each robot in the virtual rigid body can automatically realize formation maintenance and obstacle avoidance. It is convenient for the operator to focus on the centralized control of multiple mobile robot platforms.

(3) The solution of the present invention applies line-of-sight tracking to the control loop of the multi-mobile robot system. In this way, when the multi-mobile robot system encounters complex tasks, it can rely on human's advanced cognition and decision-making ability to give reasonable control.

Description of drawings

Fig. 1 is the system framework of the multi-mobile robot formation navigation based on force feedback and line-of-sight tracking in the present invention.

Fig. 2 is a schematic diagram of a high-level task control in which the main terminal of the present invention combines gaze tracking and force feedback.

Fig. 3 is a schematic diagram of the principle of multi-mobile robots completing formation switching tasks based on the virtual rigid body algorithm in the present invention.

Fig. 4 is a control block diagram of a multi-mobile robot platform based on force feedback and line of sight tracking in the present invention.

Figure 5 is a diagram of the formation switching process of multiple mobile robots based on line-of-sight tracking.

Figure 6 shows the actual linear velocity and angular velocity of robot1.

Figure 7 shows the error of robot1.

detailed description

The working principle and working process of the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments.

Referring to Fig. 1, the mobile robot formation navigation semi-autonomous control system based on force feedback and line of sight tracking includes a master terminal 1, a communication link 2 and a slave terminal 3 between the master and slave terminals. Wherein the main terminal 1 includes an operator 1-1, a computer control platform 1-2, a finger controller 1-3 and an eye tracker 1-4, and the finger controller 1-3 of the system has six degrees of freedom input and Three degrees of freedom output;

The communication link 2 adopts the Internet wireless communication mode; the slave end 3 includes the multi-mobile robot system, the obstacles 3-2 in the environment, the target position 3-3 to be reached by the multi-robot system, and the work of the slave-end multi-robot system The status is fed back to the camera 3-4 on the master side.

The multi-mobile robot system includes a multi-mobile platform system 3-1 composed of n mobile robots 3-1-i (i=1, 2,...,n), and the camera 3-4 is mounted on the drone , can move with the movement of the multi-robot system, expanding the range of activities of the multi-robot system.

The computer control platform 1-2 can issue control instructions to the multi-mobile robot system at the slave end through the communication link 2, and at the same time feed back the information collected by the sensor to the operator 1-1 in the form of text or images. The video information collected by the camera 3-4 is displayed in real time, so that the operator 1-1 can grasp the working status of the slave-end multi-mobile robot system platform.

The operator at the master end interacts with the human-machine interface device 1-3 and the eye tracker 1-4, and sends the control command to the multi-mobile platform system 3-1 at the slave end through the communication link 2, and the eye tracker 1-4 captures The eye movement information of the operator at the master end obtains the click instruction to switch formations, and the multi-mobile platform 3-1 of the slave end 3 forms a target formation according to the instructions it receives. After receiving the advanced task instructions issued by the operator , the multi-mobile platform 3-1 automatically completes formation maintenance and obstacle avoidance according to the virtual rigid body algorithm.

In the multi-mobile platform system 3-1 at the slave end of the system, each mobile robot is equipped with sensors capable of obtaining its own position and posture information, as well as relative information such as relative distances and relative angles to obstacles.

According to the virtual rigid body algorithm, the multi-robots at the slave end are regarded as a whole, and the pose between each robot and the adjacent robot is constant, which is analogous to two vectors in a rigid body, which will not follow the movement of the rigid body and change, but different from the real rigid body, when the virtual rigid body perceives obstacles or receives formation switching instructions, the relative pose between the vectors in the virtual rigid body will change accordingly; Appears in another form, using this method to automatically realize formation maintenance and automatic obstacle avoidance. At the same time, the operator can focus on the overall control of the slave robot group without worrying whether a single robot will encounter obstacles.

Referring to Figure 2, using a visual interface, the required formation is set in advance according to the working environment that the slave multi-mobile platform may face. The eye tracker captures the operator's visual information to determine the area the operator is looking at, thereby obtaining the click stimulus. Get the target formation command, combined with the force feedback interface device, and finally determine the advanced control commands of the main end, such as formation, formation scale, target speed, etc.

For the position P(x, y, z) of the end controller of the hand controller 1-3, in this embodiment, the x-coordinate axis represents the angular velocity of the virtual rigid body, the positive direction of the x-axis is turning right, and the negative direction of the x-axis is turning left ; The z coordinate axis represents the linear velocity of the virtual rigid body, the positive direction of the z axis is backward, and the negative direction of the z axis is forward; the y coordinate axis is used to control the size of the virtual rigid body; that is, the size of the formation formed by the multi-mobile robot platform; Unexpected control commands caused by operator control instability such as hand shaking.

The expected formation for a mobile robot is defined as follows:

T＝[L ^d ,Φ ^d ]

in

共有m种队形，队形的大小由人机接口设备末端控制器的y坐标轴定义，为了防止由于操作员的抖动引起不需要的运动和意外事件，将y分割区域[y_M1,y_M2,…,y_Mm]，每个区域对应一种尺度，队形的大小主要根据改变相对距离来实现。则主端的眼动仪1-4和力反馈人机接口设备1-3与从端多移动平台系统3-1之间的对应关系为：L ^d and Φ ^d represent the relative distance and relative angle matrix between each mobile robot, respectively, and the two together determine the formation of the multi-mobile robot system

There are m types of formations, and the size of the formation is defined by the y-coordinate axis of the terminal controller of the human-machine interface device. In order to prevent unwanted movements and accidents caused by the operator's shaking, the y-divided area [y _M1 , y _M2 ,…,y _Mm ], each area corresponds to a scale, and the size of the formation is mainly realized by changing the relative distance. Then the corresponding relationship between the eye tracker 1-4 at the master end, the force feedback man-machine interface device 1-3 and the multi-mobile platform system 3-1 at the slave end is:

where v and w respectively represent the linear velocity and angular velocity of the virtual rigid body VRB in the multi-mobile robot system from the end; k _v , k _ω , k _T and k _S are the gain coefficients of linear velocity, angular velocity, formation and formation size, respectively ; [q _x ,q _y ,q _z ] ^T represents the position coordinates [x _M ,y _M ,z _M ] of the end controller of the force feedback human-machine interface device, and q _S is based on the position coordinates of the operator captured by the eye tracker The click stimulus of the target formation obtained by the line of sight is obtained.

Referring to Fig. 3, the virtual rigid body algorithm is defined as follows.

Definition 1: For {1,2,...,N} a group of mobile robots labeled N, let F _i denote the local reference frame of robot i, let r denote the position, and R _i (t)∈SO(3 ) represents the position of robot i relative to F _w within time t.

Definition 2 (virtual rigid body): The virtual rigid body consists of a group of mobile robots whose number is N and a local reference frame coordinate system _Fv . The local position of the robot is specified by a set of time-varying vectors {r ₁ (t), r ₁ (t), . . . , R _N (t)}.

Definition 3 (Formation): Formation Π is a virtual rigid body with a constant local position {r ₁ , r ₂ , ..., r _N } in F _v for a group of robots of size N for duration T _Π >0.

Definition 4 (Transformation): Transformation Φ is a virtual rigid body with a time-varying position {r ₁ (t), r ₁ (t), ..., R _N (t)} relative to the local reference frame F _v , such that A group of mobile robots whose number is N, in duration T _Φ > 0, P _v (t) ∈ R ₃ and R _v (t) ∈ SO(3) denote the origin of F _v in F _w at time t position and direction. The relationship between p _i (t) and r _i (t) of robot i is p _i =p _v +R _V *r _i , i∈{1,2,...,N}.

On the basis of the above definition, the repulsion vector from the position of the obstacle to the VRB is defined at the two-dimensional coordinate p, and its size is a Gaussian function related to the position and radius of the obstacle. It is assumed that there are n Obstacle group, the horizontal position of obstacle k in the global coordinate system F _w is expressed as o _{w, k} , then the overall repulsion vector in the vector field generated from n obstacles at two-dimensional coordinate p

in

where B _k is a positive scalar parameter related to the radius r _k of obstacle k. The value of B _k can be chosen such that the command velocity _v of VRB can still overcome the largest repelling vector, because VRB is virtual and the 2×2 matrix Σ is positive definite, which defines the long axis of the dynamic Gaussian-like function by and minor axis.

The virtual rigid body algorithm defines a stronger vector field than VRB for a single mobile robot, so that when the mobile robot approaches an obstacle, the repulsive vector can "push it away". away from obstacles.

The above formulas are expressed in the global coordinate system _Fw , while in the local coordinate system _Fv they are expressed as

In the present invention, it is the simulation result of the formation switching of the multi-mobile platform based on the line-of-sight tracking of the master-slave end. In this simulation experiment, four mobile robots first complete the parallelogram formation, and then keep the formation and do linear motion for a period of time. When the operator's gaze turns to the linear button on the computer interface, a click event is triggered, and the multi-robot system at the slave end receives a formation switching command. According to the virtual rigid body algorithm, the relative pose of VRB and each robot changes. Thus completing the transformation of formation. The initial pose and formation process of the robot and its corresponding motion trajectory are shown in Figure 5, which is the formation switching process of multiple mobile robots based on line-of-sight tracking.

Among them, the circle represents the virtual rigid body VRB, and the triangle represents the actual mobile robot. The dotted lines represent the relative positions of VRB and each mobile robot, and the solid lines represent the relative positions between mobile robots. Since the four robots have the same structure, only robot 1 is analyzed here.

As shown in Figure 7; the actual linear velocity and angular velocity error of robot1;

The error between the actual pose and the target pose of mobile robot 1 robot1.xe, robot1.ye, robot1.thetae;

As shown in Figure 6, the actual linear velocity and angular velocity of robot1. It can be seen from the figure that when the formation is changed, the speed of the robot changes suddenly, and the error increases, and then the error converges to a smaller value after 10s.

Referring to Figure 4, the control block diagram of a multi-mobile robot platform based on force feedback and line-of-sight tracking. The operator applies force F _h to the hand controller, and obtains the position information P _M (x _M , y _M , z _M ) of the end controller of the hand controller, which is converted into the line of the virtual rigid body VRB at the slave end through the processing of the corresponding controller Velocity v _Ml , angular velocity ω _Ml and corresponding formation shape T and size S. The control information enters the communication link and is sent to the slave multi-robot platform. The delay problem of the communication link is solved by passive processing method. The multi-mobile platforms at the slave end change their respective positions according to the virtual rigid body algorithm under the command of the high-level task. When interacting with the environment from multiple mobile platforms, obstacles provide environmental reactions to the mobile robot swarm, which are related to the linear velocity and angular velocity of each robot and the formation formed by the entire robot swarm. Feedback to the main end through the communication link, and get the feedback force (F _x , F _y , F _z ) in each direction.

Among them, the force in three directions of the force feedback device is defined as follows:

Claims (1)

1.the semi-autonomous control method for formation navigation of mobile robots based on sight line and force feedback is characterized by comprising the following steps: the system consists of a master end, a slave end and a communication link; the main end comprises an operator, a visual tracking device, a force feedback human-computer interface device and a control computer; the slave end comprises a multi-mobile robot system, a camera and a working environment; the communication link adopts WiFi or other wireless communication modes;

the operator at the master end interacts with the force feedback human-computer interface equipment through the visual tracking equipment and sends a control command to the multi-mobile-robot system at the slave end through a communication link;

the multi-mobile robot system comprises a multi-mobile platform system consisting of n mobile robots;

the visual tracking equipment is used for capturing eye movement information of an operator and converting the information into a target formation control instruction of the slave multi-mobile robot; the multi-mobile robot system at the slave end forms an expected formation according to the received instruction;

the force feedback man-machine interface equipment has three-degree-of-freedom output feedback and is used for feeding back the state of the slave-end multi-robot system to the master end in a force signal mode;

the speed and position information of the multi-mobile robot system is fed back to the main end in real time in the form of text or video through the control computer; the control computer predefines a target formation of the slave-end multi-mobile platform by using a visual interface, displays the target formation on the interface in the form of pictures, captures eye movement information of an operator by using visual tracking equipment, and forms a corresponding click event according to a figure watched by the operator; the camera is used for feeding back the state of the multiple robots and the slave-end environment to the main end in a video or picture mode; according to a figure watched by an operator, the multi-mobile robot system at the slave end adopts a virtual rigid body algorithm, the multi-robot system at the slave end is regarded as a whole, and each robot automatically realizes formation maintenance and automatic obstacle avoidance; the state of the multiple robots is constant between each robot and the adjacent robot, and does not change along with the movement of the virtual rigid bodies, when the virtual rigid bodies detect obstacles or receive instructions of queue form switching, the relative pose between the vectors in the virtual rigid bodies changes correspondingly and reappears with the other rigid bodies; the formation keeping and the automatic obstacle avoidance are automatically realized in the mode; the force feedback man-machine interface equipment has three-degree-of-freedom output feedback, wherein the x direction and the z direction reflect the distance and angle information between the robot and the obstacle, and the output quantity in the y direction reflects the size of the formation of the whole multi-mobile platform system; the force feedback man-machine interface equipment adopts a hand controller; the visual tracking equipment adopts an eye tracker;

setting a required formation in advance according to a working environment possibly faced by a slave-end multi-mobile platform by utilizing a visual interface; the eye tracker determines an area watched by an operator by capturing visual information of the operator, thereby acquiring a click stimulus; obtaining a target formation command, combining the force feedback interface equipment, and finally determining a high-level control command of the main end;

regarding the position P (x, y, z) of the end controller of the hand controller, representing the angular velocity of the virtual rigid body by the x coordinate axis, wherein the positive direction of the x axis is a right turn, and the negative direction of the x axis is a left turn; the z coordinate axis represents the linear velocity of the virtual rigid body, the positive direction of the z axis is backward, and the negative direction of the z axis is forward; the y coordinate axis is used for controlling the size of the virtual rigid body;

the expected formation for a mobile robot is defined as follows:

T＝[L ^d ,Φ ^d ]

wherein

L ^d And phi ^d Respectively representing the relative distance and the relative angle matrix between the mobile robots, and determining the formation of the multi-mobile robot system

Total mThe size of the formation is defined by the y coordinate axis of the human-computer interface device end controller, and the y is divided into areas [ y _M1 ,y _M2 ,…,y _Mm ](ii) a Then the corresponding relationship between the eye tracker and the force feedback human-computer interface device at the master end and the multi-mobile-platform system at the slave end is as follows:

wherein v and w respectively represent the linear velocity and the angular velocity of a virtual rigid body VRB in the slave-end multi-mobile-robot system, and the area of a formation; k is a radical of formula _v ，k _ω ，k _T And k _S Gain coefficients of linear velocity, angular velocity, formation and formation size are respectively set; [ q ] q _x ,q _y ,q _z ] ^T Position coordinate [ x ] representing force feedback human-machine interface equipment end controller _M ,y _M ,z _M ]And q is _S The target formation is obtained according to the click stimulation of the target formation obtained by capturing the sight of the operator by the eye tracker;

the virtual rigid body algorithm is defined as follows;

definition 1: for the set of mobile robots marked N {1, 2.., N }, use F _i A local reference coordinate system representing the robot i, with R representing the position, and R _i (t) ε SO (3) indicates that robot i is relative to F during time t _w The position of (a);

definition 2: the virtual rigid body is composed of a group of N mobile robots and a local reference system coordinate system F _v Composition is carried out; wherein the local position of the robot is defined by a set of time-varying vectors r ₁ (t)，r ₁ (t)，...，R _N (t) };

definition 3: form pi as a virtual rigid body, for a group of robots of size N, at F _v Has a constant local position r ₁ ，r ₂ ，...，r _N H, duration T _Π >0；

Definition 4: the transformation phi is a virtual rigid body with respect to the local reference frame F _v Time-varying position of { r } ₁ (t)，r ₁ (t)，...，R _N (T) }, such that a group of N number of mobile robots, for a duration T _Φ >In 0, P _v (t)∈R ₃ And R _v (t) ∈ SO (3) respectively represent F at time t _w Middle F _v The position and orientation of the origin; p of robot i _i (t) and r _i The relationship between (t) is p _i ＝p _v +R _V *r _i ，i∈{1,2，...，N}；

On the basis of the above definition, a repulsive vector pointing from the position of the obstacle to the VRB is defined at two-dimensional coordinates p, the magnitude of which is a gaussian function related to the position and radius of the obstacle, and assuming that there are n obstacle groups in the environment, the obstacle k is placed in the global coordinate system F _w The horizontal position in (1) is denoted as o _w，k Then the overall repulsion vector in the vector field generated from the n obstacles at two-dimensional coordinate p is:

wherein

Wherein B is _k Is the radius r from the obstacle k _k An associated positive scalar parameter; selection B _k Is such that the commanded speed v of the VRB _v The largest exclusion vector can be overcome because VRB is virtual and the 2 x 2 matrix Σ is positive definite, which defines the long and short axes of the dynamic gaussian-like function in the following way;

the virtual rigid body algorithm defines a stronger vector field for a single mobile robot than VRB, so that the repulsion vector can "push" it away when the mobile robot approaches an obstacle; away from the obstacle;

the above formula is in the global coordinate system F _w In a local coordinate system F _v In which they are expressed as

Images