Hybrid terrain trajectory planning system and advancing control system

Abstract

The invention provides a mixed terrain trajectory planning system, which is used for planning an advancing path for an advancing target in a mixed terrain environment, and comprises a sensing processing module, a target trajectory planning module, a target trajectory planning module, a target trajectory planning module and a target trajectory planning module, wherein the pre-trained semantic segmentation network is used for carrying out semantic feature extraction on RGB topographic images which are collected by a moving target and are continuous at multiple moments to obtain a multi-label topographic feature map at each moment; the encoder is used for encoding the multi-label topographic feature map at each moment to obtain a mixed topographic semantic feature map at each moment; and the trajectory planning module is used for processing the advancing state and the terrain depth map of the advancing target, which are acquired by the advancing target and are continuous at multiple moments, and the mixed terrain semantic feature map, which is transmitted by the perception processing module and is continuous at multiple moments, corresponding to the advancing state and the terrain depth map based on a strategy network configured on the trajectory planning module so as to obtain a target planning trajectory.

Description

Technical Field

[0001] The present invention relates to the field of motion planning, in particular to trajectory planning technology in the field of motion planning, and more particularly to a mixed terrain trajectory planning system and a travel control system. Background Art

[0002] Trajectory planning is a crucial part of navigation tasks in the wild environment. It provides efficient and safe execution paths for tasks such as forest monitoring, mining, plateau transportation, search and rescue, etc. Given the particularity of the wild environment, trajectory planning needs to consider not only the geometric characteristics of the terrain, but also rich semantic information. At the same time, since the parameter space for trajectory planning in the wild environment is no longer the SE(2) space similar to the urban space, but the SE(3) space with high changes, trajectory planning in the wild environment needs to obtain the wild environment information and process it to obtain the terrain information, and then plan a reasonable motion trajectory based on the acquired terrain information.

[0003] With the rapid development of computer vision technology, computer vision technology can be used to obtain terrain information in the wild environment when performing trajectory planning tasks. The specific execution process is: obtain the RGB image collected in the wild environment, and assign a single terrain category label to each pixel in the RGB image, for example, assign terrain category labels such as grass, soil, and gravel. In addition to obtaining terrain information in this way, terrain information can also be extracted by constructing a traversable map. The specific execution process is: use a classification network or a manually defined traversability standard to determine whether a certain terrain in the image is traversable, and mark the traversable area as traversable and the intraversable area as intraversable, to obtain a two-dimensional raster map.

[0004] After obtaining the terrain information, the current mainstream processing method is to perform trajectory planning through traditional planning methods or reinforcement learning methods. For traditional planning methods, it is usually based on the passable information represented in the two-dimensional grid map using RRT* or A* methods to perform trajectory planning. In simple terms, it is to search for a feasible next step position in the passable area through the passable information represented in the two-dimensional grid map to find an obstacle-free trajectory, and add linear constraints in the planning process to make the planned trajectory more reasonable and efficient. For reinforcement learning methods, it obtains terrain information through RGB images collected in real time by the moving target in the real world, and uses the obtained terrain information as input and the planned trajectory as output to train the policy network online, and passes the planned trajectory output by the policy network to the moving target for execution, and continuously optimizes the policy network based on the execution feedback of the moving target.

[0005] Although the existing technology can perform trajectory planning by acquiring terrain information, the quality of the planned trajectory obtained by the existing technology is poor. It is not only difficult to control the movement of the moving target along the planned trajectory, but also may expose the moving target to potential danger during the movement.

[0006] It should be noted that this background technology is only used to introduce the relevant information of the present invention to help understand the technical solution of the present invention, but it does not mean that the relevant information is necessarily the prior art. If there is no evidence that the relevant information has been disclosed before the application date of the present invention, the relevant information shall not be regarded as the prior art. Summary of the invention

[0007] Therefore, the object of the present invention is to overcome the defects of the above-mentioned prior art and provide a hybrid terrain trajectory planning system and a moving target moving control system.

[0008] The objectives of the present invention are achieved through the following technical solutions.

[0009] According to a first aspect of the present invention, a hybrid terrain trajectory planning system is provided, which is used to plan a travel path for a moving target in a hybrid terrain environment, and the system includes: a perception processing module, which is configured with a pre-trained semantic segmentation network and an encoder; wherein the pre-trained semantic segmentation network is used to extract semantic features from multiple continuous RGB terrain images collected by the moving target at multiple times to obtain a multi-label terrain feature map at each time; the encoder is used to encode the multi-label terrain feature map at each time to obtain a hybrid terrain semantic feature map at each time; wherein the multi-label terrain feature map represents the terrain category contained in each pixel in the RGB terrain image, and the hybrid terrain semantic feature map represents the degree of difficulty of passing different terrain categories in the RGB terrain image; a trajectory planning module, which is used to process the moving state of the moving target collected by the moving target at multiple times, the terrain depth map, and the corresponding multiple times continuous hybrid terrain semantic feature maps transmitted by the perception processing module based on the strategy network configured thereon to obtain a target planning trajectory; wherein the strategy network takes the multiple times continuous hybrid terrain semantic feature maps, the terrain depth map and the moving state as input, and takes the target planning trajectory as output, and is obtained by offline training based on a meta-reinforcement learning method.

