A control method and system for tracked vehicles based on semantic information series connection

Abstract

The present invention relates to a control method and system for a tracked vehicle based on semantic information series connection. The method comprises fusing laser radar point cloud information and image semantic information, and determining a three-dimensional semantic grid map according to the fused information, a global topological reference path, and geometric passability parameters of the tracked vehicle; a motion planning module splices and transitions motion primitives according to the three-dimensional semantic grid map, the global topological reference path, and the extended cost of motion primitives in the motion primitive cluster; a motion control module uses a trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on a model predictive control algorithm according to the updated control objective function and the trajectory with behavioral semantic information under the mechanical constraints of the actuator, the kinematic constraints of the platform, and the dynamic constraints of the platform. The present invention can improve the passability of ground unmanned mobile platforms in off-road environments.

Description

Technical Field

[0001] The present invention relates to the field of vehicle control, and in particular to a control method and system for a tracked vehicle based on semantic information serial connection. Background Art

[0002] Ground unmanned dynamic tracked vehicles can replace humans in off-road environments to complete tasks such as dangerous material transportation, disaster relief, battlefield environment reconnaissance, etc., reducing or avoiding the burden and consumption of humans in off-road dangerous environments. Ground unmanned dynamic tracked vehicles are generally composed of four basic modules: environmental perception, motion planning, motion control, and chassis execution.

[0003] Previous studies have carried out corresponding work on each module in the ground unmanned aerial tracked vehicle, but the series connection between the modules is relatively weak, making the output information between the modules of the ground unmanned aerial tracked vehicle relatively simple, which in turn causes the ground unmanned aerial tracked vehicle to have a relatively low ability to pass in off-road environments.

[0004] Therefore, in order to solve the above problems, it is urgent to provide a new control method or system to improve the navigation capability of ground unmanned aerial vehicle platforms in off-road environments. Summary of the invention

[0005] The purpose of the present invention is to provide a control method and system for tracked vehicles based on semantic information serial connection, which can improve the traffic ability of ground unmanned mobile platforms in off-road environments.

[0006] To achieve the above object, the present invention provides the following solutions:

[0007] A control method for a tracked vehicle based on semantic information serial connection, comprising:

[0008] Extracting lidar point cloud information based on lidar data acquired by the environment perception module; and extracting image semantic information based on camera images acquired by the environment perception module;

[0009] The laser radar point cloud information and the image semantic information are fused, and a three-dimensional semantic grid map is determined based on the fused information, the global topological reference path, and the geometric passability parameters of the tracked vehicle; the three-dimensional semantic grid map is a semantic grid map with local passability; the geometric passability parameters include: maximum slope, vertical wall span height, and maximum cross slope angle for overturning;

[0010] The motion planning module splices and transitions the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavioral semantic information; the motion primitive cluster includes: motion primitives formed by sampling different target postures; the motion primitive is the trajectory of the tracked vehicle from the starting posture to the target posture within the set time, under the kinematic constraints, smooth transition constraints, platform-terrain constraints and platform behavior constraints of the tracked vehicle; the extended cost includes: the lateral and longitudinal deviations of the motion primitive from the global topological reference path, the collision risk cost, the smoothness cost and the passability cost;

[0011] The motion control module uses the trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on the model predictive control algorithm according to the updated control objective function and the trajectory with behavioral semantic information under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform; the control objective function is constructed according to the lateral deviation, heading deviation and control value change rate of the trajectory tracking control.

[0012] Optionally, fusing the laser radar point cloud information and the image semantic information, and determining the three-dimensional semantic grid map according to the fused information, the global topological reference path, and the geometric passability parameters of the tracked vehicle, specifically includes:

[0013] Through the coordinate transformation relationship between the laser radar and the camera in the environment perception module, the image semantic information is projected onto the laser radar point cloud information to determine the laser radar semantic point cloud; the laser radar semantic point cloud is the fused information;

[0014] Perform the first rough extraction of the traversable area based on the LiDAR semantic point cloud;

[0015] Extract point cloud features based on the semantic point cloud information of the laser radar after the first rough extraction; the point cloud features include: road roughness information, longitudinal slope and transverse slope;

[0016] The second rough extraction of the passable area is performed based on the point cloud features and geometric passability parameters;

[0017] According to the point cloud features of the lidar semantic point cloud information after the second rough extraction, the Gaussian mixture clustering method is used to determine the passability of the position corresponding to the point cloud features; then according to the global topological reference path, the octree and Bayesian kernel reasoning are used to determine the three-dimensional semantic grid map.

[0018] Optionally, the motion planning module splices and transitions the motion primitives in the motion primitive library according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive library to determine the trajectory with behavior semantic information, specifically including:

[0019] Determine a motion primitive cluster according to a reference speed of the tracked vehicle and the passability of a passable area in front of the tracked vehicle;

[0020] Determine the optimal extended motion primitive according to the extended costs of the motion primitives in the motion primitive cluster;

[0021] All optimally extended motion primitives are spliced ​​and transitioned to determine trajectories with behavioral semantic information.

[0022] Optionally, the motion control module uses the trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on the model predictive control algorithm under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform according to the updated control objective function and the trajectory with behavioral semantic information, and the method also includes:

[0023] Obtaining a kinematic model of a tracked vehicle;

[0024] The control objective function is constructed according to the lateral deviation, heading deviation and control variable change rate of trajectory tracking control.

[0025] A control system for a tracked vehicle based on semantic information serial connection, comprising:

[0026] An information acquisition unit, used to extract lidar point cloud information based on the lidar data acquired by the environment perception module; and to extract image semantic information based on the camera image acquired by the environment perception module;

[0027] A three-dimensional semantic grid map determination unit is used to fuse the laser radar point cloud information and the image semantic information, and determine the three-dimensional semantic grid map according to the fused information, the global topological reference path and the geometric passability parameters of the tracked vehicle; the three-dimensional semantic grid map is a semantic grid map with local passability; the geometric passability parameters include: maximum slope, vertical wall span height and maximum cross slope angle for overturning;

