阅读说明：本技术 一种面向异构车辆的统一运动规划方法和系统 (Heterogeneous vehicle-oriented unified motion planning method and system ) 是由 王博洋 关海杰 陆瑶敏 龚建伟 陈慧岩 臧政 吕超 谭颖琦 于 2021-11-08 设计创作，主要内容包括：本发明涉及一种面向异构车辆的统一运动规划方法和系统。本发明在确定最优的行为基元的过程中,采用异构车辆当前时刻的与参考轨迹和期望速度的偏差代价、轨迹的平滑代价和碰撞风险代价,能够实现对转向机构不同所导致的运动特性差异进行控制,并在基于异构车辆当前时刻的与参考轨迹和期望速度的偏差代价、轨迹的平滑代价和碰撞风险代价选择出最优的行为基元之后,基于最优的行为基元得到异构车辆的运动规划结果,这就能够将转向机构不同所导致的运动特性差异融入到运动规划系统中,以显著提高异构车辆运动控制的精确性。(The invention relates to a unified motion planning method and a unified motion planning system for heterogeneous vehicles. In the process of determining the optimal behavior element, the deviation cost of the current moment of the heterogeneous vehicle from the reference track and the expected speed, the smooth cost of the track and the collision risk cost are adopted, the motion characteristic difference caused by different steering mechanisms can be controlled, the optimal behavior element is selected based on the deviation cost of the current moment of the heterogeneous vehicle from the reference track and the expected speed, the smooth cost of the track and the collision risk cost, the motion planning result of the heterogeneous vehicle is obtained based on the optimal behavior element, and the motion characteristic difference caused by different steering mechanisms can be integrated into a motion planning system, so that the accuracy of motion control of the heterogeneous vehicle is obviously improved.)

1. A unified motion planning method for heterogeneous vehicles is characterized by comprising the following steps:

splicing the extended primitives and the alternative primitives to obtain extended primitive clusters in the current-time passing area; the extended primitives are behavior primitives acquired at the last moment of the heterogeneous vehicle; the alternative primitives are behavior primitives selected from a heterogeneous vehicle motion primitive library;

in an online state, selecting an optimal behavior primitive based on the deviation cost of the heterogeneous vehicle from a reference track and an expected speed at the current moment, the smooth cost of the track and the collision risk cost by taking an extended primitive cluster as input;

and obtaining a motion planning result of the heterogeneous vehicle based on the optimal behavior primitive.

2. The unified motion planning method for heterogeneous vehicles according to claim 1, wherein in the online state, the optimal behavior primitive is selected based on the deviation cost from the reference trajectory and the expected speed, the smoothness cost of the trajectory and the collision risk cost of the heterogeneous vehicle at the current time by using the extended primitive cluster as an input, and specifically comprises:

determining a cost value of each behavior element in the extended element cluster based on deviation cost of the heterogeneous vehicle at the current moment and a reference track and an expected speed, and smoothness cost and collision risk cost of the track;

and selecting the behavior primitive with the lowest cost value as the optimal behavior primitive.

3. The unified motion planning method for heterogeneous vehicles according to claim 1, wherein the obtaining of the motion planning result for heterogeneous vehicles based on the optimal behavior primitives specifically comprises:

taking the optimal behavior primitive as a new extension primitive, and returning to the step of splicing the extension primitive and the alternative primitives to obtain an extension primitive cluster until generating a primitive sequence from a starting point to an ending point; the primitive sequence from the starting point to the ending point is composed of optimal behavior primitives at different moments;

and generating expected track time sequence points of the movement of the heterogeneous vehicles based on the primitive sequence from the starting point to the end point to obtain movement planning results of the heterogeneous vehicles.

4. The unified motion planning method for heterogeneous vehicles according to claim 1, wherein the expanding primitives and the alternative primitives are spliced to obtain an expanding primitive cluster in a current-time traffic area, and the method further comprises: constructing a heterogeneous vehicle motion element library; the behavior primitives in the heterogeneous vehicle motion primitive library comprise track points distributed at equal time intervals.

5. The unified motion planning method for heterogeneous vehicles according to claim 4, wherein the building of the heterogeneous vehicle motion primitive library specifically comprises:

setting differential behavior constraints of the heterogeneous vehicles, motion differential constraints of the heterogeneous vehicle platforms, inequality constraints of the heterogeneous vehicle platform characteristics, smooth transition constraints of the heterogeneous vehicle platform characteristics and an objective function generated by element optimization; the differentiated behavior constraints of the heterogeneous vehicles include: ackerman steered tracked vehicle behavior constraints, stepless differential speed steered tracked vehicle behavior constraints and stepped differential speed steered tracked vehicle behavior constraints;

solving an objective function generated by optimizing the primitive based on the differentiated behavior constraint of the heterogeneous vehicle, the motion differential constraint of the heterogeneous vehicle platform, the inequality constraint of the heterogeneous vehicle platform characteristic and the smooth transition constraint of the heterogeneous vehicle platform characteristic to obtain a behavior primitive under a specific behavior;

and traversing the behavior primitives under the differential behavior constraint of different heterogeneous vehicles to generate the heterogeneous vehicle motion primitive library.

6. A unified motion planning system for heterogeneous vehicles, comprising:

the element expansion module is used for splicing the expanded elements and the alternative elements to obtain an expanded element cluster in the current-time passing area; the extended primitives are behavior primitives acquired at the last moment of the heterogeneous vehicle; the alternative primitives are behavior primitives selected from a heterogeneous vehicle motion primitive library;

the element selection module is used for selecting an optimal behavior element based on the deviation cost of the current moment of the heterogeneous vehicle from a reference track and an expected speed, the smooth cost of the track and the collision risk cost by taking the extended element cluster as input in an online state;

and the motion planning module is used for obtaining a motion planning result of the heterogeneous vehicle based on the optimal behavior primitive.

7. The heterogeneous vehicle-oriented unified motion planning system of claim 6, wherein the primitive selection module comprises:

the cost value determining unit is used for determining a cost value of each behavior element in the extended element cluster based on the deviation cost of the heterogeneous vehicle at the current moment with the reference track and the expected speed, the smooth cost of the track and the collision risk cost;

and the element selection unit is used for selecting the behavior element with the lowest cost value as the optimal behavior element.

8. The heterogeneous vehicle-oriented unified motion planning system of claim 6, wherein the motion planning module comprises:

the primitive sequence generating unit is used for taking the optimal behavior primitive as a new extension primitive and returning to the step of splicing the extension primitive and the alternative primitives to obtain an extension primitive cluster until a primitive sequence from a starting point to a terminating point is generated; the primitive sequence from the starting point to the ending point is composed of optimal behavior primitives at different moments;

and the motion planning unit is used for generating expected track time sequence points of the motion of the heterogeneous vehicles based on the primitive sequence from the starting point to the ending point to obtain the motion planning result of the heterogeneous vehicles.

9. The heterogeneous vehicle-oriented unified motion planning system of claim 6, further comprising:

the motion element base building module is used for building a heterogeneous vehicle motion element base; the behavior primitives in the heterogeneous vehicle motion primitive library comprise track points distributed at equal time intervals.

10. The heterogeneous vehicle-oriented unified motion planning system of claim 9, wherein the motion primitive library building module comprises:

the device comprises a setting unit, a calculating unit and a calculating unit, wherein the setting unit is used for setting differential behavior constraint of the heterogeneous vehicles, motion differential constraint of the heterogeneous vehicle platforms, inequality constraint of the heterogeneous vehicle platform characteristics, smooth transition constraint of the heterogeneous vehicle platform characteristics and an objective function generated by element optimization; the differentiated behavior constraints of the heterogeneous vehicles include: ackerman steered tracked vehicle behavior constraints, stepless differential speed steered tracked vehicle behavior constraints and stepped differential speed steered tracked vehicle behavior constraints;

the behavior element determining unit is used for solving an objective function generated by element optimization based on differential behavior constraints of the heterogeneous vehicle, motion differential constraints of the heterogeneous vehicle platform, inequality constraints of the heterogeneous vehicle platform characteristics and smooth transition constraints of the heterogeneous vehicle platform characteristics to obtain behavior elements under specific behaviors;

and the motion element base generation unit is used for traversing the behavior elements under the constraint of the differential behaviors of different heterogeneous vehicles to generate the heterogeneous vehicle motion element base.

Technical Field

The invention relates to the technical field of unmanned vehicle motion planning, in particular to a unified motion planning method and system for heterogeneous vehicles.

Background

The motion planning method of the unmanned vehicle can generate a collision-free track from a starting pose to an ending pose in a passable area by combining the motion characteristic constraint of the vehicle, and is an important component of the unmanned system.