[0010] In some embodiments of the present invention, the pre-trained semantic segmentation network is configured to perform terrain classification on each pixel in the RGB terrain image at each moment to obtain the probability that each pixel in the RGB terrain image at each moment belongs to each terrain category, and the terrain category corresponding to all terrain category probabilities in each pixel greater than a preset classification threshold is used as the terrain category label of the pixel to obtain a multi-label terrain feature map.

[0011] In some embodiments of the present invention, the encoding module is configured to encode the multi-label terrain feature map in the following manner to obtain a mixed terrain semantic feature map: based on the preset terrain category difficulty, calculate the summed mean of the terrain difficulty corresponding to all terrain category labels corresponding to each pixel in the multi-label terrain feature map.

[0012] In some embodiments of the present invention, the strategy network includes a first front network and a second front network connected in parallel, and multiple fully connected layers, a long short-term memory network and an output layer connected in series after the first front network and the second front network; wherein the first front network is composed of multiple fully connected layers, which is used to extract state feature information of continuous travel states at multiple moments; the second front network is composed of multiple convolutional layers, which is used to extract terrain semantic information of continuous mixed terrain semantic feature maps at multiple moments and terrain geometric information of continuous terrain depth maps at multiple moments; the fully connected layer is used to fuse the state feature information of multiple moments continuously transmitted by the first front network and the terrain semantic information and terrain geometric information of multiple moments transmitted by the second front network to obtain sequence fusion features; the long short-term memory network is used to extract dependencies in sequence fusion features to obtain time series features; the output layer is used to perform trajectory prediction based on time series features to obtain target planning trajectory.

[0013] In some embodiments of the present invention, the policy network is configured to be trained in the following manner: a training set is constructed based on existing path data, wherein the training set includes multiple sequence data, each sequence data has a corresponding standard planning trajectory, and each sequence data includes multiple continuous RGB terrain images at multiple moments, corresponding terrain depth maps, and travel status; a perception processing module is used to process multiple continuous RGB terrain images at multiple moments in each sequence data to obtain a mixed terrain semantic feature map at each moment in the sequence data, and multiple rounds of training are performed using the processed multiple sequence data as input to the policy network and the target planning trajectory as output, and the parameters of the policy network are updated based on preset training objectives.

[0014] Preferably, the preset training goal is:

[0015]

[0016] in, represents the policy network; θ represents the parameters of the policy network; T represents the time length; t represents the moment; γ t represents the discount factor; r(·) represents the reward function; X t Represents sequence data; q t Represents sequence data X t The corresponding standard planning trajectory; represents the total reward obtained by executing the standard planning trajectory; qt ′ Represents the policy network based on sequence data X t The target planning trajectory obtained; represents the total reward obtained by executing the target planning trajectory; D KL (π θ ||π β ) represents π θ With π β The KL divergence between θ represents the policy network before update, π β Indicated by π θ is the policy network after multiple updates of the benchmark; ∈ represents the hyperparameter.

[0017] In some embodiments of the present invention, the system also includes: a self-correction module, which is used to collect all actual motion trajectories obtained after the moving target moves according to the target planning trajectory and whose Euclidean distance with the corresponding target planning trajectory is greater than or equal to a preset distance threshold as a correction trajectory set, and pass the correction trajectory set to the trajectory planning module so that the policy network configured on the trajectory planning module is retrained based on the correction trajectory set, wherein the Euclidean distance is used to measure the similarity between the actual motion trajectory and its corresponding target planning trajectory.

[0018] In some embodiments of the present invention, the policy network is configured to be retrained in the following manner: the RGB terrain images, terrain depth maps and travel states at multiple moments corresponding to the target planning trajectory corresponding to each actual motion trajectory in the corrected trajectory set are used as retraining sequence data, and the actual motion trajectory is used as the standard motion trajectory of its corresponding retraining sequence data, and all the retraining sequence data and their corresponding standard motion trajectory constitute a retraining data set; the perception processing module is used to process the RGB terrain images at multiple moments in each retraining sequence data in the retraining data set to obtain a mixed terrain semantic feature map at each moment, and multiple rounds of training are performed with the processed multiple retraining sequence data as input and the new target planning trajectory as output, and the parameters of the policy network are updated based on the new training objectives.

[0019] Preferably, the new training goal is:

[0020]

[0021] in, represents the retrained policy network, C t Represents the retraining sequence data, k t Represents the retraining sequence data C t The corresponding standard motion trajectory, r(C t ,k t) represents the reward obtained by executing the standard motion trajectory, k t ′ indicates that the policy network is based on the retraining sequence data C t The new target planning trajectory obtained, r(C t ,k t ′) represents the reward obtained by executing the new target planning trajectory, and α represents the Euclidean distance between the standard motion trajectory and the new target planning trajectory.

