A robot navigation and obstacle avoidance method with full range information recovery capability

Abstract

The present disclosure provides a robot navigation and obstacle avoidance method with full-range information recovery capability, including obtaining surrounding environment data, obtaining observation data of original radar and current radar, constructing a historical observation data queue, inserting the latest radar observation data and posture into the historical queue, fusing the historical observation data with the current observation data, and forming a new observation perception result of the environment within a 360° range around the robot; using the probability distribution output by the new perception result in the optimized and updated strategy function network to select the next obstacle avoidance action of the robot until the target point is reached; and being able to retain a certain degree of memory of obstacles that have been passed but will still affect its subsequent actions based on the current observation and in combination with the historical observation data within a certain range.

Description

Technical Field

[0001] The present disclosure relates to the technical field of robot obstacle avoidance, and in particular to a robot navigation obstacle avoidance method and system with full-range information recovery capability. Background Art

[0002] The statements in this section merely provide background information related to the present disclosure and do not necessarily constitute prior art.

[0003] Obstacle avoidance is one of the essential functions of robot navigation. Obstacle avoidance algorithms can be divided into two types: map-based and reactive. Map-based algorithms often require large amounts of computation due to the need to update obstacle maps in real time, and their effect on avoiding dynamic obstacles is relatively poor. Reactive algorithms directly analyze and calculate the information obtained from sensors to obtain robot control quantities, which significantly improves the real-time obstacle avoidance effect of dynamic obstacles and has strong adaptability to different environments. Existing reactive obstacle avoidance algorithms often only consider obstacles within the current perception range. If a dynamic obstacle leaves the perception range during movement, but it is still within a certain range around the robot, it will still affect the movement of the robot. This situation will cause a secondary collision of the robot, causing robot movement safety issues. Summary of the invention

[0004] In order to solve the above problems, the present disclosure proposes a robot navigation and obstacle avoidance method with full-range information recovery capability, which can retain a certain degree of memory of obstacles that have been passed but will still affect its subsequent actions based on current observations and combined with historical observation data within a certain range, thereby avoiding the situation where a robot has just avoided an obstacle but has a secondary collision with a historical obstacle due to avoiding an obstacle in another direction.

[0005] According to some embodiments, the present disclosure adopts the following technical solutions:

[0006] A robot navigation and obstacle avoidance method with full range information recovery capability, the training steps are:

[0007] Establish the robot coordinate system in the three-dimensional space of the simulation environment, and randomly generate the positions of obstacles, as well as the starting point and target point of the robot in a certain area;

[0008] Obtain surrounding environment data and establish an environment model based on state space, action space, reward function, state transition probability, observation space, and observation probability distribution;

[0009] Obtain the observation data of the original radar and the current radar, build a historical observation data queue, insert the latest radar observation data and posture into the historical queue, merge the historical observation data with the current observation data, and form a new observation perception result of the environment within 360° around the robot;

[0010] The new perception results are used to output the probability distribution in the optimized updated policy function network to select the robot's next obstacle avoidance action until it reaches the target point.

[0011] Furthermore, the established observation space includes the robot's perception results after the original data is completed, the robot's current speed, and the coordinate position of the target point in the robot's local coordinate system.

[0012] Compared with the prior art, the present invention has the following beneficial effects:

[0013] This paper uses the PPO reinforcement learning algorithm based on the Actor-Critic framework as the basic training algorithm to establish a robot obstacle avoidance navigation model, and makes improvements in the observation data acquisition and reward setting steps of this algorithm. Historical information is added to the observation data, and multiple frames of original radar data are processed and fused accordingly to obtain perception information with a certain memory function that can represent the distance information of obstacles within a 360° range around the robot, so that it has the ability to restore information in the full range, which also includes the time characteristics of the relevant distance information, and sends this perception information to the deep learning network to obtain the mapping relationship between the observed action. The robot's autonomous navigation and obstacle avoidance in an unknown environment is realized, and the obstacle avoidance performance of the traditional reactive obstacle avoidance algorithm for dynamic obstacles is improved; the situation of limited radar perception is improved to avoid the secondary collision problem caused by the obstacle leaving the perception area due to the movement of the robot, but it may still affect the subsequent movement of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The accompanying drawings constituting a part of the present disclosure are used to provide a further understanding of the present disclosure. The illustrative embodiments of the present disclosure and their descriptions are used to explain the present disclosure and do not constitute an improper limitation on the present disclosure.