[0028] The trajectory determination unit with behavior semantic information is used for the motion planning module to splice and transition the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavior semantic information; the motion primitive cluster includes: motion primitives formed by sampling different target postures; the motion primitive is the trajectory of the tracked vehicle from the starting posture to the target posture within a set time, under the kinematic constraints, smooth transition constraints, platform-terrain constraints and platform behavior constraints of the tracked vehicle; the extended cost includes: the lateral and longitudinal deviations between the motion primitive and the global topological reference path, the collision risk cost, the smoothness cost and the passability cost;

[0029] The control unit of a tracked vehicle is used for a motion control module to update the weight coefficient of a control objective function in real time using a trajectory with behavioral semantic information, and to control the tracked vehicle based on a model predictive control algorithm under the mechanical constraints of an actuator, the kinematic constraints of a platform, and the dynamic constraints of a platform according to the updated control objective function and the trajectory with behavioral semantic information; the control objective function is constructed according to the lateral deviation, heading deviation, and control amount change rate of trajectory tracking control.

[0030] Optionally, the three-dimensional semantic grid map determining unit specifically includes:

[0031] The LiDAR semantic point cloud determination subunit is used to project the image semantic information onto the LiDAR point cloud information through the coordinate conversion relationship between the LiDAR and the camera in the environment perception module, and determine the LiDAR semantic point cloud; the LiDAR semantic point cloud is the fused information;

[0032] The first rough extraction subunit is used to perform the first rough extraction of the traversable area according to the lidar semantic point cloud;

[0033] The point cloud feature extraction subunit is used to extract point cloud features based on the semantic point cloud information of the lidar after the first rough extraction; the point cloud features include: road roughness information, longitudinal slope and transverse slope;

[0034] The second rough extraction subunit is used to perform the second rough extraction of the passable area according to the point cloud features and the geometric passability parameters;

[0035] The three-dimensional semantic grid map determination subunit is used to determine the passability of the position corresponding to the point cloud feature based on the point cloud features of the lidar semantic point cloud information after the second rough extraction, using the Gaussian mixture clustering method; and then determine the three-dimensional semantic grid map based on the global topological reference path, using the octree and Bayesian kernel reasoning.

[0036] Optionally, the trajectory determination unit with behavior semantic information specifically includes:

[0037] A motion primitive cluster determination subunit, used for determining a motion primitive cluster according to a reference speed of the tracked vehicle and a passability of a passable area in front of the tracked vehicle;

[0038] An optimally extended motion primitive determining subunit, used to determine the optimally extended motion primitive according to the extension costs of the motion primitives in the motion primitive cluster;

[0039] The trajectory determination subunit with behavioral semantic information is used to splice and transition all the optimally extended motion primitives to determine the trajectory with behavioral semantic information.

[0040] Optionally, it also includes:

[0041] A kinematic model determination unit, used for obtaining a kinematic model of a tracked vehicle;

[0042] The control objective function construction unit is used to construct the control objective function according to the lateral deviation, heading deviation and control amount change rate of trajectory tracking control.

[0043] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0044] The present invention provides a control method and system for tracked vehicles based on semantic information series connection. The environment perception module uses the fusion perception method of camera and laser radar, uses image semantic information and laser radar point cloud features, and combines platform geometric passability parameters to determine the three-dimensional semantic grid map; compared with the plane binary grid map, it can provide more environmental passability information. The motion planning module splices and transitions the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path, and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavioral semantic information; in the motion planning, the platform posture and the influence of platform stability are more considered, which improves the stability of the unmanned system in complex off-road environments. The motion control module uses the trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on the model predictive control algorithm under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform according to the updated control objective function and the trajectory with behavioral semantic information; that is, the environmental perception module, motion planning module and motion control module are connected in series through semantic information, which enriches the output information between the unmanned driving system modules and improves the passability of the ground unmanned driving platform in off-road environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the embodiments of the present invention 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 invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0046] Figure 1 A schematic flow chart of a control method for a tracked vehicle based on semantic information serial connection provided by the present invention;

[0047] Figure 2 It is a schematic diagram of the tracked vehicle framework based on the connection of semantic information;

[0048] Figure 3 It is a three-dimensional accessibility semantic grid map;

[0049] Figure 4 This is the effect diagram of projecting image semantic information onto the lidar point cloud information;

[0050] Figure 5 This is a schematic diagram of the longitudinal forces on a tracked vehicle;

[0051] Figure 6 This is a schematic diagram of the lateral forces on a tracked vehicle;

[0052] Figure 7 A simplified diagram of the process of a tracked vehicle crossing a vertical wall;

[0053] Figure 8 It is a schematic diagram of the horizontal coordinate system of the vehicle body;

[0054] Fig. 9 This is a schematic diagram of the control parameter semantic matching principle;

[0055] Fig.10 This is a schematic diagram of the control system structure of a tracked vehicle based on semantic information series connection provided by the present invention. DETAILED DESCRIPTION

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

[0057] The purpose of the present invention is to provide a control method and system for tracked vehicles based on semantic information serial connection, which can improve the traffic ability of ground unmanned mobile platforms in off-road environments.

[0058] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0059] Figure 1 A flow chart of a control method for a tracked vehicle based on semantic information serial connection provided by the present invention. Figure 2 It is a schematic diagram of the tracked vehicle framework based on semantic information concatenation, such as Figure 1 and Figure 2 As shown, the present invention provides a control method for a tracked vehicle based on semantic information series connection, comprising:

[0060] S101, extracting laser radar point cloud information based on the laser radar data obtained by the environment perception module; and extracting image semantic information based on the camera image obtained by the environment perception module; the laser radar point cloud information includes obstacles, undulating terrain, and surface roughness;

[0061] S102, fusing the laser radar point cloud information and the image semantic information, and determining a three-dimensional semantic grid map based on the fused information, the global topological reference path, and the geometric passability parameters of the tracked vehicle, and Figure 3 As shown; the three-dimensional semantic grid map is a semantic grid map with local passability; the geometric passability parameters include: maximum slope, vertical wall span height and maximum cross slope angle for overturning;

