Mobile robot online navigation and obstacle avoidance method based on clf and cbf

Abstract

This invention discloses an online navigation and obstacle avoidance method for mobile robots based on CLF and CBF. The invention targets a single-vehicle model mobile robot controlled by linear and angular velocities. Navigation is achieved by constructing a CLF function, and obstacle avoidance is achieved by constructing a CBF function based on the rear axle center coordinates to represent the center position coordinates. An optimization control problem is constructed based on QP form and solved online to navigate the robot to the destination and avoid collisions with obstacles. This invention overcomes the problem of traditional CBF-based obstacle avoidance controllers, which struggle to directly control all control variables for collision avoidance. It also designs CLF constraints that still enable navigation in extreme conditions. Finally, by constructing a CLF-CBF-QP optimization problem, the online solution of the problem is guaranteed.

Description

Technical Field

[0001] This invention relates to a mobile robot navigation and obstacle avoidance control method in the field of artificial intelligence and robotics, specifically a navigation control method based on a control Lyapunov function and an obstacle avoidance control method based on a control obstacle function. Background Technology

[0002] Robotics technology has developed rapidly and is widely used in various fields such as logistics, autonomous driving, and rescue operations. Therefore, designing a safety-critical controller for mobile robots is crucial. If the robot's controller cannot guarantee safe operation during work, it can lead to irreversible and devastating consequences. A safety-critical controller prioritizes robot safety above all other indicators, such as trajectory tracking, always placing robot safety as the primary control objective.

[0003] In practical applications, robot safety is primarily ensured by avoiding collisions with obstacles. Common methods include Model Predictive Control (MPC), artificial potential field methods, reachability analysis, and neural networks. Recently, methods based on Control Barrier Functions (CBFs) have been widely applied in safety-critical control. These methods often construct a continuous-time optimization control problem using Quadratic Programming (QP), incorporating CBFs and Control Lyapunov Functions (CLFs) as constraints into the problem (CLF-CBF-QP). By rapidly solving the QP optimization control problem, optimal control commands for the mobile robot can be obtained online, achieving critical safety control.

[0004] However, existing methods based on the aforementioned optimization control problem form are difficult to directly apply to single-vehicle mobile robots. This is because safety-critical controllers often cannot directly control all control variables to achieve safe operation of the mobile robot; they can often only control the linear velocity, not the angular velocity. Therefore, some studies have designed controllers based on High Order Control Barrier Functions (HOCBFs). However, the introduction of HOCBFs means that only angular velocity can be controlled for obstacle avoidance, and the linear velocity needs to be fixed in this case. Other methods combine CBFs with MPCs to overcome the shortcomings in the continuous time domain, solving for the optimal control commands for the mobile robot in the discrete time domain. However, when the number of obstacles increases, solving in the discrete time domain becomes difficult to meet real-time requirements. Therefore, achieving online safety-critical control for single-vehicle mobile robots remains a challenging problem. Summary of the Invention

[0005] In view of this, in order to solve the problems existing in the background technology, this invention proposes an online navigation and obstacle avoidance method for mobile robots based on CLF and CBF. By designing the CBF function based on the rear axle center of the mobile robot with the characteristics of a single vehicle model, the optimization control problem (CLF-CBF-QP) can simultaneously control the linear velocity and angular velocity of the mobile robot in the continuous time domain for obstacle avoidance control, and finally drive the robot to the destination position.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] Step 1: Collect information on all obstacles in the environment and save it in the mobile robot's map information, as well as determine the mobile robot's current position and destination position;

[0008] Step 2: Determine the mobile robot model and convert it into an affine control form;

[0009] Step 3: Based on the current position and destination position of the mobile robot and the obstacle information in the map, construct CLF constraints to ensure that the robot moves towards the destination and CBF constraints to ensure that the mobile robot avoids collisions with obstacles.