[0015] Figure 1 The flowchart of the present invention is to fuse the current radar frame with the historical radar frame to obtain the perception result.

[0016] Figure 2 It is a structural diagram of the obstacle avoidance network model in the present invention.

[0017] Figure 3This is a comparison of the radar visualization effects of the current frame radar and the radar visualization effects of the fused historical frame perception results in the example of the present invention. The blue rectangle represents the robot. The right figure shows that the robot maintains the memory of historical observation data during the process of walking from position ① to position ②. DETAILED DESCRIPTION

[0018] The present disclosure is further described below in conjunction with the accompanying drawings and embodiments.

[0019] It should be noted that the following detailed descriptions are all illustrative and are intended to provide further explanation of the present disclosure. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present disclosure belongs.

[0020] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present disclosure. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0021] Example 1

[0022] The present disclosure discloses a robot navigation obstacle avoidance method with full range information recovery capability, comprising the following training steps:

[0023] Step 1: Establish the robot coordinate system in the three-dimensional space of the simulation environment, and randomly generate the positions of obstacles, as well as the starting point and target point of the robot in a certain area;

[0024] Step 2: Obtain surrounding environment data and establish an environment model based on state space, action space, reward function, state transition probability, observation space, and observation probability distribution;

[0025] Step 3: Obtain the observation data of the original radar and the current radar, build a historical observation data queue, insert the latest radar observation data and posture into the historical queue, fuse the historical observation data with the current observation data, and form a new observation perception result of the environment within 360° around the robot;

[0026] Step 4: Use the probability distribution output by the new perception results in the optimized updated policy function network to select the robot's next obstacle avoidance action until it reaches the target point.

[0027] Specifically, the environment model established in step 2 includes state space S, action space A, reward function R, state transition probability P, observation space Ω, and observation probability distribution O.

[0028] Furthermore, the established observation space includes the robot's perception results after the original data is completed, the robot's current speed, and the coordinate position of the target point in the robot's local coordinate system.

[0029] Step 3 includes the perception results after the original data is completed, specifically including the distance information of obstacles within the surrounding 360° range and the corresponding time information after fusing the historical observation data with the current observation data; the current speed of the robot includes the current linear speed and angular velocity of the differential robot; the coordinate position includes the vector composed of the coordinate values ​​of the target point in the local coordinate system of the robot.

[0030] Specifically, the established observation space Ω includes the perception results of the robot after 360° completion of the original data obtained by the radar Current speed of the robot The coordinate position of the target point in the robot's local coordinate system in, It includes the distance information of obstacles within the surrounding 360° range after fusing the historical radar frame with the current radar frame and the corresponding time information. Including the current linear velocity and angular velocity of the differential robot, A vector consisting of the (x, y) coordinate values ​​of the target point in the robot's local coordinate system.

[0031] Step 3 specifically includes a processing method for converting the raw radar data into a representation of the observation perception results within a 360° range around the robot, such as Figure 1 As shown:

[0032] (1) Convert the acquired raw radar data into a polar coordinate system and normalize it;

[0033] (2) establishing a queue for storing a certain number of frames of historical observation data and a queue for robot postures corresponding to the historical observation data, wherein the postures include the coordinates of the robot in the Cartesian coordinate system in the world coordinate system and the yaw angle of the robot;

[0034] (3) Update the historical observation data queue according to certain judgment conditions;

[0035] Specifically, the determination conditions in processing method (3) are:

[0036] Each time, the posture at the end of the historical posture queue is compared with the current posture, and the threshold of the allowable range of position difference and the threshold of the yaw angle are set. If the distance difference between the posture at the last update and the current posture is greater than the set threshold or the yaw angle difference is greater than the set threshold, the current observation data and the current posture are inserted at the end of the historical observation and historical posture queues respectively, and the data at the head of the queue is deleted.

[0037] (4) Based on the previously obtained historical observation queue and the robot's latest observation data, they are integrated into a new perception result that can represent the environment within a 360° range around the robot. The 360° perception result includes the current observation data and the "recall observation data" with a certain degree of uncertainty memorized based on the historical observation data; the "recall observation data" is the observation data with a certain degree of uncertainty memorized based on the historical observation data. And according to the time difference between the data in the historical queue and the current moment, a one-dimensional time feature is assigned to each of them. This feature represents the robot's confidence in the observation information.

[0038] The processing method (4) for the historical observation queue and the robot's latest observation data includes the following steps:

[0039] The observation data in the historical observation queue are uniformly converted to the current coordinate system, that is, the historical observation data are first converted from the polar coordinate system to the Cartesian coordinate system of each time, and then the corresponding points are converted to the Cartesian coordinate system of the current time, and finally a step of conversion from the Cartesian coordinate system to the polar coordinate system is performed;

[0040] Connect the current observation data with the historical observation data into a complete set of observations; divide the 360° range into N parts, and use the polar angle of the observation data to calculate the corresponding id value within the 360° range;

[0041] After filtering the data, a one-dimensional feature is assigned according to the time difference between the observed data and the current moment; then the generated 360° radar data is combined with its corresponding time feature and returned.

[0042] The specific implementation of step 4 above is to convert the raw radar data into the observation perception results representing the 360° range around the robot. In the processing method, the incoming raw radar data is converted into a polar coordinate system and normalized. The observation data described below are all the results after polar coordinate conversion.

[0043] Establish a queue to store a certain number of frames of historical observation data and a queue of robot postures corresponding to the data. The posture includes the (x, y) coordinates of the robot in the Cartesian coordinate system in the world coordinate system and the yaw angle yaw of the robot.

[0044] Update the historical observation data queue based on certain judgment conditions.

[0045] The judgment condition is: each time the posture at the end of the historical posture queue (i.e., inserted in the last update) is compared with the current posture, and a certain threshold value dis is set. update and theta update, if the distance difference between the last updated pose and the current pose is greater than dis update Or the yaw angle difference is greater than theta update , then the current observation data and current posture are inserted into the tail of the historical observation and historical posture queues respectively, and the older data at the head of the queue is deleted.

[0046] Based on the historical observation queues obtained previously and the latest observation data of the robot, they are integrated into a new perception result that can represent the environment within the 360° range around the robot. The 360° perception result includes the absolutely correct current observation data and the "recall observation data" with a certain degree of uncertainty memorized based on the historical observation data; and according to the time difference between the data in the historical queue and the current moment, a one-dimensional time feature t_feature is assigned to each of them. This feature also represents the robot's confidence in the observation information. The specific processing process is:

[0047] The observation data in the historical observation queue are uniformly converted to the current coordinate system. Since the historical observation data are all in the polar coordinate system, and rotation and translation conversion cannot be directly performed in the polar coordinate system, the historical observation data are first converted from the polar coordinate system to the Cartesian coordinate system of each moment, and then the corresponding points are converted to the Cartesian coordinate system of the current moment, and finally a step of conversion from the Cartesian coordinate system to the polar coordinate system is performed.

[0048] Connect the current observation data with the historical observation data into a complete set of observations, and according to the following formula,

[0049] id=θ / (2π / N)

[0050] Where id represents the position of a certain observation data within the 360° range, and N represents the number of parts into which the 360° range is divided.

[0051] The 360° range is divided into N parts, and the polar angle of the observed data is used to calculate the corresponding ID value within the 360° range, that is, to determine which part of the N parts of the 360° range the radar point falls in.

[0052] Since the historical observation data and the current observation data partially overlap, that is, the observation data from different times may be assigned the same ID, the data is further screened: ① The current observation data is completely certain, so all of them are retained; ② If the historical observation data has the same corresponding ID, the data with a closer distance is retained based on the distance between the radar point and the robot. ③ Finally, the filtered observation data representing the obstacle situation within the 360° range around the robot is obtained.

[0053] According to the time difference between the historical observation data and the current moment, a one-dimensional feature is assigned to it. Specifically, ① the feature value increases from 0 to 1.0 from far to near the current moment; ② the feature value of the unknown area (that is, the area that has never been detected by historical data) is -1.

[0054] The generated 360° radar data is combined with its corresponding time features and returned.

[0055] In step 2, the established spatial motion includes a series of attainable speeds in a continuous space, and the speed of the differential robot includes linear speed and angular speed, that is,

[0056] a t =[v t ,ω t ].

[0057] The robot selects actions based on observations and strategies. After taking actions, it interacts with the environment to obtain new observations and calculates the rewards. The observations, actions, rewards, and the robot's terminal state are stored for sample sampling training.

[0058] A reward function is established for whether the robot reaches the target point. The established reward function is designed as follows:

[0059]

[0060] where r t Represents the reward function received at time t, given by and composition.

[0061] Specifically, ① the robot will be award:

[0062]

[0063] Among them, r arrival is an adjustable parameter, representing the reward for the robot when it reaches the target point; ω g It is also an adjustable parameter used to give rewards to the robot for its unknown approach to the target point; p t-1 and p t They represent the positions of the robot at the previous moment and the current moment respectively; g represents the position of the target point.