[0062] First, the BiSeNet V2 network is trained on the dataset to extract image features to complete the semantic segmentation of the input image. In the categories of accessible semantics, such as dirt roads and grass, the image segmentation results are then converted into semantic point clouds based on the sensor coordinate system transformation relationship. The relative position of the lidar and the camera can be expressed as a rotation matrix and translation vector According to formula (1), point P in the laser radar coordinate system can be l Project to the camera coordinate system to get point P c , and then transformed from the camera coordinate system to the image pixel coordinate system through formula (2). Formula (2) is the pinhole camera model of camera imaging, which includes a series of transformations from the camera coordinate system to the image physical coordinate system and the image pixel coordinate system, where (u, v) is the pixel coordinate value in the image pixel coordinate system, d x , d y Respectively represent the physical size of a single pixel in the U-axis and V-axis directions, u0, v0 represent the pixel translation from the origin of the image physical coordinate system to the origin of the image pixel coordinate system, f is the focal length of the camera, (x c ,y c , z c ) is the midpoint P of the camera coordinate system cThe coordinate value of .

[0063]

[0064]

[0065] S102 specifically includes:

[0066] Through the coordinate transformation relationship between the laser radar and the camera in the environment perception module, the image semantic information is projected onto the laser radar point cloud information to determine the laser radar semantic point cloud; the laser radar semantic point cloud is the fused information; points within a certain distance and height range are selected from the point cloud and projected onto the image and covered on the original pixel. The color of the pixel of the projected point is set to the category color corresponding to the pixel position, and the same classification information is given, such as Figure 3 As shown in the figure, the left side is the original point cloud, and the right side is the original image. That is, the inaccessible semantic categories such as vehicles and bushes are directly assigned as inaccessible areas, and then the lidar semantic point cloud is used to roughly extract the accessible areas.

[0067] Perform the first rough extraction of the traversable area based on the LiDAR semantic point cloud;

[0068] Extract point cloud features based on the semantic point cloud information of the laser radar after the first rough extraction; the point cloud features include: road roughness information, longitudinal slope and transverse slope;

[0069] The second rough extraction of the passable area is performed based on the point cloud features and geometric passability parameters;

[0070] According to the point cloud features of the lidar semantic point cloud information after the second rough extraction, the Gaussian mixture clustering method is used to determine the passability of the position corresponding to the point cloud features; then according to the global topological reference path, the octree and Bayesian kernel reasoning are used to determine the three-dimensional semantic grid map.

[0071] The process of determining the geometric passability parameters is as follows:

[0072] Considering the longitudinal stability of the platform (tracked vehicle) without overturning, its limit uphill angle α1 and limit downhill angle α2 can be obtained by equations (3) and (4). The longitudinal force diagram of the platform is as follows: Figure 5 shown.

[0073]

[0074]

[0075] In the formula, l f h is the distance from the front road wheel landing point to the platform center of mass; g is the height of the platform center of mass; L is the ground contact length of the crawler track.

[0076] Considering the longitudinal stability of the platform without sliding down, the limit sliding angle α′ can be obtained by formula (5):

[0077]

[0078] In the formula, It is the maximum adhesion coefficient of the ground and is related to the ground properties.

[0079] Therefore, the longitudinal uphill and downhill stability angles of the platform are and

[0080] In addition, platforms of different sizes should have different considerations for the continuous length of the longitudinal slope. In this article, it is believed that the longitudinal slope length should be greater than or equal to the ground contact length of the crawler track.

[0081] Considering the lateral stability of the platform without overturning, the lateral force diagram of the platform is as follows: Figure 6 In the figure, β is the cross slope angle, B is the track center distance, b is the track ground width, e is the distance that the center of mass deviates from the longitudinal plane, and F z1 , F z2 is the vertical reaction force of the ground, F y1 , F y2 is the lateral reaction force of the ground, and G is the gravity of the platform.

[0082] When the platform begins to overturn on the horizontal slope, the gravity line passes through the outer edge of the left track, so the platform's limit horizontal slope angle β is:

[0083]

[0084] Considering the lateral stability of the platform without side slip, the limit glide angle β′ can be obtained by formula (7):

[0085]

[0086] In the formula, It is the lateral adhesion coefficient of the ground and is related to the ground properties.

[0087] Therefore, the lateral stability angle of the platform is β max =min(β, β′).

[0088] Platforms of different sizes should also have different considerations for the continuous length of the cross slope. It is believed that the cross slope length should be greater than or equal to the center distance of the crawler track.

[0089] When a tracked vehicle crosses a vertical wall, it is divided into four stages: the front track contacts the wall edge, the front road wheel rises to the wall edge, the center of gravity moves to a position that coincides with the vertical wall edge line, and the top plane of the vertical wall is gently contacted. The process is shown in the following figure. Figure 7 shown.

[0090] For the first stage, when the front track contacts the wall edge, if you want to pass through this vertical wall, you have:

[0091] H1≤h d -r d (8)

[0092] Where H1 is the height of the vertical wall; h d is the distance between the center of the driving wheel and the ground; r d is the radius of the driving wheel.

[0093] For the third stage, when the center of gravity moves to a position that coincides with the vertical wall edge line, we have:

[0094] H2=(Ll f -h g tanδ)sinδ (9)

[0095] Where δ is the track inclination angle at this time, When the height of the vertical wall continues to increase, the center of gravity of the platform can never coincide with the vertical wall edge line, that is, it cannot pass through the vertical wall.

[0096] Therefore, the maximum height of the vertical wall that the platform can cross is H max =min(H1,H2).

[0097] The point cloud feature extraction process is as follows:

[0098] Consider a query point P in the point cloud q , whose neighborhood is represented by (m is the number of points in the neighborhood), then point P q The local descriptor is defined as The definitions of the elements are shown in formula (10) and formula (11).

[0099]

[0100]

