Safety predictive control method for anti-disturbance and triggered obstacle avoidance of wheeled mobile robots

Abstract

The application provides a safety prediction control method for anti-interference and trigger obstacle avoidance of a wheeled mobile robot, and relates to the technical field of automatic control. The method mainly comprises the following steps: a kinematics model of a four-wheel independent steering and four-wheel independent driving mobile robot is established, and a linear time-varying model is converted; a bounded error disturbance observer is designed to observe the external interference of the robot, an upper bound of an observation error is obtained, and an input-to-state safety control barrier function obstacle avoidance constraint condition is established based on the disturbance observation value and the error upper bound; a distance trigger obstacle avoidance mechanism is constructed according to current state information and the distance to an obstacle; and a safety prediction controller is designed, and the distance trigger mechanism and the obstacle avoidance constraint condition are introduced into the solving process of the optimization control problem. The safety prediction control method provided by the application is helpful to improve the adaptability and robustness of the control system to interference, so that the robot can still maintain a stable operating state when facing various uncertain external interferences.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot control, and in particular to a safety prediction control method for anti-interference and triggered obstacle avoidance of a wheeled mobile robot. Background Art

[0002] With the rapid development of robotics, wheeled mobile robots have found widespread application in healthcare, industrial automation, the service industry, intelligent transportation, and inspection. These robots must autonomously navigate complex and dynamic environments while ensuring precise and safe movement, a current research hotspot.

[0003] To ensure the safety of robotic systems, researchers have developed a variety of control methods, including artificial potential field methods, reachability analysis, and nonlinear model predictive control. Recently proposed control barrier functions restrict the system state to a safe region by introducing a barrier function. This directly links the control input with the system's safety constraints, ensuring that the system state remains stable while satisfying the constraints. However, in practical systems, even with obstacle avoidance constraints, perturbations inevitably affect the constraints, necessitating consideration of their impact on system safety. Summary of the Invention

[0004] The technical purpose of this application is to further solve the impact of disturbances on the safety of robot systems, provide a safe predictive control method for anti-interference and triggered obstacle avoidance of wheeled mobile robots, reduce the impact of disturbances on system safety, and thus ensure safe and stable motion control of the robot.

[0005] In order to achieve the above technical objectives, this application adopts the following technical solutions.

[0006] The present invention provides a method for predictive control of interference rejection and triggered obstacle avoidance for a wheeled mobile robot. The method is applicable to a four-wheel independent drive and four-wheel independent steering mobile robot system. The method includes:

[0007] Establishing a kinematic model of a four-wheel independent drive and four-wheel independent steering mobile robot, and converting the kinematic model into a linear time-varying model subject to external interference;

[0008] Based on the linear time-varying model, a bounded error disturbance observer is designed to observe the external disturbance to the mobile robot system to obtain a disturbance observation value, and an upper bound of the disturbance observation error is determined. According to the disturbance observation value and the upper bound of the disturbance observation error, an obstacle avoidance constraint condition of a control obstacle function is constructed;

[0009] Calculating the distance between the robot and the target obstacle based on the acquired position state information of the robot and the perceived target obstacle information; and when the distance triggers a pre-constructed distance trigger mechanism, designing a safety predictive controller based on the linear time-varying model using the obstacle avoidance constraint condition;

[0010] The safety prediction controller is used to perform safety prediction control.

[0011] Furthermore, the kinematic model of the four-wheel independent drive and four-wheel independent steering mobile robot is expressed as follows:

[0012]

[0013] in, is the speed of the robot in the X direction in the global coordinate system, is the speed of the robot in the Y direction in the global coordinate system, is the rate of change of the robot’s heading angle φ in the global coordinate system,

[0014] c i =cos(φ+δ i ), s i =sin(φ+δ i ), (x i ,y i ) are the coordinates of the four wheels in the robot coordinate system, i = fl, rl, rr, fr, v i represents the linear velocity of the four wheels, γ i Represents the angular velocity of the four wheels.

[0015] Furthermore, the linear time-varying model is expressed as:

[0016] ξ(k+i|k)=a(k)ξ(k+i-1|k)+b(k)u(k+i-1|k)+d(k|k);

[0017] Where ξ(k+i|k) represents the system state variables predicted i steps later at the current time k, ξ=[X,Y,Φ] T Represent the robot's position X, position Y and heading angle φ in the global coordinate system, respectively. I is the unit matrix of the corresponding dimension, T is the sampling time interval, is the Jacobian matrix of f with respect to the state variable ξ(k), is the Jacobian matrix of f with respect to the control variable u(k), u(k+i-1|k) represents the control input variable applied to the system at the current time k, predicted i-1 steps later, and d(k|k) is the high-order term after discretization and linearization and the external interference term.

[0018] Furthermore, the bounded error disturbance observer is expressed as:

[0019]