[0064] ②When the robot collides with an obstacle in the environment, punish:

[0065]

[0066] Among them, r collisionis an adjustable parameter, representing the penalty value for the robot when it collides. t Indicates the current position of the robot, B k Represents the position of obstacle k. When the distance between the robot and the obstacle is less than R, it is judged to be in collision state.

[0067] ③In addition, add a penalty item for active obstacle avoidance:

[0068] First calculate dis ca All radar points x in range i The cost of active obstacle avoidance

[0069]

[0070] Then map each radar point to an angle α between -π / 2 and π / 2 i middle:

[0071] α i =π*i / l data -π / 2

[0072]

[0073]

[0074] l data That is the length of the radar data. Each radar data x i Represents the distance of the point returned by the sensor. coeff is an adjustable parameter; cost sum This is the total cost of a radar frame, and the final penalty for active obstacle avoidance. It's cost sum The result after taking the negative value.

[0075] ④ To encourage the robot to move smoothly, a small penalty Used to penalize large rotation speeds:

[0076]

[0077] Setting arrival =17,ω g =2.7, r collision =-17,dis ca =0.2,coeff=0.001,w ω =-0.15. data That is the length of the radar data, and each radar data xi represents the distance of the point returned by the sensor.

[0078] As mentioned in step 4, the probability distribution output by the new perception result in the optimized updated strategy function network is used to select the robot's next obstacle avoidance action until it reaches the target point.

[0079] Specifically, a robot navigation and obstacle avoidance neural network model is established, such as Figure 2 As shown, the fused radar data The shape is 2x1080. First, a one-dimensional convolution layer is input, whose output is 32 channels and uses a convolution kernel of size 5*5. Then a one-dimensional convolution layer with an output of 32 channels and a convolution kernel size of 3*3 is input. Then a fully connected layer with an output layer size of 256 is input. Then the output of the previous step is combined with the other two inputs: the current speed of the robot The coordinate position of the target point in the robot's local coordinate system Connect them together, and then input them into a fully connected layer with an output layer size of 128; finally, input the output of the previous step into two fully connected layers with an output layer size of 128 to obtain a value of size 2 Then get the corresponding From an obedient The final action value a is sampled from the positive distribution of t .

[0080] The PPO reinforcement learning algorithm based on the Actor-Critic framework is used. According to the observed data, the actions generated by the Actor network are executed at each step to interact with the environment, and the rewards and new states of the environment are obtained. The interaction data of a certain number of times are saved, and then the Critic network uses this data to evaluate the action to obtain the action value, and updates the parameters of the Actor and Critic networks based on the obtained state, reward and action value.

[0081] The action is selected according to the probability distribution of the output of the optimized and updated policy function network. The original radar data of multiple frames is processed and fused accordingly to obtain the perception information with a certain memory function, which can represent the distance information of obstacles within 360° around the robot, so that it has the ability to recover the full range of information, including the time characteristics of the relevant distance information, and send this perception information to the deep learning network to obtain the mapping relationship between the observed action.

[0082] In the specific implementation, the radar visualization effect of the current frame is compared with the radar visualization effect of the fused historical frame perception result, such as Figure 3 As shown, the process is:

[0083] Move the car from position ① to position ②. Now it is in position ②. Figure 3The left picture in the middle shows the visualization effect of the original 180° lidar data. Except for the car model, the other black points are radar data points; the right picture shows the visualization effect after the fusion of current data and historical frame data. The data within the 180° range in front of the car model is the current frame data, and the rest is the historical frame data memorized in the process from position ① to position ②. It can be seen that at this time, the robot not only has the currently observable data, but also the previously retained historical data for its decision-making, which improves the safety of decision-making.

[0084] Those skilled in the art will appreciate that the embodiments of the present disclosure may be provided as methods, systems, or computer program products. Therefore, the present disclosure may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present disclosure may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0085] The present disclosure is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present disclosure. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0086] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0087] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0088] The above description is only a preferred embodiment of the present disclosure and is not intended to limit the present disclosure. For those skilled in the art, the present disclosure may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present disclosure shall be included in the protection scope of the present disclosure.

[0089] Although the above describes the specific implementation methods of the present disclosure in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present disclosure. Technical personnel in the relevant field should understand that on the basis of the technical solution of the present disclosure, various modifications or variations that can be made by those skilled in the art without creative work are still within the scope of protection of the present disclosure.