[0010] Step 4: Determine the objective function of the optimization control problem, and combine the CLF constraint to ensure the robot moves towards the endpoint, the CBF constraint to ensure the mobile robot avoids collisions with obstacles, and the physical constraints of the control variables to construct the optimization control problem;

[0011] Step 5: After solving the optimization control problem, obtain the optimal control quantity and send it to the control system of the mobile robot. Combined with the mobile robot model of affine control, control the mobile robot to move.

[0012] Step 6: Check if the mobile robot has reached the destination. If it has not reached the destination, repeat steps 3-5 until the mobile robot reaches the destination.

[0013] In step 1, all obstacles are circular, and the information of each obstacle includes its center position, radius, and moving speed; the mobile robot is circular in shape, and the coordinates of the rear axis center of the mobile robot are used as the position of the mobile robot.

[0014] In step 2, the formula for the mobile robot model is as follows:

[0015]

[0016] Where, x p， y p The x and y coordinates of the rear axis of the mobile robot are represented; θ represents the heading angle of the mobile robot; v and ω represent the linear velocity and angular velocity that control the movement of the mobile robot, respectively.

[0017] The formula for the affine control system corresponding to the mobile robot model is as follows:

[0018]

[0019]

[0020]

[0021]

[0022] u = [v, w] T

[0023] in, Let x and y represent the derivatives of the rear axis center x and y coordinates and the yaw angle of the mobile robot, respectively; u is the control variable; and T represents the transpose operation. Let f(x) represent the derivative of the state variable, f(x) represent the control state function, and g(x) represent the control input function.

[0024] Step 3 specifically involves:

[0025] Step 3.1: Construct a control Lyapunov function V(x) based on the current position and the destination position of the mobile robot, and then construct a CLF constraint based on the control Lyapunov function V(x) to ensure that the robot moves towards the destination.

[0026] Step 3.2: Construct a control obstacle function based on the current position of the mobile robot and the obstacle information in the map, and then construct a CBF constraint based on the control obstacle function to ensure that the mobile robot avoids collisions with obstacles.

[0027] In step 3.1, the formula controlling the Lyapunov function V(x) is as follows:

[0028]

[0029] e = [x p -x g y p -y g ,θ-θ g ] T

[0030] Where e represents the difference between the current position and the endpoint position, T represents the transpose operation, R is the error weight matrix, a1, a2, a3 are the first to third weights on the diagonal, and b1 and b2 are the first and second cross terms, respectively.

[0031] The formula for the constraint that guarantees the robot's movement toward the destination is as follows:

[0032]

[0033] Where f(x) represents the control state function, g(x) represents the control input function, u is the control quantity, and γ represents a K-type function.

[0034] In step 3.2, the formula for the control barrier function is as follows:

[0035]

[0036] Among them, h i (x) represents the control obstacle function of the mobile robot regarding the i-th obstacle. and Represents obstacle O i x, y axis coordinates, r and These represent the radii of the mobile robot and the obstacle, respectively.

[0037] In step 3.2, when the obstacle is a static obstacle, the mobile robot is guaranteed to avoid collisions with the i-th obstacle O. i The collision constraints are given by the following formula:

[0038]

[0039] Where f(x) represents the control state function, g(x) represents the control input function, u is the control variable, and α represents class K. ∞ function.

[0040] In step 3.2, when the obstacle is a dynamic obstacle, the mobile robot is guaranteed to avoid collisions with the i-th obstacle O. i The collision constraints are given by the following formula:

[0041]

[0042]

[0043]

[0044] Among them, h i (x, t) represents the time-varying obstacle control function of the mobile robot with respect to the i-th obstacle. This represents the impact of changes in the position of a dynamic obstacle on the control variable. Let f(x) represent the position information of the dynamic obstacle, g(x) represent the control state function, g(x) represent the control input function, u be the control variable, and α represent class K. ∞ function.

[0045] In step 4, the formula for the optimization control problem is as follows:

[0046]

[0047]

[0048]