However, because different types of unmanned vehicles have significant differences in the composition of the steering mechanism, how to model the motion characteristic differences caused by the different steering mechanisms and integrate the motion characteristic differences into a motion planning system is an urgent problem to be solved in establishing a unified motion planning method for heterogeneous vehicles.

Disclosure of Invention

In order to solve the problems in the prior art, the invention provides a unified motion planning method and system for heterogeneous vehicles.

In order to achieve the purpose, the invention provides the following scheme:

a unified motion planning method for heterogeneous vehicles comprises the following steps:

splicing the extended primitives and the alternative primitives to obtain extended primitive clusters in the current-time passing area; the extended primitives are behavior primitives acquired at the last moment of the heterogeneous vehicle; the alternative primitives are behavior primitives selected from a heterogeneous vehicle motion primitive library;

in an online state, selecting an optimal behavior primitive based on the deviation cost of the heterogeneous vehicle from a reference track and an expected speed at the current moment, the smooth cost of the track and the collision risk cost by taking an extended primitive cluster as input;

and obtaining a motion planning result of the heterogeneous vehicle based on the optimal behavior primitive.

Preferably, in the online state, the selecting an optimal behavior primitive based on the deviation cost from the reference trajectory and the expected speed of the heterogeneous vehicle at the current time, the smoothing cost of the trajectory, and the collision risk cost by using the extended primitive cluster as an input specifically includes:

determining a cost value of each behavior element in the extended element cluster based on deviation cost of the heterogeneous vehicle at the current moment and a reference track and an expected speed, and smoothness cost and collision risk cost of the track;

and selecting the behavior primitive with the lowest cost value as the optimal behavior primitive.

Preferably, the obtaining of the motion planning result of the heterogeneous vehicle based on the optimal behavior primitive specifically includes:

taking the optimal behavior primitive as a new extension primitive, and returning to the step of splicing the extension primitive and the alternative primitives to obtain an extension primitive cluster until generating a primitive sequence from a starting point to an ending point; the primitive sequence from the starting point to the ending point is composed of optimal behavior primitives at different moments;

and generating expected track time sequence points of the movement of the heterogeneous vehicles based on the primitive sequence from the starting point to the end point to obtain movement planning results of the heterogeneous vehicles.

Preferably, the splicing the extended primitive and the alternative primitive to obtain the extended primitive cluster in the current-time passing area further includes: constructing a heterogeneous vehicle motion element library; the behavior primitives in the heterogeneous vehicle motion primitive library comprise track points distributed at equal time intervals.

Preferably, the building of the heterogeneous vehicle motion primitive library specifically includes:

setting differential behavior constraints of the heterogeneous vehicles, motion differential constraints of the heterogeneous vehicle platforms, inequality constraints of the heterogeneous vehicle platform characteristics, smooth transition constraints of the heterogeneous vehicle platform characteristics and an objective function generated by element optimization; the differentiated behavior constraints of the heterogeneous vehicles include: ackerman steered tracked vehicle behavior constraints, stepless differential speed steered tracked vehicle behavior constraints and stepped differential speed steered tracked vehicle behavior constraints;

solving an objective function generated by optimizing the primitive based on the differentiated behavior constraint of the heterogeneous vehicle, the motion differential constraint of the heterogeneous vehicle platform, the inequality constraint of the heterogeneous vehicle platform characteristic and the smooth transition constraint of the heterogeneous vehicle platform characteristic to obtain a behavior primitive under a specific behavior;

and traversing the behavior primitives under the differential behavior constraint of different heterogeneous vehicles to generate the heterogeneous vehicle motion primitive library.

According to the specific embodiment provided by the invention, the invention discloses the following technical effects:

according to the unified motion planning method for the heterogeneous vehicles, in the process of determining the optimal behavior primitive, the deviation cost between the current moment of the heterogeneous vehicles and the reference track and the expected speed, the smooth cost of the track and the collision risk cost are adopted, the motion characteristic difference caused by different steering mechanisms can be controlled, the optimal behavior primitive is selected based on the deviation cost between the current moment of the heterogeneous vehicles and the reference track and the expected speed, the smooth cost of the track and the collision risk cost, the motion planning result of the heterogeneous vehicles is obtained based on the optimal behavior primitive, and the motion characteristic difference caused by different steering mechanisms can be integrated into a motion planning system, so that the accuracy of motion control of the heterogeneous vehicles is obviously improved.

