Vision-based unmanned platform motion planning method and system in unstructured environment

Abstract

The invention discloses a vision-based unmanned platform motion planning method and system in an unstructured environment, and relates to the technical field of computer vision, and the method comprises the steps: obtaining a target RGB image; inputting the target RGB image into the trained trafficability cost graph model to obtain a trafficability cost graph with the same height and width as the target RGB image; performing action sampling in a set motion space by using an MPPI algorithm according to the trafficability cost graph to obtain a sampling track; and carrying out cost calculation on the sampling track by taking the trafficability of the unstructured environment and the distance to the end point as indexes, and determining the optimal action of passing through the unstructured environment. Based on the motion planning method provided by the invention, more efficient motion planning and control can be realized in an unstructured environment.

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and in particular to a vision-based unmanned platform motion planning method and system in an unstructured environment. Background Art

[0002] In unstructured environments, vision-based local motion planning shows significant application potential. This technology can use real-time visual data to perform adaptive adjustments and dynamic path tracking, thereby achieving effective navigation and motion decision-making in complex and unpredictable environments.

[0003] However, current vision-based motion planning technology still faces two major challenges. First, its ability to generalize the evaluation of traversability in unstructured scenes is limited. Second, existing motion planning methods are usually divided into two consecutive stages: "path planning" and "motion tracking". This sequential execution method limits the efficiency of motion planning and cannot achieve parallel processing. Summary of the invention

[0004] The purpose of this application is to provide a vision-based unmanned platform motion planning method and system in an unstructured environment, which can achieve more efficient motion planning and control in an unstructured environment.

[0005] To achieve the above objectives, this application provides the following solutions: In a first aspect, the present application provides a vision-based unmanned platform motion planning method in an unstructured environment, comprising: A target RGB image is obtained; the target RGB image is an RGB image in a non-structured scene.

[0006] The target RGB image is input into a trained passability cost map model to obtain a passability cost map with the same height and width as the target RGB image; the trained passability cost map model is a deep neural network model trained with a sample RGB image in a non-structured environment as input and the pixel-level semantic classification annotations corresponding to the sample RGB image as labels.

[0007] According to the passability cost map, the MPPI algorithm is used to perform action sampling in the set motion space to obtain a sampling trajectory.

[0008] Taking the passability of the unstructured environment and the distance to the end point as indicators, the cost of the sampled trajectory is calculated, and based on minimizing the cost, the optimal action to pass through the unstructured environment is determined.

[0009] Optionally, the semantics in the pixel-level semantic classification annotation include: concrete road, asphalt road, gravel road, grass, dirt road, sand road, rock, rock bed, puddle, background, signboard, obstacle, tree and building.

[0010] Optionally, the passability cost values ​​corresponding to different semantics include: The passability cost of concrete roads and asphalt roads is 0; the passability cost of gravel roads, grass, dirt roads, and sandy roads is 0.25; the passability cost of rocks and rock beds is 0.5; the passability cost of puddles is 0.75; the passability cost of backgrounds, signs, obstacles, trees, and buildings is 1.

[0011] Optionally, the loss function used in the training of the passability cost map model is an L1 loss function; the formula expression of the L1 loss function is: .

[0012] in, n is the number of training samples, y i is the true passability proxy value, is the predicted passability cost.

[0013] Optionally, before performing action sampling in a set motion space using an MPPI algorithm according to the passability cost map to obtain a sampling trajectory, the method further includes: Construct a vehicle dynamics model; the formula expression of the vehicle dynamics model is: .

[0014] in, For vehicles t Moment x , y Position and yaw angle, is the vehicle at time t+1 x Location, is the vehicle at time t+1 y Location, The time of vehicle t+1 x The yaw angle of the direction, For vehicles t time x The yaw angle of the direction, v t ,ω t are the linear velocity and angular velocity respectively, To control the time interval.

[0015] Optionally, according to the passability cost map, using the MPPI algorithm, action sampling is performed in a set motion space to obtain a sampling trajectory, specifically including: According to the passability cost map, a strategy based on the MPPI algorithm is adopted to optimize the action sampling process in the motion space by combining the Numba library with the Nvidia GPU acceleration technology.