[0049] u min ≤u≤u max

[0050] Where H represents the weight matrix of the control quantity, p represents the weight coefficient of the slack variable, δ is the slack variable, and R... m+1 This indicates that the optimization variable has an m+1 dimension, where M represents the number of obstacles; u min u max These are the minimum and maximum values ​​of the control variable u, respectively.

[0051] In step 6, if the mobile robot has not reached the destination within the maximum control time, the mobile robot returns to the starting point along the original route.

[0052] The beneficial effects of this invention are as follows:

[0053] This invention is based on the CLF-CBF-QP optimization control problem form, which can guarantee the rapid online solution of the optimization control problem and ensure real-time performance. At the same time, this invention constructs the CBF function based on the robot's rear axis center, which can guarantee the simultaneous control of linear velocity and angular velocity to achieve collision avoidance. In order to realize the navigation function of the mobile robot, this invention also designs a CLF function containing the cross term between position information error and heading angle error, which ensures that the controller can simultaneously control linear velocity and angular velocity to achieve navigation function under any circumstances. Attached Figure Description

[0054] Figure 1 This is a flowchart of the method of the present invention.

[0055] Figure 2 This is a schematic diagram of a mobile robot based on a single-vehicle model, which is the subject of this invention.

[0056] Figure 3 This is the navigation simulation result of the present invention in the presence of static obstacles.

[0057] Figure 4 This is the navigation simulation result of the present invention in the presence of dynamic obstacles. Detailed Implementation

[0058] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the scope of protection of the present invention is not limited thereto.

[0059] Figure 1 The flowchart illustrates the method of the present invention, and will be explained in detail below.

[0060] Step 1: Collect information on all obstacles in the environment (including obstacle size, movement speed, etc.) and save it in the mobile robot's map information. Also, determine the mobile robot's current position and destination position, and send this information along with the map information to the mobile robot's controller. Set all obstacles to be circular, and the information for each obstacle includes its center position, radius, and movement speed. Similarly, set the mobile robot's shape to circular, and use the rear axis coordinates as the mobile robot's position. That is, given the mobile robot's current position (x... p y p ,θ) and endpoint position (x g y g θ g All coordinates represent the rear axis center coordinates of the robot, not the center coordinates of the circular robot. The conversion relationship between the center coordinates and the rear axis center coordinates is as follows: Figure 2 As shown:

[0061] X c =x p+l cosθ

[0062] y c =y p +l sinθ

[0063] Where, x c and y c The x and y coordinates of the center of the circular robot are respectively represented by the x and y axes. p and y p θ represents the x and y coordinates of the rear axis center of the circular robot, l represents the axial distance between the rear axis center and the center of the circle, and θ represents the heading angle of the mobile robot, that is, the angle between the current orientation of the mobile robot and the horizontal x-axis.

[0064] Step 2: Determine the single-vehicle model of the mobile robot and convert the single-vehicle model of the mobile robot into an affine control form;

[0065] In step 2, the formula for the single-vehicle model of the mobile robot is as follows:

[0066]

[0067] Where, x p y p The x and y coordinates of the rear axis of the mobile robot are represented; θ represents the heading angle of the mobile robot; v and w represent the linear velocity and angular velocity that control the movement of the mobile robot, respectively.

[0068] The formula for the affine control system corresponding to the single-vehicle model of the mobile robot is as follows:

[0069]

[0070]

[0071]

[0072]

[0073] u = [v, w] T

[0074] in, Let x represent the derivatives of the x and y coordinates of the rear axis center of the mobile robot, and the derivative of the heading angle, respectively, x∈R. n ,u∈R m R n and R m Let u and ω represent the state variables of the mobile robot, respectively, with an n-dimensional state and an m-dimensional control variable, both of which need to be fixed within a reasonable range. u is the control variable, which controls the robot's linear velocity v and angular velocity ω. T represents the transpose operation. Let f(x) represent the derivative of the state variable, f(x) represent the control state function, and g(x) represent the control input function.

[0075] Step 3: Based on the current position and destination position of the mobile robot and the obstacle information in the map, construct CLF constraints to ensure that the robot moves towards the destination and CBF constraints to ensure that the mobile robot avoids collisions with obstacles.

[0076] Step 3 specifically involves:

[0077] Step 3.1: Construct a control Lyapunov function (CLF) V(x) based on the current position and the destination position of the mobile robot, and then construct CLF constraints based on the control Lyapunov function V(x) to ensure that the robot moves towards the destination.

[0078] Step 3.2: Construct a Control Barrier Function (CBF) based on the mobile robot's current position and obstacle information on the map (specifically, based on the Euclidean distance between the robot's center position and the obstacle position). i (x), where h i (x) represents the CBF function constructed by the mobile robot with respect to the i-th obstacle, and then the CBF constraint that ensures the mobile robot avoids collision with the i-th obstacle is constructed based on this obstacle control function.