[0020] Among them, g(k) represents the intermediate variable of the observer, g(k+1) is the intermediate variable of the next moment, is the disturbance estimate, d(k) is the external disturbance of the system, κ is a diagonal matrix, κ=diag(κ1,κ2,…,κ p ), |κ i |<1,i=1,2,…,p, p is the dimension, I p is the p-dimensional identity matrix, b d is the disturbance matrix, a and b are both system matrices, x(k) is the system state variable, and u(k) is the system control variable.

[0021] Furthermore, the upper bound of the disturbance observation error is expressed as:

[0022]

[0023] Among them, e d (k) represents the actual external disturbance d(k) and the disturbance observer estimate at time k The error vector between di (k)(i=1,2,…,p), e di (k) is the perturbation observation error vector e d The i-th component of (k), ζ i is a preset fixed value, κ i is the i-th element of the diagonal matrix κ.

[0024] Furthermore, the distance trigger mechanism is expressed as:

[0025]

[0026] Among them, h a is the given distance trigger threshold, and h(ξ(k)) is the distance between the robot and the target obstacle.

[0027] Furthermore, the design of a safety predictive controller based on the linear time-varying model includes:

[0028] Constructing a predictive model for a safety predictive controller;

[0029] establishing a cost function based on the prediction model and the initial control increment sequence;

[0030] Establish a target constraint condition for the cost function, determine the cost at the current moment according to the cost function, take minimizing the cost as the goal, solve the cost function based on the target constraint condition, and obtain the control increment sequence at the current moment.

[0031] Furthermore, establishing a cost function based on the prediction model and the initial control increment sequence includes: establishing a cost function that takes terminal cost into consideration based on the prediction model and the initial control increment sequence.

[0032] Compared with the prior art, the safe predictive control method for anti-interference and triggered obstacle avoidance of a wheeled mobile robot provided in the embodiments of the present application has the following beneficial technical effects: a kinematic model of a four-wheel independent drive and four-wheel independent steering mobile robot is established and converted into a linear time-varying model subject to external interference, which can more accurately describe the complex situations of the robot in actual operation and provide an accurate model foundation for the design of subsequent control strategies. A bounded error disturbance observer is designed to observe external interference, which not only obtains interference information in real time but also determines the upper bound of the disturbance observation error. This helps to improve the adaptability and robustness of the control system to interference, allowing the robot to maintain a stable operating state in the face of various uncertain external interferences. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] The drawings described herein are for illustrative purposes only and are not intended to limit the scope of the present application in any way. In addition, the shapes and proportional dimensions of the components in the drawings are only schematic and are used to help understand the present application. They do not specifically limit the shapes and proportional dimensions of the components of the present application. Those skilled in the art can select various possible shapes and proportional dimensions to implement the present application according to the specific circumstances under the guidance of the present application. In the drawings:

[0034] Figure 1 A schematic flow chart of a safe predictive control method for anti-interference and triggered obstacle avoidance of a wheeled mobile robot provided in an embodiment;

[0035] Figure 2 Schematic diagram of the position and structure of the four-wheel independent drive and four-wheel independent steering robot in the world coordinate system in the embodiment;

[0036] Figure 3 Schematic diagram of the obstacle avoidance of the four-wheel independent drive and four-wheel independent steering robot in the embodiment;

[0037] Figure 4 Schematic diagram of the obstacle avoidance effect of the distance trigger mechanism in the embodiment;

[0038] Figure 5Schematic diagram of linear velocity input of four wheels in the embodiment;

[0039] Figure 6 Schematic diagram of angular velocity input of four wheels in the embodiment;

[0040] Figure 7 Schematic diagram of the observation effect of the disturbance observer in the embodiment;

[0041] Figure 8 Schematic diagram of obstacle avoidance effect under different ρ parameters in the embodiment. DETAILED DESCRIPTION

[0042] In order to enable those skilled in the art to better understand the technical solutions in this application, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the drawings in the embodiments of this application. Obviously, the described embodiments are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of this application.

[0043] The present invention provides a method for predictive control of interference rejection and triggered obstacle avoidance for a wheeled mobile robot. The control method is applicable to a four-wheel independent drive and four-wheel independent steering mobile robot system. The method includes:

[0044] Step 1: Establish a kinematic model of a four-wheel independent drive and four-wheel independent steering mobile robot and transform the kinematic model into a linear time-varying model subject to external interference;

[0045] Step 2: Based on the linear time-varying model, a bounded error disturbance observer is designed to observe the external disturbances on the mobile robot system to obtain disturbance observation values, and the upper bound of the disturbance observation error is determined. According to the disturbance observation values ​​and the upper bound of the disturbance observation error, the obstacle avoidance constraint conditions are constructed;

