Wheeled robot path tracking method in dynamic narrow environment

Abstract

The invention provides a wheeled robot path tracking method in a dynamic narrow environment. In a narrow environment with dynamic dense obstacles, due to strict safety constraints, an existing method is difficult to find a safe and feasible path for a wheeled robot. The invention designs an improved local path tracking algorithm based on model predictive control (MPC) and a dynamic control barrier function (DCBF). According to the algorithm, slack variables (SV) are combined with DCBF, and strict constraints are converted into soft constraints. In order to adjust the value of the SV, a relaxation penalty term is added into a cost function of the MPC to adjust the size of a relaxation factor. In a dynamic obstacle sparse region, the algorithm selects a relatively small relaxation factor to ensure the safety of the system; in a dynamic obstacle dense area, a relaxation factor is properly increased in an algorithm to ensure that a system finds a feasible solution. According to the method, the safe path tracking problem of the wheeled robot in the dynamic obstacle dense region is realized, and the navigation capability in the dynamic obstacle dense region is improved.

Description

Technical Field

[0001] The present invention relates to an autonomous motion and planning method for wheeled robots, specifically a path tracking algorithm for wheeled robots in dynamic narrow areas. Background Art

[0002] In the chemical production environment, the autonomous navigation and motion planning of inspection robots have become a research hotspot. The chemical production environment is highly dynamic and complex. Especially in narrow areas and scenes with dense obstacles, robots face severe challenges in path planning and obstacle avoidance. Inspection robots need to efficiently and safely perform tasks in a restricted space. The traditional manual inspection method has limitations in terms of efficiency and safety and cannot meet the increasingly complex requirements of modern factories.

[0003] To address this problem, there are currently three main types of motion planning methods: graph search-based path planning algorithms, heuristic optimization algorithms, model predictive control (MPC) algorithms, etc. Graph search algorithms such as the A* algorithm and its extended algorithms (such as Kinodynamic A*) find the global optimal path through heuristic search. However, in a dynamic obstacle environment, this method lacks real-time performance. Heuristic optimization algorithms such as genetic algorithms and particle swarm algorithms can find suboptimal paths in complex environments but have weak adaptability to dynamic environments. The MPC algorithm predicts future states and combines environmental information for dynamic obstacle avoidance and path tracking, which can overcome the deficiencies of the above algorithms to a certain extent.

[0004] However, in an environment with dense dynamic obstacles, inspection robots need to consider more non-holonomic constraints, such as kinematic constraints and dynamic constraints. Existing control algorithms are difficult to find feasible solutions that satisfy the safe movement of the robot. The traditional control barrier function method (CBF) can ensure obstacle avoidance for the robot, but its safety constraints are too loose, resulting in the robot being unable to ensure real-time safety in a complex dynamic environment; the method of adding obstacle information to CBF, the dynamic control barrier function method (DCBF), has overly strict safety constraint conditions, causing the robot to easily fail to find a feasible solution in an area with dense dynamic obstacles and then collide with the environment.

[0005] In summary, for the motion planning and control problems of wheeled robots in dynamic narrow areas, designing corresponding path planning and motion control algorithms is a challenging task, and there is an urgent need for efficient and robust algorithms to solve such problems. Summary of the Invention

[0006] The present invention aims to solve the problem of safe path tracking of wheeled robots in dynamic narrow areas. Aiming at the problems of slow solution speed and difficult safe tracking of traditional algorithms in an environment with dense dynamic obstacles, the present invention proposes a path tracking method combining MPC and CBF, which can effectively improve the obstacle avoidance ability and path tracking accuracy of inspection robots in complex dynamic environments.

[0007] To this end, an object of the present invention is to propose an autonomous navigation and control method for chemical inspection robots applicable to narrow dynamic environments. By introducing CBF into the MPC algorithm and combining a relaxation factor, this method can effectively handle complex constraints in areas with dense dynamic obstacles, ensuring that the robot can perform collision-free navigation in dynamic environments.

