A local path planning method for robots in dense smoke and foggy environments

Abstract

A local path planning method for a robot in a dense smoke and foggy environment comprises a millimeter wave radar and a laser radar installed on the robot, and comprises the following steps: using the laser radar to collect three-dimensional point cloud map data of a target area in a real scene, and recording it as the laser radar distance data; simulating a virtual environment with dense smoke and fog in a physical simulation environment, and training the robot to perform autonomous navigation in the virtual environment; obtaining obstacle data perceived by the millimeter wave radar in the virtual environment, and recording it as the millimeter wave radar distance data; training a comparative reinforcement learning network through the laser radar distance data and the millimeter wave radar distance data, and using the trained comparative reinforcement learning network as a local path planning network; obtaining local path planning information through the local path planning network; using a millimeter wave radar that can penetrate smoke to perceive obstacles in the environment, and using a deep reinforcement learning method to enable the robot to continuously learn the optimal navigation strategy in dense smoke and foggy scenes.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous navigation, and in particular to a local path planning method for a robot in a dense smoke and foggy environment. Background Art

[0002] In recent years, there have been frequent cases of people being trapped and unable to leave the fire scene in time due to indoor fires, posing a great threat to people's lives and property. The emergence of firefighting robots on the market has greatly improved the efficiency of rescue. When a fire occurs, they can enter the fire scene as soon as possible and use their own sensors to autonomously explore the internal environment of the fire scene, which can help firefighters find trapped people faster, thereby improving the overall search and rescue efficiency. However, the fire scene environment in real scenarios is complex and changeable. Robots have to perform tasks in harsh environments such as high temperature, toxicity, and smoke. Especially in terms of autonomous positioning and navigation, commonly used navigation devices such as lidar or visual cameras will be blocked by particles or gases in the air, and debris will lose some of their perception capabilities, resulting in their inability to accurately detect obstacles in the surrounding environment. Ultimately, the robot may partially lose or completely lose its autonomous navigation capabilities in the smoke and fog environment of the fire scene. Summary of the invention

[0003] In view of the problems in the prior art, the present invention provides a local path planning method for a robot in a smoke and foggy environment, aiming to achieve autonomous navigation in semi-transparent scenes such as smoke and fog.

[0004] A local path planning method for a robot in a dense smoke and foggy environment, comprising a robot with an autonomous navigation function in a real scene, a millimeter wave radar and a laser radar installed on the robot, characterized in that it comprises the following steps:

[0005] Step 1: Use laser radar to collect 3D point cloud map data of the target area in the real scene and record it as laser radar distance data;

[0006] Step 2: Simulate a virtual environment with thick smoke and fog in a physical simulation environment and train the robot to perform autonomous navigation in the virtual environment;

[0007] Step 3: Obtain obstacle data perceived by the millimeter-wave radar in the virtual environment and record it as millimeter-wave radar distance data;

[0008] Step 4: Train the comparative reinforcement learning network using the lidar distance data and the millimeter wave radar distance data, and use the trained comparative reinforcement learning network as the local path planning network;

[0009] The contrastive reinforcement learning network includes a fixed encoder and a trainable encoder, both of which are fixed key encoders with Euclidean distance, the input of the fixed encoder is the laser radar distance data, and the input of the trainable encoder is the millimeter wave radar distance data; the contrast loss between the fixed encoder and the trainable encoder is calculated by the contrast loss function, the trainable encoder is updated according to the contrast loss and a navigation encoder is obtained; the local path planning network is obtained by the navigation encoder;

[0010] Step 5: Obtain local path planning information through the local path planning network.

[0011] Further, in step 2, the process of training the robot to perform autonomous navigation in a virtual environment is:

[0012] The action performed by the robot at the current moment comes from the current navigation strategy. Each time the robot's spatial state is transferred, the robot will receive a reward, so that the robot can learn the optimal navigation strategy that maximizes the expected reward and use it as the navigation strategy for the next moment.

[0013] Optimal navigation strategy for: ;

[0014] in, is the discount factor at time t, For expectations, is the reward function; is the strategy parameter.

[0015] Further:

[0016] Reward Function is the total reward score at time t, that is:

[0017] in, To reach the reward score, is a negative reward score, is the distance gradient reward score, Bonus points for time cost.

[0018] Further:

[0019] Use gradient ascent method to update policy parameters ,Right now:

[0020] in, is the learning rate, For parameters The gradient of is the objective function, that is:

[0021]

[0022]

[0023] in, is the expectation at time t, is the cutoff constant and is set to 0.1, is the truncation function, is the navigation strategy at the current moment, Navigation strategy for the next moment; is the robot’s current action.

[0024] Further: the robot's current action for:

[0025]

[0026]

[0027] Where λ is a positive number less than 1. is the state value and is obtained through the output of the Critic network; T is the corresponding time when the collected trajectory ends;

[0028] Robot space status , as the robot moves And the state transition probability matrix PP The action performed by the robot at time t is changed. From the current navigation strategy, ∈ Action space A, state space From LiDAR distance data , millimeter wave radar distance data 、The robot's current linear speed and angular velocity And the coordinates of the target point relative to the robot Composing multidimensional vectors , the action space A consists of two elements obtained by sampling from the continuous action space, namely the linear velocity and the angular velocity, which form a two-dimensional vector .