[0022] According to a second aspect of the present invention, a moving target travel control system is provided, the system comprising: a mixed terrain trajectory planning system as described in the first aspect of the present invention, used to plan a travel path for a moving target in a mixed terrain environment to obtain a target planned trajectory; a motion control module, used to calculate the angular velocity and linear velocity of the moving target when executing the target planned trajectory based on the target planned trajectory; and a PID control module, used to control the moving target to move according to the target planned trajectory based on the angular velocity and linear velocity obtained by the motion control module.

[0023] Compared with the prior art, the advantages of the present invention are: (1) more comprehensive terrain information is obtained by performing multi-label terrain classification on RGB images, thereby avoiding the problem of key information loss caused by insufficient terrain information extraction; (2) offline meta-reinforcement learning is used to train the policy network, avoiding the dangerous planning faced by interacting with the real world during online training. At the same time, meta-reinforcement learning can establish the optimal planning strategy for different terrain types, and train a policy network that can cope with complex and changeable terrain environments by taking planning tasks on different terrains as different meta-tasks; (3) a self-correction module is set to realize self-correction and optimization of the policy network, so that the policy network maintains good adaptability and stability in complex and changeable terrain environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] The embodiments of the present invention are further described below with reference to the accompanying drawings, in which:

[0025] Figure 1 A schematic diagram of the structure of a hybrid terrain trajectory planning system according to an embodiment of the present invention;

[0026] Figure 2 is a schematic diagram of an example of a hybrid terrain semantic feature according to an embodiment of the present invention;

[0027] Figure 3 A schematic diagram of a strategy network structure according to an embodiment of the present invention;

[0028] Figure 4 It is a schematic diagram of the structural composition of a moving target control system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0029] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail by specific embodiments in conjunction with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0030] As mentioned in the background technology section, although the existing technology can perform trajectory planning by acquiring terrain information, the quality of the planned trajectory obtained by the existing technology is poor. Not only is it difficult to control the movement of the moving target along the planned trajectory, but it may also expose the moving target to potential danger during the movement.

[0031] In order to solve the above defects, the inventor analyzed the prior art and found that the existing terrain information extraction and trajectory planning have defects. The defects in the terrain information extraction and trajectory planning process are analyzed one by one below.

[0032] In the process of terrain information extraction, the existing technology mainly relies on two strategies: one is the terrain category labeling strategy, and the other is the passability annotation strategy. Among them, the terrain category labeling strategy is to assign a single terrain category label to each pixel in the RGB image to identify and extract terrain information. Although this processing method can identify the basic type of terrain, it assumes that each pixel in the RGB image represents only one independent terrain category; however, in the complex and changeable natural environment, mixed terrain is extremely common, such as muddy land interweaving with grass, grass mixed in gravel land, sandstone land and mud land mixed terrain environment. In these mixed terrain environments, simply performing a single terrain classification on each pixel obviously cannot fully and accurately reflect the true situation of the terrain, resulting in deviations or deficiencies in the extracted terrain information, which in turn leads to poor quality of the planned trajectory. The passability annotation strategy uses a classification network or a manually defined passability standard to determine whether a certain terrain in the image is passable, so as to identify and extract terrain information. Although this processing method can identify passable areas and inaccessible areas, this processing method generates a two-dimensional raster map. However, in the complex and ever-changing environment of the wild, the terrain is undulating and changing, and the two-dimensional grid map often loses a lot of key information. Therefore, it is difficult to ensure that a safe and reliable route is planned when only relying on the passability annotations on the two-dimensional grid map for trajectory planning. It can be seen that the existing terrain information extraction strategies cannot fully and accurately reflect the real situation of the terrain, and the extracted terrain information has deviations or deficiencies.

[0033] In the process of trajectory planning, the existing technology mainly relies on two planning strategies: one is the traditional planning strategy, and the other is the planning strategy based on reinforcement learning. Among them, the traditional planning strategy usually relies on search or sampling methods, combined with a two-dimensional grid map to plan a trajectory that is collision-free and can reach the destination. However, a significant limitation of this traditional planning strategy is that it cannot distinguish different types of terrain such as grass, mud, and gravel, and only relies on the passable and impassable markings on the two-dimensional grid map for planning, which leads to unsatisfactory quality of the planned trajectory. In order to improve the safety and reliability of the planned trajectory, the traditional planning strategy will also introduce linear constraints, such as maximum speed, obstacle distance and other linear constraints to adjust the planned trajectory. However, in the natural environment, there are a large number of nonlinear constraints, such as complex and changeable terrain constraints, non-integrity motion constraints and other nonlinear constraints, which have an important impact on the feasibility and safety of the trajectory. Therefore, the traditional planning strategy is limited to considering linear constraints, and ignores or cannot effectively handle these nonlinear constraints. Then the planned trajectory is likely to be unable to fully adapt to the complexity and variability of the natural environment, thereby affecting the actual travel effect and safety of the moving target. The planning strategy based on reinforcement learning uses images labeled with terrain categories to train the policy network online, and uses the planned trajectory output by the policy network to control the movement of the moving target, and then optimizes the policy network according to the execution feedback of the moving target. Although the policy network can learn the mapping of terrain information to trajectory and handle various nonlinear constraints to a certain extent, it can generate a more reasonable planning trajectory. However, due to the extremely rich terrain types in the wild environment and the different optimal trajectory strategies for each terrain, it is difficult for the policy network to achieve optimal performance in all terrains during training. In addition, online training requires interaction with the real world to obtain a large amount of data, which makes training difficult and takes a long time. In addition, continuously optimizing the policy network through online training means that the moving target needs to interact with the real environment in real time during training. However, before the policy network reaches the requirements of stable and reliable use, this real-time interaction may cause dangerous planning, thereby affecting the actual operation effect and safety of the moving target. It can be seen that the existing trajectory planning strategies cannot cope with complex and changeable terrain environments, and it is difficult to plan safe and reliable trajectories.