Corresponding to the unified motion planning method for the heterogeneous vehicles, the invention also provides a unified motion planning system for the heterogeneous vehicles, which comprises the following steps:

the element expansion module is used for splicing the expanded elements and the alternative elements to obtain an expanded element cluster in the current-time passing area; the extended primitives are behavior primitives acquired at the last moment of the heterogeneous vehicle; the alternative primitives are behavior primitives selected from a heterogeneous vehicle motion primitive library;

the element selection module is used for selecting an optimal behavior element based on the deviation cost of the current moment of the heterogeneous vehicle from a reference track and an expected speed, the smooth cost of the track and the collision risk cost by taking the extended element cluster as input in an online state;

and the motion planning module is used for obtaining a motion planning result of the heterogeneous vehicle based on the optimal behavior primitive.

Preferably, the primitive selection module comprises:

the cost value determining unit is used for determining a cost value of each behavior element in the extended element cluster based on the deviation cost of the heterogeneous vehicle at the current moment with the reference track and the expected speed, the smooth cost of the track and the collision risk cost;

and the element selection unit is used for selecting the behavior element with the lowest cost value as the optimal behavior element.

Preferably, the motion planning module comprises:

the primitive sequence generating unit is used for taking the optimal behavior primitive as a new extension primitive and returning to the step of splicing the extension primitive and the alternative primitives to obtain an extension primitive cluster until a primitive sequence from a starting point to a terminating point is generated; the primitive sequence from the starting point to the ending point is composed of optimal behavior primitives at different moments;

and the motion planning unit is used for generating expected track time sequence points of the motion of the heterogeneous vehicles based on the primitive sequence from the starting point to the ending point to obtain the motion planning result of the heterogeneous vehicles.

Preferably, the method further comprises the following steps:

the motion element base building module is used for building a heterogeneous vehicle motion element base; the behavior primitives in the heterogeneous vehicle motion primitive library comprise track points distributed at equal time intervals.

Preferably, the motion primitive library construction module includes:

the device comprises a setting unit, a calculating unit and a calculating unit, wherein the setting unit is used for setting differential behavior constraint of the heterogeneous vehicles, motion differential constraint of the heterogeneous vehicle platforms, inequality constraint of the heterogeneous vehicle platform characteristics, smooth transition constraint of the heterogeneous vehicle platform characteristics and an objective function generated by element optimization; the differentiated behavior constraints of the heterogeneous vehicles include: ackerman steered tracked vehicle behavior constraints, stepless differential speed steered tracked vehicle behavior constraints and stepped differential speed steered tracked vehicle behavior constraints;

the behavior element determining unit is used for solving an objective function generated by element optimization based on differential behavior constraints of the heterogeneous vehicle, motion differential constraints of the heterogeneous vehicle platform, inequality constraints of the heterogeneous vehicle platform characteristics and smooth transition constraints of the heterogeneous vehicle platform characteristics to obtain behavior elements under specific behaviors;

and the motion element base generation unit is used for traversing the behavior elements under the constraint of the differential behaviors of different heterogeneous vehicles to generate the heterogeneous vehicle motion element base.

The technical effect achieved by the unified motion planning system for the heterogeneous vehicles provided by the invention is the same as that achieved by the unified motion planning method for the heterogeneous vehicles provided by the invention, so that the technical effect is not repeated herein.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.

Fig. 1 is a flowchart of a unified motion planning method for heterogeneous vehicles according to the present invention;

FIG. 2 is a flowchart of an overall implementation of the unified movement planning method for heterogeneous vehicles according to the present invention;

FIG. 3 is a schematic diagram of a coordinate system of a heterogeneous vehicle platform according to an embodiment of the present invention;

FIG. 4 is a diagram illustrating the primitive expansion provided by the embodiment of the present invention;

fig. 5 is a schematic structural diagram of the unified motion planning system for heterogeneous vehicles according to the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

The invention aims to provide a unified motion planning method and a unified motion planning system for heterogeneous vehicles, which are used for modeling motion characteristic differences caused by different steering mechanisms and integrating the motion characteristic differences into a motion planning system so as to improve the accuracy of motion control of the heterogeneous vehicles.

In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.