[0079] In step 3.1, this invention adds two cross terms, b1 and b2, between the position coordinate error and the heading angle error, thereby ensuring that even in extreme cases, this invention can navigate the robot to its destination. The form of the control Lyapunov function V(x) is as follows:

[0080]

[0081] e = [x p -x g y p -y g ,θ-θ g ] T

[0082] Where e represents the difference between the current position and the destination position, T represents the transpose operation, P is the error weight matrix, which is required to be positive definite, a1, a2, a3 are the first to third weights on the diagonal, b1 and b2 are the first and second cross terms, and by adding these two cross terms, it can be ensured that the present invention can also navigate the robot to its destination in extreme cases.

[0083] The formula for the constraint that guarantees the robot's movement toward the destination is as follows:

[0084]

[0085] Here, γ represents a K-type function, which is taken as a positive real number. As long as the control input satisfies the above constraints, the robot can be guaranteed to move towards the destination.

[0086] In step 3.2, the formula for the control barrier function is as follows:

[0087]

[0088] Among them, h i (x) represents the control obstacle function of the mobile robot regarding the i-th obstacle. and Represents obstacle O i The x and y axis coordinates are used in this invention. To represent the location information of the obstacle, r and These represent the radii of the mobile robot and the obstacle, respectively.

[0089] Therefore, as long as h is guaranteed i If (x) > 0, collision avoidance by the robot can be guaranteed. Then, based on this CBF function, this invention constructs a mechanism to ensure the mobile robot avoids collisions with the i-th obstacle O. i The collision constraints are given by the following formula:

[0090]

[0091] Where α represents class K ∞ The function is defined here as a positive real number. As long as the control input satisfies the above constraints, collision avoidance between the mobile robot and obstacles can be guaranteed.

[0092] This invention can also achieve collision avoidance between mobile robots and dynamic obstacles. This invention uses... To represent the location information of dynamic obstacles, where and They represent obstacles O respectively. i The position at time t. Then, this invention constructs constraints for collision avoidance with dynamic obstacles based on a time-varying CBF function:

[0093]

[0094]

[0095] in, This represents the impact of a dynamic obstacle's position change on the control variable. If the obstacle is a static obstacle, then... Therefore, when facing dynamic obstacles, as long as the control quantity meets the above constraints, collision avoidance can be achieved with the dynamic obstacles.

[0096] This invention represents the robot's center position based on the rear axis, and then constructs a collision avoidance network (CBF) with the obstacle using Euclidean distance. This ensures that the constraints constructed based on the CBF can simultaneously control the robot's linear velocity and angular velocity to achieve collision avoidance.

[0097] Meanwhile, the CBF function constructed in this invention can avoid singularity, that is, it can guarantee... This ensures the validity of the CBF function. Conversely, if x p and y p If the center position of the circle represents the robot's position, certain singularities will occur, making it difficult to guarantee the effectiveness of the CBF function. Furthermore, if a higher-order CBF function is used, collision avoidance can only be achieved by controlling the angular velocity, because the linear velocity is fixed as a constant, thus limiting the control performance.

