Safety prediction control method for resisting disturbance and triggering obstacle avoidance of wheeled mobile robot

Abstract

The invention 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, and the method mainly comprises the steps: building a kinematics model of a four-wheel independent steering and four-wheel independent driving mobile robot, and carrying out the linear time-varying model conversion; designing a bounded error disturbance observer for observing external interference of the robot, obtaining an observation error upper bound, and establishing a control obstacle function obstacle avoidance constraint condition input to state safety based on a disturbance observation value and the error upper bound; constructing a distance triggering obstacle avoidance mechanism according to the current state information and the obstacle distance; and designing a safety prediction controller, and introducing a distance triggering mechanism and an obstacle avoidance constraint condition into a solving process of an optimization control problem. According to the safety prediction control method provided by the invention, the adaptability and robustness of the control system to interference can be improved, so that the robot can still keep a stable operation state when facing various uncertain external interferences.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot control, and more particularly, to a safety predictive control method for a wheeled mobile robot to resist disturbances and trigger obstacle avoidance. Background Art

[0002] With the rapid development of robot technology, wheeled mobile robots have been widely used in fields such as medical treatment, industrial automation, service industry, intelligent transportation, and security inspection. When performing tasks, these robots need to navigate autonomously in complex and dynamic environments to ensure the accuracy and safety of their movements, which has become a hot topic in current research.

[0003] To ensure the safety of the robot system, researchers have developed various control methods, including artificial potential field method, reachability analysis, nonlinear model predictive control, etc. The control barrier function proposed in recent years can directly establish the connection between the control input and the system safety constraints by introducing a barrier function, restricting the system state within the safe region, and ensuring that the system state tends to be stable while satisfying the constraints. However, in actual systems, even with obstacle avoidance constraints, disturbances will inevitably affect the constraint conditions, so it is necessary to consider the impact of disturbances on system safety. Summary of the Invention

[0004] The technical objective of this application is to further solve the impact of disturbances on the safety of the robot system, and provide a safety predictive control method for a wheeled mobile robot to resist disturbances and trigger obstacle avoidance, reducing the impact of disturbances on system safety, thereby ensuring the safe and stable motion control of the robot.

[0005] To achieve the above technical objective, the following technical solutions are adopted in this application.

[0006] An embodiment of this application provides a safety predictive control method for a wheeled mobile robot to resist disturbances and trigger obstacle avoidance. The control method is applicable to a four-wheel independent drive and four-wheel independent steering mobile robot system, and the method includes:

[0007] 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 affected by external disturbances;

[0008] Based on the linear time-varying model, design a bounded error disturbance observer to observe the external disturbances received by the mobile robot system to obtain disturbance observation values, and determine the upper bound of the disturbance observation error. According to the disturbance observation values and the upper bound of the disturbance observation error, construct a control barrier function obstacle avoidance constraint condition;

[0009] According to the obtained position status information of the robot and the sensed target obstacle information, calculate the distance between the robot and the target obstacle. When the distance triggers a pre-constructed distance trigger mechanism, use the obstacle avoidance constraint conditions to design a safety prediction controller based on the linear time-varying model;

[0010] Use the safety prediction controller for safety prediction control.

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

[0012]

[0013] Where, is the velocity of the robot in the X direction of the position in the global coordinate system, is the velocity of the robot in the Y direction of the position in the global coordinate system, is the change rate of the heading angle φ of the robot 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 velocities of the four wheels, γ i represents the angular velocities of the four wheels.

[0015] Further, 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 variable predicted i steps after the current moment k, ξ = [X, Y, Φ] T respectively represent the position X, position Y, and heading angle φ of the robot in the global coordinate system, I is the identity 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 i-1 steps after prediction at the current time k. d(k∣k) is the discretized and linearized high-order term and the external disturbance term.

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

[0019]