Claims

1. A robot navigation and obstacle avoidance method with full range information recovery capability, characterized in that: The training steps include: Establish the robot coordinate system in the three-dimensional space of the simulation environment, and randomly generate the positions of obstacles, as well as the starting point and target point of the robot in the set area; Obtain surrounding environment data and establish an environment model based on state space, action space, reward function, state transition probability, observation space, and observation probability distribution; Obtain the observation data of the original radar and the current radar, build a historical observation data queue, insert the latest radar observation data and posture into the historical queue, merge the historical observation data with the current observation data, and form a new observation perception result of the environment within 360° around the robot; The processing method for converting the original radar data into a new observation perception result representing the environment within the 360° range around the robot is as follows: (1) Convert the acquired raw radar data into a polar coordinate system and normalize it; (2) establishing a queue for storing a set number of historical observation data and a queue for storing robot postures corresponding to the historical observation data, wherein the postures include the coordinates of the robot in the Cartesian coordinate system in the world coordinate system and the yaw angle of the robot; (3) Update the historical observation data queue according to certain judgment conditions; (4) Based on the previously obtained historical observation queue and the robot's latest observation data, they are integrated into a new perception result representing the environment within a 360° range around the robot. The 360° perception result includes the current observation data and the "recall observation data" with a certain degree of uncertainty memorized based on the historical observation data; and based on the time difference between the data in the historical queue and the current moment, a one-dimensional time feature is assigned to each of them. This feature represents the robot's confidence in the observation information; Among them, the certain judgment conditions in processing method (3) are: Each time, the posture at the end of the historical posture queue is compared with the current posture, and the threshold of the position difference allowable range and the yaw angle threshold are set. If the distance difference between the posture at the last update and the current posture is greater than the set threshold or the yaw angle difference is greater than the set threshold, the current observation data and the current posture are respectively inserted into the end of the historical observation and historical posture queues, and the data at the head of the queue is deleted; The new perception results are used to output the probability distribution in the optimized updated policy function network to select the robot's next obstacle avoidance action until it reaches the target point.

2. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 1, characterized in that: The established observation space includes the robot's perception results after the original data is completed, the robot's current speed, and the coordinate position of the target point in the robot's local coordinate system.

3. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 2, characterized in that: The perception result after completing the original data includes the distance information of obstacles within the surrounding 360° range after fusing the historical observation data with the current observation data and the corresponding time information; the current speed of the robot includes the current linear speed and angular velocity of the differential robot; the coordinate position includes the vector composed of the coordinate values ​​of the target point in the local coordinate system of the robot.

4. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 1, characterized in that: The processing method (4) for the historical observation queue and the robot's latest observation data includes the following steps: The observation data in the historical observation queue are uniformly converted to the current coordinate system, that is, the historical observation data are first converted from the polar coordinate system to the Cartesian coordinate system of each time, and then the corresponding points are converted to the Cartesian coordinate system of the current time, and finally a step of conversion from the Cartesian coordinate system to the polar coordinate system is performed; Connect the current observation data with the historical observation data into a complete set of observations; divide the 360° range into N parts, and use the polar angle of the observation data to calculate the corresponding id value within the 360° range; After filtering the data, a one-dimensional feature is assigned according to the time difference between the observed data and the current moment; then the generated 360° radar data is combined with its corresponding time feature and returned.

5. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 1, characterized in that: The established action space includes a series of achievable speeds in the continuous space. The speed of the differential robot includes linear speed and angular speed, that is, 。 6. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 1, characterized in that: A reward function is established for whether the robot reaches the target point. The established reward function is designed as follows: in Represents the reward function received at time t, given by , and composition.

7. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 6, characterized in that: Specifically, the robot will be award: ； When the robot collides with an obstacle in the environment, punish: in, is an adjustable parameter, representing the penalty value for the robot when it collides. Indicates the current position of the robot. Represents the position of obstacle k. When the distance between the robot and the obstacle is less than R, it is judged to be in collision state.

8. A robot navigation and obstacle avoidance method with full range information recovery capability as claimed in claim 7, characterized in that: Add a penalty term for active obstacle avoidance: First calculate All radar points within range The cost of active obstacle avoidance : Each radar point is then mapped to An angle middle: ； That is, the length of each frame of radar data. Represents the distance of the point returned by the sensor; coeff is an adjustable parameter; This is the total cost of a radar frame, and the final penalty for active obstacle avoidance. that is The result after taking the negative value.

Images