[0101] In the formula, is the normal vector of each point, which can be obtained by calculating the eigenvector corresponding to the minimum eigenvalue of the covariance matrix constructed by the neighborhood points; They are the unit vectors of the three axes of the vehicle horizontal coordinate system. The vehicle horizontal coordinate system OXYZ is obtained by correcting the vehicle coordinate system O′X′Y′Z′ by the platform pitch angle and roll angle. The right side of the platform is the positive direction of the X′ axis, the front is the positive direction of the Y′ axis, and the upward direction is the positive direction of the Z′ axis. The schematic diagram is as follows Figure 8 . By summing the normal vectors of the points in the neighborhood of the query point, we get provides global orientation information of the local surface, while The local roughness is evaluated and can be interpreted as a local negative roughness index.

[0102] Based on the inhomogeneous point local descriptor, the local feature vector of the point cloud is further extracted. The features are explained as follows:

[0103] The mean of the modulus of the sum of the normal vectors of the query point and its neighborhood points can represent the roughness information of the road.

[0104]

[0105] The normal vector of the query point and its neighborhood points and the angle between the projection of the yoz plane in the horizontal coordinate system of the vehicle body and the Z axis represent the longitudinal slope.

[0106]

[0107] The normal vector of the query point and its neighborhood points and the angle between the projection of the xoz plane in the horizontal coordinate system of the vehicle body and the Z axis represent the slope of the cross slope.

[0108]

[0109] The clustering model is trained by using the Gaussian mixture clustering method (GMM), and the input point cloud is estimated online in real time and divided into three categories, namely easy to pass, passable and difficult to pass.

[0110] In the process of extracting point cloud features, the point cloud posture is first corrected using the platform's onboard inertial navigation, and then all point cloud features are calculated in the platform's horizontal coordinate system. All environmental terrain features are judged based on the platform's current position.

[0111] The Gaussian clustering model L(D) of point cloud features is based on the point cloud sample set D = {p1, p2, ...p m} is the m points after filtering, and the local feature vector of each point is The goal of Gaussian mixture clustering is to maximize the log-likelihood function, as shown in equation (15).

[0112]

[0113] Among them, m is the number of point cloud samples, is a Gaussian mixture distribution, k is the number of passable degree labels, so μ i ,∑ i is the mean vector and covariance matrix of the i-th Gaussian mixture component, and α i is the prior probability value of each Gaussian component.

[0114] Among them, the adjacent area selects the area occupied by the platform geometry. The feature vector is normalized as shown in formula (16), and multivariate clustering is performed using GMM to obtain three types of passability labels: easy to pass, passable, and difficult to pass.

[0115]

[0116] In the actual online perception process, the feature vector of the point cloud is input into the GMM model to obtain the passability of the corresponding environment of the current point cloud.

[0117] Finally, octree data is used to manage map data, and raster attribute reasoning based on Bayesian kernel reasoning is used to model the passability attributes of the three-dimensional grid, so as to infer the attributes of the grid in the missing part of the point cloud to achieve continuous three-dimensional mapping.

[0118] S103, the motion planning module splices and transitions the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavioral semantic information; the motion primitive cluster includes: motion primitives formed by sampling different target postures; the motion primitive is the trajectory of the tracked vehicle from the starting posture to the target posture within a set time, under the kinematic constraints, smooth transition constraints, platform-terrain constraints and platform behavior constraints of the tracked vehicle; the extended cost includes: the lateral and longitudinal deviations of the motion primitive from the global topological reference path, the collision risk cost, the smoothness cost and the passability cost;

[0119] S103 is based on the optimal control theory to generate two-dimensional motion primitives and online selection of motion primitives based on three-dimensional semantic grid maps and global reference paths. By projecting two-dimensional motion primitives into a three-dimensional environment, the final output is a trajectory with three-dimensional posture information.

[0120] T=[s x ,s y ,s z ,s yaw ,s pitch ,s roll ,s v ,s type ].

[0121] Among them, s x ,s y ,s z is the three-dimensional coordinate of the trajectory point in the local three-dimensional coordinate system of the platform, s yaw ,s pitch ,s roll is the rotation angle of the trajectory point around the local three-dimensional coordinate system z, y, x axis of the platform, s vis the expected velocity at the trajectory point, s type Is the type of motion primitive corresponding to the trajectory point.

[0122] S103 specifically includes:

[0123] Determine a motion primitive cluster according to a reference speed of the tracked vehicle and the passability of a passable area in front of the tracked vehicle;

[0124] Determine the optimal extended motion primitive according to the extended costs of the motion primitives in the motion primitive cluster;

[0125] All optimally extended motion primitives are spliced ​​and transitioned to determine trajectories with behavioral semantic information.

[0126] Before S103, the following step also includes: building an offline motion primitive library, and the specific building process is as follows:

[0127] The generation of motion primitives takes into account the basic kinematic constraints of tracked vehicles, smooth transition constraints, platform-terrain constraints, and platform behavior constraints. The goal is to generate motion primitives as smooth as possible, and generate a motion primitive library for online planning offline. The optimization problem for generating motion primitives is defined as Equation (17).

[0128]

[0129] Where f is the kinematic constraint of the tracked vehicle; g is the objective function that considers the smoothness of the generated motion primitives in the path curvature and velocity layer; B is the behavioral constraint that distinguishes the motion primitives; U e Consider the inequality constraints between the platform and the terrain on the control quantity and the state quantity; U p is the inequality constraint considering the mechanical structure and kinematic constraints of the platform; T is the constraint considering the transition of motion primitive splicing. The optimized control quantity is u(t) = [η x (t), α Δv (t)] T , η x (t) is the impact degree of the platform in the longitudinal direction, α Δv (t) is the derivative of the rate of change of the speed difference; the state parameter of the platform

[0130] s(t)=[x(t),y(t),θ(t),v x (t), a x (t), Δv(t), ω Δv (t)] T , where x(t), y(t) and θ(t) are the position coordinates and heading angle in the global coordinate system, v x (t) is the longitudinal velocity along the x-axis of the platform coordinate system, a x(t) is the corresponding acceleration, Δv(t) is the speed difference between the two sides of the track, ω Δv (t) is the rate of change of the speed difference between the two sides of the track; t1 is the starting time of primitive generation, t g It is the termination time of primitive generation.

[0131] The kinematic differential constraint f of the dual-side independently driven unmanned tracked vehicle is as shown in formula (18).