[0008] To achieve the above object, the present invention proposes a path tracking algorithm for inspection robots, which includes the following steps: First, construct a kinematic model of the wheeled inspection robot and a prediction model of dynamic obstacles; then, in the path planning stage, adopt a global path planning method based on the Kinodynamic A* algorithm and optimize the path by combining B-spline curves to generate a smooth and collision-free global path; in the path tracking stage, predict and control the robot's movement based on the MPC algorithm, and constrain its movement through a control obstacle function; in a complex dynamic environment, adjust the control input by combining a relaxation factor to ensure that the robot can still find a feasible solution in the area with dense obstacles, and finally realize the safe, efficient and autonomous navigation of the inspection robot. Further, the specific steps of the present invention are as follows:

[0009] Step 1: Path planning of the wheeled robot in a dynamic narrow area, including the following steps:

[0010] Step 1.1: By acquiring the robot's lidar point cloud data and odometer data, the robot constructs an Euclidean signed distance field map for environmental characterization. The odometer information obtained from the robot system positioning algorithm by the path planning algorithm is used as the initial point of the current path search, and then the end point is set according to the task target requirements. At the same time, two priority queues are initialized to store the nodes to be expanded in the subsequent path planning.

[0011] Step 1.2: Sample in the state space according to the state space equation of the robot to construct a Lattice Graph. This process can be described as given the starting state and the final state, finding an optimal control path, that is, a two-point boundary optimal control problem (OBVP). When solving this problem, first construct an OBVP problem model. Considering that the system dimension will be relatively high if the x and y directions of the wheeled robot are calculated together, the x and y dimensions are decoupled and each dimension is optimized separately. The constructed OBPV problem model, where the objective function of OBPV is defined as J∑ It is obtained by summing two objective functions in the x and y directions. J k Represents the objective functions of the robot in the x and y directions respectively.

[0012] Where j m (t) represents the derivatives of the accelerations in the x and y directions at t = 1, 2 respectively. The ultimate goal of the OBVP problem is to find a suitable control input u such that J between two points ∑ is minimized to achieve the most energy-efficient purpose. To solve this OBVP problem, the present invention uses the Hamiltonian method to solve the problem. Define the Hamiltonian equation: H(s, u, λ) = g(s, u) + λ T f(s, u) (1 - 2)

[0013] Where s represents the state of the robot and u represents the control input of the robot. f(s, u) is the kinematic equation of the robot. g(s, u) is the transition cost during the state transition of the robot, and H(s, u, λ) is the Hamiltonian equation of the OBVP problem, is the undetermined coefficient in the Hamiltonian method, which are the weight coefficients of the velocity (υ), acceleration (a) and jerk (j) in a single direction respectively. Equation (1 - 2) can be transformed into:

[0014] If we want to solve the optimal control problem for (1 - 3), we need to make H(s, u, λ) in (1 - 3) minimum under the condition that the robot state s takes the optimal solution s * . At this time, the obtained u is the optimal control input between two points. According to the Pontryagins minimum principle, taking the partial derivative of (1 - 3) with respect to s gives:

[0015] Construct a solution of λ for (1 - 4), where α, β, γ are all parameters introduced to be solved, that is:

[0016] Combining (1 - 4) and (1 - 5) gives that the optimal control input u * (t) is:

[0017] Combining the initial state s(0) and the final state s * (T) gives the objective function of the final OBPV problem:

[0018] The above equation is a polynomial function of time T. Therefore, to minimize the cost function, we only need to take the derivative of Equation (1-7) to find the extreme value. After the above steps, the optimal control input between two points is found, the state space is discretized, and sampling is performed to construct the Lattice Graph.

[0019] Step 1.3: Use the current position of the robot as the initial node in the Lattice Graph for heuristic path search. Expand in four directions: up, down, left, and right in the grid map, and store the position nodes of the reachable area in the priority queue. During the node expansion process, calculate the cost value for each node according to the heuristic function. When expanding each node, pop out the node with the minimum cost value and continue to expand to achieve the purpose of heuristic path search. The heuristic function is defined as: f(n) = h(n) + g(n) (1-8)

[0020] where n represents the current node, g(n) represents the distance cost from the initial node to the current node, h(n) represents the distance cost from the current node to the target position, and f(n) represents the total distance cost of the current node in the path search. Through the heuristic function, calculate the cost value of each node during the search process, and finally achieve the heuristic path search.

[0021] Step 1.4: During the node expansion process, when it is found that the cost h(n) of the current node to the target node is less than the set threshold, it is considered that the node is already close to the target point. At this time, perform path backtracking to find the path from the starting point to the current node, and push it into a list for output to obtain a rough path.