[0034] Based on the analysis of the prior art, in view of the defects of the existing terrain information extraction strategy, the inventor has found through research that more comprehensive terrain information can be obtained by multi-label terrain classification of RGB images, so as to avoid the problem of key information loss caused by insufficient terrain information extraction. In view of the defects of the existing trajectory planning strategy, the inventor has found that the policy network can be trained by offline meta-reinforcement learning, wherein the offline training method can avoid the dangerous planning faced by interacting with the real world during online training, and meta-reinforcement learning can establish the optimal planning strategy for different terrain types respectively, and train a policy network that can cope with complex and changeable terrain environments by taking planning tasks on different terrains as different meta-tasks. Furthermore, the inventor proposes a new trajectory planning system for complex outdoor environments, in which a perception processing module capable of multi-label terrain classification processing is provided, and a trajectory planning module configured with a policy network obtained by offline training based on meta-reinforcement learning.

[0035] In summary, if Figure 1 As shown, the present invention provides a hybrid terrain trajectory planning system, which is used to plan a travel path for a moving target in a hybrid terrain environment. The system includes: a perception processing module, which is configured with a pre-trained semantic segmentation network and an encoder; wherein the pre-trained semantic segmentation network is used to extract semantic features from multiple continuous RGB terrain images collected by the moving target at multiple times to obtain a multi-label terrain feature map at each time; the encoder is used to encode the multi-label terrain feature map at each time to obtain a hybrid terrain semantic feature map at each time; wherein the multi-label terrain feature map represents the terrain category contained in each pixel in the RGB terrain image, and the hybrid terrain semantic feature map represents the degree of difficulty of passing different terrain categories in the RGB terrain image; a trajectory planning module, which is used to process the moving state of the moving target collected at multiple times continuously by the moving target, the terrain depth map, and the corresponding multiple times continuous hybrid terrain semantic feature maps transmitted by the perception processing module based on the strategy network configured thereon to obtain the target planning trajectory; wherein the strategy network takes the multiple times continuous hybrid terrain semantic feature maps, terrain depth maps and moving state as input, and takes the target planning trajectory as output, and is obtained by offline training based on meta-reinforcement learning.

[0036] In order to better understand the present invention, the components of the trajectory planning system are described in detail below in conjunction with specific embodiments.

[0037] 1. Perception Processing Module

[0038] The perception processing module is used to pre-process the RGB image collected by the moving target to extract terrain information in the RGB image. The perception processing module is configured with a pre-trained semantic segmentation network and an encoder, and the pre-trained semantic segmentation network and the encoder are respectively introduced below.

[0039] The pre-trained semantic segmentation network is used to extract semantic features from multiple continuous RGB terrain images collected by the moving target at multiple moments to obtain a multi-label terrain feature map at each moment. According to one embodiment of the present invention, the pre-trained semantic segmentation network is configured to perform terrain classification on each pixel in the RGB terrain image at each moment to obtain the probability that each pixel in the RGB terrain image at each moment belongs to each terrain category, and the terrain category corresponding to all terrain category probabilities greater than the preset classification threshold in each pixel is used as the terrain category label of the pixel to obtain a multi-label terrain feature map. In addition to using the terrain category corresponding to all terrain category probabilities greater than the preset classification threshold in each pixel as the terrain category label of the pixel, the terrain category probabilities corresponding to each pixel can also be sorted from large to small, and the three terrain categories that are ranked first and greater than the preset classification threshold are selected as the terrain category labels of the pixel according to the sorting results. For example, when the terrain classification result corresponding to a certain pixel is: gravel 0.7, grass 0.8, mud 0.7, cement 0.2, asphalt 0.1, assuming that the preset classification threshold is 0.6, gravel, grass, and mud can be selected as the terrain category labels of the pixel. It should be noted that by transforming the original single terrain label into multiple terrain labels, so as to distinguish situations such as grass on gravel or grass on muddy ground that are classified as grass by existing technologies, terrain information can be obtained more accurately, solving the problem of insufficient terrain information extraction in mixed terrain environments.