[0016] Optionally, before calculating the cost of the sampled trajectory based on the passability of the unstructured environment and the distance to the end point as indicators, and determining the optimal action for passing through the unstructured environment based on minimizing the cost, the method further includes: Transform a point (x, y) in real-world coordinates to the image plane using known camera parameters, including: Determine the intrinsic parameters of the camera as K ;in, f x and f y They are x and y The focal length in the direction, ( c x , c y ) is the intersection of the optical axis and the image plane.

[0017] According to the formula , project the point onto the image coordinates; where, h is the height from the ground to the camera.

[0018] According to the formula , convert image coordinates into pixel coordinates; where ( u p , v p ) are pixel coordinates on the image plane.

[0019] Optionally, the cost function for calculating the cost of the sampling trajectory includes running the cost function and the final cost function .

[0020] Optionally, the running cost function The formula expression is: .

[0021] The final state cost function The formula expression is: .

[0022] in, is the final target position; pt For vehicles at time t location; s default is the default speed used to estimate the time it takes for the vehicle to reach the destination; is an indicator function. When any state x τ (0≤ τ ≤ T )achieve If yes, it returns 1, otherwise it returns 0; For location p t The cost of accessibility; w dist >0 and w tra >0 are the weights of penalizing the distance to the end point and the cost of passability.

[0023] In a second aspect, the present application provides a vision-based unmanned platform motion planning system in an unstructured environment, comprising: The image acquisition module is used to acquire a target RGB image; the target RGB image is an RGB image in a non-structured scene.

[0024] The image conversion module is used to input the target RGB image into a trained passability cost map model to obtain a passability cost map with the same height and width as the target RGB image; the trained passability cost map model is a deep neural network model trained by taking a sample RGB image in a non-structured environment as input and using the pixel-level semantic classification annotations corresponding to the sample RGB image as labels.

[0025] The trajectory sampling module is used to perform action sampling in a set motion space according to the passability cost map and use the MPPI algorithm to obtain a sampling trajectory.

[0026] The calculation module is used to calculate the cost of the sampled trajectory based on the passability of the unstructured environment and the distance to the end point as indicators, and determine the optimal action to pass through the unstructured environment based on minimizing the cost.

[0027] According to the specific embodiments provided in this application, this application discloses the following technical effects: The present application provides a vision-based unmanned platform motion planning method and system in an unstructured environment. By inputting the target RGB image into a trained deep neural network model, a passability cost map can be quickly obtained, thereby realizing real-time motion planning; the deep neural network model trained by pixel-level semantic classification annotation can accurately identify obstacles and feasible areas in unstructured environments; the MPPI algorithm is used for action sampling, and combined with cost calculation, an optimized trajectory with the highest passability and the closest distance to the end point in an unstructured environment can be found, thereby improving the motion efficiency of the unmanned platform. By calculating the cost of the sampled trajectory and determining the optimal action based on minimizing the cost, the unmanned platform can effectively avoid collisions with obstacles during movement, thereby improving movement safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0029] Figure 1 A flowchart of a vision-based unmanned platform motion planning method in an unstructured environment is provided in one embodiment of the present application.

[0030] Figure 2 A schematic diagram of the structure of a vision-based unmanned platform motion planning system in an unstructured environment provided in one embodiment of the present application. DETAILED DESCRIPTION

[0031] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0032] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0033] Embodiment 1 like Figure 1 As shown, this embodiment provides a vision-based unmanned platform motion planning method in an unstructured environment, including: Step 101: Acquire a target RGB image; the target RGB image is an RGB image in a non-structured scene.

[0034] Step 102: Input the target RGB image into a trained passability cost map model to obtain a passability cost map with the same height and width as the target RGB image; the trained passability cost map model is a deep neural network model trained with a sample RGB image in a non-structured environment as input and pixel-level semantic classification annotations corresponding to the sample RGB image as labels.

[0035] Step 103: According to the passability cost map, use the MPPI algorithm to perform action sampling in the set motion space to obtain a sampling trajectory.

[0036] Step 104: Taking the passability of the unstructured environment and the distance to the end point as indicators, cost calculation is performed on the sampled trajectory, and based on minimizing the cost, an optimal action for passing through the unstructured environment is determined.

[0037] When executing steps 101-102, the specific steps may be as follows: A target RGB image is obtained, which corresponds to an RGB image in an unstructured scene; and the target RGB image is input into a trained passability cost map model to generate a passability cost map having the same size as the target RGB image.