[0046] Step 3: Based on the acquired robot position information and the perceived target obstacle information, the distance between the robot and the target obstacle is calculated. When the distance triggers the pre-constructed distance trigger mechanism, the obstacle avoidance constraints are used to design a safety predictive controller based on the linear time-varying model.

[0047] Step 4: Use the safety predictive controller to perform safety predictive control.

[0048] In the embodiment, Figure 1 As shown, step 1 includes step 1.1: establishing a kinematic model of the omnidirectional and all-wheel drive mobile robot according to the speed relationship between the four wheels of the robot.

[0049] Before establishing the kinematic model of the four-wheel independent drive and four-wheel independent steering wheeled mobile robot, the following assumptions are made about the system:

[0050] Assumption 1: The four-wheel independent drive and four-wheel independent steering wheeled mobile robot moves on a flat and hard ground without vertical movement.

[0051] like Figure 2 The figure shows the robot's position at a certain moment in the global coordinate system, X C O C Y C is the global coordinate system, xoy is the robot's own coordinate system, x i oy i (i=fl,fr,rl,rr) is the wheel coordinate system, X and Y represent the position of the robot in the global coordinate system, φ represents the heading angle of the robot in the global coordinate system, and β represents the yaw angle of the robot. v is the longitudinal distance from the front and rear wheels to the center of the robot, 2l t is the distance between the left and right wheels of the robot, [v x ,v y ,γ] T represents the longitudinal velocity, lateral velocity and angular velocity in the robot coordinate system, [v xi ,v yi ,γ i ] T (i=fl,fr,rl,rr) represents the longitudinal velocity, lateral velocity and angular velocity of each wheel coordinate system. i (i=fl,fr,rl,rr) represents the linear velocity of the four wheels, δ i (i=fl,fr,rl,rr) represents the turning angle of the four wheels, γ i (i=fl, fr, rl, rr) represents the angular velocity of the four wheels.

[0052] Based on the above known conditions, the relationship between the robot's four wheel speeds, rotation angles and the robot's posture in the global coordinate system can be obtained as follows:

[0053]

[0054] Among them, c i =cos(φ+δ i ), s i =sin(φ+δ i ), (x i ,y i )i=fl, rl, rr, fr are the coordinates of the four wheels in the robot coordinate system, which are (l v ,l t ), (-lv ,l t ), (-l v ,-l t ), (l v ,-l t ). Thus, the kinematic equation of the four-wheel independent drive and four-wheel independent steering mobile robot can be obtained as:

[0055]

[0056] For the convenience of derivation, the nominal system (Formula (2)) is expressed as:

[0057]

[0058] where ξ=[X,Y,φ] T is the state variable, u=[v fl ,v rl ,v rr ,v fr ,γ fl ,γ rl ,γ rr ,γ fr ] T is the input variable.

[0059] In the embodiment, step 1 further includes step 1.2: taking into account the external interference to the system, the kinematic model is converted into a linear time-varying model subject to external interference using a first-order quotient difference method.

[0060] When using nonlinear model predictive control to solve optimization control problems, the complexity of the robot system and the consideration of various constraints make the solution extremely complex. To ensure the real-time performance of the four-wheel independent drive and four-wheel independent steering robot system, it is necessary to use linear time-varying model predictive control to solve it. In this embodiment, equation (3) is first linearized.

[0061] Assume that the control input obtained by optimization at the previous time k-1 is u(k-1). At the current time k, the control input remains unchanged in the prediction domain, thus obtaining the reference system expression:

[0062]

[0063] At the sampling point (ξ R (k),u R (k+1)), linearize formula (4) and discretize formula (3) using the first-order quotient difference method, and convert the four-wheel independent drive and four-wheel independent steering nonlinear model into a linear time-varying model:

[0064] ξ(k+i|k)=a(k)ξ(k+i-1|k)+b(k)u(k+i-1|k) (5)

[0065] The linear time-varying model considering external interference is:

[0066] ξ(k+i|k)=a(k)ξ(k+i-1|k)+b(k)u(k+i-1|k)+d(k|k) (6)

[0067] Among them, d(k|k) is the high-order term after discretization and linearization and the external interference term, which can be collectively referred to as the unknown term. is the Jacobian matrix of f with respect to the state variables, is the Jacobian matrix of the control variable f, T is the time interval, and I is the identity matrix of the corresponding dimension.

[0068] In the embodiment, step 2 further includes step 2.1: based on the established linear time-varying model subject to external interference, designing a bounded error disturbance observer to observe the external interference received by the system, and determining an upper bound of the disturbance observation error.

[0069] In order to improve the system's anti-interference ability, the impact of external interference should be considered during the design of the safety predictive controller. Since the expression of external interference is difficult to determine directly, it is necessary to observe the disturbance. In order to be able to use the control barrier function input to the state safety, it is necessary to observe the uncertain terms in the control barrier function. At the same time, to further ensure safety, it is necessary to obtain the upper bound of the disturbance observation error, using the disturbance observation value and the upper bound of the observation error to replace the unknown terms. Therefore, the design goal of the disturbance observer in the embodiment is to obtain the upper bound of the disturbance observation error while observing the disturbance.