[0132]

[0133] Among them, B is the center distance of the track contact points on both sides, L is the track contact length, and Δv is the speed difference between the left and right sides of the track.

[0134] The platform behavior constraints mainly restrict the starting and target states of the generated motion primitives. Common behaviors include straight-line driving, right-angle turning, U-shaped turning with a large angle, U-turn, double lane change, uphill and downhill, etc. The constraints of the starting and target states are shown in formula (19).

[0135]

[0136] Among them, d is the lateral movement distance of the first and last postures, and β is the angular difference between the first and last postures.

[0137] In the process of motion primitive generation, due to the mechanical structure constraints of the platform actuator and the relationship between the platform and the terrain, the inequality constraints need to be satisfied in the process of motion primitive generation as shown in formula (20).

[0138]

[0139] Among them, v min 、v max 、a max , η max are the upper and lower limits of the platform longitudinal parameters, Δv max , are the upper and lower limits of the platform's lateral parameters. Using the steering dynamics model of the tracked vehicle and the external characteristic curve of the drive motor in the chassis drive module, the values ​​of the upper and lower limits under different working conditions can be obtained.

[0140] Since the final output trajectory of motion planning is composed of a sequence of motion primitives, the smooth transition of the lateral and longitudinal parameters needs to be considered when generating primitives offline. The constraint form is as shown in formula (21).

[0141]

[0142] The objective function generated by the motion primitive takes into account the smoothness of the horizontal and vertical parameters from both horizontal and vertical aspects. The discretized objective function is expressed as shown in formula (22).

[0143]

[0144] Among them, c1, c2, c3, c4, and c5 are the weight coefficients of each parameter in the objective function.

[0145] The global planning in this paper adopts the environmental topology map made based on the Google Earth satellite map. On the basis of the environmental topology map, A* search is used to complete the global planning of passing through the necessary points and output a global reference trajectory with good guidance effect.

[0146] Therefore, the trajectory points on the selected motion primitive need to have the smallest possible lateral deviation, heading deviation, and velocity deviation from the reference trajectory matching points. The cost calculation is shown in formula (23).

[0147]

[0148] Among them, N p is the total number of equally spaced sampling points on the motion primitive, p is the normal distribution function, j n It is the cost of the horizontal and vertical deviation between the generated trajectory sampling point and the reference line matching point.

[0149] ω d ,ω h ,ω v They are the coefficients of lateral deviation, heading angle deviation and speed deviation respectively.

[0150] The calculation of the collision risk cost first approximates the outer structure of the ground unmanned platform using the covering circle method, and uses the difference between the distance from the center of the covering circle to the nearest obstacle and the radius of the covering circle as the collision risk cost of the motion primitive, as shown in Formula (24).

[0151]

[0152] The smoothness evaluation of the motion primitive adopts the trajectory energy function of the comprehensive curvature and length. The smaller the value, the lower the energy of the motion primitive and the smoother the trajectory. The cost value is calculated as shown in formula (25).

[0153]

[0154] Among them, κ n is the curvature value at the primitive sampling point, Δs n is the spacing between adjacent sampling points.

[0155] Under the three-dimensional semantic grid map provided by environmental perception, the calculation of the passability cost of the motion primitive includes two aspects: on the one hand, the angle between the normal vector of the fitted plane of the platform shape structure area near the grid and the vertical direction of the motion primitive trajectory point matching, and on the other hand, the average of the passability cost of the matching area. The calculation method of the fitted plane normal vector is shown in formula (26).

[0156]

[0157] in: and cov is the coordinate mean and covariance matrix of all grids in the platform size area near the matching grid; eig represents the eigenvalue of the matrix; λ is the normal vector of the fitting plane. The final cost is calculated as shown in formula (27).

[0158]

[0159] Where: p it is the passability of the grid, and I is the vertical z-axis unit vector.

[0160] The final selection of motion primitives is a comprehensive evaluation of multiple targets, and the selection cost is calculated as shown in formula (28).

[0161]

[0162] Among them, ω s ,ω t ,ω c is the weight coefficient of the corresponding cost.

[0163] The principle of online primitive selection is to select the motion primitive with the smallest total cost, and to expand forward by selecting the primitive. When the final state of the motion primitive is in the planned end area, the expansion is completed to form the final three-dimensional trajectory with spatiotemporal information.

[0164] In the selection process, the transition primitives between adjacent motion primitives are variable speed linear motions, and cubic Hermite interpolation is used to complete the generation of the velocity in the transition section.

[0165] S104, the motion control module uses the trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on the model predictive control algorithm according to the updated control objective function and the trajectory with behavioral semantic information under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform; the control objective function is constructed according to the lateral deviation, heading deviation and control amount change rate of the trajectory tracking control.

[0166] Before S104, it also included:

[0167] Obtaining a kinematic model of a tracked vehicle;

[0168] The speed control method is used to perform trajectory tracking control, and the control quantity is the speed of the left and right tracks, that is, The state quantity is the kinematic model of the platform as shown in formula (29).

[0169]

[0170] Among them, X, Y are the coordinates of the platform in the global coordinate system, θ is the heading angle of the platform, and y l ,y r They are the ordinates of the ground contact points of the left and right tracks of the platform in the platform coordinate system, They are the winding speeds of the left and right tracks of the platform respectively.

[0171] The control objective function is constructed based on the lateral deviation, heading deviation and control amount change rate of trajectory tracking control. The trajectory composed of motion primitives includes semantic information such as road slope, road roughness, curvature characteristics, etc. The above information provides more reference information for motion control model constraints and objective function design. In the process of establishing the model prediction trajectory tracking control model, the mechanical constraints of the actuator, the kinematic constraints of the platform and the platform dynamic constraints are mainly considered. According to the road type, curvature characteristics, platform speed and other information, the dynamics of the motor external characteristics and the ramp steering of the tracked vehicle are analyzed, and the constraints on the backup power of the drive system are added to ensure the stability of the track tracking of the tracked vehicle under extreme working conditions. When the platform performs low-speed and uniform steering on the ramp, the platform posture and planned speed on the reference trajectory are combined to predict the power and torque required for the vehicle to track on the ramp. Combined with the boundary value of the motor external characteristics, the control amount of the vehicle is constrained so that the platform can perform stable trajectory tracking on the ramp. Finally, the constraint inequality as shown in formula (30) is established.