[0040] The encoder is used to encode the multi-label terrain feature map at each moment to obtain the mixed terrain semantic feature map at each moment. According to one embodiment of the present invention, the encoding module is configured to encode the multi-label terrain feature map in the following manner to obtain the mixed terrain semantic feature map: based on the preset terrain category pass difficulty, calculate the summed mean of the terrain pass difficulty corresponding to all terrain category labels corresponding to each pixel in the multi-label terrain feature map. In order to better understand the mixed terrain semantic feature map, Figure 2 The mixed terrain shown is used as an example to illustrate. Figure 2 It can be seen that Figure 2 (a) shows two different RGB images containing three types of images: fallen leaves, grass, and land; Figure 2 (b) shows Figure 2(a) The mixed terrain semantic feature map corresponding to the two images. The red marked part in the figure indicates that the terrain corresponding to the area is dangerous and difficult to pass, and the green marked part indicates that the terrain corresponding to the area is safe and easy to pass; Figure 2 (c) shows the use of existing terrain information extraction strategies Figure 2 In (a), the feature maps of the two images after processing are almost all marked with yellow to indicate the difficulty of terrain (green is safe, yellow is between safe and dangerous, and red is dangerous), which fails to accurately distinguish the difficulty of different terrains. Figure 2 It can be seen that the mixed terrain semantic feature map actually uses the corresponding color mark to distinguish the difficulty of each terrain, and the color mark corresponds to the summed mean of the difficulty of the terrain corresponding to all terrain category labels corresponding to each pixel, where the larger the summed mean corresponding to a pixel, the redder the color corresponding to the pixel; conversely, the smaller the summed mean corresponding to a pixel, the greener the color corresponding to the pixel. It should also be noted that the difficulty of terrain category passage is obtained by conducting experiments on different terrains with the target. The specific experimental process is: the target is driven on the flat ground of the corresponding terrain type, the linear speed of the target is controlled to be 2 meters per second, and the angular velocity is gradually accelerated from 0 to 1.5 meters per second. The angular velocity of the target when it slips or vibrates too much during the acceleration process is recorded, and the recorded angular velocity is divided by the maximum angular velocity of 1.5 to obtain the difficulty of the current terrain.

[0041] 2. Trajectory Planning Module

[0042] The trajectory planning module is used to process the moving state of the moving target collected at multiple times continuously by the moving target, the terrain depth map, and the corresponding multiple times continuous mixed terrain semantic feature maps transmitted by the perception processing module based on the strategy network configured thereon to obtain the target planning trajectory. The strategy network configured thereon is described in detail below.

[0043] According to one embodiment of the present invention, Figure 3As shown, the strategy network includes a first front network and a second front network connected in parallel, and a plurality of fully connected layers, a long short-term memory network and an output layer connected in series after the first front network and the second front network; wherein the first front network is composed of a plurality of fully connected layers, and is used to extract state feature information of continuous travel states at multiple moments; the second front network is composed of a plurality of convolutional layers, and is used to extract terrain semantic information of continuous mixed terrain semantic feature maps at multiple moments and terrain geometric information of continuous terrain depth maps at multiple moments; the fully connected layer is used to fuse the state feature information of multiple moments continuous transmitted by the first front network and the terrain semantic information and terrain geometric information of multiple moments transmitted by the second front network to obtain sequence fusion features; the long short-term memory network is used to extract dependencies in sequence fusion features to obtain time series features; the output layer is used to perform trajectory prediction based on time series features to obtain the target planning trajectory. wherein, in Figure 3 In the strategy network shown, the first front network consists of two fully connected layers containing 256 neurons and one fully connected layer containing 128 neurons; the second front network consists of a convolution layer configured with a convolution kernel size of 5×5 and a filter number of 32 and a convolution layer configured with a convolution kernel size of 3×3 and a filter number of 64; the first front network and the second front network are serially connected in sequence, and there is a fully connected layer containing 512 neurons and a fully connected layer containing 256 neurons; the output layer is a fully connected layer containing 12 neurons, and the output layer outputs the control points of the B-spline, which uniquely determine a B-spline, that is, the target planning trajectory. It should be noted that the strategy network can adjust the network structure and the configuration of the network layer according to actual needs, and the present invention does not impose specific restrictions on the strategy network.

[0044] According to one embodiment of the present invention, the policy network is configured to be trained in the following manner: construct a training set based on existing path data, wherein the training set includes multiple sequence data, each sequence data has a corresponding standard planning trajectory, and each sequence data includes multiple continuous RGB terrain images at multiple moments, corresponding terrain depth maps and travel states; use a perception processing module to process multiple continuous RGB terrain images at multiple moments in each sequence data to obtain a mixed terrain semantic feature map at each moment in the sequence data, use the processed multiple sequence data as the input of the policy network, and the target planning trajectory as the output for multiple rounds of training, and update the parameters of the policy network based on the preset training target. It should be noted that the travel state includes the speed of the travel target, specifically including angular velocity, six-axis acceleration and linear velocity; it also includes the posture of the travel target, that is, the configuration state of the direction and position of the travel target in space relative to a certain coordinate system, and the posture describes the spatial posture information such as the orientation, tilt or rotation of the travel target at a certain moment; it also includes the motion state of the travel target, which indicates the state of the travel target during driving, such as whether there is shaking, whether there is slipping, whether there is an obstacle, etc.