[0070] In some embodiments, step 2 is for the following discrete linear system with external interference:

[0071] x(k+1)=ax(k)+bu(k)+b d d(k) (7)

[0072] Where x(k)∈R m is the state variable, u(k)∈R n is the system control variable, d(k)∈R p For external disturbance.

[0073] Assumption 2: Assume that the input matrix b and the perturbation matrix b in the system d are all full-rank matrices, i.e., rank(b) = n, rank(b d )=p.

[0074] Assumption 3: External disturbance d(k)=[d1(k),d2(k),...d p (k)] T and its increment △d(k)=d(k+1)-d(k)=[△d1(k),△d2(k),...△d p (k)] T are bounded, that is, there is a constant τ i and Make |d i (k)|≤τ i ,

[0075] For the disturbed discrete linear system (6), the following disturbance observer is designed to estimate the disturbance d(k) in the system:

[0076]

[0077] Among them, g(k) is the intermediate variable of the observer, g(k+1) is the intermediate variable of the next moment, is the disturbance estimate, κ=diag{κ1,κ2,...,κ p}Satisfy|κ i |<1,i=1,2,...,p.

[0078] Theorem 1: When the two assumptions above are met, the disturbance observer (8) is used to observe the disturbance in the discrete system (7). The disturbance observation error of the external disturbance is Bounded and converges to the bounded region

[0079] In the embodiment, step 2 further includes step 2.2: constructing an obstacle avoidance constraint condition input to a state-safe control obstacle function based on the disturbance observation value and the upper bound of the observation error obtained in step 2.1.

[0080] Safety is a crucial characteristic of robot tracking control. It is crucial to ensure that the robot remains in a safe zone during tracking. Inspired by the control Lyapunov function, which is used to ensure system stability, the control obstacle function employs the concept of forward invariant sets to ensure the safety of the control system.

[0081] For a nonlinear affine system and a perturbed nonlinear affine system:

[0082]

[0083] in, are system state variables and control variables, respectively. is a time-varying bounded disturbance. f,g are local Lipschitz functions, and the disturbance d is also a local Lipschitz function.

[0084] First, the relevant definitions of control barrier function and safety are given:

[0085] Definition 1: If a continuous function α:[0,∞)→[0,∞) is strictly monotonically increasing, satisfies α(0)=0 and as r→∞, α(r)=∞, then α belongs to k ∞ Class function.

[0086] Definition 2: If the continuous function α:(-∞,∞)→(-∞,∞) is strictly monotonically increasing, satisfies α(0)=0 and as r→-∞, α(r)=-∞, α(r)=∞, then α belongs to the extended k ∞ Class function (k ∞,e ).

[0087] Definition 3: Consider a set and a continuously differentiable function h(ξ) satisfies:

[0088]

[0089] in, Not empty and There are no isolated points, i.e. and

[0090] For system (9), given a closed set Satisfying (11), given any initial value Any t>0 satisfies It is called a set It is positive and invariant.

[0091] If the collection Satisfying the forward invariance, the system (Formula (9)) in the set It is safe. It can ensure the collection The forward invariant controller is called a protection controller. Therefore, in the embodiment, a suitable protection controller can be obtained by controlling the barrier function constraint.

[0092] Definition 4: Given a continuously differentiable function If there is a control set and α∈k ∞,e Make all satisfy:

[0093]

[0094] Then h(ξ) is defined on the set The control barrier function on . Among them, L f and L g is the Lie derivative of h with respect to f and g.

[0095] The control barrier function expanded to discrete form is:

[0096] △h(ξ(k),u(k))≥-αh(ξ(k)),0<α≤1 (13)

[0097] Among them, △h(ξ(k),u(k))=h(ξ(k+1))-h(ξ(k)).

[0098] Definition 5: Set Satisfying the forward invariance, the system (Formula (9)) in the set If there is a slightly larger set and ι∈k ∞ Satisfying the forward invariance, the disturbed system (Formula (10)) in the set is input-to-state safe, is defined as follows:

[0099]

[0100] Among them, ||d|| ∞ is the infinite norm of the perturbation. Due to the influence of the perturbation, the original set The security of the system may not be guaranteed when the forward invariance is satisfied, but the larger set after the perturbation d is considered The security of the system can be guaranteed when the positive invariance is satisfied.

[0101] Definition 6: For a disturbed system (Formula (10)), given a continuously differentiable function If there is a control set α∈k ∞,e and ρ∈k ∞ Make all satisfy:

[0102]

