Robot following system

Abstract

A robot following system comprises an error following module, a kinematics control module, a nonlinear active disturbance rejection control module, a dynamics control module and an error following module. The error following module is used for detecting a relative distance and an azimuth angle between the mobile robot and a moving target and inputting the relative distance and the azimuth angle into the kinematics control module; the kinematics control module is used for calculating expected angular velocities of left and right driving wheels of the mobile robot; the nonlinear active-disturbance-rejection control module is used for the mobile robot to quickly and accurately track expected angular speeds of left and right driving wheels of the robot, and estimating and compensating unknown disturbance of a driving motor of the mobile robot in real time; the dynamics control module is used for acquiring the actual voltage U and controlling the angular velocity of left and right driving wheels of the mobile robot; according to the invention, the number of input quantities is reduced, the difficulty of data acquisition is reduced, and the difficulty of actual following is reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of robot following, and particularly to a robot following system. Background Art

[0002] The human-robot following technology based on the leading mode is an advanced technology that can reflect the future characteristics of human-robot coexistence. It combines computer vision, machine learning, and sensor technology, enabling robots or drones to identify humans and automatically follow their movements. The target position is obtained through sensors such as cameras and UWB, and machine learning algorithms analyze and identify the target. At the same time, sensors are used to perceive the environment in real time and adjust actions to ensure accurate and safe following.

[0003] Human-robot coexistence is regarded as a core feature of future intelligent robots. In the process of realizing this coexistence, human-robot following is considered crucial. In recent years, with the rapid development of visual target recognition and multi-sensor positioning technology, the application of following robots has been promoted, and this technology has important theoretical and practical significance.

[0004] Traditional human-robot following kinematic control, in addition to the relative error between humans and robots, also relies on the direct measurement of the robot's pose information or the direct measurement of acceleration. However, too many state variables correspond to too many sensors, which will have more complex requirements for the optimization of the robot's structure and algorithms.

[0005] The following control of mobile robots is affected by various factors such as the non-holonomic constraints and non-linear dynamics of mobile robots, the complexity of the friction coefficient in the mobile scenario, and the uncertainty of external disturbances. High-speed, high-precision, and high-safety human-robot following control has become one of the research hotspots in this field.

[0006] In the human-robot following control of the leading mode, the dynamic controller designed based on the dynamic model of the mobile robot often converts the user's operation instructions into the desired input of the robot through hardware or software to follow or track the target. In the actual dynamic model of the mobile robot, it is affected by factors such as unmeasurable system parameters, wheel slippage, and unknown external disturbances, and the actuator is affected by physical constraints, resulting in a problem of mismatch between its output and the controller output, causing the actuator to enter a saturation state. Summary of the Invention

[0007] In view of the above-mentioned disadvantages of the prior art, the purpose of the present invention is to provide a robot following system to solve the technical problems of a large number of traditional detection variables and the mismatch between the robot output and the controller output.

[0008] To achieve the above object, the present invention provides a robot following system, including an error following module, a kinematic control module, a nonlinear active disturbance rejection control module, a dynamic control module, and an error following module;

[0009] The error following module is used to detect the relative distance and azimuth angle between the mobile robot and the mobile target, and input them into the kinematic control module;

[0010] The kinematic control module is used to calculate the desired angular velocities of the left and right drive wheels of the mobile robot; the kinematic model of the kinematic control module has the following relationship:

[0011]

[0012] where k1, k2, k3, and k4 are parameters, k1>0, k2>0, k3>0, k4>0; is the following distance error of the mobile robot, l d is the desired distance between the mobile robot and the mobile target; r is the radius of the drive wheel of the mobile robot; R is the length from the center point where the two drive wheels of the mobile robot are connected to the left or right drive wheel; ω ld is the desired angular velocity of the left wheel of the mobile robot, ω rd is the desired angular velocity of the right wheel of the mobile robot; V = [v, ω] T , v is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot;

[0013] The nonlinear active disturbance rejection control module is used for the mobile robot to quickly and accurately track the desired angular velocities of the left and right drive wheels, and to perform real-time estimation and compensation on the unknown disturbances of the drive motors of the mobile robot;

[0014] The dynamic control module is used to obtain the actual voltage U and control the angular velocities of the left and right drive wheels of the mobile robot.

[0015] The technical solution of the present invention enables the motion trajectory of the mobile robot to always maintain a relative desired spacing and desired direction angle with the trajectory of the pedestrian, transforms the human-machine trajectory following problem into a stabilization problem, and the error following module uses the camera to input the relative distance and direction angle between the mobile robot and the mobile target as state variables, reducing the number of input variables, reducing the difficulty of data acquisition, and reducing the state variables of the mobile target following control, reducing the difficulty of actual following; in addition, the nonlinear active disturbance rejection control module is used to solve problems such as various disturbances of the mobile robot and changes in control process parameters, improving the stability of the human-machine following control. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a schematic framework diagram of the present invention;