[0029] Further: In step 4, the contrast loss formula is as follows:

[0030]

[0031] in, is the laser radar distance data, is the millimeter wave radar distance data, is the expected value; is a trainable encoder, is a fixed encoder; k and q are the outputs of the fixed encoder and the trainable encoder, respectively.

[0032] The beneficial effects of the present invention are as follows: a millimeter-wave radar that can penetrate smoke to sense obstacles in the environment is used, and a deep reinforcement learning method is used to enable the robot to continuously learn the optimal navigation strategy in thick smoke and fog scenes; and a contrast learning algorithm is used to regard the millimeter-wave radar data as an enhancement of the lidar data. By continuously training the encoder, local path planning can be achieved in a thick smoke and foggy environment using only data from the millimeter-wave radar, which is used for local path planning of the robot, and combined with a pre-established three-dimensional grid map, ultimately achieving autonomous navigation of the robot in a thick smoke and foggy environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is a flow chart of the present invention. DETAILED DESCRIPTION

[0034] The present invention is described in detail below in conjunction with the accompanying drawings. The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements with the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be interpreted as limiting the present invention. The directional terms such as left, middle, right, top, and bottom in the embodiments of the present invention are only relative concepts or are based on the normal use state of the product, and should not be considered as restrictive.

[0035] A local path planning method for a robot in a dense smoke and foggy environment includes a robot with autonomous navigation function in a real scene, a Dijkstra algorithm and a navigation framework are set in the robot, and before autonomous navigation, the three-dimensional point cloud map data is converted into a three-dimensional grid map that can be used for robot navigation through the Octomap function package, and during autonomous navigation, a global optimal path pointing to a final target point is automatically planned based on the three-dimensional point cloud map data using the Dijkstra algorithm; then the three-dimensional grid map is loaded into the robot's navigation framework, and the robot autonomously navigates in a given real scene based on the global optimal path; a millimeter wave radar and a laser radar are installed on the robot, such as Figure 1 As shown, the following steps are included:

[0036] Step 1: Use laser radar to collect 3D point cloud map data of the target area in the real scene and record it as laser radar distance data;

[0037] Step 2: Simulate a virtual environment with thick smoke and fog in a physical simulation environment and train the robot to perform autonomous navigation in the virtual environment;

[0038] The process of training a robot to navigate autonomously in a virtual environment is:

[0039] The action performed by the robot at the current moment comes from the current navigation strategy. Each time the robot's spatial state is transferred, the robot will receive a reward, so that the robot can learn the optimal navigation strategy that maximizes the expected reward and use it as the navigation strategy for the next moment.

[0040] Optimal navigation strategy for:

[0041] in, is the discount factor at time t, For expectations, is the reward function, the reward function is the total reward score at time t, that is:

[0042] in, To reach the reward score, is a negative reward score, is the distance gradient reward score, To reward points for time cost;

[0043] The strategy parameters can be adjusted through continuous training of the network. Update and repeat the above steps with new policy parameters until the policy performance no longer improves to obtain the optimal navigation policy. ; Use the gradient ascent method to update the policy parameters ,Right now:

[0044] in, is the learning rate, For parameters The gradient of is the objective function, that is:

[0045]

[0046]

[0047] in, is the expectation at time t, is the cutoff constant and is set to 0.1, is the truncation function, is the navigation strategy at the current moment, Navigation strategy for the next moment; is the robot’s current action;

[0048] The robot's current action for;

[0049]

[0050]

[0051] Where λ is a positive number less than 1. is the state value and is obtained through the output of the Critic network; T is the corresponding time when the collected trajectory ends; specifically, the navigation strategy is executed in the environment To obtain a set of interaction data, these data include , , Then, the robot state is defined as the laser radar data, millimeter wave radar data, mobile robot speed information, and the coordinate information of the target point relative to the mobile robot. , As network input data, the lidar branch needs to use the LSTM module to process the time series in the input signal. Then, the lidar branch extracts high-dimensional features through a fully connected layer with ReLu as the activation function. On the one hand, it passes through the Actor network and then passes through a fully connected layer with Tanh and Linear as the activation function to calculate the action and output the action at the current moment. On the other hand, the Critic network is used to output the state value ;

[0052] Robot space status , as the robot moves And the state transition probability matrix PP The action performed by the robot at time t is From the current navigation strategy, ∈ ∈ action space A, state space From LiDAR distance data , millimeter wave radar distance data 、The robot's current linear speed and angular velocity And the coordinates of the target point relative to the robot Composing multidimensional vectors , the action space A consists of two elements obtained by sampling from the continuous action space, namely the linear velocity and the angular velocity, which form a two-dimensional vector ;

[0053] Step 3: Obtain obstacle data perceived by the millimeter-wave radar in the virtual environment and record it as millimeter-wave radar distance data;

[0054] Step 4: Train the comparative reinforcement learning network using the lidar distance data and the millimeter wave radar distance data, and use the trained comparative reinforcement learning network as the local path planning network;