[0045] According to one embodiment of the present invention, the preset training target is:

[0046]

[0047] in, represents the policy network; θ represents the parameters of the policy network; T represents the time length; t represents the moment; γ t represents the discount factor; r(·) represents the reward function; X t Represents sequence data; q t Represents sequence data X t The corresponding standard planning trajectory; represents the total reward obtained by executing the standard planning trajectory; q t ′ Represents the policy network based on sequence data X t The target planning trajectory obtained; represents the total reward obtained by executing the target planning trajectory; D KL (π θ ||π β ) represents π θ With π β The KL divergence between θ represents the policy network before update, π β Indicated by π θis the policy network after multiple updates of the benchmark; ∈ represents a hyperparameter. It should be noted that the purpose of the preset training goal is to minimize the difference between the total reward obtained by executing the standard planning trajectory and the total reward obtained by executing the target planning trajectory. The smaller the difference, the closer the target planning trajectory is to the standard planning trajectory, so that the policy network can learn how to plan the trajectory; the preset training goal also sets a constraint condition D KL (π θ ||π β )≤∈ to constrain the update distance to increase training stability, where the value of the hyperparameter ∈ is determined according to actual needs. It should also be noted that represents the total reward obtained by executing the standard planning trajectory from time step t = 0 until the end of time step t = T, γ t r(X t ,q t ) represents the immediate reward obtained by executing the corresponding action at time step t according to the standard planning trajectory; represents the total reward obtained from executing the target planning trajectory starting at time step t = 0 until the end of execution at time step t = T, γ t r(X t ,q t ′ ) represents the immediate reward obtained by executing the corresponding action of the standard planning trajectory at time step t.

[0048] 3. Self-correction module

[0049] According to one embodiment of the present invention, Figure 1 As shown, the system also includes: a self-correction module, which is used to collect all actual motion trajectories obtained after the moving target moves according to the target planning trajectory and whose Euclidean distance with the corresponding target planning trajectory is greater than or equal to a preset distance threshold as a correction trajectory set, and pass the correction trajectory set to the trajectory planning module so that the policy network configured on the trajectory planning module is retrained based on the correction trajectory set, wherein the Euclidean distance is used to measure the similarity between the actual motion trajectory and its corresponding target planning trajectory. It should be noted that the preset distance threshold can be set to 4, or it can be determined according to actual needs, and the present invention does not impose specific restrictions on it. It should also be noted that the main purpose of setting up the self-correction module is to achieve self-correction and optimization of the policy network, so that the policy network maintains good adaptability and stability in complex and changeable terrain environments, and outputs a target planning trajectory with better quality.

[0050] According to one embodiment of the present invention, the policy network is configured to be retrained in the following manner: the RGB terrain images, terrain depth maps and travel states at multiple moments corresponding to the target planning trajectory corresponding to each actual motion trajectory in the corrected trajectory set are used as retraining sequence data, and the actual motion trajectory is used as the standard motion trajectory of its corresponding retraining sequence data, and all the retraining sequence data and their corresponding standard motion trajectory constitute a retraining data set; the perception processing module is used to process the RGB terrain images at multiple moments in each retraining sequence data in the retraining data set to obtain a mixed terrain semantic feature map at each moment, and multiple rounds of training are performed with the processed multiple retraining sequence data as input and the new target planning trajectory as output, and the parameters of the policy network are updated based on the new training objectives.

[0051] According to one embodiment of the present invention, the new training objective is:

[0052]

[0053] in, represents the retrained policy network, C t Represents the retraining sequence data, k t Represents the retraining sequence data C t The corresponding standard motion trajectory, r(C t ,k t ) represents the reward obtained by executing the standard motion trajectory, k t ′ Represents the policy network based on the retraining sequence data C t The new target planning trajectory obtained, r(C t ,k t ′ ) represents the reward obtained by executing the new target planning trajectory, and α represents the Euclidean distance between the standard motion trajectory and the new target planning trajectory. It should be noted that the Euclidean distance between the standard motion trajectory and the new target planning trajectory is added as a constraint in the new training objective to improve the stability of the policy network.

[0054] The trajectory planning system described in the above embodiment can plan a path for a moving target in a mixed terrain environment, so that the moving target moves based on the planned path, thereby achieving the working task of the moving target in a real environment. Figure 4As shown, the present invention also provides a moving target control system, the system comprising: a mixed terrain trajectory planning system as described in the aforementioned embodiment, used to plan a moving path for a moving target in a mixed terrain environment to obtain a target planning trajectory; a motion control module, used to calculate the angular velocity and linear velocity of the moving target when executing the target planning trajectory based on the target planning trajectory; a PID control module, used to control the moving target to move according to the target planning trajectory based on the angular velocity and linear velocity obtained by the motion control module.

[0055] In order to verify that the trajectory planning system proposed in the present invention has a better trajectory planning effect than the trajectory planning scheme proposed in the prior art, comparative experiments and ablation experiments were performed to obtain the experimental results shown in Table 1.