[0017] Figure 2 It is a schematic diagram of the framework for the non - linear active disturbance rejection control module and the dynamic control module;

[0018] Figure 3 It is a schematic diagram of a mobile robot following a pedestrian;

[0019] Figure 4 It is the trajectory of the mobile robot and the pedestrian under a straight - line trajectory;

[0020] Figure 5 It is the relative distance between the pedestrian and the mobile robot under a straight - line trajectory;

[0021] Figure 6 It is the relative angle between the pedestrian and the mobile robot under a straight - line trajectory;

[0022] Figure 7 It is the angular velocity of the pedestrian and the mobile robot under a straight - line trajectory;

[0023] Figure 8 It is the linear velocity of the pedestrian and the mobile robot under a straight - line trajectory;

[0024] Figure 9 It is the disturbance and disturbance estimation of the left driving wheel under a straight - line trajectory;

[0025] Figure 10 It is the disturbance and disturbance estimation of the right driving wheel under a straight - line trajectory;

[0026] Figure 11 It is the angular velocity error of the left and right driving wheels under a straight - line trajectory;

[0027] Figure 12 It is the input voltage of the left and right driving wheels under a straight - line trajectory;

[0028] Figure 13 It is the trajectory of the mobile robot and the pedestrian under a regular arc - shaped trajectory;

[0029] Figure 14 It is the relative distance between the pedestrian and the mobile robot under a regular arc - shaped trajectory;

[0030] Figure 15 It is the relative angle between the pedestrian and the mobile robot under a regular arc - shaped trajectory;

[0031] Figure 16 It is the angular velocity of the pedestrian and the mobile robot under a regular arc - shaped trajectory;

[0032] Figure 17 It is the linear velocity of the pedestrian and the mobile robot under a regular arc - shaped trajectory;

[0033] Figure 18 It is the disturbance and disturbance estimation of the left driving wheel under a regular arc - shaped trajectory;

[0034] Figure 19 For the disturbance and disturbance estimation of the right drive wheel under a regular arc trajectory;

[0035] Figure 20 For the angular velocity error of the left and right drive wheels under a regular arc trajectory;

[0036] Figure 21 For the input voltages of the left and right drive wheels under a regular arc trajectory;

[0037] Figure 22 For the trajectories of the mobile robot and the pedestrian under a random trajectory;

[0038] Figure 23 For the relative distance between the pedestrian and the mobile robot under a random trajectory;

[0039] Figure 24 For the relative angle between the pedestrian and the mobile robot under a random trajectory;

[0040] Figure 25 For the angular velocities of the pedestrian and the mobile robot under a random trajectory;

[0041] Figure 26 For the linear velocities of the pedestrian and the mobile robot under a random trajectory;

[0042] Figure 27 For the disturbance and disturbance estimation of the left drive wheel under a random trajectory;

[0043] Figure 28 For the disturbance and disturbance estimation of the right drive wheel under a random trajectory;

[0044] Figure 29 For the angular velocity error of the left and right drive wheels under a random trajectory;

[0045] Figure 30 For the input voltages of the left and right drive wheels under a random trajectory;

[0046] Figure 31 For the actual tracked relative distance between the human and the machine;

[0047] Figure 32 For the actual tracked relative angle between the human and the machine. Specific implementation manners

[0048] The following uses specific specific examples to illustrate the implementation manners of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation manners, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention.

[0049] Such as Figure 1As shown in the figure, the present invention provides a robot following system, including an error following module, a kinematic control module, a nonlinear active disturbance rejection control module, and a dynamic control module. The error following module is used to detect the relative distance l and azimuth angle α between the mobile robot and the mobile target, and input them into the kinematic control module.

[0050] The kinematic control module is used to calculate the desired angular velocities of the left and right driving wheels of the mobile robot.

[0051] The nonlinear active disturbance rejection control module is used for the mobile robot to quickly and accurately track the desired angular velocities of the left and right driving wheels, and to perform real-time estimation and compensation on the unknown disturbances of the driving motors of the mobile robot.

[0052] The dynamic control module is used to obtain the actual voltage U and control the angular velocities of the left and right driving wheels of the mobile robot. The actual voltage U is the actual required voltage that can meet the desired distance between the mobile robot and the mobile target after compensation, and is required for the angular velocities of the left and right driving wheels.

[0053] The present invention enables the motion trajectory of the mobile robot to always maintain a relative desired distance l d and desired direction angle α d with the trajectory of the pedestrian, and transforms the human-robot trajectory following problem into a stabilization problem. The error following module uses the camera to input the relative distance and direction angle between the mobile robot and the mobile target as state variables, reducing the number of input variables, lowering the difficulty of data acquisition, and reducing the state variables of the mobile target following control, thus reducing the difficulty of actual following. In addition, the nonlinear active disturbance rejection control module is used to solve problems such as various disturbances of the mobile robot and changes in control process parameters, improving the stability of the human-robot following control.