[0103] Then h(ξ) is defined on the set The input to the state safety control barrier function, ||d|| ∞ is the infinite norm of the perturbation.

[0104] Similarly, the control barrier function form expanded to discrete input to state safety is:

[0105] △h(ξ(k),u(k),d(k))≥-αh(ξ(k))-ρ||d(k)||∞ (16)

[0106] Among them, 0<α≤1, 0<ρ≤1, △h(ξ(k),u(k),d(k))=h(ξ(k+1))-h(ξ(k)).

[0107] Assumption 4: All obstacles in the environment can be regarded as a global coordinate system with position O obs =[O X ,O Y ] T , with a radius of R obs circular obstacle.

[0108] like Figure 3 As shown in the figure, a four-wheel independent drive and four-wheel independent steering robot travels from the starting point to the end point. To ensure that the robot avoids collisions with environmental obstacles during operation, safe obstacle avoidance control is achieved. Based on assumption 2, the safety constraints of the robot during driving are:

[0109] ||P(k)-O obs ||≥R obs (17)

[0110] Where P(k) = [X(k), Y(k)] T is the position of the robot at time k in the global coordinate system

[0111] From this we can get the control obstacle function of the robot:

[0112] h(ξ(k))=||P(k)-O obs ||-R obs (18)

[0113] The set that can ensure system security is

[0114]

[0115] When the system is disturbed, Slightly larger collection When forward invariance is satisfied, the set can be guaranteed It is a set of inputs to the state security, which can ensure the security of the system:

[0116]

[0117] From Definition 6, when there is disturbance interference in the system, when (18) satisfies the following conditions, it can be guaranteed that Satisfies forward invariance:

[0118] △h(ξ(k),u(k),d(k))≥-αh(ξ(k))-ρ||d(k)|| ∞ (twenty one)

[0119] The disturbance term in formula (21) is the unknown term d(k) in the linear time-varying model. Its value needs to be estimated. The discrete disturbance observer in step 2.1 is used to estimate its unknown value. The disturbance observer is designed as follows:

[0120]

[0121] Among them, g(k|k) is the intermediate variable of the observer, is the perturbation estimate, κ=diag{κ1,κ2,κ3} satisfies |κ i |<1, i=1,2,3. At the same time, from Theorem 1, we can know that the disturbance observer (22) is used to observe the disturbance in the discrete system (6), and the disturbance observation error of the external disturbance is Bounded and converges to the bounded region

[0122] In order to ensure the collection under disturbance Satisfy the forward invariance, that is, the system satisfies the control barrier function constraint condition input to the state safety. It is necessary to estimate the unknown term in the control barrier function constraint (Formula (21)) input to the state safety. In the embodiment, the disturbance observation value is used. and the perturbation observation upper bound To construct the unknown terms in the constraints, the constructed control obstacle avoidance constraint is expressed as follows:

[0123]

[0124] When the disturbance is too large, directly compensating for the disturbance may cause the control input to exceed the system input constraint, thereby affecting the normal operation of the system. To avoid this, this application combines the value of the disturbance observer with the model predictive control, using the disturbance observation value to replace the disturbance in the linear time-varying model (Formula (6)) for the design of the model predictive controller. The linear time-varying model (Formula (6)) can be rewritten as:

[0125]

[0126] Since the sampling time is very short and the disturbance is unpredictable, it is assumed that the disturbance value is

[0127] The application constructs a control barrier function to avoid barrier constraints according to disturbance observation values and error upper bounds, provides an effective theoretical basis and constraint mechanism for robot obstacle avoidance, and ensures the safety and reliability of the robot in the obstacle avoidance process.

[0128] It should be noted that direct compensation of disturbance can only respond according to the current feedback signal, and may not effectively handle the constraint conditions, resulting in unstable system operation or violation of constraints. The MPC considering disturbance can add a disturbance prediction model in the controller design and consider the influence of disturbance in the optimization process, thereby enhancing the robustness and stability of the system. At the same time, since the constraint influence is considered in the MPC solving process, the constraint requirements of the system can be better met.

[0129] In some embodiments, the specific steps of step 3 are as follows: the constraint conditions established above can ensure that the robot realizes the safe obstacle avoidance task under the disturbance condition, but when there is a long-distance obstacle, the obstacle avoidance constraint condition causes unnecessary computational burden for the solution of the robot controller, and the robot can complete the obstacle avoidance task by executing the obstacle avoidance instruction at a certain distance from the obstacle.

[0130] As can be seen from equation (18), the Euclidean distance between the robot and the obstacle is:

[0131] h(ξ(k))=||P(k)-O obs ||-R obs (25)

[0132] The robot obstacle avoidance trigger mechanism can be established according to the distance condition as follows:

[0133]

[0134] where h a is a given distance trigger threshold, and through the obstacle avoidance trigger mechanism, it can be selected whether to execute the obstacle avoidance task.

