Intelligent transfer robot dynamic coordination obstacle avoidance method based on reinforcement learning
Through reinforcement learning, the artificial potential field method and model prediction control are improved, combined with path planning and speed planning, the safety and stability of intelligent handling robots in dynamic obstacle environments are solved, and the efficiency of dynamic coordination of obstacle avoidance and trajectory tracking is achieved. control.
Patent Information
- Application Number
- CN202410998234.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-24
- Publication Date
- 2024-11-12
AI Technical Summary
The traditional intelligent transport robot path planning method ignores speed planning, resulting in insufficient safety and stability in dynamic obstacle environments. The trajectory tracking method based on model prediction control is fixed parameters and cannot adapt to different environments.
Using the improved artificial potential field method-model prediction control method (IAPF-MPC) based on reinforcement learning, the obstacle avoidance path is planned by improving artificial potential field method, and collision detection is carried out based on path information to plan the speed, combined with reinforcement learning, adaptive adjustment of the model Predictive control parameters to achieve dynamic coordination and obstacle avoidance.
The obstacle avoidance safety and driving stability of intelligent handling robots in multi-dynamic obstacle environments are improved. Through real-time trajectory planning and parameter adaptive tracking control, the robot can effectively avoid obstacles in complex environments.
Smart Images

Figure CN118938919A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of trajectory planning and tracking control of an intelligent handling robot, and relates to a dynamic coordinated obstacle avoidance method for an intelligent handling robot in a dynamic obstacle environment. Background Art
[0002] Intelligent handling robots can be used in various real-world scenarios, such as logistics, warehouse management, and industrial production. In the process of intelligent handling robots moving, there are often other mobile robots and various environmental restrictions, which will affect the safety of the robot itself and the surrounding environment. Therefore, it is of great significance to develop a trajectory planning and control method for dynamic coordinated obstacle avoidance of intelligent handling robots based on reinforcement learning.
[0003] The paper "An Optimization-Based Path Planning Approach for Autonomous Vehicles Using the DynEFWA-Artificial Potential Field", published in IEEE Transactions on Intelligent Vehicles, Vol. 7, No. 2, 2022, proposes a real-time vehicle path planning method based on the combination of the dynamic enhanced fireworks algorithm and the artificial potential field method. This method can improve the smoothness of the path; however, traditional path planning methods ignore speed planning, which greatly limits the application scenarios of intelligent handling robots. Therefore, a time-space integrated trajectory planning method is proposed to plan the speed while planning the path. In addition, the paper "A Comparison of Trajectory Planning and Control Frameworks for Cooperative Autonomous Driving", published in Journal of Dynamic Systems, Measurement, and Control, Vol. 143, No. 7, 2021, proposes a vehicle cooperative trajectory planning and tracking control method under dual-lane lane change conditions based on distributed model predictive control. This method can improve the safety of vehicle driving. However, the traditional trajectory tracking control method based on model predictive control cannot adapt to all environments due to its fixed parameters, resulting in poor trajectory tracking effect and stability, which will endanger the safety of the robot in extreme environments. Therefore, a parameter adaptive trajectory tracking control method based on reinforcement learning is proposed to achieve efficient tracking of the robot and improve the stability of the robot. Summary of the invention
[0004] In view of the above-mentioned problems existing in the prior art, in order to enable the intelligent transport robot to be applied in different scenarios and ensure the safety of itself and the surrounding environment, as well as the driving stability of the intelligent transport robot, the present invention proposes a dynamic coordination obstacle avoidance method of the intelligent transport robot based on reinforcement learning.
[0005] To achieve the above object, the present invention adopts the following technical solutions:
[0006] A dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, which can realize autonomous planning of obstacle avoidance trajectory and tracking control in a multi-dynamic obstacle environment. It is an improved artificial potential field method-model predictive control method (IAPF-MPC) based on reinforcement learning. First, the method plans the obstacle avoidance path by improving the artificial potential field method; secondly, collision detection is performed based on the planned path information to plan the speed; finally, the parameters of the model predictive control are adaptively adjusted through reinforcement learning to achieve dynamic coordination obstacle avoidance and ensure the safety and driving stability of the robot. Specifically, the following steps are included:
[0007] Step 1: Improve the artificial potential field method, that is, treat the environment and obstacles as different potential field functions for path planning. Establish a three-dimensional potential field function for the road environment based on the road structure, a three-dimensional potential field function for static obstacles based on the shape of static obstacles, and a three-dimensional potential field function for dynamic obstacles based on the shape, direction and speed of dynamic obstacles, as follows:
[0008] Step 1.1: Define the width L of a single lane for the intelligent transport robot to travel w The width of the robot is twice that of the robot, and the overall road for the intelligent handling robot is divided into several lanes with a width of L w The road potential field is established by dividing the road width into two lanes. The three-dimensional potential field function is established by taking the road environment divided into two lanes as an example. The three-dimensional potential field function of multiple lanes is established in the same way as the three-dimensional potential field function of two lanes.
[0009] Step 1.2: Set the intelligent handling robot to drive on the right, that is, the three-dimensional potential field function value of the center line of the right lane is lower than the three-dimensional potential field function value of the center line of the left lane, and the three-dimensional potential field function A from the center line of the left lane to the lane dividing line l and the three-dimensional potential field function A from the center line of the right lane to the lane dividing line r , the sum of the three-dimensional potential field function A between the center lines of the left and right lanes is as follows:
[0010]
[0011] Where, Y is the longitudinal position of the robot, Y m Indicates the longitudinal coordinate of the lane dividing line, L wIt represents the width of a single lane; p m0 is the coefficient that determines the three-dimensional potential field function value at the center line of the left lane, p m is the coefficient, which determines the value of the three-dimensional potential field function at the dividing line between the two lanes;
[0012] Step 1.3: Introduce the road boundary potential field to restrict the intelligent handling robot to keep moving within the road. Therefore, when the intelligent handling robot approaches the road boundary, its potential field should be designed to be infinite. Design the three-dimensional potential field function B from the center line of the left lane to the road boundary l , design the three-dimensional potential field function B from the center line of the right lane to the right of the road boundary r , then the sum of the three-dimensional potential field functions B from the centerline of the left and right lanes to the left and right boundaries is as follows:
[0013]
[0014] Among them, Y l Indicates the longitudinal coordinate of the center line of the left lane, Y r Indicates the longitudinal coordinate of the center line of the right lane;
[0015] Step 1.4: Integrate the three-dimensional potential field functions shown in the above formulas (1) and (2) to obtain the complete road three-dimensional potential field function J that meets the requirements road as follows:
[0016]
[0017] Among them, S1 is the action coefficient of the sum of the three-dimensional potential field function A between the center lines of the left and right lanes, S2 is the action coefficient of the sum of the three-dimensional potential field function B from the center lines of the left and right lanes to the left and right boundaries respectively, and J road is the three-dimensional potential field function of the complete road, A is the sum of the three-dimensional potential field functions between the center lines of the left and right lanes, and B is the sum of the three-dimensional potential field functions from the center lines of the left and right lanes to the left and right boundaries respectively;
[0018] Step 1.5: Establish the static obstacle three-dimensional potential field function J according to the obstacle shape s_obs as follows:
[0019]
[0020] Among them, (X,Y), (X obs ,Y obs ) represent the coordinates of the intelligent handling robot and the obstacle in the global coordinate system, J s_obs is the static obstacle potential field established; c1 is the weight of the lateral distance between the robot and the obstacle, c2 is the weight of the longitudinal distance between the robot and the obstacle, and ρ represents the threshold for determining the range of the repulsive potential field;
[0021] Step 1.6: Introduce dynamic factors and construct the dynamic obstacle three-dimensional potential field function J d_obs as follows:
[0022]
[0023] Among them, k o is a scaling weight constant value greater than zero, v o is the speed of the obstacle, θ o For vector r = [XX obs ,YY obs ] and v o The angle, J s_obs is the static obstacle potential field established, D d is the dynamic factor, J d_obs is the three-dimensional potential field function of the dynamic obstacle.
[0024] Step 2: Based on the road three-dimensional potential field function J shown in formula (3) road and the dynamic obstacle three-dimensional potential field function J shown in formula (5) d_obs Establish the objective function and use the solver to solve the desired path, as follows:
[0025] Step 2.1: Establish the kinematic model of the intelligent handling robot as follows:
[0026]
[0027] Among them, (X, Y) and (x, y) are the positions of the center of mass of the intelligent handling robot in the global coordinate system and its own coordinate system respectively; are the longitudinal speed and lateral speed of the intelligent handling robot, represents the yaw angle of the intelligent handling robot, and γ represents the yaw angular velocity of the intelligent handling robot;
[0028] Step 2.2: Establish the stability constraints of the path planning module as follows:
[0029]
[0030] Among them, v y =βv x , and is the abbreviation in the above formula, l r is the distance from the rear axle to the center of mass of the intelligent handling robot; α r,sat is the maximum slip angle of the rear wheel within the linear range; μ is the road friction coefficient; g is the acceleration of gravity; β is the center of mass slip angle;
[0031] Step 2.3: The established road three-dimensional potential field function J road, dynamic obstacle three-dimensional potential field function J d_obs and the designed control quantity u p Integrating into the objective function, the objective function is as follows:
[0032]
[0033] Where J = [J road ,J d_obs ] T , J is the overall three-dimensional potential field function of the road and dynamic obstacles, J road is the three-dimensional potential field function of the road, J d_obs is the three-dimensional potential field function of the dynamic obstacle; N p Represents the prediction time domain; N c Indicates the control time domain; Q P represents the weight matrix of the overall three-dimensional potential field function; R p represents the control weight matrix; t represents the current time as time t; i represents the i-th time domain; u p represents the control quantity; J(t+i|t) represents the three-dimensional potential field function value at time t+i predicted at time t; u p (t+i|t) represents the path planning control quantity at time t+i predicted at time t;
[0034] Step 2.4: For the objective function shown in formula (8), obtain N through the solver p Path discrete points Use a fifth-order polynomial to fit this N p discrete points, the expected path is as follows:
[0035]
[0036] Among them, a i represents the fitting constant of the expected horizontal coordinate, i = 0, 1, 2, 3, 4, 5, i represents the i-th coefficient; and b i represents the fitting constant of the desired yaw angle, i = 0, 1, 2, 3, 4, 5, i represents the i-th coefficient; Y ref represents the expected horizontal axis; represents the desired yaw angle; X represents the ordinate.
[0037] Step 3: Based on the expected path obtained by the improved artificial potential field method, detect the N position of the intelligent handling robot on the expected path. p Whether the discrete points will collide with the surrounding obstacles, and the total collision coefficient J is obtained sumcol , according to the total collision coefficient J sumcol Carry out speed planning, as follows:
[0038] Step 3.1: Check the path planning Np The collision between the intelligent transport robot and the obstacle at the discrete points of the trajectory. If the intelligent transport robot collides with the obstacle at the i-th discrete point, then let J col (i) = 1, otherwise let J col (i) = 0. P The total collision coefficient of discrete points is:
[0039]
[0040] Step 3.2: If J sumcol >0, it means that if the intelligent transport robot continues to travel at the current speed, it will collide with an obstacle on the expected path, that is, the expected path obtained in step 2.4 is dangerous. At this time, the speed of the intelligent transport robot is planned, and the expected speed of the intelligent transport robot is:
[0041]
[0042] Among them, V ref is the expected speed of the robot at time t in the prediction time domain, v0 is the speed of the obstacle with collision risk closest to the robot on the expected path, and a dmod To ease the deceleration, t dc1 is the time required for the robot to slow down to v0 with a gentle deceleration, a dmax is the maximum deceleration, t dc2 It is the time required for the robot to decelerate to v0 at the maximum deceleration; flag indicates the current speed planning state of the intelligent handling robot;
[0043] Step 3.3: If J sumcol =0, it means that if the intelligent transport robot continues to travel at the current speed, it will not collide with obstacles on the expected path, that is, the expected path obtained in step 2.4 is safe, and the expected speed of the intelligent transport robot is:
[0044]
[0045] Among them, v max is the robot target speed, a amax is the maximum acceleration, t ac The time it takes to accelerate from the current speed to the target speed.
[0046] Obstacle avoidance trajectory autonomously planned by the intelligent handling robot is obtained through steps 1 to 3, and steps 1 to 3 are trajectory planning modules of the intelligent handling robot;
[0047] Step 4: Based on the dynamic model of the intelligent handling robot, a model predictive control method is established to realize the tracking control of the robot's obstacle avoidance trajectory, as follows:
[0048] Step 4.1: Establish the dynamic model of the intelligent handling robot. The dynamic equation is as follows:
[0049]
[0050] Among them, m, I z They represent the total mass and the moment of inertia of the intelligent handling robot around the z axis respectively; C si , C αi The axes are the longitudinal stiffness and lateral stiffness of the front and rear wheels respectively; l f , l r Respectively represent the distance between the center of mass of the intelligent handling robot and the front and rear axes; s f 、s r are the wheel slip rates of the front and rear wheels, δ f is the front wheel steering angle, F xi 、F yi They represent the longitudinal force and lateral force of the wheel respectively, and τ is the time constant required for the actual acceleration of the intelligent handling robot to reach the expected acceleration, which is usually set to 0.5s; a des is the expected acceleration of the robot;
[0051] Step 4.2: Linearize the nonlinear dynamic model, and the transformed state space representation is as follows
[0052]
[0053] in, is the state variable, u c =[δ f ,a des ] T is the input control quantity of the intelligent handling robot, As output; δ f is the front wheel steering angle, a des Expected acceleration for the intelligent handling robot;
[0054] Step 4.3: To achieve the goal of predicting the state quantity at the future moment through the dynamic model established in step 4.1, the "prediction model" shown in formula (14) after linearization is discretized using the forward Euler method. The discretization form is as follows:
[0055] χ(k+1)=A k χ(k)+B k u c (k) (15)
[0056] Among them, A k =I7+T t A, A k represents the coefficient matrix, I7 represents the identity matrix; Bk =T t B, B k represents the control weight matrix; T t represents the sampling time of the model predictive controller; χ(k+1) represents the state quantity at time k+1; χ(k) represents the state quantity at time k; u c (k) represents the control quantity of the trajectory tracking system at time k;
[0057] Step 4.4: In order to enable the intelligent handling robot to track the expected trajectory and improve the stability of the intelligent handling robot, the optimization goal is to minimize the error between the output path position value of the intelligent handling robot and the expected path and the error between the output speed value of the intelligent handling robot and the expected speed. At the same time, in order to avoid the robot being out of control due to excessive control increments, the optimization goal is to minimize the control increment as much as possible. The constructed objective function is as follows:
[0058]
[0059] Among them, Q c Represents the weight matrix of the state quantity, R c represents the weight matrix of the control increment, p is the weight coefficient, ε is the relaxation factor; H p represents the prediction time domain of trajectory tracking control; H c represents the control time domain of trajectory tracking control; ΔU c To control the increment;
[0060] Step 4.5: Considering the trajectory tracking capability, actuator constraints and motion conditions of the intelligent handling robot, the optimization problem shown in formula (16) can be transformed into:
[0061]
[0062] in, D represents the coefficient matrix, represents the identity matrix; ΔU min , ΔU max Respectively represent the minimum and maximum values of the control increment; U min , U max Respectively represent the minimum and maximum values of the control amount;
[0063] Step 5: In order to improve the trajectory tracking accuracy, it is necessary to adjust the weight matrix Q in the objective function as shown in formula (17) in real time. c,, therefore, real-time parameter adjustment is performed based on reinforcement learning; reinforcement learning is an experience-driven autonomous learning strategy, which mainly studies the maximization of rewards in complex and uncertain environments through reinforcement learning. Traditional reinforcement learning algorithms are mainly divided into value function-based algorithms and policy-based algorithms, and convergence speed is slow; therefore, the present invention adopts the Actor-Critic framework, combines the value function-based algorithm with the policy-based algorithm, and is mainly divided into two parts: Actor and Critic. Critic consists of two neural networks, the online Q network and the target Q network, which are responsible for evaluating the value of the action. The probability of the action being selected is affected by the evaluated value, that is, the parameters of the Actor are updated based on the evaluation of the action. The Actor is used to estimate the deterministic policy function and is responsible for selecting the action according to the environment and state (s t ,a t ,r t ,s t+1 ), outputs specific actions through the strategy algorithm, that is, the strategy algorithm calculates the weight matrix Q according to the robot's current environment and its own state c , passed to the objective function as in formula (17);
[0064] Step 6: Update the weight matrix Q of the objective function as shown in formula (17) through reinforcement learning method c , and solve the optimization problem shown in formula (17) to obtain the optimal control increment sequence in the control time domain, and take the first control increment in the optimal control increment sequence as As the actual control increment, the control amount can be obtained:
[0065]
[0066] Among them, δ f (k) is the front wheel steering angle at time k; δ f (k-1) is the front wheel steering angle at time k-1; is the front wheel steering angle increment at time k; a des (k) is the expected acceleration of the intelligent handling robot at time k; a des (k-1) is the expected acceleration of the intelligent handling robot at time k-1; is the expected acceleration increment of the intelligent handling robot at time k;
[0067] Through steps 4 to 6, the intelligent handling robot tracks obstacle avoidance and outputs the final control amount. Steps 4 to 6 are the trajectory tracking module of the intelligent handling robot.
[0068] Step 7: The control quantity finally obtained in step 6 is transmitted to the intelligent transport robot, the state information of the intelligent transport robot is output, and then the state information of the intelligent transport robot is transmitted to the trajectory planning and trajectory tracking module to update the next state information and perform dynamic coordinated obstacle avoidance.
[0069] The beneficial effects of the present invention are:
[0070] (1) The present invention provides a trajectory planning method based on improved artificial potential field theory, which can plan the optimal obstacle avoidance path according to the current environment of the intelligent handling robot, and at the same time plan the speed according to this path, which greatly improves the safety of the intelligent handling robot and completes dynamic coordinated obstacle avoidance;
[0071] (2) The present invention provides an adaptive trajectory tracking control method based on reinforcement learning. Driven by reinforcement learning data, the weight matrix of the model predictive control method is updated in real time according to the current state of the intelligent handling robot and the surrounding environment, thereby improving the tracking trajectory accuracy and stability of the intelligent handling robot;
[0072] (3) The present invention is a dynamic coordinated obstacle avoidance trajectory planning and control strategy for dynamic environments based on reinforcement learning. Through real-time trajectory planning and parameter adaptive trajectory tracking control methods, the safety and stability of the robot can be ensured while effectively avoiding obstacles. BRIEF DESCRIPTION OF THE DRAWINGS
[0073] Figure 1 It is the three-dimensional potential field function diagram of two-lane road;
[0074] Figure 2 It is a schematic diagram of speed planning rules;
[0075] Figure 3 It is a framework diagram of dynamic coordinated obstacle avoidance trajectory planning and control of intelligent handling robots. DETAILED DESCRIPTION
[0076] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention is described in detail below with reference to the accompanying drawings and embodiments.
[0077] like Figure 1-Figure 3 As shown, the present invention comprises the following steps:
[0078] Step 1: Improve the artificial potential field method, that is, treat the environment and obstacles as different potential field functions for path planning. Establish a three-dimensional potential field function for the road environment based on the road structure, a three-dimensional potential field function for static obstacles based on the shape of static obstacles, and a three-dimensional potential field function for dynamic obstacles based on the shape, direction and speed of dynamic obstacles, as follows:
[0079] Step 1.1: Define the width L of a single lane for the intelligent transport robot to travelw The width of the robot is twice that of the robot, and the overall road for the intelligent handling robot is divided into several lanes with a width of L w The road potential field is established by taking the road environment where the road width is divided into two lanes as an example. Figure 1 The three-dimensional potential field function shown in the figure is established in the same way as the three-dimensional potential field function of two lanes.
[0080] Step 1.2: Set the intelligent handling robot to drive on the right, that is, the three-dimensional potential field function value of the center line of the right lane is lower than the three-dimensional potential field function value of the center line of the left lane, and the three-dimensional potential field function A from the center line of the left lane to the lane dividing line l and the three-dimensional potential field function A from the center line of the right lane to the lane dividing line r , the sum of the three-dimensional potential field function A between the center lines of the left and right lanes is as follows:
[0081]
[0082] Where Y is the longitudinal position of the robot, Y m Indicates the longitudinal coordinate of the lane dividing line, L w It represents the width of a single lane; p m0 is the coefficient that determines the three-dimensional potential field function value at the center line of the left lane, p m is the coefficient, which determines the value of the three-dimensional potential field function at the dividing line between the two lanes;
[0083] Step 1.3: Introduce the road boundary potential field to restrict the intelligent handling robot to keep moving within the road. Therefore, when the intelligent handling robot approaches the road boundary, its potential field should be designed to be infinite. Design the three-dimensional potential field function B from the center line of the left lane to the road boundary l , design the three-dimensional potential field function B from the center line of the right lane to the right of the road boundary r , then the sum of the three-dimensional potential field functions B from the centerline of the left and right lanes to the left and right boundaries is as follows:
[0084]
[0085] Among them, Y l Indicates the longitudinal coordinate of the center line of the left lane, Y r Indicates the longitudinal coordinate of the center line of the right lane;
[0086] Step 1.4: Integrate the three-dimensional potential field functions shown in the above formulas (1) and (2) to obtain the complete road three-dimensional potential field function J that meets the requirements road as follows:
[0087]
[0088] Among them, S1 is the action coefficient of the sum of the three-dimensional potential field function A between the center lines of the left and right lanes, S2 is the action coefficient of the sum of the three-dimensional potential field function B from the center lines of the left and right lanes to the left and right boundaries respectively, and J road is the three-dimensional potential field function of the complete road, A is the sum of the three-dimensional potential field functions between the center lines of the left and right lanes, and B is the sum of the three-dimensional potential field functions from the center lines of the left and right lanes to the left and right boundaries respectively;
[0089] like Figure 1 As shown, Y=0 is the lane on the right. It can be clearly seen from the figure that the potential field function value at the center line of the right lane is relatively low, which complies with the rule that the robot drives on the right. At the same time, the closer to the boundary, the higher the potential field function value is, which prevents the robot from driving off the road and ensures the safety of the robot.
[0090] Step 1.5: Establish the static obstacle three-dimensional potential field function J according to the obstacle shape s_obs as follows:
[0091]
[0092] Among them, (X,Y), (X obs ,Y obs ) represent the coordinates of the intelligent handling robot and the obstacle in the global coordinate system, J s_obs is the static obstacle potential field established; c1 is the weight of the lateral distance between the robot and the obstacle, c2 is the weight of the longitudinal distance between the robot and the obstacle, and ρ represents the threshold for determining the range of the repulsive potential field;
[0093] Step 1.6: Introduce dynamic factors and construct the dynamic obstacle three-dimensional potential field function J d_obs as follows:
[0094]
[0095] Among them, k o is a scaling weight constant value greater than zero, v o is the speed of the obstacle, θ o For vector r = [XX obs ,YY obs ] and v o The angle, J s_obs is the static obstacle potential field established, D d is the dynamic factor, J d_obs is the three-dimensional potential field function of the dynamic obstacle.
[0096] Step 2: Based on the road three-dimensional potential field function J shown in formula (3) road and the dynamic obstacle three-dimensional potential field function J shown in formula (5) d_obs Establish the objective function and use the solver to solve the desired path, as follows:
[0097] Step 2.1: Establish the kinematic model of the intelligent handling robot as follows:
[0098]
[0099] Among them, (X, Y) and (x, y) are the positions of the center of mass of the intelligent handling robot in the global coordinate system and its own coordinate system respectively; are the longitudinal speed and lateral speed of the intelligent handling robot, represents the yaw angle of the intelligent handling robot, and γ represents the yaw angular velocity of the intelligent handling robot;
[0100] Step 2.2: Establish the stability constraints of the path planning module as follows:
[0101]
[0102] Among them, v y =βv x , and is the abbreviation in the above formula, l r is the distance from the rear axle to the center of mass of the intelligent handling robot; α r,sat is the maximum slip angle of the rear wheel within the linear range; μ is the road friction coefficient; g is the acceleration of gravity; β is the center of mass slip angle;
[0103] Step 2.3: The established road three-dimensional potential field function J road , dynamic obstacle three-dimensional potential field function J d_obs and the designed control quantity u p Integrating into the objective function, the objective function is as follows:
[0104]
[0105] Where J = [J road ,J d_obs ] T , J is the overall three-dimensional potential field function of the road and dynamic obstacles, J road is the three-dimensional potential field function of the road, J d_obs is the three-dimensional potential field function of the dynamic obstacle; N p Represents the prediction time domain; N c Indicates the control time domain; Q P represents the weight matrix of the overall three-dimensional potential field function; R p represents the control weight matrix; t represents the current time as time t; i represents the i-th time domain; u p represents the control quantity; J(t+i|t) represents the three-dimensional potential field function value at time t+i predicted at time t; u p(t+i|t) represents the path planning control quantity at time t+i predicted at time t;
[0106] Step 2.4: For the objective function shown in formula (8), obtain N through the solver p Path discrete points Use a fifth-order polynomial to fit this N p discrete points, the expected path is as follows:
[0107]
[0108] Among them, a i represents the fitting constant of the expected horizontal coordinate, i = 0, 1, 2, 3, 4, 5, i represents the i-th coefficient; and b i represents the fitting constant of the desired yaw angle, i = 0, 1, 2, 3, 4, 5, i represents the i-th coefficient; Y ref represents the expected horizontal axis; represents the desired yaw angle; X represents the ordinate.
[0109] Step 3: Based on the expected path obtained by the improved artificial potential field method, detect the N position of the intelligent handling robot on the expected path. p Whether the discrete points will collide with the surrounding obstacles, and the total collision coefficient J is obtained sumcol , according to the total collision coefficient J sumcol Carry out speed planning, as follows:
[0110] Step 3.1: Check the path planning N p The collision between the intelligent transport robot and the obstacle at the discrete points of the trajectory. If the intelligent transport robot collides with the obstacle at the i-th discrete point, then let J col (i) = 1, otherwise let J col (i) = 0. P The total collision coefficient of discrete points is:
[0111]
[0112] Step 3.2: If J sumcol >0, it means that if the intelligent transport robot continues to travel at the current speed, it will collide with an obstacle on the expected path, that is, the expected path danger obtained in step 2.4. Figure 2 The speed planning rule shown is used to plan the speed of the intelligent handling robot. The expected speed of the intelligent handling robot is:
[0113]
[0114] Among them, V refis the expected speed of the robot at time t in the prediction time domain, v0 is the speed of the obstacle with collision risk closest to the robot on the expected path, and a dmod To ease the deceleration, t dc1 is the time required for the robot to slow down to v0 with a gentle deceleration, a dmax is the maximum deceleration, t dc2 It is the time required for the robot to decelerate to v0 at the maximum deceleration; flag indicates the current speed planning state of the intelligent handling robot;
[0115] Step 3.3: If J sumcol = 0, it means that if the intelligent transport robot continues to travel at the current speed, it will not collide with obstacles on the expected path, that is, the expected path obtained in step 2.4 is safe. Figure 2 The speed planning rule shown is used to plan the speed of the intelligent handling robot. The expected speed of the intelligent handling robot is:
[0116]
[0117] Among them, v max is the robot target speed, a amax is the maximum acceleration, t ac The time it takes to accelerate from the current speed to the target speed.
[0118] Through steps 1 to 3, the obstacle avoidance trajectory planned autonomously by the intelligent handling robot is obtained. Steps 1 to 3 are the trajectory planning module of the intelligent handling robot. The framework of the trajectory planning module is as follows: Figure 3 As shown;
[0119] Step 4: Based on the dynamic model of the intelligent handling robot, a model predictive control method is established to realize the tracking control of the robot's obstacle avoidance trajectory, as follows:
[0120] Step 4.1: Establish the dynamic model of the intelligent handling robot. The dynamic equation is as follows:
[0121]
[0122] Among them, m, I z They represent the total mass and the moment of inertia of the intelligent handling robot around the z axis respectively; C si , C αi The axes are the longitudinal stiffness and lateral stiffness of the front and rear wheels respectively; l f , l r Respectively represent the distance between the center of mass of the intelligent handling robot and the front and rear axes; s f 、s r are the wheel slip rates of the front and rear wheels, δ f is the front wheel steering angle, Fxi 、F yi They represent the longitudinal force and lateral force of the wheel respectively, and τ is the time constant required for the actual acceleration of the intelligent handling robot to reach the expected acceleration, which is usually set to 0.5s; a des is the expected acceleration of the robot;
[0123] Step 4.2: Linearize the nonlinear dynamic model, and the transformed state space representation is as follows
[0124]
[0125] in, is the state variable, u c =[δ f ,a des ] T is the input control quantity of the intelligent handling robot, As output; δ f is the front wheel steering angle, a des Expected acceleration for the intelligent handling robot;
[0126] Step 4.3: To achieve the goal of predicting the state quantity at the future moment through the dynamic model established in step 4.1, the "prediction model" shown in formula (14) after linearization is discretized using the forward Euler method. The discretization form is as follows:
[0127] χ(k+1)=A k χ(k)+B k u c (k) (15)
[0128] Among them, A k =I7+T t A, A k represents the coefficient matrix, I7 represents the identity matrix; B k =T t B, B k represents the control weight matrix; T t represents the sampling time of the model predictive controller; χ(k+1) represents the state quantity at time k+1; χ(k) represents the state quantity at time k; u c (k) represents the control quantity of the trajectory tracking system at time k;
[0129] Step 4.4: In order to enable the intelligent handling robot to track the expected trajectory and improve the stability of the intelligent handling robot, the optimization goal is to minimize the error between the output path position value of the intelligent handling robot and the expected path and the error between the output speed value of the intelligent handling robot and the expected speed. At the same time, in order to avoid the robot being out of control due to excessive control increments, the optimization goal is to minimize the control increment as much as possible. The constructed objective function is as follows:
[0130]
[0131] Among them, Q c Represents the weight matrix of the state quantity, R c represents the weight matrix of the control increment, p is the weight coefficient, ε is the relaxation factor; H p represents the prediction time domain of trajectory tracking control; H c represents the control time domain of trajectory tracking control; ΔU c To control the increment;
[0132] Step 4.5: Considering the trajectory tracking capability, actuator constraints and motion conditions of the intelligent handling robot, the optimization problem shown in formula (16) can be transformed into:
[0133]
[0134] in, D represents the coefficient matrix, represents the identity matrix; ΔU min , ΔU max Respectively represent the minimum and maximum values of the control increment; U min , U max Respectively represent the minimum and maximum values of the control amount;
[0135] Step 5: In order to improve the trajectory tracking accuracy, it is necessary to adjust the weight matrix Q in the objective function as shown in formula (17) in real time. c ,, therefore, real-time parameter adjustment is performed based on reinforcement learning; reinforcement learning is an experience-driven autonomous learning strategy, which mainly studies the maximization of rewards in complex and uncertain environments through reinforcement learning. Traditional reinforcement learning algorithms are mainly divided into value function-based algorithms and policy-based algorithms, and convergence speed is slow; therefore, the present invention adopts the Actor-Critic framework, combines the value function-based algorithm with the policy-based algorithm, and is mainly divided into two parts: Actor and Critic. Critic consists of two neural networks, the online Q network and the target Q network, which are responsible for evaluating the value of the action. The probability of the action being selected is affected by the evaluated value, that is, the parameters of the Actor are updated based on the evaluation of the action. The Actor is used to estimate the deterministic policy function and is responsible for selecting the action according to the environment and state (s t ,a t ,r t ,s t+1 ), outputs specific actions through the strategy algorithm, that is, the strategy algorithm calculates the weight matrix Q according to the robot's current environment and its own state c , passed to the objective function as in formula (17), the reinforcement learning framework is as follows Figure 3 As shown;
[0136] Step 6: Update the weight matrix Q of the objective function as shown in formula (17) through reinforcement learning method c , and solve the optimization problem shown in formula (17) to obtain the optimal control increment sequence in the control time domain, and take the first control increment in the optimal control increment sequence as As the actual control increment, the control amount can be obtained:
[0137]
[0138] Among them, δ f (k) is the front wheel steering angle at time k; δ f (k-1) is the front wheel steering angle at time k-1; is the front wheel steering angle increment at time k; a des (k) is the expected acceleration of the intelligent handling robot at time k; a des (k-1) is the expected acceleration of the intelligent handling robot at time k-1; is the expected acceleration increment of the intelligent handling robot at time k;
[0139] Through steps 4 to 6, the intelligent handling robot is controlled to track and avoid obstacles, and the final control amount is output. Steps 4 to 6 are the trajectory tracking module of the intelligent handling robot. The framework of the trajectory tracking module is as follows: Figure 3 As shown;
[0140] Step 7: The control quantity finally obtained in step 6 is transmitted to the intelligent handling robot, and the state information of the intelligent handling robot is output. Then, the state information of the intelligent handling robot is transmitted to the trajectory planning and trajectory tracking module to update the next state information and perform dynamic coordinated obstacle avoidance. The dynamic coordinated obstacle avoidance framework of the intelligent handling robot is as follows: Figure 3 shown.
[0141] The present invention is a dynamic coordinated obstacle avoidance trajectory planning and control strategy for an intelligent handling robot based on reinforcement learning, which can effectively avoid obstacles while ensuring the safety and stability of the robot.
[0142] What is disclosed above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.
Claims
1. A dynamic coordination obstacle avoidance method for intelligent handling robots based on reinforcement learning. It is characterized in that The dynamic coordinated obstacle avoidance method for the intelligent handling robot first plans the obstacle avoidance path by improving the artificial potential field method; secondly, collision detection is performed based on the planned path information to plan the speed; finally, the parameters of the model predictive control are adaptively adjusted through reinforcement learning to achieve dynamic coordinated obstacle avoidance and ensure the safety and driving stability of the robot; the dynamic coordinated obstacle avoidance method for the intelligent handling robot can realize autonomous planning of obstacle avoidance trajectory and tracking control in a multi-dynamic obstacle environment.
2. According to claim 1, a dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, It is characterized in that The following steps are involved: Step 1: Improve the artificial potential field method, that is, the environment and obstacles are equivalent to different potential field functions for path planning; establish a three-dimensional potential field function of the road environment according to the road structure, establish a three-dimensional potential field function of the static obstacle according to the shape of the static obstacle, and establish a three-dimensional potential field function of the dynamic obstacle according to the shape, direction and speed of the dynamic obstacle; Step 2: The three-dimensional potential field function J of the road established in step 1 road and dynamic obstacle three-dimensional potential field function J d_obs Establish the objective function and use the solver to solve the desired path, as follows: Step 2.1: Establish the kinematic model of the intelligent handling robot as follows: Among them, (X, Y) and (x, y) are the positions of the center of mass of the intelligent handling robot in the global coordinate system and its own coordinate system respectively; are the longitudinal speed and lateral speed of the intelligent handling robot, represents the yaw angle of the intelligent handling robot, and γ represents the yaw angular velocity of the intelligent handling robot; Step 2.2: Establish stability constraints for the path planning module; Step 2.3: The established road three-dimensional potential field function J road , dynamic obstacle three-dimensional potential field function J d_obs and the designed control quantity u p Integrating into the objective function, the objective function is as follows: s.t.C s u p ≤b s (8) Where J = [J road ,J d_obs ] T , J is the overall three-dimensional potential field function of the road and dynamic obstacles, J road is the three-dimensional potential field function of the road, J d_obs is the three-dimensional potential field function of the dynamic obstacle; N p Represents the prediction time domain; N c Indicates the control time domain; Q P represents the weight matrix of the overall three-dimensional potential field function; R p represents the control weight matrix; t represents the current time as time t; i represents the i-th time domain; u p represents the control quantity; J(t+i|t) represents the three-dimensional potential field function value at time t+i predicted at time t; u p (t+i|t) represents the path planning control quantity at time t+i predicted at time t; Step 2.4: For the objective function shown in formula (8), obtain N through the solver p Path discrete points Use a fifth-order polynomial to fit this N p discrete points, the expected path is as follows: Among them, a i represents the fitting constant of the expected horizontal coordinate, i = 0, 1, 2, 3, 4, 5, i represents the i-th coefficient; and b i represents the fitting constant of the desired yaw angle, i = 0, 1, 2, 3, 4, 5, i represents the i-th coefficient; Y ref represents the expected horizontal axis; represents the desired yaw angle; X represents the ordinate; Step 3: Based on the expected path obtained by the improved artificial potential field method, detect the N position of the intelligent handling robot on the expected path. p Whether the discrete points will collide with the surrounding obstacles, and the total collision coefficient J is obtained sumcol , according to the total collision coefficient J sumcol Conduct speed planning; Obstacle avoidance trajectory autonomously planned by the intelligent handling robot is obtained through steps 1 to 3, and steps 1 to 3 are trajectory planning modules of the intelligent handling robot; Step 4: Based on the dynamic model of the intelligent handling robot, a model predictive control method is established to realize the tracking control of the robot's obstacle avoidance trajectory, as follows: Step 4.1: Establish the dynamic model of the intelligent handling robot. The dynamic equation is as follows: Among them, m, I z They represent the total mass and the moment of inertia of the intelligent handling robot around the z axis respectively; C si , C αi The axes are the longitudinal stiffness and lateral stiffness of the front and rear wheels respectively; l f , l r Respectively represent the distance between the center of mass of the intelligent handling robot and the front and rear axes; s f 、s r are the wheel slip rates of the front and rear wheels, δ f is the front wheel steering angle, F xi 、F yi They represent the longitudinal force and lateral force of the wheel respectively, and τ is the time constant required for the actual acceleration of the intelligent handling robot to reach the expected acceleration; a des is the expected acceleration of the robot; Step 4.2: Linearize the nonlinear dynamic model; Step 4.3: To achieve the goal of predicting the state quantity at the future moment through the dynamic model established in step 4.1, the "prediction model" shown after linearization is discretized using the forward Euler method; Step 4.4: The optimization goal is to minimize the error between the output path position value of the intelligent handling robot and the expected path, and the error between the output speed value of the intelligent handling robot and the expected speed, and to minimize the increment as much as possible. The constructed objective function is as follows: Among them, Q c Represents the weight matrix of the state quantity, R c represents the weight matrix of the control increment, p is the weight coefficient, ε is the relaxation factor; H p represents the prediction time domain of trajectory tracking control; H c represents the control time domain of trajectory tracking control; ΔU c To control the increment; Step 4.5: Considering the trajectory tracking capability, actuator constraints and motion conditions of the intelligent handling robot, the optimization problem shown in formula (16) is transformed into: in, D represents the coefficient matrix, represents the identity matrix; ΔU min , ΔU max Respectively represent the minimum and maximum values of the control increment; U min , U max Respectively represent the minimum and maximum values of the control amount; Step 5: Use the Actor-Critic framework, which is mainly divided into two parts: Actor and Critic. Critic consists of two neural networks: online Q network and target Q network. It is responsible for evaluating the value of the action. The probability of the action being selected is affected by the evaluated value. That is, the parameters of the Actor are updated based on the evaluation of the action. The Actor is used to estimate the deterministic policy function and is responsible for selecting the action according to the environment and state (s t ,a t ,r t ,s t+1 ), outputs specific actions through the strategy algorithm, that is, the strategy algorithm calculates the weight matrix Q according to the robot's current environment and its own state c , passed to the objective function as in formula (17); Step 6: Update the weight matrix Q of the objective function as shown in formula (17) through reinforcement learning method c , and solve the optimization problem shown in formula (17) to obtain the optimal control increment sequence in the control time domain, and take the first control increment in the optimal control increment sequence as As the actual control increment, the control amount is obtained: Among them, δ f (k) is the front wheel steering angle at time k; δ f (k-1) is the front wheel steering angle at time k-1; is the front wheel steering angle increment at time k; a des (k) is the expected acceleration of the intelligent handling robot at time k; a des (k-1) is the expected acceleration of the intelligent handling robot at time k-1; is the expected acceleration increment of the intelligent handling robot at time k; Through steps 4 to 6, the intelligent handling robot tracks obstacle avoidance and outputs the final control amount. Steps 4 to 6 are the trajectory tracking module of the intelligent handling robot. Step 7: The control quantity finally obtained in step 6 is transmitted to the intelligent transport robot, the state information of the intelligent transport robot is output, and then the state information of the intelligent transport robot is transmitted to the trajectory planning and trajectory tracking module to update the next state information and perform dynamic coordinated obstacle avoidance.
3. According to claim 2, a dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, It is characterized in that The step 1 is specifically as follows: Step 1.1: Define the width L of a single lane for the intelligent transport robot to travel w The width of the robot is twice that of the robot, and the overall road for the intelligent handling robot is divided into several lanes with a width of L w The road potential field is established by dividing the road width into two lanes. The three-dimensional potential field function is established by taking the road environment divided into two lanes as an example. The three-dimensional potential field function of multiple lanes is established in the same way as the three-dimensional potential field function of two lanes. Step 1.2: Set the intelligent handling robot to drive on the right, that is, the three-dimensional potential field function value of the center line of the right lane is lower than the three-dimensional potential field function value of the center line of the left lane, and the three-dimensional potential field function A from the center line of the left lane to the lane dividing line l and the three-dimensional potential field function A from the center line of the right lane to the lane dividing line r , the sum of the three-dimensional potential field function A between the center lines of the left and right lanes is as follows: Where Y is the longitudinal position of the robot, Y m Indicates the longitudinal coordinate of the lane dividing line, L w It represents the width of a single lane; p m0 is the coefficient that determines the three-dimensional potential field function value at the center line of the left lane, p m is the coefficient, which determines the value of the three-dimensional potential field function at the dividing line between the two lanes; Step 1.3: When the intelligent handling robot approaches the road boundary, its potential field should be designed to be infinite; design the three-dimensional potential field function B from the center line of the left lane to the road boundary l , design the three-dimensional potential field function B from the center line of the right lane to the right of the road boundary r , then the sum of the three-dimensional potential field functions B from the centerline of the left and right lanes to the left and right boundaries is as follows: Among them, Y l Indicates the longitudinal coordinate of the center line of the left lane, Y r Indicates the longitudinal coordinate of the center line of the right lane; Step 1.4: Integrate the three-dimensional potential field functions shown in the above formulas (1) and (2) to obtain the complete road three-dimensional potential field function J that meets the requirements road as follows: Among them, S 1 is the action coefficient of the sum of the three-dimensional potential field function A between the center lines of the left and right lanes, S 2 is the action coefficient of the sum of the three-dimensional potential field functions B from the center line of the left and right lanes to the left and right boundaries, J road is the three-dimensional potential field function of the complete road, A is the sum of the three-dimensional potential field functions between the center lines of the left and right lanes, and B is the sum of the three-dimensional potential field functions from the center lines of the left and right lanes to the left and right boundaries respectively; Step 1.5: Establish the static obstacle three-dimensional potential field function J according to the obstacle shape s_obs as follows: Among them, (X,Y), (X obs ,Y obs ) represent the coordinates of the intelligent handling robot and the obstacle in the global coordinate system, J s_obs is the static obstacle potential field established; c 1 is the weight of the lateral distance between the robot and the obstacle, c 2 is the weight of the longitudinal distance between the robot and the obstacle, ρ represents the threshold for determining the range of the repulsive potential field; Step 1.6: Introduce dynamic factors and construct the dynamic obstacle three-dimensional potential field function J d_obs as follows: Among them, k o is a scaling weight constant value greater than zero, v o is the speed of the obstacle, θ o For vector r = [XX obs ,YY obs ] and v o The angle, J s_obs is the static obstacle potential field established, D d is the dynamic factor, J d_obs is the three-dimensional potential field function of the dynamic obstacle.
4. According to claim 2, a dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, It is characterized in that In step 2.2, the stability constraints are as follows: Among them, v y =βv x , and is the abbreviation in the above formula, l r is the distance from the rear axle to the center of mass of the intelligent handling robot; α r,sat is the maximum sideslip angle within the linear range of the rear wheel; μ is the road friction coefficient; g is the acceleration of gravity; β is the sideslip angle at the center of mass.
5. According to claim 2, a dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, It is characterized in that The step 3 is as follows: Step 3.1: Check the path planning N p The collision between the intelligent transport robot and the obstacle at the discrete points of the trajectory. If the intelligent transport robot collides with the obstacle at the i-th discrete point, then let J col (i) = 1, otherwise let J col (i) = 0; N P The total collision coefficient of discrete points is: Step 3.2: If J sumcol >0, it means that if the intelligent transport robot continues to travel at the current speed, it will collide with an obstacle on the expected path, that is, the expected path obtained in step 2.4 is dangerous. At this time, the speed of the intelligent transport robot is planned, and the expected speed of the intelligent transport robot is: Among them, V ref is the expected speed of the robot at time t in the prediction time domain, v 0 is the speed of the obstacle closest to the robot on the desired path that poses a collision risk, and a dmod To ease the deceleration, t dc1 The robot slows down to v with a gentle deceleration 0 The time required, a dmax is the maximum deceleration, t dc2 The robot decelerates to v at the maximum deceleration 0 The time required; flag indicates the current speed planning status of the intelligent handling robot; Step 3.3: If J sumcol =0, it means that if the intelligent transport robot continues to travel at the current speed, it will not collide with obstacles on the expected path, that is, the expected path obtained in step 2.4 is safe, and the expected speed of the intelligent transport robot is: Among them, v max is the robot target speed, a amax is the maximum acceleration, t ac The time it takes to accelerate from the current speed to the target speed.
6. According to claim 2, a dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, It is characterized in that In step 4.2, the state space representation of the linearized nonlinear dynamic model is as follows: in, is the state variable, u c =[δ f ,a des ] T is the input control quantity of the intelligent handling robot, As output quantity; δ f is the front wheel steering angle, a des Expected acceleration for the intelligent handling robot.
7. According to claim 2, a dynamic coordination obstacle avoidance method for an intelligent handling robot based on reinforcement learning, It is characterized in that In step 4.3, the discretization form is as follows: x(k+1)=A k x(k)+B k you c (k) (15) Among them, A k =I 7 +T t A, A k represents the coefficient matrix, I 7 represents the identity matrix; B k =T t B, B k represents the control weight matrix; T t represents the sampling time of the model predictive controller; χ(k+1) represents the state quantity at time k+1; χ(k) represents the state quantity at time k; u c (k) represents the control quantity of the trajectory tracking system at time k.
Citation Information
Cited By
Mobile charging robot obstacle avoidance method and system based on multi-sensor fusion
CN119148722A
Multi-humanoid robot distributed collaborative carrying method based on model predictive control
CN119847023A
Dynamic collaborative transportation operation method for multiple autonomous mobile robots based on data driving
CN120010482A
Warehouse management method and system based on AGV robot
CN120218813A
Warehouse management method and system based on AGV robot
CN120218813B