[0038] Among them, the training of the trained passability cost map model is as follows: The passability estimation uses an RGB image as the original input (assuming that the image shape is 3×H×W, where 3 is the three channels of the three primary colors R, G, and B of the image, and H and W are the height and width of the image, respectively), and uses a deep neural network as the image feature extractor (for example, the ResUNet framework can be used). The final output of the neural network is a passability cost map with the same height and width as the RGB image (that is, the output shape is 1×H×W, corresponding to each pixel of the image, there is a corresponding passability cost value corresponding to it). In order to realize the training of the above network, a training set and an objective function need to be given.

[0039] The training set can be obtained by improving the existing unstructured environment semantic segmentation dataset (such as the RUGD dataset). The RUGD dataset contains a series of images in unstructured environments and the corresponding pixel-level semantic classification annotations of the images, and the passability costs corresponding to different semantic definitions (the higher the cost, the more difficult it is for ground unmanned platforms to pass through the area), thereby realizing the transformation of the dataset from a classification problem to a regression problem.

[0040] Specifically, the semantics in the pixel-level semantic classification annotation include: concrete road, asphalt road, gravel road, grass, dirt road, sand road, rock, rock bed, puddle, background, sign, obstacle, tree and building. The passability cost values ​​corresponding to different semantics include: The passability cost of concrete roads and asphalt roads is 0; the passability cost of gravel roads, grass, dirt roads, and sandy roads is 0.25; the passability cost of rocks and rock beds is 0.5; the passability cost of puddles is 0.75; the passability cost of backgrounds, signs, obstacles, trees, and buildings is 1. The details are shown in Table 1: Table 1 Passability costs corresponding to different semantics

[0041] It should be noted that the passability cost of the above obstacles / backgrounds is set to 1 to make it close to the passability cost of other categories to facilitate network training. However, in the subsequent motion control cost function, the cost of obstacles / backgrounds will be set to 1000 to prevent ground unmanned vehicles from entering these areas.

[0042] For the training function of the network, the L1 loss function is used here, which is defined as: .

[0043] Where n is the number of training samples, y i is the true passability proxy value, is the predicted passability cost.

[0044] In some embodiments, before executing step 103, the method further includes: Construct a vehicle dynamics model; the formula expression of the vehicle dynamics model is: .

[0045] in, are the vehicle's x , y Position and yaw angle, is the x position of the vehicle at time t+1, is the vehicle at time t+1 y Location, The time of vehicle t+1 x The yaw angle of the direction, For vehicles t time x The yaw angle of the direction, v t ,ω t are the linear velocity and angular velocity respectively, To control the time interval.

[0046] In some embodiments, when executing step 203, it specifically includes: According to the passability cost map, a strategy based on the MPPI algorithm is adopted to optimize the action sampling process in the motion space by combining the Numba library with the Nvidia GPU acceleration technology.

[0047] Specifically, the basic process of the MPPI algorithm is as follows: Assume that the system dynamics is described by the following continuous-time state-space equation: .

[0048] in x(t) is the state vector, u(t) is the control input vector, f is a nonlinear function of the system dynamics, w(t) is the process noise that follows a Gaussian distribution with a mean of 0 and a variance of Q.

[0049] The goal of MPPI is to minimize the cost function within a limited prediction horizon T. The cost function is defined as: .

[0050] in is a trajectory containing state and control input pairs, To run the cost function, used to evaluate the t instantaneous cost. is the final state cost function, evaluating the final state The cost.

[0051] To minimize the cost function, MPPI samples control trajectories from the control sequence of the noise perturbation distribution. Specifically, N control trajectories are generated by adding Gaussian noise: .

[0052] in The mean is 0 and the variance is ∑ Gaussian noise is sampled independently for each trajectory and each time step.

[0053] For each sampled trajectory , simulate the system dynamics and calculate the cost function , then, based on the cost function, the weight of each trajectory is calculated using the Boltzmann distribution: .

[0054] where w i is the weight of the ith trajectory. >0 is the temperature parameter that controls the sensitivity of the weight to cost.

[0055] The optimal control quantity is obtained by taking a weighted average of the sampled control trajectories: .

[0056] Therefore, MPPI is a control method in the receding horizon.