[0135] The embodiment can obtain the position state information of the robot and the target obstacle information in real time, calculate the distance between the two, and combine the pre-constructed distance trigger mechanism to trigger the obstacle avoidance action in time when the robot approaches the obstacle, realize fast and accurate obstacle avoidance, and avoid collision accidents.

[0136] In one embodiment, the design of the safety predictive controller based on the linear time-varying model in step 4 includes: constructing a predictive model of the safety predictive controller (the following formula (31)); establishing a cost function (the following formula (37)) based on the predictive model and the initial control increment sequence; establishing target constraints of the cost function (the following formulas (38)-(41)); determining the cost at the current moment based on the cost function, and solving the cost function based on the target constraints with the goal of minimizing the cost to obtain the control increment sequence at the current moment.

[0137] In another embodiment, in step 4, a prediction model of a safety prediction controller is constructed (the following formula (31)); based on the prediction model and the initial control increment sequence, a cost function considering the terminal cost is established (the following formula (43)); the target constraint conditions of the cost function are established (the following formulas (44)-(47)); the cost at the current moment is determined according to the cost function, and with the minimum cost as the goal, the cost function is solved based on the target constraint conditions to obtain the control increment sequence at the current moment.

[0138] Further explanation is given below.

[0139] Based on the linear time-varying model (Formula (24)), a safety predictive controller is designed. First, the control input variable u(k+i-1|k) in the linear time-varying model (Formula (24)) is converted into the form of control increment △u(k+i-1|k) to satisfy the control increment constraint.

[0140] First, define a new state variable:

[0141]

[0142] Let △u(k|k)=u(k|k)-u(k-1|k), then formula (24) can be rewritten as:

[0143]

[0144] in, N x =3, N u =8 are the number of state variables and control variables of the original system respectively.

[0145] The output equation of the system is:

[0146]

[0147] in,

[0148] Define N p 、N c are the prediction time domain and control time domain of the prediction model respectively, where Np ≥N c At the current moment, the control increment sequence acts on the system. After each iteration, the system predicts the time domain N p The output of can be expressed as:

[0149]

[0150] Rewriting formula (30) into a matrix form, we can obtain the prediction model of the safety predictive controller:

[0151]

[0152] in, Predict the output of each step in the time domain for the system, To control the increment sequence, is the perturbation sequence, Θ(k) and Ξ(k) are the corresponding coefficient matrices.

[0153] Based on the prediction model (Formula (31)), the following cost function is established:

[0154]

[0155] in, is the weight coefficient, and ε is the relaxation factor. Q and R are the weight matrices of the state variables and the control increment, respectively. The former represents the restriction of the system state, and the latter is the restriction of the control increment, which ensures the continuity of the controller and the responsiveness of the actuator. Substituting formula (31) into formula (32) can simplify the objective function to:

[0156]

[0157] in, The matrix M is a constant at each sampling moment, so it can be omitted and the objective function in the standard quadratic programming form can be obtained as follows:

[0158]

[0159] Among them, △U ε =[△U(k) T ,ε],

[0160] Due to the actuator constraints, the constraints on the linear and angular velocities of the four wheels are as follows:

[0161]

[0162] in, represents the Kronecker product, and I8 is the identity matrix of dimension 8.

[0163] The constraints on the velocity increments and angular velocity increments of the four wheels are as follows:

[0164] △U min (k)≤△U(k)≤△U max (k) (36)

[0165] With the above cost function and constraints, as well as the safety constraints of the control obstacle function input to the state safety, the problem of solving the optimal control increment sequence △U(k) is transformed into an optimization control problem. During the optimization solution process, a distance-triggered obstacle avoidance mechanism is introduced. The solution includes the following two embodiments:

[0166] One embodiment: establishing a cost function (Formula (37)) based on the prediction model and the initial control increment sequence; establishing target constraints of the cost function (Formula (38)-Formula (41))

