A 3D point cloud-based D3-TD3 robot path planning method

Abstract

The application relates to the technical field of path planning, in particular to a D3-TD3 robot path planning method based on a 3D point cloud, which comprises the following steps: initializing parameters of a Critic network and an Actor network, and designing a state space and an action space of a D3-TD3 algorithm; designing a D3-TD3 network structure and a reward function; creating a three-dimensional point cloud space model, and simplifying point cloud data. The application introduces the creation of the three-dimensional point cloud space model, solves the problems that image processing is huge based on visual navigation and is greatly limited by light conditions, and navigation cannot be completed in a very dark environment, and introduces a deep dense structure on the basis of the TD3 algorithm, the structure retains important information of cross-layer input, and divides the network into a value function and an advantage function, so that faster convergence is realized when solving complex tasks, the learning rate is faster, and the collision frequency is lower.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a path planning method for a D3-TD3 robot based on 3D point clouds. Background Technology

[0002] With the surge in interest in artificial intelligence, autonomous robot navigation has garnered widespread attention and research. Path planning is one of the key technologies for achieving autonomous robot navigation, aiming to plan a path that satisfies the robot's own constraints based on task objectives. In recent decades, the rapid development of artificial intelligence, particularly reinforcement learning, has provided new possibilities for the path planning problem. Reinforcement learning does not require prior knowledge of complex environmental models, which helps achieve high-level artificial intelligence and has become an attractive method in fields such as autonomous driving, video games, and robot control. Research on deep reinforcement learning in the field of artificial intelligence has been a hot topic in recent years, and certain progress has been made in this area. Summary of the Invention

[0003] To address the shortcomings of existing algorithms, this invention introduces the creation of a 3D point cloud spatial model, solving the problems of massive image processing requirements and significant limitations imposed by lighting conditions on vision-based navigation, which makes navigation impossible in low-light environments. Furthermore, it introduces a deep, dense structure based on the TD3 algorithm. This structure preserves important information from cross-layer inputs and divides the network into value functions and dominance functions, thus achieving faster convergence when solving complex tasks, resulting in a faster learning rate and fewer collisions.

[0004] The technical solution adopted in this invention is: a D3-TD3 robot path planning method based on 3D point clouds, comprising the following steps:

[0005] Step 1: Initialize the parameters of the Critic network and Actor network, and design the state space and action space of the D3-TD3 algorithm (Deep Dense Dueling TD3 algorithm);

[0006] Furthermore, specifically, this includes: selecting action a based on the robot's current state s and adding noise, executing action a to obtain a reward r and the next state s′, and storing the quadruple (s,a,r,s′) into the experience replay pool, and sampling m samples (s) from the experience pool. j ,a j ,r j ,s j+1 (j = 1, 2, ..., m), calculate the expected reward of the action through the Critic target network and update the Critic network parameters; every d steps, update the Actor network parameters through the deterministic policy gradient. and target network parameters θ′ i ,

[0007] The state space includes: LiDAR data, the current angular velocity and linear velocity of the mobile robot, the angular velocity and linear velocity of the mobile robot at the previous moment, and the direction and distance of the target point.

[0008] Furthermore, the motion space includes the angular velocity of the mobile robot's rotation around the Z-axis and its linear velocity along the X-axis in the robot's body coordinate system.

[0009] Step 2: Design the D3-TD3 network structure and total reward function;

[0010] Furthermore, specifically, the D3-TD3 network structure is divided into a value function and a dominance function. Specifically, the information obtained from the LiDAR is used as the input state s of the Actor network, which consists of four fully connected layers, with the state s added to the last three fully connected layers. The input to the Critic network is a pair of states s and actions a, and the Critic network also consists of four fully connected layers, with the state s and action a added to the first three fully connected layers. After four fully connected layers, the network is decomposed into a dominance function and a value function, and the Q-value is obtained using the following formula.

[0011] Q(s,a,ω,α,β)=V(s,ω,α)+A(s,a,ω,β) (1)

[0012] The first part is related only to the state s and is called the value function, denoted as V(s,ω,α); the other part is related to the state s and the action a and is called the dominance function, denoted as A(s,a,ω,β), where ω is the common parameter of V and A, α and β are the parameters of V and A respectively, s represents the state, and a represents the action.