[0055] The contrastive reinforcement learning network includes a fixed encoder and a trainable encoder, both of which are fixed key encoders with Euclidean distance, the input of the fixed encoder is the laser radar distance data, and the input of the trainable encoder is the millimeter wave radar distance data; the contrast loss between the fixed encoder and the trainable encoder is calculated by the contrast loss function, the trainable encoder is updated according to the contrast loss and a navigation encoder is obtained; the local path planning network is obtained by the navigation encoder;

[0056] The contrast loss formula is as follows:

[0057] in, is the laser radar distance data, is the millimeter wave radar distance data, is the expected value; is a trainable encoder, is a fixed encoder; k and q are the outputs of the fixed encoder and the trainable encoder, respectively.

[0058] Step 5: Obtain local path planning information through the local path planning network.

[0059] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A local path planning method for a robot in a dense smoke and foggy environment, comprising a robot with autonomous navigation function in a real scene, a millimeter wave radar and a laser radar installed on the robot, characterized in that: The following steps are involved: Step 1: Use laser radar to collect 3D point cloud map data of the target area in the real scene and record it as laser radar distance data; Step 2: Simulate a virtual environment with thick smoke and fog in a physical simulation environment and train the robot to perform autonomous navigation in the virtual environment; Step 3: Obtain obstacle data perceived by the millimeter-wave radar in the virtual environment and record it as millimeter-wave radar distance data; Step 4: Train the comparative reinforcement learning network using the laser radar distance data and the millimeter wave radar distance data, and use the trained comparative reinforcement learning network as the local path planning network; The contrastive reinforcement learning network includes a fixed encoder and a trainable encoder, both of which are fixed key encoders with Euclidean distance, the input of the fixed encoder is the laser radar distance data, and the input of the trainable encoder is the millimeter wave radar distance data; the contrast loss between the fixed encoder and the trainable encoder is calculated by the contrast loss function, the trainable encoder is updated according to the contrast loss and a navigation encoder is obtained; the local path planning network is obtained by the navigation encoder; Step 5: Obtain local path planning information through the local path planning network.

2. The local path planning method for a robot in a dense smoke and foggy environment according to claim 1 is characterized in that: In step 2, the process of training the robot to perform autonomous navigation in a virtual environment is: The action performed by the robot at the current moment comes from the current navigation strategy. Each time the robot's spatial state is transferred, the robot will receive a reward, so that the robot can learn the optimal navigation strategy that maximizes the expected reward and use it as the navigation strategy for the next moment. The optimal navigation strategy π* is: p * =θarg maxE[∑γ t r t ] Among them, γ t is the discount factor at time t, E is the expectation, r t is the reward function; θ is the policy parameter.

3. The local path planning method for a robot in a dense smoke and foggy environment according to claim 2 is characterized in that: Reward function r t is the total reward score at time t, that is: r t =R t (goal)+R t (col)+R t (d s )+R t (time) Among them, R t (goal) is the reward score, R t (col) is the negative reward score, R t (d s ) is the distance gradient reward score, R t (time) is the time cost reward score.

4. The local path planning method for a robot in a dense smoke and foggy environment according to claim 2 is characterized in that: Use the gradient ascent method to update the policy parameters θ, namely: Among them, α is the learning rate, is the gradient of parameter θ, L CLIP (θ) is the objective function, that is: L CLIP (θ)=E t [min(r t (i)A t ,clip(r t (θ), 1-ε, 1+ε)A t )] r t (θ)=π new / p θ Among them, E t is the expectation at time t, ε is the cutoff constant and is set to 0.1, clip is the cutoff function, and π new is the navigation strategy at the current moment, π θ A is the navigation strategy for the next moment; t is the robot’s current action.

5. The local path planning method for a robot in a dense smoke and foggy environment according to claim 4 is characterized in that: The robot's current action A t for: A t =d t +(gl)d t+1 +…+(cl) T-t+1 d T+1 δ t =r t +γV(S t+1 )-V(S t ) Among them, λ is a positive number less than 1, V(S t ) is the state value and is obtained through the output of the Critic network; T is the corresponding time when the collected trajectory ends; the robot spatial state S t , as the robot moves A t And the state transition probability matrix P(S t+1 |S t , A t ), where the action A performed by the robot at time t t From the current navigation strategy, S t ∈ state space S, a t ∈ action space A, state space S is composed of lidar distance data x l , millimeter wave radar distance data x m , the robot's current linear velocity v and angular velocity ω, and the coordinates g of the target point relative to the robot form a multidimensional vector (x l , x m , v, ω, g), the action space A is composed of two elements obtained by sampling from the continuous action space, namely the linear velocity and the angular velocity, which form a two-dimensional vector (v, ω).

6. The local path planning method for a robot in a dense smoke and foggy environment according to claim 1, characterized in that: In step 4, the contrast loss formula is as follows: Among them, x l is the laser radar distance data, x m is the millimeter wave radar distance data, is the expected value; q m is a trainable encoder, q L is a fixed encoder; k and q are the outputs of the fixed encoder and the trainable encoder, respectively.

Images