[0167] △U * (k)=argminJ(Y(k),△U(k) (37)

[0168]

[0169] △U min ≤△U(k)≤△U max (39)

[0170] ξ(k+N p |k)∈X ξ (40)

[0171]

[0172] Where P is the system terminal cost weight matrix, formula (38) is the control variable constraint, U(k-1) is the control input of the system at the previous moment, formula (39) is the control increment constraint, formula (40) is the system terminal constraint, and formula (41) is the safety constraint to ensure the robot's obstacle avoidance. A distance trigger mechanism is introduced in the obstacle avoidance constraint. When the Euclidean distance between the robot's current state and the obstacle is less than the set threshold, the obstacle avoidance constraint is considered in the controller solution process and the obstacle avoidance task is executed. By reducing the constraints of the controller in the global solution process, the computational complexity of the system is reduced.

[0173] At each time point k, the cost of the current moment is determined according to the cost function. With the minimum cost as the goal, the cost function is solved based on the target constraints to obtain the control increment sequence of the current moment:

[0174] △U * (k)=[△u(k|k),△u(k+1|k),...,△u(k+Nc -1|k)] (42)

[0175] In another embodiment, based on the prediction model and the initial control increment sequence, a cost function (Formula (43)) considering the terminal cost is established; target constraints of the cost function are established (Formula (44)-Formula (47));

[0176] △U * (k)=argmin(J(Y(k),△U(k)+ξ T (k+N p |k)Pξ(k+N p |k)) (43)

[0177]

[0178] △U min ≤△U(k)≤△U max (45)

[0179] ξ(k+N p |k)∈X ξ (46)

[0180]

[0181] Where P is the system terminal cost weight matrix, formula (44) is the control variable constraint, U(k-1) is the control input of the system at the previous moment, formula (45) is the control increment constraint, formula (46) is the system terminal constraint, and formula (47) is the safety constraint to ensure the robot's obstacle avoidance. A distance trigger mechanism is introduced in the obstacle avoidance constraint. When the Euclidean distance between the robot's current state and the obstacle is less than the set threshold, the obstacle avoidance constraint is considered in the controller solution process and the obstacle avoidance task is executed. By reducing the constraints of the controller in the global solution process, the computational complexity of the system is reduced.

[0182] At each time point k, the cost of the current moment is determined according to the cost function. With the minimum cost as the goal, the cost function is solved based on the target constraints to obtain the control increment sequence of the current moment:

[0183] △U * (k)=[△u(k|k),△u(k+1|k),...,△u(k+N c -1|k)] (48)

[0184] By adding the first element in the optimal control increment sequence to the control input at the previous moment, the system input at each current moment k can be obtained as:

[0185] u(k)=u(k-1)+△u(k|k) (49)

[0186] By inputting the control input into the robot system at each sampling moment, safe obstacle avoidance control of the robot can be achieved.

[0187] This application designs a safety prediction controller based on a linear time-varying model and uses the controller to perform safety prediction control. It can comprehensively consider the robot's motion state, external interference and obstacle avoidance requirements, achieve optimized control of the robot, and improve the robot's operating efficiency and safety.

[0188] The following is a simulation of this application, as follows:

[0189] In order to verify the effectiveness of the obstacle avoidance control method proposed in this invention, this section designs an obstacle avoidance control verification example in MATLAB for the four-wheel independent drive and four-wheel independent steering WMR system.

[0190] This application considers the process of the four-wheel independent drive and four-wheel independent steering WMR from a given starting point to a specified end point under disturbance conditions. The specific parameters of the robot are set as follows: the initial position of the robot in the global coordinate is (6,6), the end position is (0,0), and the distance between the front and rear wheels of the robot is 2l v =0.4m, the distance between the left and right wheels is 2l t =0.4m.

[0191] The sampling time interval of the discrete system is T = 0.2s, and the prediction time domain N of the safety predictive controller is p =8, control time domain N c =8, and the controller weight parameter matrices are: state constraint weight matrix Q = 50I3, control constraint weight matrix R = 40I8, and terminal constraint weight matrix P = 100I3, where I3 and I8 are identity matrices of dimensions 3 and 8, respectively. In practice, considering the speed limits of the robot's wheel motors, the four wheel speed constraints are set to (-2, 2) m / s, and the angular velocity constraint is set to (-1, 1) rad / s.

[0192] The obstacle is set to be centered at O obs =(-3,-3), radius R obs =1m. The disturbances are set as d1 = 0.01sin(0.2t), d2 = 0.01cos(0.2t), d3 = 0.01sin(0.3t), and the disturbance observer parameters are designed as κ = diag{0.1 0.10.1}. The control barrier function parameters input to the state safety are designed as α = 0.5, ρ = 0.5, and the distance trigger parameter h a =1.

[0193] like Figure 4The figure shows the robot's obstacle avoidance effect. The robot can reach the final position (0, 0) from the initial position (6, 6) and successfully avoid obstacles. The robot determines whether the obstacle avoidance trigger condition is met based on the real-time state and the distance between the obstacle. The purple area along the way is the area where the robot performs obstacle avoidance behavior.

[0194] Figure 5 and Figure 6 is the control variable obtained by solving the machine controller, Figure 5 is the linear velocity of the four wheels, Figure 6 is the angular velocity of the four wheels. Due to the independent driving and steering characteristics of the wheels, the linear velocity and angular velocity of the four wheels are different. In order to establish accurate input to the state safety control obstacle function constraint conditions, it is necessary to accurately observe the disturbance. Figure 7 This is the observation effect of the bounded error disturbance observer on the external disturbance. It can be seen that the observer can quickly observe the external disturbance from the initial value.

[0195] At the same time, in order to verify the influence of the key parameter ρ of the control obstacle function input to the state safety on the robot's obstacle avoidance effect, Figure 8 The following figure compares the obstacle avoidance performance under different parameters: ρ. The robot adjusts its distance to obstacles based on the parameter ρ. When ρ increases, the robot moves closer to the obstacle, while when ρ decreases, the robot moves away from it. Therefore, when the system is disturbed, adjusting ρ can adjust the control obstacle function constraints to achieve better obstacle avoidance.

[0196] The above is a detailed introduction to the safe predictive control method for anti-interference and triggered obstacle avoidance of a wheeled mobile robot provided by this application. This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the concept of this application and should not be understood as limiting the scope of protection of this application.

Claims

1. A safe predictive control method for anti-interference and triggered obstacle avoidance of a wheeled mobile robot, characterized in that: The control method is applicable to a four-wheel independent drive and four-wheel independent steering mobile robot system, and the method includes: Establishing a kinematic model of a four-wheel independent drive and four-wheel independent steering mobile robot, and converting the kinematic model into a linear time-varying model subject to external interference; Based on the linear time-varying model, a bounded error disturbance observer is designed to observe the external disturbance to the mobile robot system to obtain a disturbance observation value, and an upper bound of the disturbance observation error is determined. According to the disturbance observation value and the upper bound of the disturbance observation error, an obstacle avoidance constraint condition of a control obstacle function is constructed; Calculating the distance between the robot and the target obstacle based on the acquired position state information of the robot and the perceived target obstacle information; and when the distance triggers a pre-constructed distance trigger mechanism, designing a safety predictive controller based on the linear time-varying model using the obstacle avoidance constraint condition; The safety prediction controller is used to perform safety prediction control.

2. The safety prediction control method according to claim 1, characterized in that: The kinematic model of the four-wheel independent drive and four-wheel independent steering mobile robot is expressed as follows: in is the speed of the robot in the X direction in the global coordinate system, is the speed of the robot in the Y direction in the global coordinate system, is the rate of change of the robot’s heading angle φ in the global coordinate system, (x i ,y i ) are the coordinates of the four wheels in the robot coordinate system, where i = fl, rl, rr, fr; v i represents the linear velocity of the four wheels, γ i Represents the angular velocity of the four wheels.

3. The safety prediction control method according to claim 1, characterized in that: The linear time-varying model is expressed as: ξ(k+i|k)=a(k)ξ(k+i-1|k)+b(k)u(k+i-1|k)+d(k|k); Where ξ(k+i|k) represents the system state variables predicted i steps later at the current time k, ξ=[X,Y,Φ] T Represent the robot's position X, position Y and heading angle φ in the global coordinate system, respectively. I is the unit matrix of the corresponding dimension, T is the sampling time interval, is the Jacobian matrix of f with respect to the state variable ξ(k), is the Jacobian matrix of f with respect to the control variable u(k), u(k+i-1|k) represents the control input variable applied to the system at the current time k, predicted i-1 steps later, and d(k|k) is the high-order term after discretization and linearization and the external interference term.

4. The safety prediction control method according to claim 1, characterized in that: The bounded error disturbance observer is expressed as: Among them, g(k) represents the intermediate variable of the observer, g(k+1) is the intermediate variable of the next moment, is the disturbance estimate, d(k) is the external disturbance of the system, κ is a diagonal matrix, κ=diag(κ1,κ2,...,κ p ), |κ i |<1,i=1,2,...,p, p is the dimension, I p is the p-dimensional identity matrix, b d is the disturbance matrix, a and b are both system matrices, x(k) is the system state variable, and u(k) is the system control variable.

5. The safety prediction control method according to claim 4, characterized in that: The upper bound of the disturbance observation error is expressed as: Among them, e d (k) represents the actual external disturbance d(k) and the disturbance observer estimate at time k The error vector between di (k) composition, e di (k) is the perturbation observation error vector e d The i-th component of (k), ζ i is a preset fixed value, κ i is the i-th element of the diagonal matrix κ.

6. The safety prediction control method according to claim 1, characterized in that: The distance trigger mechanism is expressed as: Among them, h a is the given distance trigger threshold, and h(ξ(k)) is the distance between the robot and the target obstacle.

7. The safety prediction control method according to claim 1, characterized in that: The design of a safety predictive controller based on the linear time-varying model includes: Constructing a predictive model for a safety predictive controller; establishing a cost function based on the prediction model and the initial control increment sequence; Establish a target constraint condition for the cost function, determine the cost at the current moment according to the cost function, take minimizing the cost as the goal, solve the cost function based on the target constraint condition, and obtain the control increment sequence at the current moment.

8. The safety prediction control method according to claim 7, characterized in that: A cost function is established based on the prediction model and the initial control increment sequence, including: Based on the prediction model and the initial control increment sequence, a cost function considering the terminal cost is established.

Images