[0020] where g(k) represents the intermediate variable of the observer, and g(k + 1) is the intermediate variable at the next time. is the disturbance estimate value, d(k) is the external disturbance of the system, κ is a diagonal matrix, κ = diag(κ1, κ2, …, κ p ), |κ i | < 1, i = 1, 2, …, p, where p is the dimension, and 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] Still further, the upper bound of the disturbance observation error is expressed as:

[0022]

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

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

[0025]

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

[0027] Still further, the design of the safety prediction controller based on the linear time-varying model includes:

[0028] Construct the prediction model of the safety prediction controller;

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

[0030] Establish the target constraint conditions of the cost function, determine the cost at the current moment according to the cost function, aim at minimizing the cost, and solve the cost function based on the target constraint conditions to obtain the control increment sequence at the current moment.

[0031] Further, establishing a cost function based on the prediction model and the initial control increment sequence includes: establishing a cost function considering the terminal cost based on the prediction model and the initial control increment sequence.

[0032] Compared with the prior art, the safety predictive control method for anti-disturbance and trigger obstacle avoidance of the wheeled mobile robot provided by the embodiments of the present application has the following beneficial technical effects: establishing a kinematic model of a four-wheel independently driven and four-wheel independently steered mobile robot and transforming it into a linear time-varying model subject to external disturbances can more accurately describe the complex situation of the robot during actual operation, providing an accurate model basis for the design of subsequent control strategies. Designing a bounded error disturbance observer to observe external disturbances can not only obtain disturbance information in real time but also determine the upper bound of the disturbance observation error, which helps improve the adaptability and robustness of the control system to disturbances, enabling the robot to maintain a stable operating state in the face of various uncertain external disturbances. 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 disclosure of the present application in any way. Additionally, the shapes and proportional dimensions of the components in the figures are only schematic and are used to assist in understanding the present application, rather than specifically limiting the shapes and proportional dimensions of the components of the present application. Those skilled in the art can, under the teaching of the present application, select various possible shapes and proportional dimensions according to specific circumstances to implement the present application. In the drawings:

[0034] Figure 1 is a schematic flowchart of the safety predictive control method for anti-disturbance and trigger obstacle avoidance of the wheeled mobile robot provided in the embodiment;

[0035] Figure 2 is a schematic diagram of the pose and structure of a four-wheel independently driven and four-wheel independently steered robot in the world coordinate system in the embodiment;

[0036] Figure 3 is a schematic diagram of obstacle avoidance of a four-wheel independently driven and four-wheel independently steered robot in the embodiment;

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

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

[0039] Figure 6 Schematic diagram of the angular velocity input of the 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 the obstacle avoidance effect under different ρ parameters in the embodiment. Detailed implementation manners

[0042] In order to enable those skilled in the art to better understand the technical solutions in this application, the following will clearly and completely describe the technical solutions in the embodiments of this application with reference to the accompanying drawings in the embodiments of this application. Obviously, the described embodiments are only a part of the embodiments of this application, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in this application without creative efforts shall fall within the protection scope of this application.

[0043] The embodiment of this application provides a safety prediction control method for anti-disturbance and trigger obstacle avoidance of 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 affected by external disturbances;

[0045] Step 2: Based on the linear time-varying model, design a bounded error disturbance observer to observe the external disturbances received by the mobile robot system to obtain disturbance observation values, and determine the upper bound of the disturbance observation error. According to the disturbance observation values and the upper bound of the disturbance observation error, construct an obstacle avoidance constraint condition;

[0046] Step 3: According to the obtained position state information of the robot and the perceived target obstacle information, calculate the distance between the robot and the target obstacle. When the distance triggers a pre-constructed distance trigger mechanism, use the obstacle avoidance constraint condition to design a safety prediction controller based on the linear time-varying model;

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

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

[0049] Before establishing the kinematic model of a four-wheel independently driven and four-wheel independently steered wheeled mobile robot, the following assumptions are made for the system:

[0050] Assumption 1: The four-wheel independently driven and four-wheel independently steered wheeled mobile robot moves on a flat and hard ground, and there is no vertical movement.