[0054] The mobile robot of the present invention is a two-wheel differential drive mobile robot model, which consists of two driving wheels and one steering wheel. Among them, the two driving wheels are independently driven by two motors, and the steering wheel only plays a supporting role. By controlling the rotational speeds of the two driving wheel motors, the angular velocities of the left and right wheels of the mobile robot are controlled, realizing changes in the yaw angle and position of the mobile robot. That is, the system of the present invention has three degrees of freedom and two control variables, and the system of the present invention is a typical underactuated nonholonomic control system. The mobile target of the present invention is a pedestrian.

[0055] The present invention is based on establishing multiple model derivations as the basis for designing the kinematic control module, nonlinear active disturbance rejection control module, dynamic control module, and error following module. The specific model derivations are as follows.

[0056] As Figure 3As shown in the figure, the present invention selects OXY as the world coordinate system, and the coordinate system oxy is a moving coordinate system with the geometric center of the mobile robot as the origin. The center of gravity of the mobile robot coincides with the geometric centers P of the two driving wheels. The vector q of point P in the world coordinate system OXY is q = [x, y, θ] T ∈R 3 , which is the pose of the mobile robot in the world coordinate system, where (x, y) and θ are the position and yaw angle of the mobile robot in the world coordinate system respectively.

[0057] For the kinematic control module of the present invention, a kinematic model is established in advance. The error following module establishes an error following model for the pedestrian and mobile robot system.

[0058] Assumption 1: The two driving wheels do not slip on the ground and only perform pure rolling. Then the kinematic model of the mobile robot in the world coordinate system can be expressed as:[[]]

[0059]

[0060] In the formula, V = [v, ω] T ∈R 2 is the linear velocity and angular velocity of the mobile robot, which is called the generalized velocity vector here. Since the center of gravity of the robot coincides with the geometric center, the transformation matrix T(θ) is:[[]]

[0061]

[0062] Considering target following without global pose measurement of the mobile robot and the pedestrian, assuming that the distance l between the mobile robot and the pedestrian and the azimuth angle α between the linear velocity direction of the mobile robot and the direction from the pedestrian to the mobile robot are measurable, the mobile robot error following model is:[[]]

[0063]

[0064] Among them, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, l is the relative distance for detecting the mobile robot and the moving target, and α is the azimuth angle for detecting the mobile robot and the moving target.

[0065] When l > 0, these relationships hold, and this condition is always satisfied by the asymptotic reduction of l to zero. Because the mobile robot follows the pedestrian with an expected distance l d > 0, there is always l > 0.

[0066] The pose of the mobile robot is adjusted by controlling the angular velocities of the left and right driving wheels. Therefore, the control task is to adjust the appropriate angular velocities of the left and right driving wheels so that the mobile robot follows the movement trajectory of the pedestrian. The relationship between the linear velocity v and angular velocity ω of the mobile robot and the angular velocities of the left and right driving wheels is:[[]]

[0067]