[0172]

[0173] Among them, u min 、u max The constraint solving of is mainly to combine the external characteristics of the motor, the road ramp information, the vehicle dynamics model, etc., and obtain the vehicle speed boundary value under this working condition under the premise of ensuring that the vehicle has sufficient backup power and steering torque. min , Δu max N is the change rate constraint of the drive motor speed regulation. p , N c They are the prediction time domain and the control time domain respectively. b It is the constraint output of the state quantity.

[0174] The motion control module based on the model predictive control method tracks different semantically classified trajectories using multiple sets of control objective function weight coefficients according to the reference trajectories with semantic classification composed of behavioral motion primitives provided by the motion planning module.

[0175] According to the control objective function as shown in formula (31), the trajectory tracking stability with the lateral deviation, heading deviation and control variable change rate as indicators of trajectory tracking control is constructed to evaluate the optimal trajectory tracking control accuracy. In this way, the influence of different control objective function weight coefficients and different coefficient ratios on the final trajectory tracking control effect is obtained, and the control strategy data set under different semantic trajectories is established.

[0176]

[0177] Among them, l e is the lateral deviation; h e is the heading deviation; v d is the expected speed value; the last two terms are the change rates of the track winding speeds on both sides; ω1 is the weight coefficient of the lateral deviation; ω h is the weight coefficient of heading deviation; ω v is the weight coefficient of speed deviation; ω u is the weight coefficient of the control variable change rate; k is the time in the prediction time domain;

[0178] Through the accumulation and analysis of experimental data, within the feasible domain where the platform can perform stable trajectory tracking control, the trajectory tracking effect of the platform is mainly affected by the lateral tracking weight ω l and the heading tracking weight ω h At the same time, the proportional relationship between the two weight coefficients has a great difference in the performance of the platform under different semantic trajectories.

[0179] like Fig. 9 As shown, when the platform tracks a reference trajectory with semantic classification, it uses different control parameters on trajectories with different semantic attributes to improve the accuracy and stability of trajectory tracking.

[0180] Fig.10 A schematic diagram of the control system structure of a tracked vehicle based on semantic information series connection provided by the present invention is shown in FIG. Fig.10 As shown, the control system of a tracked vehicle based on semantic information series connection provided by the present invention comprises:

[0181] The information acquisition unit 1001 is used to extract the laser radar point cloud information according to the laser radar data acquired by the environment perception module; and to extract the image semantic information according to the camera image acquired by the environment perception module;

[0182] The three-dimensional semantic grid map determination unit 1002 is used to fuse the laser radar point cloud information and the image semantic information, and determine the three-dimensional semantic grid map according to the fused information, the global topological reference path and the geometric passability parameters of the tracked vehicle; the three-dimensional semantic grid map is a semantic grid map with local passability; the geometric passability parameters include: maximum slope, vertical wall span height and maximum cross slope angle for overturning;

[0183] The trajectory determination unit 1003 with behavior semantic information is used for the motion planning module to splice and transition the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavior semantic information; the motion primitive cluster includes: motion primitives formed by sampling different target postures; the motion primitive is the trajectory of the tracked vehicle from the starting posture to the target posture within a set time, under the kinematic constraints, smooth transition constraints, platform-terrain constraints and platform behavior constraints of the tracked vehicle; the extended cost includes: the lateral and longitudinal deviations between the motion primitive and the global topological reference path, the collision risk cost, the smoothness cost and the passability cost;

[0184] The control unit 1004 of the tracked vehicle is used for the motion control module to update the weight coefficient of the control objective function in real time using the trajectory with behavioral semantic information, and to control the tracked vehicle based on the model predictive control algorithm under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform according to the updated control objective function and the trajectory with behavioral semantic information; the control objective function is constructed according to the lateral deviation, heading deviation and control amount change rate of the trajectory tracking control.

[0185] The three-dimensional semantic grid map determining unit 1002 specifically includes:

[0186] The LiDAR semantic point cloud determination subunit is used to project the image semantic information onto the LiDAR point cloud information through the coordinate conversion relationship between the LiDAR and the camera in the environment perception module, and determine the LiDAR semantic point cloud; the LiDAR semantic point cloud is the fused information;

[0187] The first rough extraction subunit is used to perform the first rough extraction of the traversable area according to the lidar semantic point cloud;

[0188] The point cloud feature extraction subunit is used to extract point cloud features based on the semantic point cloud information of the lidar after the first rough extraction; the point cloud features include: road roughness information, longitudinal slope and transverse slope;

[0189] The second rough extraction subunit is used to perform the second rough extraction of the passable area according to the point cloud features and the geometric passability parameters;

[0190] The three-dimensional semantic grid map determination subunit is used to determine the passability of the position corresponding to the point cloud feature based on the point cloud features of the lidar semantic point cloud information after the second rough extraction, using the Gaussian mixture clustering method; and then determine the three-dimensional semantic grid map based on the global topological reference path, using the octree and Bayesian kernel reasoning.

[0191] The trajectory determination unit 1003 with behavior semantic information specifically includes:

[0192] A motion primitive cluster determination subunit, used for determining a motion primitive cluster according to a reference speed of the tracked vehicle and a passability of a passable area in front of the tracked vehicle;

[0193] An optimally extended motion primitive determining subunit, used to determine the optimally extended motion primitive according to the extension costs of the motion primitives in the motion primitive cluster;

[0194] The trajectory determination subunit with behavioral semantic information is used to splice and transition all the optimally extended motion primitives to determine the trajectory with behavioral semantic information.

[0195] The control system of a tracked vehicle based on semantic information series connection provided by the present invention further includes:

[0196] A kinematic model determination unit, used for obtaining a kinematic model of a tracked vehicle;

[0197] The control objective function construction unit is used to construct the control objective function according to the lateral deviation, heading deviation and control amount change rate of trajectory tracking control.