[0051] As Figure 2 shown is the pose of the robot 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. l 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 in each wheel coordinate system. v i (i = fl, fr, rl, rr) represents the linear velocities of the four wheels, δ i (i = fl, fr, rl, rr) represents the steering angles of the four wheels, and γ i (i = fl, fr, rl, rr) represents the angular velocities of the four wheels.

[0052] Based on the above known conditions, the relationships between the speeds and steering angles of the four wheels of the robot and the robot's pose 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 ), (-l​v , l t ), (-l v , -l t ), (l v , -l t ). Thus, the kinematic equation of the four-wheel independently driven and four-wheel independently steered mobile robot can be obtained as follows:

[0055]

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

[0057]

[0058] where ξ = [X, Y, φ] T is the state variable, and 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: Considering the situation where the system is affected by external disturbances, the kinematic model is converted into a linear time-varying model affected by external disturbances by using the first-order quotient difference method.

[0060] When using nonlinear model predictive control to solve the optimal control problem, due to the complexity of the robot system and various constraints considered, the solution becomes extremely complex. To ensure the real-time performance of the four-wheel independently driven and four-wheel independently steered robot system, linear time-varying model predictive control needs to be used for solution. In the embodiment, Equation (3) is first linearized.

[0061] Assume that at the previous moment k - 1, the control input u(k - 1) is obtained through optimal solution. At the current moment k, the control input within the prediction horizon remains unchanged, thus obtaining the reference system expression:

[0062]

[0063] At the sampling point (ξ R (k), u R (k + 1)), Equation (4) is linearized and Equation (3) is discretized by using the first-order quotient difference method to convert the four-wheel independently driven and four-wheel independently steered 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 disturbances is as follows:

[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 and external disturbance term after discretization and linearization, which can be collectively referred to as the unknown term. is the Jacobian matrix of f with respect to the state variable, is the Jacobian matrix of f with respect to the control variable, T is the adopted 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 affected by external disturbances, design a bounded error disturbance observer to observe the external disturbances received by the system and determine the upper bound of the disturbance observation error.

[0069] To improve the anti-interference ability of the system, the influence of external disturbances should be considered in the design process of the safety prediction controller. Since the expression of external disturbances is difficult to directly determine, it is necessary to observe the disturbances. In order to be able to use the control barrier function for input-to-state safety, it is necessary to observe the uncertain terms in the control barrier function. At the same time, in order to further ensure safety, it is necessary to obtain the upper bound of the disturbance observation error, and use the disturbance observation value and the upper bound of the observation error to replace the unknown term. Therefore, the design goal of the disturbance observer in the embodiment is to observe the disturbance and obtain the upper bound of the disturbance observation error at the same time.

[0070] In some embodiments, step 2 is directed to the following discrete linear system with external disturbances:

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

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

[0073] Assumption 2: Assume that the input matrix b and the disturbance matrix b d in the system are both column full-rank matrices, that is, rank(b) = n, rank(b d ) = p.

[0074] Hypothesis 3: The external disturbance \(d(k)=[d_1(k),d_2(k),\cdots,d p (k)] T and its increment \(\Delta d(k)=d(k + 1)-d(k)=[\Delta d_1(k),\Delta d_2(k),\cdots,\Delta d p (k)] T are all bounded, that is, there exist constants \(\tau i and such that \(|d i (k)|\leq\tau i ,

[0075] For the perturbed discrete linear system (6), design the following disturbance observer to estimate the disturbance \(d(k)\) in the system:

[0076]

[0077] where \(g(k)\) is the intermediate variable of the observer, \(g(k + 1)\) is the intermediate variable at the next moment, is the disturbance estimation value, \(\kappa=\text{diag}\{\kappa_1,\kappa_2,\cdots,\kappa p}\) satisfies \(|\kappa i |\lt1, i = 1,2,\cdots,p\).

[0078] Theorem 1: Under the premise that the previous two hypotheses hold, when using the disturbance observer (8) to observe the interference existing in the discrete system (7), the disturbance observation error of the external disturbance is bounded and converges to a bounded region

[0079] Step 2 in the embodiment further includes Step 2.2: Based on the disturbance observation value and the upper bound of the observation error obtained in Step 2.1, construct an input-to-state safety control barrier function for obstacle avoidance constraints.

[0080] Safety is an important characteristic in the robot tracking control process, and it is necessary to ensure that the state of the robot is always in a safe area during the tracking process. The control Lyapunov function is used to ensure the stability of the system. Inspired by the control Lyapunov function, the control barrier function uses the idea of forward invariant sets to ensure the safety of the control system.

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

[0082]

[0083] where, are the system state variables and control variables respectively, It is a time-varying bounded disturbance. f and g are locally Lipschitz functions, and the perturbation d is also locally Lipschitz.

[0084] First, the related definitions of control barrier functions and safety are given:

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

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

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

[0088]

[0089] where, is non-empty and has no isolated points, that is, and

[0090] For system (9), given a closed set satisfying (11), given any initial value for any t > 0 satisfying then the set is said to be forward invariant.

[0091] If the set satisfies forward invariance, then system (Equation (9)) is safe on the set . A controller that can guarantee the forward invariance of the set is called a protective control. Therefore, in the embodiments, a suitable protective controller can be obtained through the constraint conditions of the control barrier function.

[0092] Definition 4: Given a continuously differentiable function if there exists a control set and α ∈ k ∞,e such that for all satisfies:

[0093]

[0094] then h(ξ) is said to be defined on the set The control barrier function on it. Among them, L f and L g are the Lie derivatives of h with respect to f and g.

[0095] The extended discrete control barrier function 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: The set satisfies forward invariance, then the system (Equation (9)) is safe on the set If there exists a slightly larger set and ι ∈ k ∞ satisfies forward invariance, then the perturbed system (Equation (10)) is input-to-state safe on the set The definition of is as follows:

[0099]

[0100] Among them, ||d|| ∞ is the infinity norm of the perturbation. Due to the influence of the perturbation, the original set satisfying forward invariance may not guarantee the safety of the system, but considering the larger set satisfying forward invariance after the action of the perturbation d may guarantee the safety of the system.

[0101] Definition 6: For the perturbed system (Equation (10)), given a continuously differentiable function If there exists a control set α ∈ k ∞,e and ρ ∈ k ∞ such that all satisfy:

[0102]

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

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

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

[0106] Among them, \(0 \lt lpha\leq1\), \(0 \lt ho\leq1\), \(\Delta h(\xi(k), u(k), d(k)) = h(\xi(k + 1)) - h(\xi(k))\).

[0107] Assumption 4: All obstacles existing in the environment can be regarded as a circular obstacle with a position of \(O\) = \([O_{x}, O_{y}]\) in the global coordinate system and a radius of \(R\). obs =[O X ,O Y T , with a radius of \(R\) obs circular obstacle.

[0108] As Figure 3 shown, the four-wheel independently driven and four-wheel independently steered robot travels from the starting point to the end point. To ensure that the robot avoids collisions with environmental obstacles during operation and realizes safe obstacle avoidance control. From Assumption 2, the safety constraint of the robot during driving can be obtained as:

[0109] \(\left\lVert P(k)-OightVert\geq R\) obs (17) obs (17)

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

[0111] Thus, the control barrier function of the robot can be obtained as:

[0112] \(h(\xi(k))=\left\lVert P(k)-OightVert - R\) obs (18) obs (18)

[0113] The set that can ensure the safety of the system is

[0114]

[0115] When the system is disturbed, when a set slightly larger than satisfies forward invariance, it can ensure that the set is input-to-state safe and can ensure the safety of the system: .

[0116]

[0117] According to Definition 6, when there are disturbance interferences in the system, when (18) satisfies the following conditions, it can ensure that satisfies forward invariance:

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

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

[0120]

[0121] where g(k|k) is the intermediate variable of the observer, is the disturbance estimated value, κ = diag{κ1, κ2, κ3} satisfies |κ i | < 1, i = 1, 2, 3. At the same time, it can be seen from Theorem 1 that the disturbance observer (22) is used to observe the disturbance existing in the discrete system (6), and the disturbance observation error of the external disturbance is bounded and converges to a bounded region

[0122] To ensure that the set satisfies forward invariance under the disturbed condition, that is, the system satisfies the control barrier function constraint condition of input-to-state safety, it is necessary to estimate the unknown term in the input-to-state safety control barrier function constraint (formula (21)). In the embodiment, the disturbance observation value and the disturbance observation upper bound are used to construct the unknown term in the constraint condition, and the constructed control barrier function obstacle avoidance constraint condition is expressed as follows:

[0123]

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

[0125]

[0126] Due to the short sampling time and the unpredictability of the disturbance, it is assumed that the disturbance value is

[0127] This application constructs an obstacle avoidance constraint condition for the control barrier function based on the disturbance observation value and the error upper bound, providing an effective theoretical basis and constraint mechanism for the robot to avoid obstacles, and ensuring the safety and reliability of the robot during the obstacle avoidance process.

[0128] It should be noted that directly compensating for the disturbance can only respond based on the current feedback signal, and may not be able to effectively handle the constraint conditions, resulting in unstable system operation or violation of the constraints. MPC considering the disturbance can add a disturbance prediction model to the controller design and consider the influence of the disturbance during the optimization process, thereby enhancing the robustness and stability of the system. At the same time, since the influence of the constraints is considered during the MPC solution process, it can better meet the constraint requirements of the system.

[0129] In some embodiments, the specific steps of step 3 are as follows: The established constraint conditions above can ensure that the robot can achieve the safe obstacle avoidance task under disturbance. However, when there are obstacles at a long distance, the obstacle avoidance constraint conditions impose an unnecessary computational burden on the solution of the robot controller. The robot can complete the obstacle avoidance task by executing the obstacle avoidance command at a certain distance from the obstacle.

[0130] It can be seen from formula (18) that the Euclidean distance between the robot and the obstacle is:

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

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

[0133]

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

[0135] The embodiment can calculate the distance between the two by obtaining the position state information of the robot and the target obstacle information in real time, and combine the pre-constructed distance trigger mechanism, which can trigger the obstacle avoidance action in time when the robot approaches the obstacle, achieve fast and accurate obstacle avoidance, and avoid the occurrence of 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 prediction model of the safety predictive controller (the following formula (31)); establishing a cost function based on the prediction model and the initial control increment sequence (the following formula (37)); establishing the objective constraint conditions of the cost function (the following formulas (38)-(41)); determining the cost at the current moment according to the cost function, aiming at the minimum cost, solving the cost function based on the objective constraint conditions, and obtaining the control increment sequence at the current moment.

[0137] In another embodiment, in step 4, construct a prediction model of the safety predictive controller (the following formula (31)); based on the prediction model and the initial control increment sequence, establish a cost function considering the terminal cost (the following formula (43)); establish the objective constraint conditions of the cost function (the following formulas (44)-(47)); determine the cost at the current moment according to the cost function, aiming at the minimum cost, solve the cost function based on the objective constraint conditions, and obtain the control increment sequence at the current moment.

[0138] The following is a further explanation.

[0139] Based on the linear time-varying model (formula (24)), for the design of the safety predictive controller, first convert the control input variable u(k+i-1|k) in the linear time-varying model (formula (24)) into the form of the control increment △u(k+i-1|k) to facilitate meeting the control increment constraint.

[0140] First, define new state variables:

[0141]

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

[0143]

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

[0145] The output equation of the system is:

[0146]

[0147] where,

[0148] Define N p 、N c as the prediction time domain and the 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 step of iteration, the output within the system prediction time domain N p can be expressed as:

[0149]

[0150] Rewriting formula (30) into matrix form, the prediction model of the safety prediction controller can be obtained:

[0151]

[0152] where, is the output at each step within the system prediction time domain, is the control increment sequence, is the disturbance sequence, and Θ(k), Ξ(k) are the corresponding coefficient matrices.

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

[0154]

[0155] where, is the weight coefficient, and ε is the relaxation factor. Q and R are the weight matrices of the state variable and the control increment respectively. The former term represents the limitation of the system state, and the latter term is the limitation of the control increment, ensuring the continuity of the controller and the response ability of the actuator. Substituting formula (31) into formula (32) can simplify the objective function to:

[0156]

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

[0158]

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

[0160] Due to the actuator constraints, the constraint conditions for the linear velocity and angular velocity of the four wheels are as follows:

[0161]

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

[0163] The constraint conditions for the speed increment and angular velocity increment of the four wheels are as follows:

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

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

[0166] One of the embodiments: Establish a cost function (Equation (37)) based on the prediction model and the initial control increment sequence; establish the objective constraint conditions of the cost function (Equations (38) - (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] Among them, P is the system terminal cost weight matrix. Equation (38) is the control variable constraint, U(k - 1) is the control input of the system at the previous moment, Equation (39) is the control increment constraint, Equation (40) is the system terminal constraint, and Equation (41) is the safety constraint to ensure the robot's obstacle avoidance. A distance-triggered 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 during the controller solution process, and the obstacle avoidance task is executed. By reducing the constraint conditions in the global solution process of the controller, the computational complexity of the system is reduced.

[0173] At each moment k, determine the cost at the current moment according to the cost function. With the goal of minimizing the cost, solve the cost function based on the objective constraint conditions to obtain the control increment sequence at 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 considering the terminal cost is established (Equation (43)); the objective constraint conditions of the cost function are established (Equations (44)-(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, Equation (44) is the control variable constraint, U(k - 1) is the control input of the system at the previous moment, Equation (45) is the control increment constraint, Equation (46) is the system terminal constraint, and Equation (47) is the safety constraint to ensure the robot avoids obstacles. A distance trigger mechanism is introduced in the obstacle avoidance constraint. When the Euclidean distance between the current state of the robot and the obstacle is less than the set threshold, the obstacle avoidance constraint is considered in the controller solution process to perform the obstacle avoidance task. By reducing the constraint conditions in the global solution process of the controller, the computational load of the system is reduced.

[0182] At each moment k, the cost at the current moment is determined according to the cost function. With the goal of minimizing the cost, the cost function is solved based on the objective constraint conditions to obtain the control increment sequence at the current moment:

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

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

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

[0186] By inputting the control at each sampling moment into the robot system, 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 this controller for safety prediction control. It can comprehensively consider the motion state of the robot, external disturbances, and obstacle avoidance requirements, achieve optimized control of the robot, and improve the operating efficiency and safety of the robot.

[0188] The following conducts simulation and emulation on this application, specifically as follows:

[0189] To verify the effectiveness of the proposed obstacle avoidance control method of the present invention, in this section, an obstacle avoidance control verification example is designed in MATLAB for a four-wheel independently driven and four-wheel independently steered WMR system.

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

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

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

[0193] As Figure 4The figure shows the effect diagram of the robot avoiding obstacles. The robot can reach the end position (0, 0) from the initial position (6, 6), successfully avoid obstacles, and judge whether the obstacle avoidance trigger condition is met according to the distance between the real-time state and the obstacles. The purple area in the figure is the area where the robot performs obstacle avoidance behavior.

[0194] Figure 5 and Figure 6 are the control variables solved by the machine controller. Figure 5 are the linear velocities of the four wheels. Figure 6 are the angular velocities of the four wheels. Due to the independent drive and steering characteristics of the wheels, there are differences in the linear velocities and angular velocities of the four wheels. In order to establish accurate input-to-state safety control barrier function constraint conditions, it is necessary to accurately observe the disturbance quantity. Figure 7 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 input-to-state safety control barrier function on the obstacle avoidance effect of the robot, Figure 8 is the comparison diagram of the obstacle avoidance effects under different parameters ρ. The robot adjusts the distance from the obstacle according to different parameters ρ. When ρ increases, the robot approaches the obstacle during obstacle avoidance. When ρ decreases, the robot moves away from the obstacle during obstacle avoidance. Therefore, when there are disturbances in the system, the control barrier function constraint conditions can be adjusted by adjusting ρ, so as to achieve a better obstacle avoidance effect.

[0196] The above has introduced in detail the anti-disturbance and trigger obstacle avoidance safety prediction control method of the wheeled mobile robot provided by this application. Specific examples are used in this article to elaborate on the principle and implementation manner of this application. The description of the above embodiments is only used to help understand the concept of this application, and should not be construed as a limitation on the protection scope of this application.

Claims

1. A safety predictive control method for a wheeled mobile robot to resist disturbances and trigger obstacle avoidance, characterized in that, The described control method is applicable to a four-wheel independently driven and four-wheel independently steered mobile robot system, and the method includes: Establish a kinematic model of a four-wheel independently driven and four-wheel independently steered mobile robot, and transform the kinematic model into a linear time-varying model subject to external disturbances; Based on the linear time-varying model, design a bounded-error disturbance observer to observe the external disturbances received by the mobile robot system to obtain disturbance observation values, and determine the upper bound of the disturbance observation error. According to the disturbance observation values and the upper bound of the disturbance observation error, construct an obstacle avoidance constraint condition for the control barrier function; According to the obtained position state information of the robot and the perceived target obstacle information, calculate the distance between the robot and the target obstacle. When the distance triggers a pre-constructed distance trigger mechanism, then use the obstacle avoidance constraint condition to design a safety prediction controller based on the linear time-varying model; Use the safety prediction controller to perform safety prediction control.

2. The safety prediction control method according to claim 1, characterized in that, The kinematic model of a four-wheel independently driven and four-wheel independently steered mobile robot is expressed as follows: where is the velocity of the robot in the X direction of the position in the global coordinate system, is the velocity of the robot in the Y direction of the position in the global coordinate system, is the change rate of the heading angle φ of the robot in the global coordinate system, are the coordinates of the four wheels in the robot coordinate system, i = fl, rl, rr, fr, v i represents the linear speed 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); Among them, ξ(k+i|k) represents the system state variable predicted i steps after the current time k, and ξ = [X, Y, Φ] T respectively representing the position X, position Y, and heading angle φ of the robot in the global coordinate system, I is the identity matrix for the corresponding dimension, and 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 predicted i-1 steps after the current time k, and d(k∣k) is the discretized and linearized high-order term and the external disturbance 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, and g(k + 1) is the intermediate variable at the next moment. is the disturbance estimation value, d(k) is the external disturbance of the system, κ is a diagonal matrix, κ = diag(κ1, κ2, …, κ p ), |κ i | < 1, i = 1, 2, …, p, where p is the dimension, and 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, wherein, The upper bound of the disturbance observation error is expressed as: Among them, e d (k) represents the error vector between the true external disturbance d(k) and the estimated value of the disturbance observer at time k , which is composed of multiple components e di (k) (i = 1, 2,..., p). e di (k) is the i-th component of the disturbance observation error vector e d (k), ζ i is a preset fixed value, and κ 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: where 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, Designing a safety prediction controller based on the linear time-varying model includes: Construct a prediction model of the safety prediction controller; Establish a cost function based on the prediction model and the initial control increment sequence; Establish the target constraint condition of the cost function, determine the cost at the current moment according to the cost function, and take the minimum cost as the goal to solve the cost function based on the target constraint condition to obtain the control increment sequence at the current moment.

8. The safety prediction control method according to claim 7, wherein Establishing a cost function based on the prediction model and the initial control increment sequence includes: Based on the prediction model and the initial control increment sequence, establish a cost function considering the terminal cost.

Images