[0098] Step 4: Determine the objective function of the optimization control problem, and combine the CLF constraint to ensure that the robot moves towards the destination, the CBF constraint to ensure that the mobile robot avoids collisions with obstacles, and the physical constraints of adding control variables to construct the optimization control problem (CLF-CBF-QP).

[0099] When the obstacle is a static obstacle, the optimization control problem takes the following form:

[0100]

[0101]

[0102]

[0103] u min ≤u≤u max

[0104] Where H represents the weight matrix of the control input, p represents the weight coefficient of the slack variable, δ is the slack variable, and δ∈R, the control input and slack variable together constitute the optimization variables, and R m+1 The optimization variables are m+1 dimensional, and N represents the number of obstacles. When CBF and CLF constraints conflict, the CBF constraint is prioritized; therefore, a slack variable δ is added to relax the CLF constraint. min u max These are the minimum and maximum values ​​of the control variable u, representing the constraints on the control variable, satisfying u min =[v min w min ] T u max

[0105] [v max w max ] T The objective function consists of two parts: one part aims to minimize the control variable, and the other part represents that the constraints cannot be relaxed too much.

[0106] Step 5: After solving the optimization control problem, obtain the optimal control quantity and send it to the control system of the mobile robot. Determine the simulation step size and control the mobile robot's single-vehicle model in the form of affine control. In fact, this invention decomposes the overall optimization control problem into multiple QP problems for online solution.

[0107] Step 6: Check if the mobile robot has reached the destination. If it has not reached the destination, repeat steps 3-5 until the mobile robot reaches the destination.

[0108] If the mobile robot fails to reach the destination within the maximum control time, it will return to the starting point. In practice, if the mobile robot fails to reach the destination within the maximum control time, the control process will terminate, and the simulation will end.

[0109] This invention has been verified through simulation experiments. The maximum control time was set to 30 seconds, and the simulation step size was 0.1 seconds. The simulation results are as follows: Figure 3 , Figure 4 (a)- Figure 4 As shown in (d), where Figure 3 This is for obstacles in an environment that are only static. Figure 4 This is for dynamic obstacles in the environment. Obstacles are represented by black circles with dashed edges, black circles with solid edges represent the robot's current position, gray circles represent the robot's starting and target endpoint positions, and black solid lines represent the robot's past movement trajectory. The movement trajectory of dynamic obstacles is represented by black dashed lines. Figure 3 Simulation results show that the method of the present invention can realize the collision avoidance of a single-vehicle model mobile robot controlled by linear velocity and angular velocity with static obstacles and navigate the mobile robot to its destination position. Figure 4 The simulation results show the simulation results at four different moments in the entire control process. By observing the simulation results at the four moments, it can be found that the present invention achieves collision avoidance between the mobile robot and dynamic obstacles, and controls the mobile robot to travel to the destination.