As shown in fig. 1 and fig. 2, the unified motion planning method for heterogeneous vehicles according to the present invention includes:

step 100: and splicing the extended primitives and the alternative primitives to obtain an extended primitive cluster in the current-time passing area. The extended primitives are behavior primitives acquired at the last moment of the heterogeneous vehicle. The alternative primitives are selected behavior primitives in the heterogeneous vehicle motion primitive library. When the position is at the initial position, the primitive cluster with the initial speed of 0 is directly placed without completing splicing with the initial position. The extension of the primitives is shown in figure 4.

And (3) finishing the generation of the speed in the transition section by adopting three times of Hermite interpolation, and constructing the transition between the primitives by variable-speed linear motion. The specific form is as follows:

in the formula:l ₀=(t-t ₁)/(t ₀-t ₁)，l ₁=(t-t ₀)/(t ₁-t ₀) Since the starting and ending point accelerations to be associated with the transition primitives are all zero, i.e.v _n And = 0. Thus passing throughv(t) derivation to obtain a function of the acceleration of the transition elementa(t) the function is int _a=(t ₁-t ₀) The maximum value is obtained at the position of/2, and the value at the position of the maximum value is shown as the following formula:

in the formula: deltav=v ₁ -v ₀Is the speed deviation of the primitive to be transited, and t is obtained by solving₁The speed values of all points of the whole transition section can be generated to form the transition section of the variable speed linear element.

Step 101: in an online state, an extended primitive cluster is used as input, and an optimal behavior primitive is selected based on the deviation cost of the current time of the heterogeneous vehicle from a reference track and an expected speed, the smooth cost of the track and the collision risk cost. The implementation process of the step can be as follows:

and determining a cost value of each behavior element in the extended element cluster based on the deviation cost of the heterogeneous vehicle at the current moment from the reference track and the expected speed, the smooth cost of the track and the collision risk cost.

And selecting the behavior primitive with the lowest cost value as the optimal behavior primitive.

Step 102: and obtaining a motion planning result of the heterogeneous vehicle based on the optimal behavior primitive. The implementation process of the step can be as follows:

and taking the optimal behavior primitive as a new extension primitive, and returning to the step of splicing the extension primitive and the alternative primitives to obtain an extension primitive cluster until generating a primitive sequence from a starting point to an ending point. The primitive sequence from the start point to the end point is composed of optimal behavior primitives at different time instants.

And generating expected track time sequence points of the movement of the heterogeneous vehicles based on the primitive sequence from the starting point to the ending point to obtain movement planning results of the heterogeneous vehicles.

The specific implementation process of the steps 101 and 102 is as follows:

1) the cost of deviation from the reference trajectory and the desired speed:

in the formula:N _p is the total number of equally spaced sampling points on the generated trace,pis a function of the gaussian function and,j _n is the cost of the deviation between the generated trajectory sample point and the reference line match point.

In the formula: gaussian functionpIs a standard normal distribution, omega_dAnd omega_hRespectively the weight coefficients of the distance deviation and the heading deviation,d _n is the deviation of the distance, Δ, from the reference lineθ _n Is the heading deviation from the reference line.

In the formula:v _r is the desired velocity at the reference line match point,v _end is the velocity at the end of the cell.

2) Smoothness penalty of the trajectory:

the smoothness of the track directly reflects the average change of the curvature of each point on the element, the smaller the value is, the smoother the curvature is, and the definition of the smoothness of the track is shown as the following formula:

（23）

in the formula:is the curvature value, Δ, at the primitive sample points _n Is the spacing between adjacent sample points.The weight coefficients are set according to the primitive types and the vehicle platform types. Weighting factor for stepless steering vehicles (including Ackerman steering wheeled vehicles and speed difference steering tracked vehicles)Is defined as shown in equation (24), and the weight coefficient of the step-steered tracked vehicleIs as defined in formula (25).

（24）

（25）

In the formula:the subscript M of (a) indicates a direction maintenance, R indicates a direction reversal, the superscript s indicates a prescribed turning radius, and g indicates a non-prescribed turning radius.

S33, collision risk cost:

the vehicle body is represented by the approximate six-covered circle, the distance from the obstacle to the center of the circle is calculated to serve as an index of collision risk cost, and a specific calculation formula is as follows:

（26）

in the formula:is the closest distance between the center of the circle and the obstacle,r _l is the radius of the vehicle body covering circle.