[0057] Among them, in the MPPI algorithm, it is necessary to sample the control sequence to obtain the optimal control with the goal of minimizing the cost function. Due to the independence of each control sequence sampling, this embodiment uses the Numba library in combination with the Nvidia GPU to accelerate the sampling process. Numba is a just-in-time compiler for Python, which converts a subset of Python and NumPy code into efficient machine code at runtime, thereby achieving significant performance improvements. The efficiency and speed of the MPPI algorithm are improved by utilizing the computing and parallel processing capabilities of the Nvidia GPU. Numba and Nvidia GPU parallel acceleration technology provide a fast solution for the sampling of a large number of control sequences and the calculation of corresponding costs in the MPPI algorithm, achieving more efficient motion planning and control in complex, dynamic and unstructured environments.

[0058] At each time step, the algorithm samples the control trajectory with Gaussian noise based on the current state, then simulates the system dynamics for each sampled trajectory and calculates the corresponding cost function. Finally, the weights are calculated based on these cost functions, the optimal control sequence is obtained using the weighted average, and the first control input is applied to the system.

[0059] In some embodiments, before calculating the cost of the sampled trajectory based on the passability of the unstructured environment and the distance to the end point as indicators and determining the optimal action for passing through the unstructured environment based on minimizing the cost, the method further includes: To deduce the position The passability cost at , requires using known camera parameters to convert the point in real world coordinates ( x , y ) to the image plane, including: Determine the intrinsic parameters of the camera as K ;in, f x and f y They are x and y The focal length in the direction, ( c x , c y ) is the intersection of the optical axis and the image plane.

[0060] According to the formula , project the point onto the image coordinates; where, h is the height from the ground to the camera.

[0061] According to the formula , convert image coordinates into pixel coordinates; where ( u p , v p ) is the pixel coordinate on the image plane, u , v , w are homogeneous coordinates.

[0062] After converting the real-world points into the image plane, the corresponding passability cost can be obtained according to the passability cost map .

[0063] In some embodiments, when executing step 104, the specific steps may be as follows: Determine the cost function for calculating the cost of the sampling trajectory, the cost function includes running the cost function and the final cost function .

[0064] Among them, the running cost function The formula expression is: .

[0065] The final state cost function The formula expression is: .

[0066] In the formula, The final target location. p t For vehicles at time t location. s default is the default speed used to estimate the time it will take the vehicle to reach its destination. is an indicator function, when any state x τ (0≤τ≤T ) reaches It returns 1 if the value is true, otherwise it returns 0. For location p t The cost of accessibility. w dist >0 and w tra >0 are the weights of penalizing the distance to the end point and the cost of passability.

[0067] Embodiment 2 like Figure 2 As shown, this embodiment provides a vision-based unmanned platform motion planning system in an unstructured environment, including: The image acquisition module 201 is used to acquire a target RGB image; the target RGB image is an RGB image in a non-structured scene.

[0068] The image conversion module 202 is used to input the target RGB image into the trained passability cost map model to obtain a passability cost map with the same height and width as the target RGB image; the trained passability cost map model is a deep neural network model trained with a sample RGB image in a non-structured environment as input and pixel-level semantic classification annotations corresponding to the sample RGB image as labels.

[0069] The trajectory sampling module 203 is used to perform action sampling in a set motion space according to the passability cost map and use the MPPI algorithm to obtain a sampling trajectory.

[0070] The calculation module 204 is used to calculate the cost of the sampled trajectory based on the passability of the unstructured environment and the distance to the end point as indicators, and determine the optimal action to pass through the unstructured environment based on minimizing the cost.

[0071] The technical features of the above embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0072] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A vision-based unmanned platform motion planning method in an unstructured environment, characterized in that: The unmanned platform motion planning method comprises: Acquire a target RGB image; the target RGB image is an RGB image in a non-structured scene; The target RGB image is input into a trained passability cost map model to obtain a passability cost map having the same height and width as the target RGB image; the trained passability cost map model is a deep neural network model trained by taking a sample RGB image in a non-structured environment as input and using pixel-level semantic classification annotations corresponding to the sample RGB image as labels; According to the passability cost map, using the MPPI algorithm, action sampling is performed in the set motion space to obtain a sampling trajectory; Taking the passability of the unstructured environment and the distance to the end point as indicators, the cost of the sampled trajectory is calculated, and based on minimizing the cost, the optimal action to pass through the unstructured environment is determined.