[0056] The experimental results shown in Table 1 include five evaluation indicators, namely, success, smoothness, standardized trajectory length, number of dangerous planning, and trajectory tracking difficulty. Among them, success indicates the probability that the moving target can successfully reach the destination according to the planned trajectory; smoothness indicates the continuity and fluency of the moving target in the actual movement process according to the planned trajectory (a smooth trajectory means that the moving target does not have a sharp change of direction during the movement process, and the smaller the smoothness, the better the planned trajectory); standardized trajectory length indicates the average length of the planned trajectory (the shorter the length, the better the planned trajectory); the number of dangerous planning indicates the number of times the moving target encounters danger in the actual movement process according to the planned trajectory (the fewer the number of dangerous planning, the better the planned trajectory); trajectory tracking difficulty indicates the Euclidean distance between the planned trajectory and the actual movement trajectory (the smaller the distance, the better the planned trajectory).

[0057] The comparative experiment in Table 1 is used to evaluate the difference between the existing trajectory planning method and the trajectory planning scheme proposed in this invention. The comparative experiment includes four existing trajectory planning methods, among which the traditional method uses fast RRT and model predictive control to plan trajectories in the wild environment; the RAORN method uses single-category terrain classification combined with a learning-based planning method to plan trajectories in the wild environment; the Terp method converts the terrain environment into a traversable map in the form of rewards combined with reinforcement learning to plan trajectories in the wild environment (the larger the reward, the easier it is to pass); the BADGR method directly uses the data collected by the original sensor combined with reinforcement learning to plan trajectories in the wild environment.

[0058] The ablation experiment in Table 1 is used to evaluate the trajectory planning effect after simplifying some processing methods in the trajectory planning scheme proposed by the present invention. Among them, removing historical data means deleting part of the training data when training the policy network, that is, reducing the amount of training data; removing self-correction means that the policy network does not perform retraining; removing mixed terrain processing means that only a single terrain classification is performed on the RGB image.

[0059] It can be seen from Table 1 that, compared with the other four existing trajectory planning methods, the present invention is slightly inferior to the traditional method in terms of smoothness, while the performance of the present invention in other indicators is better than that of the other four existing trajectory planning methods.

[0060] Table 1

[0061]

[0062] Compared with the prior art, the present invention has the following beneficial effects: (1) more comprehensive terrain information is obtained by performing multi-label terrain classification on RGB images, thereby avoiding the problem of key information loss caused by insufficient terrain information extraction; (2) offline meta-reinforcement learning is used to train the policy network, thereby avoiding the dangerous planning faced by interacting with the real world during online training. At the same time, meta-reinforcement learning can establish the optimal planning strategy for different terrain types, and train a policy network that can cope with complex and changeable terrain environments by taking planning tasks on different terrains as different meta-tasks; (3) a self-correction module is set to realize self-correction and optimization of the policy network, so that the policy network maintains good adaptability and stability in complex and changeable terrain environments.

[0063] It should be noted that although the above describes the various steps in a specific order, it does not mean that the various steps must be executed in the above specific order. In fact, some of these steps can be executed concurrently or even in a different order as long as the required functions can be achieved.

[0064] The present invention may be a system, a method and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for causing a processor to implement various aspects of the present invention.

[0065] Computer readable storage medium can be a tangible device that holds and stores instructions used by an instruction execution device. Computer readable storage medium can include, for example, but is not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination thereof. More specific examples (non-exhaustive list) of computer readable storage medium include: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disk read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, for example, a punch card or a protruding structure in a groove on which instructions are stored, and any suitable combination thereof.

[0066] The embodiments of the present invention have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.

Claims

1. A mixed terrain trajectory planning system for planning a path for a moving target in a mixed terrain environment, characterized in that: The system comprises: A perception processing module, which is configured with a pre-trained semantic segmentation network and an encoder; wherein the pre-trained semantic segmentation network is used to extract semantic features from multiple continuous RGB terrain images collected by the moving target at multiple moments to obtain a multi-label terrain feature map at each moment; the encoder is used to encode the multi-label terrain feature map at each moment to obtain a mixed terrain semantic feature map at each moment; wherein the multi-label terrain feature map represents the terrain category contained in each pixel in the RGB terrain image, and the mixed terrain semantic feature map represents the degree of difficulty of passing different terrain categories in the RGB terrain image; The trajectory planning module is used to process the moving state of the moving target collected at multiple moments continuously, the terrain depth map and the corresponding multiple moments continuous mixed terrain semantic feature map transmitted by the perception processing module based on the strategy network configured thereon to obtain the target planning trajectory; wherein the strategy network takes the multiple moments continuous mixed terrain semantic feature map, the terrain depth map and the moving state as input, takes the target planning trajectory as output, and is obtained by offline training based on meta-reinforcement learning.

2. The system according to claim 1, characterized in that The pre-trained semantic segmentation network is configured to perform terrain classification on each pixel in the RGB terrain image at each moment to obtain the probability that each pixel in the RGB terrain image at each moment belongs to each terrain category, and the terrain category corresponding to all terrain category probabilities in each pixel that are greater than a preset classification threshold is used as the terrain category label of the pixel to obtain a multi-label terrain feature map.