S34, optimizing and selecting the primitives:

the online selection of the primitive needs to comprehensively evaluate the defined selection cost, and the total cost of primitive selection is shown as the following formula:

in the formula:ω _p，ω _v ，ω _s，ω _cis the weight coefficient of the corresponding cost index. And the online selection module selects the primitive with the lowest cost from the primitive cluster based on the calculated cost value, and gradually expands and generates a primitive sequence from a starting point to an end point to form a final expected track time sequence point.

In order to further improve the accuracy of motion planning, the construction process of the heterogeneous vehicle motion element library adopted by the invention comprises the following steps:

under an off-line state, based on differential behavior constraint of heterogeneous vehicles, motion differential constraint of vehicle platforms, inequality constraint of vehicle platform characteristics and smooth transition constraint of elements, smoothness of a track is taken as an optimization target to obtain heterogeneous motion element libraries of three types of vehicle platforms, and an alternative set is provided for expansion and selection of subsequent motion elements.

In the optimized generation of the motion primitive, the definition and parameters of the vehicle platform coordinate system are defined as follows:

the state parameters of the wheeled Ackermann steering vehicle and the crawler type differential speed steering vehicle are respectively as follows:

wherein, the schematic diagram of the ackermann steering vehicle platform and the differential steering vehicle platform is shown in fig. 3. Wherein x and y are position coordinates in a global coordinate system,θis the angle of the course of the vehicle,andrespectively, velocity and acceleration along the x-axis of the body coordinate system.δAndω _δ are the front wheel slip angle and angular rate of a wheeled ackermann steered vehicle. DeltavAndω _vΔthe speed difference and the corresponding change rate of the crawler belts on the two sides.

Andthe control quantities of the wheeled vehicle platform and the tracked vehicle platform, respectively, wherein,is the jerk along the x-axis of the vehicle body,α _δ is thatω _δ The rate of change of (a) is,α _vΔis thatω _vΔRate of change of t₁And t_gAre the start and end times of the primitive.

1) And setting differential behavior constraints of the heterogeneous vehicles according to the structural characteristics of the steering mechanisms of the three heterogeneous vehicles.

The heterogeneous vehicles related to the invention mainly comprise three types, namely ackerman steering wheeled vehicles, stepless speed difference steering tracked vehicles and stepped speed difference steering tracked vehicles. Typical driving behaviors include: straight driving, turning around, U-shaped bending, right-angled bending, lane changing and the like. Each behavior category corresponds to a set of set starting and ending point poses.

Taking the turning behavior as an example, the differentiated behavior constraints of three different types of vehicle platforms are as follows:

the behavioral constraint of Ackerman steered tracked vehicles isThe behavior constraint of the stepless speed difference steering crawler vehicle isThe behavior constraint of the step-steered tracked vehicle is。

The behavior constraint of the step-steered tracked vehicle isFirstly, setting starting point and ending point constraints of a starting pose and an ending pose. Secondly, willAs a starting and ending state constraint of the Dubins curve, the Dubins curve is generated by combining a fixed radius arc with a straight line, wherein the value of the fixed radius arc is set to be the same as the specified steering radius of the step-steered tracked vehicle. Finally, selecting Dubins curveN _s Behavior constraint of step steering tracked vehicle formed by sampling pointsWherein the starting point is removedb ₁And end pointThe position and the course are required to be constrained, and other sampling points are required to be constrainedb _n Only the position is constrained. As shown in fig. 4.

2) Respectively setting motion differential constraints of the heterogeneous vehicle platforms, wherein the constraint settings of the step/stepless speed difference steering tracked vehicle are consistentThe restriction of the Ackerman steered wheeled vehicle is。

WhereinLIs the distance between the front axle and the rear axle of the wheeled vehicle,Bis the spacing between the tracks on both sides.WhereinAndis the speed of the left and right side tracks.

3) Respectively setting inequality constraints of the characteristics of the heterogeneous vehicle platforms, wherein the constraints of the step/stepless speed difference steering tracked vehicle are set consistentlyU _t The restriction of the Ackerman steered wheeled vehicle isUw。

4) Respectively setting smooth transition constraints of the characteristics of the heterogeneous vehicle platforms, wherein the constraints of the step/stepless speed difference steering tracked vehicle are set to be consistentT _t The restriction of the Ackerman steered wheeled vehicle isT _w。