2. According to the vision-based unmanned platform motion planning method in an unstructured environment according to claim 1, it is characterized in that: The semantics in the pixel-level semantic classification annotation include: concrete road, asphalt road, gravel road, grass, dirt road, sand road, rock, rock bed, puddle, background, signboard, obstacle, tree and building.

3. The method for motion planning of an unmanned platform based on vision in an unstructured environment according to claim 2, characterized in that: The passability cost values ​​corresponding to different semantics include: The passability cost of concrete roads and asphalt roads is 0; the passability cost of gravel roads, grass, dirt roads, and sandy roads is 0.25; the passability cost of rocks and rock beds is 0.5; the passability cost of puddles is 0.75; the passability cost of backgrounds, signs, obstacles, trees, and buildings is 1.

4. The method for motion planning of an unmanned platform based on vision in an unstructured environment according to claim 1, characterized in that: The loss function used in the training of the passability cost graph model is L1 Loss function; L1 The formula of the loss function is: ； in, n is the number of training samples, y i is the true passability proxy value, is the predicted passability cost.

5. The vision-based unmanned platform motion planning method in an unstructured environment according to claim 1, characterized in that: Before sampling actions in a set motion space using the MPPI algorithm according to the passability cost map to obtain a sampling trajectory, the method further includes: Construct a vehicle dynamics model; the formula expression of the vehicle dynamics model is: ； in, For vehicles t Moment x , y Position and yaw angle, For vehicles t The x position at time +1, For vehicles t +1 time y Location, For vehicles t +1 Moment x The yaw angle of the direction, For vehicles t time x The yaw angle of the direction, v t ,ω t are the linear velocity and angular velocity respectively, To control the time interval.

6. The vision-based unmanned platform motion planning method in an unstructured environment according to claim 1, characterized in that: According to the passability cost map, the MPPI algorithm is used to perform action sampling in the set motion space to obtain a sampling trajectory, which specifically includes: According to the passability cost map, a strategy based on the MPPI algorithm is adopted to optimize the action sampling process in the motion space by combining the Numba library with the Nvidia GPU acceleration technology.

7. The vision-based unmanned platform motion planning method in an unstructured environment according to claim 1, characterized in that: Before calculating the cost of the sampled trajectory based on the passability of the unstructured environment and the distance to the end point as indicators and determining the optimal action for passing through the unstructured environment based on minimizing the cost, the method further includes: Transform a point (x, y) in real-world coordinates to the image plane using known camera parameters, including: Determine the intrinsic parameters of the camera as K ;in, f x and f y They are x and y The focal length in the direction, ( c x , c y ) is the intersection of the optical axis and the image plane; According to the formula , project the point onto the image coordinates; where, h is the height from the ground to the camera; According to the formula , convert image coordinates into pixel coordinates; where ( u p , v p ) is the pixel coordinate on the image plane, u , v , w are homogeneous coordinates.

8. The vision-based unmanned platform motion planning method in an unstructured environment according to claim 1, characterized in that: The cost function for calculating the cost of the sampled trajectory includes running the cost function and the final cost function .

9. The vision-based unmanned platform motion planning method in an unstructured environment according to claim 8, characterized in that: The running cost function The formula expression is: ; The final state cost function The formula expression is: ; in, is the final target position; p t For vehicles at time t location; s default is the default speed used to estimate the time it takes for the vehicle to reach the destination; is an indicator function. When any state x τ (0≤τ≤T) to reach It returns 1 when , otherwise it returns 0; For location p t The cost of accessibility; w dist >0 and w tra >0 are the weights of penalizing the distance to the end point and the cost of passability.

10. A vision-based unmanned platform motion planning system in an unstructured environment, characterized in that: include: An image acquisition module is used to acquire a target RGB image; The target RGB image is an RGB image in a non-structured scene; An image conversion module is used to input the target RGB image into a trained passability cost map model to obtain a passability cost map having the same height and width as the target RGB image; the trained passability cost map model is a deep neural network model trained by taking a sample RGB image in a non-structured environment as input and using pixel-level semantic classification annotations corresponding to the sample RGB image as labels; A trajectory sampling module, used to perform action sampling in a set motion space according to the passability cost map and use the MPPI algorithm to obtain a sampling trajectory; The calculation module is used to calculate the cost of the sampled trajectory based on the passability of the unstructured environment and the distance to the end point as indicators, and determine the optimal action to pass through the unstructured environment based on minimizing the cost.

Images