[0013] Furthermore, the reward function of the D3-TD3 algorithm specifically includes: the reward function of the D3-TD3 algorithm is designed based on whether the mobile robot reaches the target point, the change in the distance between the mobile robot and the target point, and whether it collides with obstacles.

[0014] Furthermore, a reward function is designed based on whether the mobile robot reaches the target point:

[0015]

[0016] Where, x t d1 represents the distance between the mobile robot and the target point at time t, and d1 is a set threshold. When the distance between the mobile robot and the target point is less than d1, it means that the target point has been reached.

[0017] The reward function is designed based on whether the mobile robot collides with an obstacle:

[0018]

[0019] Where, d i This represents the distance to the nearest obstacle detected by the LiDAR, where d2 is a set constant threshold. When the distance between the mobile robot and the obstacle is less than d2, it indicates that a collision has occurred.

[0020] To guide the mobile robot toward the target point, immediate rewards will be given based on its current linear velocity and angular velocity.

[0021] R distance =v-|w| (4)

[0022] Where v represents the robot's current linear velocity and w represents the robot's current angular velocity.

[0023] The overall reward function is:

[0024] Reward = R arrival +R collision +R distance (5)

[0025] Step 3: Create a 3D point cloud spatial model and simplify the point cloud data;

[0026] Furthermore, the specific steps include: First, converting laser scanning data into 3D point cloud data to collect point cloud data during robot movement for environmental modeling; second, transforming the coordinate system of the point cloud data, mapping it from the local coordinate system of the LiDAR sensor to the global coordinate system; third, using statistical filtering to remove outliers to improve the accuracy of the 3D point cloud data scanned by the LiDAR; and finally, simplifying the acquired point cloud data.

[0027] Furthermore, coordinate system transformation of point cloud data includes:

[0028] First, the point cloud data is transformed using a matrix for calculation:

[0029]

[0030] Where, p l =[x l y l z l ] T These are the coordinates of the point cloud in the local coordinate system of the lidar; p w =[x w y w z w ] TR is the coordinates of the point cloud in the global coordinate system; R is a 3×3 rotation matrix, representing the rotational transformation between the local coordinate system and the world coordinate system; t is a 3×1 translation vector, representing the translational transformation between the local coordinate system and the world coordinate system.