[0198] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0199] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, 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 the present invention.

Claims

1. A control method for a tracked vehicle based on semantic information serial connection, characterized in that: include: Extracting lidar point cloud information based on lidar data acquired by the environment perception module; And extract image semantic information based on the camera image obtained by the environment perception module; The LiDAR point cloud information and image semantic information are fused, and a three-dimensional semantic grid map is determined based on the fused information, the global topological reference path, and the geometric passability parameters of the tracked vehicle. The three-dimensional semantic grid map is a semantic grid map with local passability; the geometric passability parameters include: maximum slope, vertical wall span height and maximum cross slope angle for overturning; The motion planning module splices and transitions the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavioral semantic information; the motion primitive cluster includes: motion primitives formed by sampling different target postures; the motion primitive is the trajectory of the tracked vehicle from the starting posture to the target posture within the set time, under the kinematic constraints, smooth transition constraints, platform-terrain constraints and platform behavior constraints of the tracked vehicle; the extended cost includes: the lateral and longitudinal deviations of the motion primitive from the global topological reference path, the collision risk cost, the smoothness cost and the passability cost; The motion control module uses the trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on the model predictive control algorithm under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform according to the updated control objective function and the trajectory with behavioral semantic information; the control objective function is constructed according to the lateral deviation, heading deviation and control amount change rate of the trajectory tracking control; The laser radar point cloud information and the image semantic information are fused, and a three-dimensional semantic grid map is determined according to the fused information, the global topological reference path, and the geometric passability parameters of the tracked vehicle, specifically including: Through the coordinate transformation relationship between the laser radar and the camera in the environment perception module, the image semantic information is projected onto the laser radar point cloud information to determine the laser radar semantic point cloud; the laser radar semantic point cloud is the fused information; Perform the first rough extraction of the traversable area based on the LiDAR semantic point cloud; Extract point cloud features based on the semantic point cloud information of the laser radar after the first rough extraction; the point cloud features include: road roughness information, longitudinal slope and transverse slope; The second rough extraction of the passable area is performed based on the point cloud features and geometric passability parameters; According to the point cloud features of the LiDAR semantic point cloud information after the second rough extraction, the Gaussian mixture clustering method is used to determine the passability of the corresponding position of the point cloud features; then, according to the global topological reference path, the octree and Bayesian kernel reasoning are used to determine the three-dimensional semantic grid map; The motion planning module splices and transitions the motion primitives in the motion primitive library according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive library to determine the trajectory with behavior semantic information, specifically including: Determine a motion primitive cluster according to a reference speed of the tracked vehicle and the passability of a passable area in front of the tracked vehicle; Determine the optimal extended motion primitive according to the extended costs of the motion primitives in the motion primitive cluster; All optimally extended motion primitives are spliced ​​and transitioned to determine trajectories with behavioral semantic information; When generating primitives offline, the smooth transition of the horizontal and vertical parameters is considered, and the constraint form T t for: Among them, η x (t1) and η x (t g ) are the starting time t1 and the ending time t2 of primitive generation respectively. g The impact degree of the platform in the longitudinal direction, ωΔv(t1) and ωΔv(t g ) are the starting time t1 and the ending time t2 of primitive generation respectively. g The speed difference of the tracks on both sides, ω Δv (t1) and ω Δv (t g ) are the starting time t1 and the ending time t2 of primitive generation respectively. g The rate of change of the speed difference between the two sides of the track, α Δv (t1) and α Δv (t g ) are the starting time t1 and the ending time t2 of primitive generation respectively. g The derivative of the rate of change of the speed difference, a x (t1) and a x (t g ) are the starting time t1 and the ending time t2 of primitive generation respectively. g The corresponding acceleration, the superscript T is the transpose; The discretized objective function g is: in, is the discretized corresponding acceleration, is the discretized impact of the platform in the longitudinal direction, Δv i is the discretized speed difference between the two tracks, is the discretized rate of change of the track speed difference on both sides, is the derivative of the discretized speed difference change rate, N is the number of motion primitives, i is the i-th motion primitive; c1, c2, c3, c4, c5 are the weight coefficients of each parameter in the objective function; Adopt the environmental topology map made based on Google Earth satellite map, and use A* search on the basis of the environmental topology map to complete the global planning of passing through the necessary points, and output the global reference trajectory with good guidance effect; The cost J between the trajectory point and the reference trajectory matching point in lateral deviation, heading deviation, and speed deviation o Calculated as: j n =ω d d n +oh h Dth n +oh v △v Among them, N p is the total number of equally spaced sampling points on the motion primitive, n is the equally spaced sampling points on the nth motion primitive, p() is the normal distribution function, e is the base of the natural logarithm, π is the circumference, μ is the mean, σ is the standard deviation, x is the random value, j n is the cost of the horizontal and vertical deviation between the generated trajectory sampling point and the reference line matching point; ω d ,ω h ,ω v They are the coefficients of lateral deviation, heading angle deviation and speed deviation respectively; The calculation of the collision risk cost is to first approximate the outer structure of the ground unmanned platform using the covering circle method, and use the difference between the distance from the center of the covering circle to the nearest obstacle and the radius of the covering circle as the collision risk cost J of the motion primitive. c : Among them, d obs_i is the distance from the center of the 6-covering circle to the nearest obstacle, r i is the radius of the covering circle; The smoothness of the motion primitive is evaluated by using the trajectory energy function that combines curvature and length, and the cost J s Calculated as: Among them, κ n is the curvature value at the nth primitive sampling point, κ n-1 is the curvature value at the n-1th primitive sampling point, Δs n is the spacing between adjacent sampling points; Under the three-dimensional semantic grid map provided by environmental perception, the calculation of the passability cost of the motion primitive includes two aspects. On the one hand, the angle between the normal vector of the fitted plane of the platform shape structure size area near the grid that matches the trajectory point of the motion primitive and the vertical direction, and on the other hand, the average of the passability cost of the matching area; the calculation method of the fitted plane normal vector is: in: and cov is the coordinates p of all grids in the platform size area near the matching grid i mean and covariance matrix; eig means finding the eigenvalue of the matrix; λ is the normal vector of the fitting plane; finally the cost J t The calculation is: Where: p it is the passability of the grid, I is the vertical z-axis unit vector; The final selection of motion primitives is a comprehensive evaluation of multiple targets, and the selection cost J(mp) is calculated as: Among them, ω s ,ω t ,ω c is the weight coefficient of the corresponding cost, mp is the multi-objective; The principle of online primitive selection is to select the motion primitive with the smallest total cost, and then expand forward by selecting the primitive. When the final state of the motion primitive is in the planned end area, the expansion is completed to form the final three-dimensional trajectory with spatiotemporal information. The final constraint inequality established is: Among them, u min 、u max is the vehicle speed boundary value, u(k+i) and u(k+i-1) are the vehicle speeds at time k+i and time k+i-1, Δu min , Δu max is the change rate constraint of the drive motor speed regulation, Δu(k+i) is the change rate of the drive motor speed regulation, N p , N c are respectively the prediction time domain and the control time domain, y b (k+i) is the state output, y min (k+i) and y max (k+i) are state quantity input constraints respectively; The control objective function J(ξ(k),u(k)) is: Among them, l e is the lateral deviation; h e is the heading deviation; v d is the expected speed value; the last two terms are the change rates of the track winding speeds on both sides; ω1 is the weight coefficient of the lateral deviation; ω h is the weight coefficient of heading deviation; ω v is the weight coefficient of speed deviation; ω u is the weight coefficient of the control variable change rate; k is the time in the prediction time domain.