[0110] The above embodiments are used to explain and illustrate the present invention, but not to limit the present invention. Any modifications and changes made to the present invention within the spirit and scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A CLF and CBF based online navigation and obstacle avoidance method for mobile robots, characterized in that, Includes the following steps: Step 1: Collect information on all obstacles in the environment and save it in the mobile robot's map information, as well as determine the mobile robot's current position and destination position; Step 2: Determine the mobile robot model and convert it into an affine control form; Step 3: Based on the current position and destination position of the mobile robot and the obstacle information in the map, construct CLF constraints to ensure the robot moves towards the destination and CBF constraints to ensure the mobile robot avoids collisions with obstacles. Step 3 specifically involves: Step 3.1: Construct the control Lyapunov function based on the current position and destination position of the mobile robot. Then, based on the control Lyapunov function Construct CLF constraints to ensure the robot moves toward the destination; Step 3.2: Construct a control obstacle function based on the current position of the mobile robot and the obstacle information in the map, and then construct a CBF constraint based on the control obstacle function to ensure that the mobile robot avoids collisions with obstacles; In step 3.1, the control Lyapunov function is formulated as follows: in, This represents the difference between the current location and the destination location. This indicates the transpose operation. Here is the error weight matrix. These are the first to third weights on the diagonal. , These are the first and second cross terms, respectively; The rear axis center indicates the current position of the mobile robot. axis coordinate values, This indicates the heading angle of the mobile robot at its current position. The rear axis center of the mobile robot at the destination. axis coordinate values, The heading angle indicating the mobile robot's location at the destination; The formula for the constraint that guarantees the robot's movement toward the destination is as follows: in, This represents the control state function. Indicates the control input function. To control the quantity, Representative Class function; Step 4: Determine the objective function of the optimization control problem, and combine the CLF constraint to ensure the robot moves towards the endpoint, the CBF constraint to ensure the mobile robot avoids collisions with obstacles, and the physical constraints of the control variables to construct the optimization control problem; Step 5: After solving the optimization control problem, obtain the optimal control quantity and send it to the control system of the mobile robot. Combined with the mobile robot model of affine control, control the mobile robot to move. Step 6: Check if the mobile robot has reached the destination. If it has not reached the destination, repeat steps 3-5 until the mobile robot reaches the destination.

2. The method of claim 1, wherein, In step 1, all obstacles are circular, and the information of each obstacle includes its center position, radius, and moving speed; the mobile robot is circular in shape, and the coordinates of the rear axis center of the mobile robot are used as the position of the mobile robot.

3. The method of claim 1, wherein, In step 2, the formula for the mobile robot model is as follows: in, Indicates the rear axis of the mobile robot axis coordinate values, Indicates the heading angle of the mobile robot; and These represent the linear velocity and angular velocity that control the movement of the mobile robot, respectively. The formula for the affine control system corresponding to the mobile robot model is as follows: = = in, These represent the rear axis centers of the mobile robot. The differentials of the axis coordinates and the differentials of the heading angle. To control the quantity, This indicates the transpose operation. The derivative of a state variable. This represents the control state function. This represents the control input function.

4. The method of claim 1, wherein, In step 3.2, the formula for the control barrier function is as follows: in, Indicates the mobile robot regarding the first The control obstacle function for each obstacle. and Representing obstacles of axis coordinate values, and These represent the rear axle centers of the mobile robot. axis coordinate values, This represents the axial distance between the rear axis center of the mobile robot and the center of the circle. This indicates the heading angle of the mobile robot. and These represent the radii of the mobile robot and the obstacle, respectively.

5. The method of claim 4, wherein, The step 3.2, when the obstacle is a static obstacle, ensures that the mobile robot avoids colliding with the first obstacle The constraint is as follows: wherein represents a control state function, represents a control input function, is a control quantity, represents a class function.

6. The method of claim 4, wherein, In step 3.2, when the obstacle is a dynamic obstacle, the constraint that the mobile robot avoids collision with the first obstacle is ensured, and the formula is as follows: ​​ in, Indicates mobile robot about the first Time-varying control obstacle function for each obstacle, This represents the impact of changes in the position of a dynamic obstacle on the control variable. This indicates the location information of dynamic obstacles. This represents the control state function. Indicates the control input function. To control the quantity, Representative Class function.

7. The online navigation and obstacle avoidance method for a mobile robot based on CLF and CBF according to claim 5, characterized in that, In step 4, the formula for the optimization control problem is as follows: in, The weight matrix represents the control quantity. The weighting coefficients of the slack variables. As slack variables, The dimension of the optimization variable is . dimension, Indicates the number of obstacles; Control Quantity The minimum and maximum values.

8. The method of claim 1, wherein, In step 6, if the mobile robot has not reached the destination within the maximum control time, the mobile robot returns to the starting point along the original route.

Images