[0031] Secondly, a spatial topological relationship is established in the point cloud P using a KD-tree to find the distance from any point P. i The k nearest points form a set P. ij ={P i1 ,P i2 ,P i3 ,…,P ik Let Nbp(i) be the local average distance between the search point and all points in its neighborhood.

[0032]

[0033] Next, calculate the global average distance μ. N and mean square error σ N :

[0034]

[0035] Finally, determine the sampling point P. i The average distance of the k-neighborhood Whether the statistical filtering criteria are met is as follows:

[0036]

[0037] if or If a point is considered an outlier, it will be removed. By removing outliers from the point cloud data, the accuracy of the 3D point cloud data scanned by the LiDAR can be improved.

[0038] Furthermore, the acquired point cloud data is streamlined, including:

[0039] In the topological set P of the point cloud ij ={P i1 ,P i2 ,P i3 ,…,P ik In the process, the centroid C of all neighboring points is first calculated. i :

[0040]

[0041] Secondly, define the distance Z between the nearest neighbor point to the search point and the search point. i (P i As shown in formula (11):

[0042]

[0043] ||C i -p i ||>λZ i (P i (12)

[0044] Finally, Z i Multiply by a fixed parameter to obtain the threshold for judging edge points, as shown in formula (12).

[0045] The beneficial effects of this invention are:

[0046] 1. A 3D point cloud spatial model creation method was introduced, solving the problem that vision-based navigation requires a large amount of image processing and is greatly limited by lighting conditions, making navigation impossible in low-light environments. Furthermore, a deep, dense structure was introduced based on the TD3 algorithm. This structure retains important information from cross-layer inputs and divides the network into value functions and dominance functions, thus achieving faster convergence when solving complex tasks, resulting in a faster learning rate and fewer collisions.

[0047] 2. Compared with TD3, Dueling TD3, and Deep Dense TD3, this invention has higher learning efficiency and fewer collisions in dynamic and unknown environments. Attached Figure Description

[0048] Figure 1 This is a diagram of the D3-TD3 network structure of the present invention;

[0049] Figure 2 These are the static environment diagram and 3D point cloud diagram of this invention;

[0050] Figure 3 This is a comparison chart of the reward values ​​of four algorithms under static conditions according to the present invention;

[0051] Figure 4 This is a comparison chart of the collision rates of the four algorithms under static conditions according to the present invention.

[0052] Figure 5 These are the dynamic and static environment diagrams and 3D point cloud diagrams of this invention;

[0053] Figure 6 This is a comparison chart of the reward values ​​of four algorithms under dynamic and static environments according to the present invention;

[0054] Figure 7 This is a comparison chart of the collision rates of four algorithms under dynamic and static environments according to the present invention. Detailed Implementation

[0055] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0056] like Figure 1 As shown, a D3-TD3 robot path planning method based on 3D point clouds includes the following steps:

[0057] Step 1: Initialize the parameters of the Critic and Actor networks. Select an action based on the robot's current state s and add noise. Execute action a to obtain the reward r and the next state s′, and store the quadruple (s, a, r, s′) into the experience replay pool. Sample m samples (sm, sm) from the experience pool. j ,a j ,r j ,s j+1 (j = 1, 2, ..., m), calculate the expected reward of the action through the Critic target network and update the Critic network parameters; every d steps, update the Actor network parameters through the deterministic policy gradient. and target network parameters θ′ i ,

[0058] A path planning model for mobile robots based on deep reinforcement learning is established, and the path planning problem of mobile robots is described as a Markov decision process.

[0059] Design the state space and action space of the D3-TD3 algorithm;

[0060] The state space of the D3-TD3 algorithm includes: 200-dimensional LiDAR data, the current angular velocity and linear velocity of the mobile robot, the angular velocity and linear velocity of the mobile robot at the previous moment, and the direction and distance of the target point;

[0061] The action space of the D3-TD3 algorithm includes: the angular velocity of the mobile robot's rotation around the Z-axis and the linear velocity along the X-axis in the robot's body coordinate system;

[0062] Step 2: Design and optimization of the network structure of the TD3 algorithm (i.e., design the D3-TD3 network structure) and design the total reward function;

[0063] Furthermore, the TD3 network structure employs dense connections to retain important cross-layer input information, and the network is divided into a value function and a dominance function. Specifically, the information obtained from the LiDAR is used as the input state s of the Actor network, which consists of four fully connected (FC) layers. The last three fully connected layers incorporate the state s to retain important cross-layer input information. The input to the Critic network is a pair of states s and actions a. This network also consists of four fully connected (FC) layers, with the first three fully connected layers incorporating the state s and action a to retain important cross-layer input information. After four fully connected layers, the network is decomposed into a dominance function and a value function. The Q-value is obtained using the following formula:

[0064] Q(s,a,ω,α,β)=V(s,ω,α)+A(s,a,ω,β) (1)

[0065] The first part relates only to the state s and is called the value function, denoted as V(s,ω,α); the other part relates to both the state s and the action a and is called the dominance function, denoted as A(s,a,ω,β), where ω is a common parameter of V and A, α and β are parameters of V and A respectively, s represents the state, and a represents the action.

[0066] Furthermore, the reward function of the D3-TD3 algorithm is designed based on the artificial potential field method as follows:

[0067] The reward function of the D3-TD3 algorithm is designed based on whether the mobile robot reaches the target point, the change in the distance between the mobile robot and the target point, and whether a collision occurs with an obstacle, as follows:

[0068] The reward function is designed based on whether the mobile robot reaches the target point:

[0069]

[0070] Where, x t d1 represents the distance between the mobile robot and the target point at time t, and d1 is a set threshold. When the distance between the mobile robot and the target point is less than d1, it means that the target point has been reached.

[0071] The reward function is designed based on whether the mobile robot collides with an obstacle:

[0072]

[0073] Where, d i This represents the distance to the nearest obstacle detected by the LiDAR, where d2 is a set constant threshold. When the distance between the mobile robot and the obstacle is less than d2, it indicates that a collision has occurred.

[0074] The change in distance between the mobile robot and the target point guides the robot toward the target point, and provides immediate rewards based on its current linear and angular velocities.

[0075] R distance =v-|w| (4)

[0076] Where v represents the robot's current linear velocity and w represents the robot's current angular velocity.

[0077] The final reward function is:

[0078] Reward = R arrival +R collision +R distance (5)

[0079] like Figure 2 , 5 The three steps are: static environment map and 3D point cloud map, dynamic and static environment map and 3D point cloud map. Step 3: Establish a spatial model starting from the 3D point cloud. Specifically, first, convert the laser scanning data into 3D point cloud data and collect the point cloud data during robot movement for environment modeling. Then, the point cloud data needs to be transformed into a coordinate system, mapping the point cloud data from the local coordinate system of the sensor to the global coordinate system. Next, a statistical filtering method is used to remove outliers to improve the accuracy of the 3D point cloud data scanned by the LiDAR. Finally, since the laser sensor has a high acquisition frequency and resolution, the point cloud density obtained by the laser sensor is relatively large, so the acquired point cloud data is simplified.

[0080] Furthermore, when acquiring data, LiDAR perceives the environment in its local coordinates, thus performing calculations on the point cloud data through matrix transformation:

[0081]

[0082] Where, p l =[x l y l z l ] T These are the coordinates of the point cloud in the local coordinate system of the lidar; p w =[x w y w z w ] T R is the coordinates of the point cloud in the global coordinate system; R is a 3×3 rotation matrix, representing the rotational transformation between the local and world coordinate systems; t is a 3×1 translation vector, representing the translational transformation between the local and world coordinate systems.

[0083] Furthermore, since the laser sensor outputs scanning data one line at a time, the topological relationships between data points are incomplete. Therefore, we first use a KD-tree to establish spatial topological relationships in the point cloud P, finding the distance from any point P. i Find the k nearest points and form a set P. ij ={P i1 ,P i2 ,P i3 ,…,P ik Let Nbp(i) be the local average distance between the search point and all points in its neighborhood.

[0084]

[0085] Then calculate the distance μ between all search points. N and mean square error σ N :

[0086]

[0087] In the formula, N represents the number of search points. This indicates the average distance.

[0088] Finally, determine the sampling point P. i The average distance of the k-neighborhood Whether the statistical filtering criteria are met is as follows:

[0089]

[0090] In the formula, α represents the proportionality coefficient, and μ N σ represents the average value. N This represents the mean squared error.

[0091] if or If a point is considered an outlier, it will be removed. By removing outliers from the point cloud data, the accuracy of the 3D point cloud data scanned by the LiDAR can be improved.

[0092] Because laser sensors have high acquisition frequency and resolution, the point cloud density they obtain is relatively high. Therefore, it is necessary to simplify the acquired point cloud data. For robot path planning, obstacle edge information is very important. Therefore, the point cloud data is downsampled while retaining obstacle edge information, thereby simplifying the point cloud data.

[0093] Furthermore, in the topological set P of the point cloud ij ={P i1 ,P i2 ,P i3 ,…,Pik In the process, the centroid C of all neighboring points is first calculated. i :

[0094]

[0095] In the formula, k represents the number of elements in the point cloud topology set, and P ij A topological set representing a point cloud.

[0096] Since point cloud density affects the distance between sampling points and centroids, an adaptive parameter is used as part of the evaluation metric to adapt to changes in point cloud density. The distance Z between the nearest neighbor of the search point and the search point is defined as Z. i (P i As shown in formula (11):

[0097]

[0098] ||C i -p i ||>λZ i (P i (12)

[0099] In the formula, C i Z represents the centroid of all neighboring points. i (P i () represents the distance between the nearest neighbor point and the search point;

[0100] Z i Multiply by a fixed parameter λ to obtain the threshold for judging edge points, as shown in formula (12); if the distance between the search point and the centroid of the neighborhood is greater than this threshold, the point is determined to be at the edge of the point cloud, and the edge point information is retained when downsampling the point cloud data.

[0101] By interacting with the environment, training data is acquired, and the sampled data is used to train the mobile robot to complete collision-free path planning. The specific steps include the following:

[0102] Step 3.1: Establish a spatial model, allow the mobile robot to interact with the environment, and store the training data in the experience pool;

[0103] Step 3.2: Sample training data from the experience pool to train the actor network and the critic network;

[0104] Step 3.3: Input the current state information of the mobile robot into the trained neural network and output the next action command of the mobile robot.

[0105] like Figure 3 , 4Figures 6 and 7 show the comparison of reward values ​​and collision rates of the present invention with TD3, Dueling TD3, and DeepDense TD3 algorithms in static and dynamic / static environments, respectively. The figures show that the present invention has the highest reward value, the highest surface learning efficiency, and the fewest collisions in both static and dynamic / static environments.

[0106] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. A D3-TD3 robot path planning method based on a 3D point cloud, characterized in that, The method comprises the following steps: Step one, initializing the parameters of Critic network and Actor network, designing the state space and action space of D3-TD3 algorithm; Step one specifically comprises: According to the current state of the robot Selecting an action And adding noise, performing the action Getting a reward And the next state And storing the quadruple Sampling m Samples from the experience pool Computing the expected return of the action through the Critic target network and updating the Critic network parameters; every d steps, updating the Actor network parameters through the deterministic policy gradient And the target network parameters ; Step two, designing D3-TD3 network structure and total reward function; Step two specifically comprises: The D3-TD3 network structure is divided into value function and advantage function, specifically: the information obtained by laser radar is taken as the input state of the Actor network The Actor network is composed of four fully connected layers, and the state is added in the last three fully connected layers The input in the Critic network is a pair of state and action The Critic network is composed of four fully connected layers, and the state and action are added in the first three fully connected layers; after four fully connected layers, the network is divided into advantage function and value function, which is obtained by the following formula Q Value: (1) where the first part, which is only related to the state , is called the value function and denoted by ; the other part, which is related to the state and action , is called the advantage function and denoted by , where is a common parameter for V and A, and are parameters for V and A, respectively, denotes the state, denotes the action; The reward function of whether the mobile robot reaches the target point is: （2） wherein, represents the distance of the mobile robot from the target point at time t, is a set threshold value, when the mobile robot is less than represents reaching the target point; The reward function of collision with obstacles is: （3） wherein, is the distance at which the laser radar detects the nearest obstacle, is a constant threshold set; The reward function of the distance change of the mobile robot and the target point is: （4） wherein, represents the current linear velocity of the robot, represents the current angular velocity of the robot; The total reward function is: （5） Step three, three-dimensional point cloud space model creation, and point cloud data simplification.

2. The 3D point cloud based D3-TD3 robot path planning method according to claim 1, wherein, The action space comprises: the angular velocity of the mobile robot around the Z-axis in the body coordinate system and the linear velocity along the X-axis.

3. The 3D point cloud based D3-TD3 robot path planning method according to claim 1, wherein, Step three specifically comprises: Firstly, the laser scanning data is converted into three-dimensional point cloud data, and the point cloud data collected when the robot moves is used for environment modeling; secondly, the point cloud data is converted in the coordinate system, and the point cloud data is mapped from the local coordinate system of the laser radar sensor to the global coordinate system; thirdly, the statistical filtering method is used to remove outliers to remove outliers of the point cloud data; and finally, the obtained point cloud data is simplified.

4. The 3D point cloud based D3-TD3 robot path planning method according to claim 3, wherein, The point cloud data is converted in the coordinate system, and the point cloud data is mapped from the local coordinate system of the laser radar sensor to the global coordinate system; secondly, the statistical filtering method is used to remove outliers to remove outliers of the point cloud data specifically comprises: Firstly, the point cloud data is calculated through matrix transformation: wherein, is the coordinates of the point cloud in the local coordinate system of the lidar; is the coordinates of the point cloud in the global coordinate system; R is a 3x3 rotation matrix representing the rotational change of the transformation from the local coordinate system to the world coordinate system; t is a 3x1 translation vector representing the translational change of the transformation from the local coordinate system to the world coordinate system; Secondly, a spatial topological relationship is established in the point cloud P using a KD tree to find the distance to any point. The recent k A set of points. Calculate the local average distance between the search point and all points in its neighborhood. : Thirdly, the global distance average is calculated and the mean square error : Finally, it is judged whether the average distance of the neighborhood of the sampling point satisfies the judgment criterion of statistical filtering: k ​​ If or , the point is considered an outlier and is rejected.

5. The 3D point cloud based D3-TD3 robot path planning method according to claim 4, wherein, The obtained point cloud data is simplified specifically comprises: In a topological set of point clouds First, the centroid of all neighboring points is calculated : Secondly, the distance between the nearest neighbor point of the search point and the search point is defined as As shown in equation (11): Finally, the fixed parameter is multiplied as a threshold value for judging the edge point as shown in Equation (12).

Images