5) And setting an objective function generated by element optimization, and solving the optimization problem by combining the constraint conditions to obtain a motion element library which highlights the unique motion characteristics of the heterogeneous vehicle. Wherein the optimal target settings of the step/stepless speed difference steering tracked vehicle are consistentg _tThe restriction of the Ackerman steered wheeled vehicle isg _w。

In the formula:c ₁,c ₂,c ₃,c ₄,c ₅are the corresponding weight coefficients. The smaller the value of the objective function, the smoother the generated trajectory.

Thus, the solution of the optimization problem is that ofg _t/wVehicle control quantity when minimum value is takenuAnd the constraint condition needs to be satisfied in the solving processf、B、U、T。

6) And 1) through the steps of 1) to 5), behavior primitives under a specific behavior can be obtained, and each behavior primitive comprises track points distributed at equal time intervals of 0.1 s. Traversing different behavioral constraintsBThen, a heterogeneous vehicle motion primitive library containing a plurality of behavior patterns is generated.

Corresponding to the unified motion planning method for heterogeneous vehicles provided above, the present invention further provides a unified motion planning system for heterogeneous vehicles, as shown in fig. 5, the system includes: a primitive extension module 1, a primitive selection module 2 and a motion planning module 3.

The primitive extension module 1 is configured to splice the extension primitives and the alternative primitives to obtain an extension primitive cluster in the current-time traffic area. The extended primitives are behavior primitives acquired at the last moment of the heterogeneous vehicle. The alternative primitives are selected behavior primitives in the heterogeneous vehicle motion primitive library.

The element selection module 2 is used for selecting an optimal behavior element based on the deviation cost of the current time of the heterogeneous vehicle from the reference track and the expected speed, the smooth cost of the track and the collision risk cost by taking the extended element cluster as input in an online state.

And the motion planning module 3 is used for obtaining a motion planning result of the heterogeneous vehicle based on the optimal behavior primitive.

Wherein, the element selection module 2 comprises: a cost value determining unit and a primitive selecting unit.

The cost value determining unit is used for determining the cost value of each behavior element in the extended element cluster based on the deviation cost of the heterogeneous vehicle at the current moment from the reference track and the expected speed, the smooth cost of the track and the collision risk cost.

The element selection unit is used for selecting the behavior element with the lowest cost value as the optimal behavior element.

The movement planning module 3 comprises: a primitive sequence generating unit and a motion planning unit.

The element sequence generating unit is used for taking the optimal behavior element as a new extension element, and returning to the step of splicing the extension element and the alternative element to obtain an extension element cluster until an element sequence from a starting point to a terminating point is generated. The primitive sequence from the start point to the end point is composed of optimal behavior primitives at different time instants.

The movement planning unit is used for generating expected track time sequence points of movement of the heterogeneous vehicles based on the primitive sequence from the starting point to the end point, and obtaining movement planning results of the heterogeneous vehicles.

The unified motion planning system for heterogeneous vehicles provided by the invention further comprises: and a motion element library construction module.

The motion element base building module is used for building a heterogeneous vehicle motion element base. The behavior primitives in the heterogeneous vehicle motion primitive library comprise track points distributed at equal time intervals.

The motion primitive library construction module may include: the device comprises a setting unit, a behavior primitive determining unit and a motion primitive library generating unit.

The setting unit is used for setting differential behavior constraints of the heterogeneous vehicles, motion differential constraints of the heterogeneous vehicle platforms, inequality constraints of the heterogeneous vehicle platform characteristics, smooth transition constraints of the heterogeneous vehicle platform characteristics and an objective function generated by element optimization. The differentiated behavior constraints of heterogeneous vehicles include: ackerman steered tracked vehicle behavior constraints, stepless differential steered tracked vehicle behavior constraints and stepped differential steered tracked vehicle behavior constraints.

The behavior element determining unit is used for solving an objective function generated by element optimization based on differential behavior constraints of the heterogeneous vehicles, motion differential constraints of the heterogeneous vehicle platforms, inequality constraints of the heterogeneous vehicle platform characteristics and smooth transition constraints of the heterogeneous vehicle platform characteristics to obtain behavior elements under specific behaviors.

The motion element base generation unit is used for traversing the behavior elements under the constraint of the differential behaviors of different heterogeneous vehicles to generate a heterogeneous vehicle motion element base.

The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.

The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.