3. The system according to claim 2, characterized in that The encoding module is configured to encode the multi-label terrain feature map in the following manner to obtain a mixed terrain semantic feature map: Based on the preset terrain category difficulty, the sum mean of the terrain difficulty corresponding to all terrain category labels corresponding to each pixel in the multi-label terrain feature map is calculated.

4. The system according to claim 3, characterized in that The strategy network includes a first front network and a second front network connected in parallel, and a plurality of fully connected layers, a long short-term memory network and an output layer connected in series after the first front network and the second front network; wherein the first front network is composed of a plurality of fully connected layers, and is used to extract state feature information of continuous travel states at multiple moments; the second front network is composed of a plurality of convolutional layers, and is used to extract terrain semantic information of continuous mixed terrain semantic feature maps at multiple moments and terrain geometric information of continuous terrain depth maps at multiple moments; the fully connected layer is used to fuse the state feature information of multiple moments continuous transmitted by the first front network and the terrain semantic information and terrain geometric information of multiple moments transmitted by the second front network to obtain sequence fusion features; the long short-term memory network is used to extract dependency relations in sequence fusion features to obtain time series features; the output layer is used to perform trajectory prediction based on time series features to obtain target planning trajectory.

5. The system according to claim 4, characterized in that The policy network is configured to be trained as follows: A training set is constructed based on the existing path data, wherein the training set includes multiple sequence data, each sequence data has a corresponding standard planning trajectory, and each sequence data includes multiple continuous RGB terrain images at multiple times, corresponding terrain depth maps and travel status; The perception processing module is used to process the continuous RGB terrain images at multiple moments in each sequence data to obtain the mixed terrain semantic feature map at each moment in the sequence data. The processed multiple sequence data are used as the input of the policy network and the target planning trajectory is used as the output for multiple rounds of training, and the parameters of the policy network are updated based on the preset training objectives.

6. The system according to claim 5, characterized in that The preset training objectives are: s.tD KL (p θ ||p β )≤∈ in, represents the policy network; θ represents the parameters of the policy network; T represents the time length; t represents the moment; γ t represents the discount factor; r(·) represents the reward function; X t represents sequence data; q t Represents sequence data X t The corresponding standard planning trajectory; represents the total reward obtained by executing the standard planning trajectory; q t ′ indicates that the policy network is based on the sequence data X t The target planning trajectory obtained; represents the total reward obtained by executing the target planning trajectory; D KL (π θ ||π β ) represents π θ With π β The KL divergence between θ represents the policy network before update, π β Indicated by π θ is the policy network after multiple updates of the benchmark; ∈ represents the hyperparameter.

7. The system according to claim 6, characterized in that The system further comprises: The self-correction module is used to collect all actual motion trajectories obtained after the moving target moves according to the target planning trajectory and whose Euclidean distance with the corresponding target planning trajectory is greater than or equal to a preset distance threshold as a correction trajectory set, and pass the correction trajectory set to the trajectory planning module so that the policy network configured on the trajectory planning module is retrained based on the correction trajectory set, wherein the Euclidean distance is used to measure the similarity between the actual motion trajectory and its corresponding target planning trajectory.

8. The system according to claim 7, characterized in that The policy network is configured to be retrained as follows: The RGB terrain images, terrain depth maps and travel states at multiple moments corresponding to the target planning trajectory corresponding to each actual motion trajectory in the corrected trajectory set are used as retraining sequence data, and the actual motion trajectory is used as the standard motion trajectory of its corresponding retraining sequence data. All retraining sequence data and their corresponding standard motion trajectories constitute a retraining data set; The perception processing module is used to process the RGB terrain images at multiple moments in each retraining sequence data in the retraining dataset to obtain the mixed terrain semantic feature map at each moment. Multiple rounds of training are carried out with the processed multiple retraining sequence data as input and the new target planning trajectory as output, and the parameters of the policy network are updated based on the new training objectives.

9. The system according to claim 8, characterized in that The new training objective is: in, represents the retrained policy network, C t Represents the retraining sequence data, k t Represents the retraining sequence data C t The corresponding standard motion trajectory, r(C t ,k t ) represents the reward obtained by executing the standard motion trajectory, k t ′ Represents the policy network based on the retraining sequence data C t The new target planning trajectory obtained, r(C t ,k t ′ ) represents the reward obtained by executing the new target planning trajectory, and α represents the Euclidean distance between the standard motion trajectory and the new target planning trajectory.

10. A moving target travel control system, characterized in that: The system comprises: A mixed terrain trajectory planning system as described in any one of claims 1 to 9, used to plan a travel path for a moving target in a mixed terrain environment to obtain a target planning trajectory; A motion control module, used for calculating the angular velocity and linear velocity of the moving target when executing the target planning trajectory based on the target planning trajectory; The PID control module is used to control the moving target to move according to the target planning trajectory based on the angular velocity and linear velocity obtained by the motion control module.

Images