[0068] [ω l , ω r T where ω is the angular velocity of the left and right driving wheels of the mobile robot; r is the radius of its driving wheels; R is the length from the center point connecting the two driving wheels of the mobile robot to the left or right driving wheel; v is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot.

[0069] Assumption 2 The angular velocity signals and their derivatives of the left and right driving wheels of the mobile robot are bounded, that is:

[0070]

[0071] The above assumption does not include the global pose of the robot. The present invention only utilizes the measurable distance and azimuth angle between the mobile robot and the pedestrian, regards the human-robot following problem as a stabilization control problem, and does not need to measure the global pose and velocity of the pedestrian. The present invention cannot control the human-robot following as a trajectory tracking problem.

[0072] Let the expected distance for the mobile robot to follow the pedestrian be l d , and let

[0073]

[0074] where is the distance error for the mobile robot to track; the expected angular velocity α d is set to 0, that is, the deviation angle error for the mobile robot to track is equal to the deviation angle α.

[0075] Substituting the above formula into the error following model of the mobile robot gives:

[0076]

[0077] The human-robot following control problem here is to find a state feedback method:

[0078]

[0079] such that α → 0 to ensure asymptotic property.

[0080] Based on the backstepping design idea, a Lyapunov function is constructed, and a kinematic control law with global uniform asymptotic stability is designed:

[0081]

[0082] where k1 and k2 are parameters, k1 > 0, k2 > 0, V L = [v L , ω L T ​​, where \(v_L\) is the desired linear velocity of the mobile robot, and \(\omega\) L is the desired angular velocity of the mobile robot, is the following distance error of the mobile robot, and \(l\) d is the desired distance between the mobile robot and the moving target;

[0083] It is obtained that under the control of the kinematic control module of the mobile robot, the theorem that \(\alpha\) approaches zero respectively.

[0084] The present invention selects a Lyapunov function to prove the above theorem

[0085]

[0086] In the formula, \(q_1>0, q_2>0\).

[0087] Obviously, the function \(V(x)\) has the first three properties of the Lyapunov function. The derivative of \(V(x)\) with respect to time along the system trajectory determined by the above formula is:

[0088]

[0089] Because \(k_1>0, k_2>0, q_1>0, q_2>0\), so According to the Lyapunov stability criterion, it can be obtained that the system asymptotically converges to zero under the control of the kinematic control module.

[0090] Since the pedestrian is moving, there will be a certain steady-state error in the actual following distance and actual azimuth angle between the human and the machine during the stabilization control process. To eliminate the steady-state error, an integral term of the following distance error and an integral term of the azimuth angle error are respectively introduced into the desired linear velocity and desired angular velocity of the mobile robot

[0091]

[0092] In the formula, \(k_3>0, k_4>0, V\) integral \(=[v\) integral , \(\omega\) integral T , \(v\) integral , \(\omega\) integral are respectively the integral term of the following distance error and the integral term of the azimuth angle error. The desired linear velocity and angular velocity of the mobile robot designed based on the backstepping design idea are combined with the integral terms to obtain the generalized velocity vector \(V\), and the expression is:

[0093]

[0094] In the formula, \(k_1, k_2, k_3, k_4\) are parameters, \(k_1>0, k_2>0, k_3>0, k_4>0\); \(V = [v, \omega]\) T ​, where \(v\) is the linear velocity of the mobile robot and \(\omega\) is the angular velocity of the mobile robot; is the following distance error of the mobile robot following, \(l\) d is the expected distance between the mobile robot and the moving target.

[0095] The angular velocities of the left and right wheels of the mobile robot and the generalized velocity vector satisfy the following relational expressions:

[0096]

[0097] Among them, \(r\) is the radius of the driving wheel of the mobile robot; \(R\) is the distance from the center point where the two driving wheels of the mobile robot are connected to the left or right driving wheel. Substituting the generalized velocity vector \(V\) into the above relational expressions, the expected angular velocity \(\omega\) of the left wheel of the mobile robot is obtained ld and the expected angular velocity \(\omega\) of the right wheel rd . The above expressions describe the association between the kinematic model and the dynamic model of the two-wheel differential drive mobile robot.

[0098] Through the above inference design, the kinematic model of the kinematic control module has the following relational expressions:

[0099]

[0100] The kinematic control module outputs the expected angular velocity \(\omega\) of the left wheel of the mobile robot through the above two relational expressions ld and the expected angular velocity \(\omega\) of the right wheel rd .

[0101] The kinematic control module designed based on the above kinematic model only solves the problem of human-machine following at the kinematic level. However, in an actual mobile robot system, the linear velocity and angular velocity of the mobile robot are controlled by the angular velocity of the driving wheel motor. For motor speed control, due to the existence of friction and other force interferences on the robot wheels, to meet high-precision human-machine following, a non-linear active disturbance rejection control module and a dynamic control module must also be designed. Before that, the dynamic model of the mobile robot system must be established.

[0102] The actuator of the mobile robot of the present invention is a permanent magnet brushless motor, and its driving motor dynamic model can be described as:

[0103]

[0104] Among them, \(\omega\) m is the angular velocity of the motor, is the angular acceleration of the motor, \(r\) a is the total resistance of the motor armature circuit, \(B\) v is the damping coefficient, \(K\) e is the back electromotive force coefficient, \(K\) t is the torque constant, \(L\)a where \(L\) is the equivalent inductance of the winding, \(J\) is the rotor inertia, \(U\) is the actual voltage applied to the motor, and \(d(t)\) is the uncertain part of the system, including model parameter perturbations and external unknown bounded disturbances.

[0105] Let Then it can be rewritten as

[0106]

[0107] Assumption 3: The total disturbance \(d(t)\) of the mobile robot system is bounded and differentiable, and the system model uncertainty function \(f(x_1,x_2,t)\) is smooth, bounded, and differentiable.

[0108] Since the mobile robot needs to overcome various disturbances such as wheel-ground friction, motor rotation error, and external disturbances during high-precision target following, the backstepping method and traditional torque controllers can design the dynamic control module of the system through recursive thinking. However, they have a high dependence on the model and a weak ability to compensate for unknown disturbances. The active disturbance rejection control algorithm is less dependent on the system model. Therefore, a nonlinear active disturbance rejection control module is designed to enable the robot to quickly and accurately track the desired linear velocity and angular velocity, estimate the unknown disturbances of the system in real time, and compensate for the estimated unknown disturbances in real time.

[0109] Combined with Figure 2 As shown in, the nonlinear active disturbance rejection control module includes an arrangement transition module, an extended state observer module, and a nonlinear combination module. The arrangement transition module is used to obtain the desired angular velocity signal of the driving motor of the mobile robot, and solve the contradiction between the overshoot and rapidity of the PID; the extended state observer module is used to estimate and compensate the unknown influencing factors of the driving motor in real time, and improve the robustness and adaptability of the mobile robot system to disturbances; the nonlinear combination module is used to output the desired voltage \(U_1\).

[0110] The arrangement transition module is used to smooth the desired angular velocity signals of the left and right wheels of the mobile robot. After these signals are added to the angular velocity and angular acceleration signals estimated by the extended state observer module for the left and right driving wheels of the mobile robot respectively, the angular velocity error and angular acceleration error of the left and right driving wheels of the mobile robot are obtained and transmitted to the nonlinear combination module;

[0111] The desired voltage \(U_1\) output by the nonlinear combination module, after adding the unknown disturbances estimated by the extended state observer module for the left and right driving motors of the robot, obtains the actual voltage \(U\), and the actual voltage \(U\) is input to the dynamic control module and the extended state observer module;

[0112] The dynamic control module controls the actual angular velocities of the left and right driving wheels of the mobile robot and outputs the actual angular velocities of the left and right driving wheels of the mobile robot to the extended state observer module.

[0113] In the described arrangement transition module, a nonlinear tracking differentiator is adopted, and the expression of its second-order sliding mode differentiator is as follows:

[0114]

[0115] Among them, ω d (t) = [ω ld (t), ω rd (t)] T , ω ld (t), ω rd (t) are respectively the expected angular velocities of the left driving wheel and the right driving wheel of the mobile robot changing with time during the process of following a pedestrian. h tracks ω d (t), and h is the smoothed expected angular velocity signal of the left and right driving wheels, representing the tracking variable, making the change of the expected angular velocities of the left and right driving wheels relatively gentle and avoiding violent fluctuations in the system; h2 tracks which is the second derivative of h1. By tracking the change of angular acceleration, the system can respond to faster dynamic changes while maintaining overall smoothness.

[0116] The nonlinear tracking differentiator can effectively suppress the noise amplification effect. In the present invention, a nonlinear tracking differentiator based on sliding mode technology proposed by Levant is adopted to arrange the transition process.

[0117] According to the dynamic model of the drive motor, the total disturbance and system uncertainty model of the mobile robot system can be regarded as the total uncertainty function F = f(x1, x2, t) + d(t) + (G - B)U, where Among them, b1 and b2 are respectively the estimated values of the input gains of the left and right drive motors; according to assumption 3, F is bounded and differentiable. Since the two drive motors of the mobile robot are the same, the expression of the dynamic model of the mobile robot with extended state variables is

[0118]

[0119] Among them, M3 = F, F = [f1, f2] T , where ω s = [ω1, ω2], ω1 and ω2 are respectively the actual angular velocities of the left and right driving wheels, and f1 and f2 are respectively the total uncertainty functions of the left and right drive motors; U = [u1, u2] T , where u1 and u2 are respectively the actual voltages of the inputs of the left and right drive motors of the mobile robot. The dynamic control module has a dynamic model of the mobile robot with extended state variables.

[0120] Furthermore, for the dynamic model of a mobile robot with an extended state variable, the extended state observation module establishes a non-linear extended state observer with the following expression:

[0121]

[0122] where e w =[e w1 ,e w2 T , where fal(e ω , ε, δ) is a saturation function, e w1 is the difference between the actual angular velocity of the left drive wheel and the estimated value of the angular velocity of the left drive wheel by the extended state observer, and e w2 is the difference between the actual angular velocity of the right drive wheel and the estimated value of the angular velocity of the right drive wheel by the state observer; ε1 and ε2 are both non-linear factors, and δ is a filtering factor; z1 = [z 11 ,z 12 T is the estimation of the angular velocities of the left and right drive wheels of the mobile robot by the extended state observation module, and z2 = [z 21 ,z 22 T is the estimation of the angular accelerations of the left and right drive wheels of the mobile robot by the extended state observation module; z3 = [z 31 ,z 32 T is the estimation of the total uncertainty function of the motors of the left and right drive wheels of the mobile robot by the extended state observation module; additionally:

[0123]

[0124] where ω0 is the bandwidth of the non-linear extended state observer, and ω0 > 0.

[0125] where the saturation function fal(e ω , ε, δ) is expressed as:

[0126]

[0127] where ε is a non-linear factor and δ is a filtering factor. When |e ω | > δ, fal(e ω , ε, δ) can make the system state rapidly approach the input signal ω s , so that the error e ω approaches δ; when |e ω | ≤ δ, the structure of fal(e ω , ε, δ) is a low-pass filter.

[0128] ​​​​For the dynamic model of the drive motor of a mobile robot with unknown interference and modeling uncertainty, in the nonlinear active disturbance rejection control module, by combining the arrangement transition module process and the nonlinear extended state observer, the following error differential equation is established:

[0129]

[0130] where, h → ω d , z1 → ω s , where ω d is the desired angular velocity of the left and right drive wheels, ω s is the actual angular velocity of the left and right drive wheels of the mobile robot; e v is the difference between the desired angular velocity of the left and right drive wheels and the estimated value of the angular velocity of the left and right drive wheels by the state observer.

[0131] Furthermore, the nonlinear combination module of the present invention adopts the following nonlinear error feedback control law:

[0132] U1 = k1fal3 + k2fal4

[0133] where, U1 is the desired voltage, k 1i > 0, k 2i > 0, i = 1, 2; fal3 = [fal(e ω1 , ε3, δ1), fal(e ω2 , ε3, δ1)] T , fal4 = [fal(e ω1 , ε4, δ1), fal(e ω2 , ε4, δ1)] T , 0 < α3 < α4; k 1i , k 2i are the proportional gain and differential gain of the control law respectively.

[0134] Compensate for the unknown disturbance estimated by the nonlinear extended state observer to satisfy the following control law

[0135] U = U1 - D

[0136] where, U is the actual voltage, D is the unknown disturbance estimated by the left and right drive wheels of the mobile robot according to the extended observation module.

[0137] Such as Figures 4 - 32As shown in the figure, the present invention conducts a simulation experiment. The controlled object of the experiment is a non-holonomic two-wheel drive mobile robot (experimental prototype). The overall experimental prototype is made of aluminum alloy. Four hub motors drive four wheels. The IMU sensor is located at the geometric center of the robot, and the power supply is placed inside the chassis. Its counterweight is adjusted to ensure that the center of gravity of the robot is at the geometric center. The present invention uses Simulink to establish the dynamic model of the mobile robot and verifies the algorithm therein; in the actual robot motion control, the robot control system (Robot operating system, ROS) is used to verify the physical algorithm.

[0138] The steps of the simulation experiment are as follows: Step 1, establish the kinematic model and dynamic model according to the above experimental prototype, and the simulation parameters are shown in the following table.

[0139]

[0140] Step 2, verify the performance of the algorithm under real parameters through Simulink simulation. The simulation is mainly divided into five parts: the motion trajectory generation part of the moving target (pedestrian), the kinematic model of the mobile robot, the motor dynamic model, the kinematic control part, and the dynamic control part. The parameters in the kinematic control algorithm and the dynamic control algorithm are shown in the following table.

[0141]

[0142]

[0143] Based on the above given simulation parameters, three kinds of pedestrian trajectories, namely straight trajectory, regular arc trajectory, and random trajectory, are set respectively to conduct the following simulation of the mobile robot. In the simulation, the differential mobile robot is used to simulate the motion trajectory of the pedestrian. The initial pose of the pedestrian is given as (0, 0, 0). In order to verify the robustness of the system and considering different factors in different following trajectories, different parameters such as pedestrian speed and angular velocity are set. For details, see below.

[0144] The trajectory of the mobile robot following the pedestrian is straight trajectory following. Considering the situation that the pedestrian will have changes in linear velocity and turning angular velocity, the pedestrian linear velocity is set to The pedestrian angular velocity is set to The starting pose of the mobile robot is (0, 3, 0), its initial linear velocity and angular velocity are 0, the running time T = 100s; the external interference of the left and right drive wheels is d(t) = 2sin(t); the expected distance between the mobile robot and the pedestrian is l d = 0.5m and the expected direction angle α d = 0 rad. At this time, the distance between the pedestrian and the mobile robot is greater than the set expected distance. Based on the above given simulation parameters, the simulation data is asFigures 4 - 12 as shown

[0145] Considering that the trajectory of the pedestrian is not necessarily a straight line, the trajectory of the pedestrian is set as a regular arc trajectory, and the linear velocity of the pedestrian is set as v c = 1.5 m / s, and the angular velocity of the pedestrian is set as The starting pose of the mobile robot is (0.2, 0.3, 0), its initial linear velocity and angular velocity are 0, the running time T = 100 s; the external disturbances of the left and right driving wheels are d(t) = 2sin(t); the desired spacing between the robot and the person is l d = 0.5 m and the desired direction angle α d = 0 rad. Based on the above given simulation parameters, the simulation data is as Figures 13 - 21 shown

[0146] Furthermore, the movement of the pedestrian is set as a random movement. Considering the instability of the pedestrian's speed and angular velocity, the linear velocity of the pedestrian is set as v c = 1.25 + 0.5*cos(0.3*t) m / s, and the angular velocity of the pedestrian is set as ω c = 0.1*sin(0.5*t) rad / s; the starting pose of the mobile robot is (0, 1, 0), its initial linear velocity and angular velocity are 0, the running time T = 100 s; the external disturbances of the left and right driving wheels are d(t) = 4*sin(t); the desired spacing between the robot and the person is l d = 1 m and the desired direction angle α d = 0 rad. Based on the above given simulation parameters, the simulation data is as Figures 22 - 30 shown

[0147] From the simulation results of the mobile robot following the pedestrian's trajectory, it can be seen that the mobile robot can accurately follow the pedestrian's motion trajectory from the initial pose, and the relative distance error asymptotically converges to the set desired distance l d after initialization, and the relative angle error asymptotically converges to α d = 0 rad. From Figures 4 - 30It can be seen that the linear velocity and angular velocity control inputs of the mobile robot are bounded, and the following process can be analyzed: When the actual distance between the pedestrian and the mobile robot is greater than the desired distance, the linear velocity of the mobile robot is greater than that of the pedestrian, gradually consuming the influence of the initial distance difference and angle difference, and finally the linear velocity and angular velocity of the mobile robot track those of the pedestrian. When the actual distance between the pedestrian and the mobile robot is less than the desired distance, the linear velocity of the mobile robot is less than that of the pedestrian, and the change of the angular velocity of the mobile robot is similar to that of its linear velocity. By comparing the three groups of simulation experiments, it can be seen that the tracking of the random trajectory takes a relatively long time to reach stability, because the trajectory is more complex, and the changes of the linear velocity and angular velocity of the mobile robot are relatively rapid and large in amplitude. Although it takes a long time, after reaching stability, the three groups of simulation experiments still maintain a good tracking effect under the sudden changes of the linear velocity and angular velocity of the pedestrian. These simulation experiment results prove that the control law for dealing with the follow-up control problem of nonholonomic wheeled mobile robots using only relative distance and angle measurements is feasible and effective.

[0148] In addition to the above simulation experiments with given data, real experiments were also carried out. The test site was the sports field of Huxi Campus of Chongqing University. The advantage of this site is that the ground is not completely flat, there will be unknown disturbances, the space is large, and experiments with relatively high speeds can be carried out. When verifying the actual robot motion control experiment, the Robot Operating System (ROS) was adopted, and the established kinematic control algorithm and dynamic control algorithm were used to control the motion of the robot. The desired spacing between the robot and the person was set as l d = 0.65 m and the desired direction angle α d = 0 rad. It should be noted that due to the low precision of the motor drive and the fact that the linear velocity and angular velocity of the pedestrian are both variables that change suddenly within a certain range, and considering problems such as the uneven ground and the relatively large mass of the robot, it is difficult for the robot to maintain a stable speed under a small voltage. Therefore, an error within 0.05 m is considered an ideal tracking situation, and the tracking data is shown in Figure 30 and 32 .

[0149] The present invention uses a nonlinear active disturbance rejection control module to control the dynamic model of the drive motor of the mobile robot, and can accurately estimate the disturbances received by the system from the outside and inside through the extended state observer module; the designed dynamic control module can well compensate these disturbances, making the angular velocity errors of the left and right drive wheels asymptotically converge, as shown in Figure 10 and 19, as shown in Figure 28; for the control input voltage of the left and right drive wheels, the input voltages of both wheels are bounded, and the degree of tremor phenomenon is low. Obviously, in the case of unknown disturbances in the mobile robot system, the system adopted by the present invention plays a very good role in controlling the wheel angular velocity of the mobile robot.

[0150] The present invention introduces a drive motor model into the dynamic model and designs an active disturbance rejection dynamic controller. This controller does not depend on the accurate mathematical model of the system, estimates the unknown disturbances of the system in real time through an extended state observer, and compensates in real time through the controller, improving the impact of complex road conditions and unpredictable disturbances on human-robot following.

[0151] The above embodiments are only illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Any person familiar with this technology can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes completed by those with ordinary knowledge in the technical field without departing from the spirit and technical ideas disclosed by the present invention should still be covered by the claims of the present invention.

Claims

1. A robot following system, characterized in that: It includes error following module, kinematic control module, nonlinear anti-disturbance control module, dynamic control module and error following module; The error following module is used to detect the relative distance l and azimuth angle α between the mobile robot and the mobile target, and input them into the kinematic control module; The kinematic control module is used to calculate the expected angular velocity of the left and right driving wheels of the mobile robot; the kinematic model of the kinematic control module has the following relationship: Among them, k1, k2, k3, k4 are parameters, k1>0, k2>0, k3>0, k4>0; is the following distance error of the mobile robot, l d is the expected distance between the mobile robot and the mobile target; r is the radius of the mobile robot's driving wheel; R is the distance from the center point where the two driving wheels of the mobile robot are connected to the left or right driving wheel; ω ld is the expected angular velocity of the left wheel of the mobile robot, ω rd is the expected angular velocity of the right wheel of the mobile robot; V = [v, ω] T , v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot; The nonlinear anti-disturbance control module is used for the mobile robot to quickly and accurately track the desired angular velocity of the left and right driving wheels of the robot, and to estimate and compensate for the unknown disturbance of the driving motor of the mobile robot in real time; The dynamics control module is used to obtain the actual voltage U and control the angular velocity of the left and right driving wheels of the mobile robot.

2. A robot following system according to claim 1, characterized in that: The nonlinear auto-disturbance rejection control module includes a transition arrangement module, an extended state observation module, and a nonlinear combination module; The arrangement transition module is used to smoothly process the expected angular velocity signals of the left and right wheels of the mobile robot, and the signals are respectively added to the angular velocity and angular acceleration signals of the left and right driving wheels of the mobile robot estimated by the expanded state observation module to obtain the angular velocity error and angular acceleration error of the left and right driving wheels of the mobile robot, and transmit them to the nonlinear combination module; The expected voltage U1 output by the nonlinear combination module is added to the unknown disturbance of the left and right driving motors of the robot estimated by the extended state observation module to obtain the actual voltage U, which is input to the dynamic control module and the extended state observation module; The dynamics control module controls the actual angular velocity of the left and right driving wheels of the mobile robot, and outputs the actual angular velocity of the left and right driving wheels of the mobile robot to the extended state observation module.

3. A robot following system according to claim 2, characterized in that: The nonlinear tracking differentiator is used in the arrangement transition module, and its second-order sliding mode differentiator expression is: in, ω d (t) = [ω ld (t),ω rd (t)] T ,ω ld (t),ω rd (t) are the expected angular velocities of the left and right driving wheels of the mobile robot changing with time during the process of following the pedestrian, and h tracks ω d (t), h2 tracking 4. A robot following system according to claim 3, characterized in that: In the dynamic control module, a mobile robot dynamic model with extended state variables is established, and its expression is as follows: Where M1 = ω s , M3=F,F=[f1,f2] T , where ω s =[ω1,ω2], ω1,ω2 are the actual angular velocities of the left and right driving forces respectively, and f1,f2 are the total uncertainty functions of the left and right driving motors respectively; Where b1 and b2 are the estimated values ​​of the input gains of the left and right drive motors respectively; U = [u1, u2] T , where u1 and u2 are the actual input voltages of the left and right drive motors of the mobile robot respectively.

5. A robot following system according to claim 4, characterized in that: For the mobile robot dynamics model with extended state variables, a nonlinear extended state observer with the following expression is established in the extended state observation module: in, e ω =[e ω1 ,e ω2 ] T , where fal(e ω ,ε,δ) is a saturation function, e ω1 is the difference between the actual angular velocity of the left driving wheel and the estimated angular velocity of the left driving wheel by the extended state observer, e ω2 is the difference between the actual angular velocity of the right driving wheel and the estimated value of the angular velocity of the right driving wheel by the state observer; ε1 and ε2 are nonlinear factors, δ is the filtering factor; z1 = [z 11 ,z 12 ] T is the estimation of the angular velocity of the left and right driving wheels of the mobile robot by the extended state observation module, z2 = [z 21 ,z 22 ] T is the estimation of the angular acceleration of the left and right driving wheels of the mobile robot by the extended state observation module; z3 = [z 31 ,z 32 ] T The estimation of the total uncertainty function of the left and right driving wheel motors of the mobile robot by the extended state observation module; and Where ω0 is the bandwidth of the nonlinear extended state observer, and ω0>0.

6. A robot following system according to claim 5, characterized in that: Saturation function fal(e ω ,ε,δ) is expressed as: Among them, ε is the nonlinear factor and δ is the filtering factor. ω |>δ,fal(e ω ,ε,δ) can make the system state quickly approach the input signal ω s , so that the error e ω Approaching δ; when |e ω |≤δ,fal(e ω ,ε,δ) is a low-pass filter.

7. A robot following system according to claim 6, characterized in that: In the nonlinear ADRC module, the following error differential equation is established by combining the arrangement of the transition module process and the nonlinear extended state observer: Where h→ω d , z1→ω s , where ω d is the desired angular velocity of the left and right driving wheels, ω s is the actual angular velocity of the left and right driving wheels of the mobile robot; e v is the difference between the desired angular velocity of the left and right driving wheels and the estimated value of the angular velocity of the left and right driving wheels by the state observer.

8. A robot following system according to claim 7, characterized in that: The nonlinear combination module adopts the following nonlinear error feedback control law: U1=k1fal3+k2fal4 Where U1 is the expected voltage, k 1i >0,k 2i >0,i=1,2;fal3=[fal(e ω1 ,ε3,δ1),fal(e ω2 ,ε3,δ1)] T ,fal4=[fal(e ω1 ,ε4,δ1),fal(e ω2 ,ε4,δ1)] T ,0<α3<α4; k 1i , k 2i They are the proportional gain and differential gain of the control law respectively.

9. A robot following system according to claim 8, characterized in that: Compensating the unknown disturbance estimated by the nonlinear extended state observer satisfies the following control law: U=U1-D Where U is the actual voltage, D is the unknown disturbance of the left and right driving wheels of the mobile robot estimated according to the extended observation module.

Images