[0022] Step 1.5: After the path is searched, use a path optimization algorithm based on B-spline optimization to optimize the path to obtain a smooth path considering obstacle information. For the pb-degree B-spline trajectory defined by N + 1 nodes Q 0 , Q 1 ,..., Q N , the present invention optimizes a subset of N + 1 - 2pb control points Q pb , Q pb+1 ,..., Q N-pb , where pb = 5. Define the objective function f total as: f total = λ s f s + λ c f c + λd(f υ + f a) (1-9)

[0023] where f s and f c represent smoothness and collision cost respectively. f υ and f a represent the cost of velocity and acceleration respectively. λ s , λ c and λ d are the weights for smoothness, safety, and dynamic feasibility respectively. In the actual process, λ s = 0.3, λ c = 0.3, λ d = 0.4. Among them, f s is defined as:

[0024] where each term F i+1，i = Q i+1 - Q i and F i-1，i = Q i-1 - Q i are the bonding forces of two springs connecting nodes Q i+1 , Q i and Q i-1 , Q i respectively. Similar to f s , the collision cost f c is:

[0025] where d(Q i ) is the sum of the distances between Q i and two adjacent obstacles. F c is a differentiable potential energy cost function, and d thr represents the consideration range of obstacles. In the actual process, d thr = 0.2. Obstacles within d thr will be considered in this optimization problem, while obstacles outside d thr will be ignored. F c is defined as:

[0026] In the definition of f υ , this method penalizes the control points on the path that exceed the maximum velocity and maximum acceleration. The one-dimensional velocity penalty is defined as: where F υ is defined as:

[0027] Similarly, the acceleration cost f a is defined as: where F a is defined as:

[0027] In the actual execution process, the searched path is optimized by using equation (1-9) to minimize the total cost function, so that the searched path becomes the optimal path considering the robot's own state.

[0028] After obtaining the optimal path planned by the robot, the algorithm proposed in step 2 is used for path tracking to control the robot to move along the planned path.

[0029] Step 2: Path tracking of the wheeled robot in a dynamic narrow area. The present invention proposes a dynamic control barrier function combined with a relaxation factor, abbreviated as DS, which will be used together with MPC in the control design of the present invention. It includes the following steps:

[0030] Step 2.1: In the present invention, the control variable X = [x, p o , η o . Among them, represents the position coordinates of the robot, represents the position coordinates of the obstacle, represents the geometric characteristics of the obstacle. n, m, s respectively represent the dimensions of each state. In the example of the present invention, a wheeled robot is considered. The safety set C suitable for the robot to operate safely is defined. This set represents the safety range of the control variable, that is, when the control variable X is in this set, it means that the robot does not collide with the environment and can safely complete path tracking. The control barrier function is defined as a continuously differentiable function satisfying the following conditions, where χ represents the set of control variables: Equation (2-1) shows that for the control variable X within the range of , the range of the robot safety set needs to satisfy h(x)≥0, indicating the calculation method of the safety set C, where the function h represents the dynamic obstacle function (DS) with a relaxation factor in this article. That is, the calculation method of the safety set C is the value set of X when h(X)≥0.

[0031] In the specific definition, DS is defined as: for all Combined with the K ∞ function γ and the relaxation factor δ, the function h in (2-1) can be defined as: there exists a control input u such that (2-2) holds

[0032] Among them, the slack term is defined as δh(X) because it adds a slack variable proportional to h(X) in the original constraint. This allows for a moderate relaxation of the constraint when the system state is close to the boundary, making it easier for the system to satisfy the safety constraints of the robot.

[0033] For a continuous-time system, the continuous-time state equation of the system can be written as: Among them, f(x) and g(x) represent the state function and input function of the robot system respectively. Based on this system, DS can be written as:

[0034] Combining (2-2) and (2-3), the control input u can be calculated as: Among them, ∈ and v represent the position of the obstacle and the geometric shape influence factor respectively, and the specific representation format is as follows:

[0035] To simplify the symbolic expression, the following symbolic expressions are made in the present invention, where L f h, L g h represent the rates of change of h(x) along the directions f(x) and g(x) respectively.

[0036] It can be obtained that the control input set K of DS DS is:

[0037] Similarly, it can be obtained that the control input set K of DCBF DCBF is:

[0038] In (2-2), for the sake of simplifying subsequent operations, γ is a constant rather than a K ∞ function. In the present invention, γ is set to 0.1. The value of the slack variable δ is calculated and adjusted by the cost function of the MPC controller during control, and this value is automatically calculated by the algorithm. When δ > 0, the K DS set will be larger than the control set K DCBF which means that the constraints on the control input are reduced and the set of the feasible region of the robot becomes larger.

[0039] In (2-2), γ is a scalar rather than a K ∞ function. The value of the slack variable δ depends on the cost function of the MPC controller. When δ > 0, KDS The set will be larger than the control set K DCBF which means that the constraints on the control input are reduced and the set of the robot's feasible regions becomes larger.

[0040] In specific applications, the control barrier function h(x) designed in the present invention is: where d s represents the safety distance of the robot.

[0041] To control the movement of the robot, an MPC controller is designed for path tracking. First, a discrete-time kinematic model of the actual robot is established as shown in Equation (2-10): x k+1 = Ax k + Bu k (2-10) where the matrices A and B represent the state transition matrix and the control input matrix of the robot, respectively, and are defined as: represents the state vector of the UGV at time step k, The symbol represents the matrix transpose. x k , y k respectively represent the position coordinates of the robot, θ k represents the heading angle of the robot, respectively represent the speed of the robot in the x direction and the speed in the y direction. represents the control input. Where υ k represents the linear velocity of the robot, ω k represents the angular velocity of the robot. ΔT s is the sampling period. The cost function of the MPC controller is defined as:

[0042] where represents the MPC cost function at time step t, u t：t+N-1|t represents the control input sequence from the current time t to the future time t+N-1. This cost function represents optimizing the control input sequence for the next N steps at the current time t to minimize the total cost, which is the core step in MPC. q(x t+k|t , u t+k|t ) represents the stage cost of the system, while p(x t+N|t ) represents the terminal cost. The stage cost and the terminal cost are defined as follows: q(x t+k|t , u t+k|t ) = xt+k|t T Qx t+k|t +u t+k|t T Ru t+k|t (2-12) p(x t+N|t )=x t+N|t T Px t+N|t (2-13) where Q, R, and P are the corresponding weight matrices, and is a diagonal matrix, with specific definitions as shown in Equation (5-13):

[0043] The stage cost q(x t+k|t , u t+k|t ) is designed to ensure that within each prediction step of the MPC, the deviation between the robot's state and the path point is minimized, and the control input u is also as small as possible, so as to ensure that the system control input is reduced to achieve the purpose of energy conservation.

[0044] The constraint conditions of the MPC are as follows: x t+k+1|t = f(x t+k|t , u t+k|t ), k = 0,..., N-1 (2-15) x t|t = x t (2-17)

[0045] where x t+k|t represents the predicted state at time step t + k at time t. (2-15) means that based on the current state x t and the input sequence u t+k|t are applied to the dynamic equation of the unmanned vehicle, where u t+k|t represents the control input predicted at time t for time step t + k. That is, the predicted state at the next moment should be derived from the dynamics. (2-16) represents that the robot state x t+k|t and the control input u t+k|t should be within a certain range, where represent the interval ranges of the robot state and control input limits respectively. N represents the optimization time domain length of the MPC controller, including the prediction and control ranges. The terminal state constraint is enforced in (2-18), where represents the set of terminal state constraints.

[0046] In an actual robot, the DS is used as the safety constraint of the system, expressed as: Δh(x t , u t ) ≥ -γh(x) - δh(x), 0 < γ < 1, δ > 0 (2-19)

[0047] In the cost function (2-11), the penalty of the slack variable is represented by βδ, where β > 0 is the penalty coefficient, and the coefficient size is set to 40. δ represents the slack variable. In the experiment, the dense obstacle area is defined as the area where more than 3 obstacles appear simultaneously within 1.5 m around the UGV. If the number of obstacles is less than 3, the area is defined as the sparse obstacle area. The slack variable plays a key role in DS, especially in the area where dense obstacles are encountered. In this case, δ can appropriately relax the constraints to find a feasible solution. In the cost function of MPC, the smallest β that satisfies all current constraint conditions is selected in each iteration process to adjust δ to a value as close to zero as possible. This allows the slack variable to only play a role when dense obstacles are encountered, and remain unchanged in other cases. This ensures that under normal circumstances, DS can still effectively control the movement of the robot and guarantee the safety of the system. This strategy flexibly adjusts δ according to the control requirements to plan the movement of the robot. Brief Description of the Drawings

[0048] Figure 1 is the flowchart of the dynamic control barrier function of the present invention combined with the relaxation factor

[0049] Figure 2 is the flowchart of the self-navigating method of the wheeled robot of the present invention.

[0050] Figure 3 is the schematic diagram of the self-navigating method of the wheeled robot in the present invention.

[0051] Figure 4 is the schematic diagram of the algorithm of the dynamic control barrier function combined with the relaxation factor in the present invention.

[0052] Figure 5 is the algorithm principle diagram of the dynamic control barrier function combined with the relaxation factor in the present invention. Detailed Embodiment

[0053] To better understand the technical solution of the present invention, the following further introduces the embodiments of the present invention in combination with the drawings and specific examples. Specific parameter settings: d s = 0.5m, γ = 0.1, N = 25, ΔT s = 0.1s.

[0054] Figure 2 is the flowchart of the self-navigation of the wheeled robot of the present invention Figure 3 is the schematic diagram of the algorithm of the dynamic control barrier function combined with the relaxation factor in the present invention. The specific steps are as follows:

[0055] Step 1: Manually give the robot target points. Construct a lidar-IMU fusion odometer by obtaining lidar point cloud and IMU information, and obtain the robot's position coordinates.

[0056] Step 2: Combine the robot's current position and the lidar point cloud to model the dynamic environment and construct an ESDF map.

[0057] Step 3: Use the Kinodynamic A* algorithm to perform path search to generate a rough path. Then use B-spline optimization to optimize the rough path and generate a fine trajectory considering the environmental information.

[0058] Step 4: Use the DBSCAN algorithm to cluster the obstacles in the environment and cluster them into circles with a radius of Next, use the Kalman filter to predict the trajectories of the obstacles. The predicted position of each obstacle at each moment can be obtained as where respectively represent the planar coordinates of the clustered obstacles in the spatial coordinate system.

[0059] Step 5: Input the generated fine path into the MPC controller as a reference, and the MPC performs path tracking. The MPC controller calculates the optimal linear velocity and angular velocity that satisfy the constraint conditions in the current environment and inputs them to the vehicle chassis for execution. The present invention has the following excellent characteristics:

[0060] Comparing the method of the present invention with the dynamic control obstacle function algorithm, it can be found that in the sparse area of dynamic obstacles, there is no significant difference between the two except for energy loss. However, in the dense area of dynamic obstacles, the method of the present invention can help the wheeled robot pass through this area, ensuring the safety of the navigation of the wheeled robot.

Claims

1. A path tracking method for a wheeled robot in a dynamic narrow environment, characterized in that: The specific steps are as follows: S1. Set the robot target point according to the specific task; construct a radar-IMU fusion odometer by acquiring the lidar point cloud and IMU information to obtain the robot's position coordinates; S2, combining the robot’s current position and the LiDAR point cloud, models the dynamic environment and constructs obstacles in the space as a 3D occupancy grid map; S3, using Kinodynamic A* algorithm to search for paths and generate a rough path; then using B-spline optimization to optimize the rough path and generate a fine path considering environmental information; S4. Use the DBSCAN algorithm to cluster the obstacles in the environment, cluster each obstacle into a circle, and define the geometric features of the obstacles as in represents the radius of the ith obstacle, represents a set of real numbers; Next, a Kalman filter is used to predict the trajectory of the obstacle; The predicted position of the obstacle at each moment is obtained as in They represent the plane coordinates of the clustered obstacles in the spatial coordinate system; S5. Input the refined path generated in S3 into the MPC controller as a reference, and MPC performs path tracking; the MPC controller calculates the optimal linear velocity and angular velocity that meets the constraints in the current environment, and inputs them to the car chassis for execution.

2. The method according to claim 1, characterized in that: The S5 comprises the following steps: S51, define the control variable X = [x, p o , η o ];in, Represents the position coordinates of the robot, Represents the location coordinates of the obstacle, represents the geometric features of the obstacle; n, m, s represent the dimensions of each state; define the safety set C for the robot to work safely, which represents the safety range of the control variable, that is, when the control variable X is in this set, it means that the robot does not collide with the environment and completes the path tracking safely; define the control obstacle function as a continuous differentiable function h: The following conditions are met, where χ represents the set of control variables: Formula (5-1) shows that for The range of the control variable X in the range, the range of the robot safety set needs to satisfy h(X)≥0, which shows the calculation method of the safety set C, where the function h represents the dynamic barrier function (DS) with a relaxation factor; that is, the calculation method of the safety set C is the set of values ​​of X when h(X)≥0 is satisfied; S52. In terms of specific definition, DS is defined as: for all Combined with K ∞ function γ and relaxation factor δ, the function h in (5-1) is defined as: there exists a control input u such that (5-2) holds Here, the relaxation term is defined as δh(X) because it adds a relaxation variable proportional to h(X) to the original constraint; S53. For a continuous-time system, the continuous-time state equation of the system is written as: Where f(x) and g(x) represent the state function and input function of the robot system respectively; based on this system, DS is written as: Combining (5-2) and (5-3), the control input u is calculated as: Among them, ∈ and v represent the position and geometric shape influencing factors of the obstacle respectively. The specific format is as follows: L f h,L g h represents the rate of change of h(x) along the directions f(x) and g(x) respectively; Get, DS control input set K DS for: Similarly, the control input set K of the control dynamic control barrier function (DCBF) without relaxation factor is DCBF for: In (5-2), set γ = 0.1; S54. The control barrier function h(x) with slack variables is: where d s Represents the safe distance between the robot and the obstacle; S55. In order to control the robot motion, an MPC controller is designed to perform path tracking. First, the actual robot is modeled in discrete time kinematics, as shown in equation (5-10): x k+1 =Ax k +Bu k (5-10) Among them, matrix A and matrix B represent the state transfer matrix and control input matrix of the robot, which are defined as: represents the state vector of UGV at time step k, and the T symbol represents matrix transposition; x k ,y k Represent the position coordinates of the robot, θ k Represents the heading angle of the robot, Represent the speed of the robot in the x direction and the speed in the y direction respectively; represents the control input; where v k represents the linear velocity of the robot, ω k Represents the angular velocity of the robot; ΔT s is the sampling period; S56. Define the cost function of the MPC controller as: in represents the MPC cost function at time step t, u t:t+N-1|t represents the control input sequence from the current time t to the future time t+N-1. The cost function represents the optimization of the control input sequence for the next N steps at the current time t to minimize the total cost; q(x t+k|t ,u t+k|t ) represents the stage cost of the system, and p(x t+N|t ) represents the terminal cost; the stage cost and terminal cost are defined as follows: Where Q, R, and P are the corresponding weight matrices, which are diagonal matrices. The specific definition is shown in formula (5-13): Stage cost q(x t+k|t ,u t+k|t ) is designed to ensure that in each prediction step of MPC, the deviation between the robot state and the path point is minimal, and the control input u is as small as possible; The constraints of MPC are as follows: x t+k+1|t =f(x t+k|t ,u t+k|t ),k=0,...,N-1 (5-15) x t|t =x t (5-17) where x t+k|t represents the state predicted at time step t+k; (5-15) represents the state predicted based on the current state x t and the input sequence u t+k|t Applied to the dynamics equation of the unmanned vehicle, where u t+k|t represents the control input predicted at time t+k; that is, the predicted state at the next moment should be derived from dynamics; (5-16) represents the robot state x t+k|t and control input u t+k|t Should be within a certain range, Represent the range of robot state and control input constraints respectively; N represents the optimization time domain length of the MPC controller, including the prediction and control range; the terminal state constraint is enforced in (5-18), where represents the set of terminal state constraints; S57. In actual robots, DS is used as the safety constraint of MPC, which is specifically expressed as: Δh(x t ,u t )≥-γh(x)-δh(x),0<γ<1,δ>0 (5-19) In the cost function (5-11), the penalty for the slack variable is represented by βδ, where β>0 is the penalty coefficient, and the coefficient is set to 40; δ represents the slack variable.

Images