2. The control method of a tracked vehicle based on semantic information series connection according to claim 1, characterized in that: The motion control module uses the trajectory with behavioral semantic information to update the weight coefficient of the control objective function in real time, and controls the tracked vehicle based on the model predictive control algorithm under the mechanical constraints of the actuator, the kinematic constraints of the platform and the dynamic constraints of the platform according to the updated control objective function and the trajectory with behavioral semantic information, which also includes: Obtaining a kinematic model of a tracked vehicle; The control objective function is constructed according to the lateral deviation, heading deviation and control variable change rate of trajectory tracking control.

3. A control system for a tracked vehicle based on semantic information series connection, applied to a control method for a tracked vehicle based on semantic information series connection as claimed in any one of claims 1 to 2, characterized in that: The control system comprises: An information acquisition unit, used to extract lidar point cloud information based on the lidar data acquired by the environment perception module; and to extract image semantic information based on the camera image acquired by the environment perception module; A three-dimensional semantic grid map determination unit is used to fuse the laser radar point cloud information and the image semantic information, and determine the three-dimensional semantic grid map according to the fused information, the global topological reference path and the geometric passability parameters of the tracked vehicle; the three-dimensional semantic grid map is a semantic grid map with local passability; the geometric passability parameters include: maximum slope, vertical wall span height and maximum cross slope angle for overturning; The trajectory determination unit with behavior semantic information is used for the motion planning module to splice and transition the motion primitives in the motion primitive cluster according to the three-dimensional semantic grid map, the global topological reference path and the extended cost of the motion primitives in the motion primitive cluster to determine the trajectory with behavior semantic information; the motion primitive cluster includes: motion primitives formed by sampling different target postures; the motion primitive is the trajectory of the tracked vehicle from the starting posture to the target posture within a set time, under the kinematic constraints, smooth transition constraints, platform-terrain constraints and platform behavior constraints of the tracked vehicle; the extended cost includes: the lateral and longitudinal deviations between the motion primitive and the global topological reference path, the collision risk cost, the smoothness cost and the passability cost; The control unit of a tracked vehicle is used for a motion control module to update the weight coefficient of a control objective function in real time using a trajectory with behavioral semantic information, and to control the tracked vehicle based on a model predictive control algorithm under the mechanical constraints of an actuator, the kinematic constraints of a platform, and the dynamic constraints of a platform according to the updated control objective function and the trajectory with behavioral semantic information; the control objective function is constructed according to the lateral deviation, heading deviation, and control amount change rate of trajectory tracking control.

4. A control system for a tracked vehicle based on semantic information serial connection according to claim 3, characterized in that: The three-dimensional semantic grid map determination unit specifically includes: The LiDAR semantic point cloud determination subunit is used to project the image semantic information onto the LiDAR point cloud information through the coordinate conversion relationship between the LiDAR and the camera in the environment perception module, and determine the LiDAR semantic point cloud; the LiDAR semantic point cloud is the fused information; The first rough extraction subunit is used to perform the first rough extraction of the traversable area according to the lidar semantic point cloud; The point cloud feature extraction subunit is used to extract point cloud features based on the semantic point cloud information of the lidar after the first rough extraction; the point cloud features include: road roughness information, longitudinal slope and transverse slope; The second rough extraction subunit is used to perform the second rough extraction of the passable area according to the point cloud features and the geometric passability parameters; The three-dimensional semantic grid map determination subunit is used to determine the passability of the position corresponding to the point cloud feature based on the point cloud features of the lidar semantic point cloud information after the second rough extraction, using the Gaussian mixture clustering method; and then determine the three-dimensional semantic grid map based on the global topological reference path, using the octree and Bayesian kernel reasoning.

5. A control system for a tracked vehicle based on semantic information serial connection according to claim 4, characterized in that: The trajectory determination unit with behavior semantic information specifically includes: A motion primitive cluster determination subunit, used for determining a motion primitive cluster according to a reference speed of the tracked vehicle and a passability of a passable area in front of the tracked vehicle; An optimally extended motion primitive determining subunit, used to determine the optimally extended motion primitive according to the extension costs of the motion primitives in the motion primitive cluster; The trajectory determination subunit with behavioral semantic information is used to splice and transition all the optimally extended motion primitives to determine the trajectory with behavioral semantic information.

6. The control system of a tracked vehicle based on semantic information series connection according to claim 3, characterized in that: Also includes: A kinematic model determination unit, used for obtaining a kinematic model of a tracked vehicle; The control objective function construction unit is used to construct the control objective function according to the lateral deviation, heading deviation and control amount change rate of trajectory tracking control.

Images