CN102591332A - Device and method for local path planning of pilotless automobile - Google Patents

Abstract

The present invention is a device and method for local path planning of unmanned vehicles. Obstacles, establishing a road boundary model and a road centerline model; the repulsion calculation device establishes a repulsion point function and calculates repulsion; the gravitational calculation device establishes a gravitational point function and calculates gravitation; the resultant force direction angle calculation device calculates the direction angle of the resultant force of repulsion and gravitation; The steering wheel angle calculating device determines the steering wheel angle according to the direction angle of the resultant force and the transmission ratio of the steering system. This method not only eliminates the problem of falling into local minimum and path oscillation caused by the repulsive force and gravitational force in the same direction in the artificial potential field method, but also can correct the deviation of the driving path caused by the interference of uncertain factors in real time.

Description

Apparatus and method for local path planning of unmanned vehicles

technical field

The invention belongs to the technical field of smart cars, and in particular relates to a device and method for unmanned driving local path planning.

Background technique

The driverless vehicle system (Autonomous Ground Vehicle referred to as AGV) is an intelligent control system that obtains environmental information, vehicle status, and location based on various sensors, and automatically controls the driving behavior of the vehicle through understanding the environment. It is mainly composed of sensors, processors, controller and other devices.

Local path planning is one of the key technologies in driverless vehicle research. Local path planning refers to: in an uncertain road environment, the control system plans the current driving of the vehicle in real time according to the information provided by the environmental perception system and the vehicle state detection system, and the goals to be achieved provided by the global path planning. path.

The artificial potential field method is a relatively mature and real-time planning method in the research of local path planning. It abstracts the environmental information of vehicles into gravitational field functions and repulsive field functions, and plans a route from the starting point through the resultant force field function. A collision-free path to a gravitational point (goal point).

Contents of the invention

The object of the present invention is to provide a device and method for local path planning of an unmanned vehicle, which calculates the driving path of the unmanned vehicle according to the environmental characteristics without establishing a complex environment model.

For achieving above object, the solution that the present invention adopts is:

A device for local path planning of an unmanned vehicle, comprising:

Environmental sensing device, used to detect obstacles, establish road boundary model and road centerline model;

A repulsion calculation device, used to establish a repulsion point function and calculate the repulsion;

A gravitational calculation device, used to establish the gravitational point function and calculate the gravitational force;

The resultant force direction angle calculation device is used to calculate the direction angle of the resultant force of repulsion and attraction;

The steering wheel angle calculation device is used to determine the steering wheel angle according to the direction angle of the resultant force and the transmission ratio of the steering system.

Further, the environment perception device is a visual sensor and a radar, the visual sensor detects the road boundary and calculates the road centerline, the radar detects obstacle information, and the road coordinate point information given by the visual sensor is fitted into a multiple curve to establish a road Boundary model and road centerline model.

The environment sensing device is a radar, which detects roadsides, fits road boundary information, calculates road centerline information, and establishes a road boundary model and a road centerline model.

The repulsion calculation device determines the two road boundaries on which the vehicle will travel to establish a repulsion point function according to the real-time information of the environment given by the environment sensing device; Position and calculate the repulsive force of the repulsive point to the unmanned vehicle.

The gravity calculation device calculates the lateral distance between the road centerline with the new boundary line as the road centerline and the road centerline of the road where the vehicle is currently located through the deviation calculation device, and at the same time, whether there are obstacles or obstacles in the road through the Gaussian membership combination function. The information of the distance between the object and the driverless car is reflected in the gravitational point function in real time; and according to the gravitational point function, the position of the gravitational point of the driverless car is calculated as the environment changes, and the gravitational force of the gravitational point on the driverless car is calculated. .

A method for local path planning of an unmanned vehicle, comprising:

Environmental awareness step, detecting obstacles, establishing road boundary model and road centerline model;

The repulsion calculation step is to establish the repulsion point function and calculate the repulsion;

The gravitational calculation step is to establish the gravitational point function and calculate the gravitational force;

The resultant force direction angle calculation step is to calculate the direction angle of the resultant force of the repulsive force and the gravitational force;

In the step of calculating the steering wheel angle, the steering wheel angle is determined according to the direction angle of the resultant force and the transmission ratio of the steering system.

Further, the environmental perception step is to detect the road boundary by the visual sensor and calculate the road centerline, detect the obstacle information by the radar, fit the road coordinate point information given by the visual sensor into a multiple curve, and establish the road boundary model and the road center wire model.

The environmental perception step is to detect the roadside by radar, fit the road boundary information, calculate the road centerline information, and establish the road boundary model and the road centerline model.

The repulsion calculation step determines the boundaries of the two roads on which the vehicle will travel to establish the repulsion point function according to the real-time information of the environment given by the environment sensing device; Position and calculate the repulsive force of the repulsive point to the unmanned vehicle.

The gravity calculation step calculates the lateral distance between the road centerline with the new boundary line as the road centerline and the road centerline of the road where the vehicle is currently located through the deviation calculation device, and at the same time, whether there are obstacles or obstacles in the road through the Gaussian membership combination function The information of the distance between the object and the driverless car is reflected in the gravitational point function in real time; and according to the gravitational point function, the position of the gravitational point of the driverless car is calculated as the environment changes, and the gravitational force of the gravitational point on the driverless car is calculated. .

Owing to having adopted above-mentioned scheme, the present invention has following characteristics:

1) By utilizing the method of Gaussian combined membership function to respond to environmental changes in real time, the present invention solves the problems of falling into local minimum and path oscillations in the traditional artificial potential field method due to repulsion and gravitation in the same direction;

2) Through the selection of the repulsion point function, when the vehicle deviates from the target driving path due to the interference of uncertain factors such as side wind and road surface unevenness, the combined force of the repulsive force and the gravitational force will guide the vehicle back to the target driving path, improving the The robustness of the vehicle to track the ideal target driving path;

3) Since there is no need to establish a complex environmental model, only an environmental feature model is required to realize local path planning, so the required amount of sensor data information is smaller and the real-time performance of data information processing is better. On the one hand, it helps to reduce sensor On the other hand, it is also conducive to meeting the high real-time responsiveness requirements of the vehicle control system;

4) The present invention has good environmental adaptability. Except for the two driving environments mentioned in the embodiments of the present invention, the present invention can realize multiple environments for other driving environments, such as intersections that are blocked but have passing paths. Automatic obstacle avoidance when getting off the vehicle;

5) Through the setting of Gaussian combined membership function coefficients, the target driving path function can also meet the constraints of vehicle kinematics and dynamics such as the minimum turning radius of the vehicle and the maximum steering angular velocity of the front wheels, so that the controlled object can be tracked well. The desired path is obtained, so as to achieve the expected optimal control.

Description of drawings

FIG. 1 is a schematic diagram of a local path planning device.

Fig. 2 is a schematic diagram of an environment characteristic model establishment device.

Fig. 3 is a schematic diagram of the device for establishing the gravitational point function.

Figure 4 is a comparison diagram of vehicle trajectories with different values of σ.

Fig. 5 is a schematic diagram of the calculation device for the resultant force direction angle.

Figure 6 is a simulation result of the driving trajectory of the unmanned vehicle in an environment without obstacles.

Figure 7 is a diagram of the expected steering wheel angle of an unmanned vehicle in an environment without obstacles.

Figure 8 is a simulation result of the driving trajectory of the unmanned vehicle in the obstacle environment.

Fig. 9 is a diagram of the expected steering wheel angle of an unmanned vehicle in an obstacle environment.

Detailed ways

The present invention will be further described below in conjunction with the embodiments shown in the accompanying drawings.

The present invention is based on the artificial potential field APF (Artificial Potential Field) method, and utilizes the Gaussian combination membership function to establish the target driving path function, that is, the gravitational point function of the artificial potential field method, and reflects the change of the environment in real time in the gravitational point function. Changes, and use this to calculate the gravitational force; get the repulsion point function based on the obstacle information and road edge information, and use this to calculate the repulsive force. Then calculate the direction of the resultant force on the driverless car through the gravitational and repulsive forces, so as to plan the path that the driverless car will travel. When the vehicle is driving along the target driving path, the two repulsive forces cancel each other out, and the gravitational force plays a leading role. Once the vehicle is disturbed by uncertain factors such as side wind, road unevenness, etc. The vehicle returns to the target travel path.

A device for gravitational calculation in the present invention includes a gravitational point function establishment device and a gravitational calculation device, wherein the gravitational point function device includes: deviation calculation, used to calculate the difference between the center line of the road with the new boundary line as the road and the current The lateral distance between the road centerlines of the road where the vehicle is located; the presence or absence of obstacles in the road and the distance between obstacles and unmanned vehicles are reflected in the gravitational point function in real time through the Gaussian membership combination function; Gravity point selection on the point function to calculate gravity.

Wherein, the deviation calculation device calculates according to the obstacle boundary point information obtained by radar (laser or millimeter wave radar) and the road centerline.

Among them, the gravity point function gives the road centerline according to the visual system, and calculates the deviation value by solving the maximum change of the road centerline deviation according to the real-time environment change, and uses the Gaussian membership combination function to reflect the information of obstacles in real time, so as to obtain gravitational point function.

Among them, the coefficients of the Gaussian membership combination function will make the target driving path satisfy the kinematics and dynamics constraints of the vehicle.

Wherein, the selection of the gravity point will be determined according to the curvature of the road and the speed of the vehicle.

A device for repulsion calculation, including a repulsion point function establishment device and a repulsion calculation device, wherein the repulsion point function is established according to the real-time information of the environment given by the visual sensor and the radar to determine the boundaries of the two roads on which the vehicle will travel; the repulsion calculation The device is used to calculate the magnitude of the repulsive force according to the repulsive force point.

The present invention also includes a device for converting resultant force into steering wheel angle, comprising the steps of: establishing the vehicle body coordinate system, decomposing the attractive force into component forces of orthogonal coordinate axes according to the magnitude of the gravitational force and the direction under the vehicle body coordinate system, and The repulsive force is decomposed into the component force of the orthogonal coordinate axis and the direction in the car body coordinate system, so as to calculate the front wheel rotation angle, and then calculate the steering wheel rotation angle according to the transmission ratio of the steering system.

The present invention intends to use a visual sensor to detect the road boundary and calculate the road centerline, use radar (laser or millimeter wave radar) to detect obstacle information, and fit the road coordinate point information given by the visual system into a multiple curve (considering the vehicle The dynamic constraints of the three or more curves) establish a road boundary model and a road centerline model (it is also possible to use only lidar as an environmental perception system, use lidar to detect road edges, fit road boundary information, and calculate the road centerline information). Based on the environment feature model obtained by the sensor, the Gaussian membership combination function item in the gravitational point function will reflect the change of the environment in the road in real time, that is, whether there is an obstacle, the distance between the obstacle and the driverless workshop, and according to the gravitational point function obtained in real time Calculate the gravitational force; establish the repulsion point function according to the environment change, and calculate the repulsion force according to the repulsion point function. The schematic diagram of the local path planning device is shown in Figure 1.

1. Environmental awareness:

According to the vision system, the two boundary function models of the road are established on the vehicle body coordinate system

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="1"\; label="y"\; filenames="HDA0000043738410000011.png,HDA0000043738410000052.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="d"\; filenames="HDA0000043738410000011.png,HDA0000043738410000052.png"\; state="\{\{state\}\}">d</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="2"\; label="y"\; filenames="HDA0000043738410000011.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Among them, a ₁ , b ₁ , c ₁ , d ₁ , and d ₂ are cubic curve coefficients.

The road centerline function model is deduced from formula (1)

y _center ＝a ₁ x ³ +b ₁ x ² +c ₁ x+(d ₁ +d ₂ )/2 (2)

When there is an obstacle in the road, the real-time coordinates (X _ob , Y _ob ) of the most dangerous boundary point of the obstacle in the same coordinate system on the current road are obtained through fusion of the road line information data given by the radar scan and the vision system, namely Obtain the data information of the two boundaries of the passable road. The basic model of environmental characteristics is obtained through the above raw data. The schematic diagram of the environment feature model establishment device is shown in Figure 2.

2. Establishment of gravitational point function

Vehicles driving on structured roads must have road boundary constraints, and there may be obstacles. There are two possibilities for obstacles, one is the widest passable path when the obstacle and the road boundary are used as the boundary line, and the other is the widest passable path. One is that when two obstacles are used as the boundary, it is the widest passable path. The device for establishing the gravitational point function is shown in Figure 3.

1) There are no obstacles in the road

At this time, the vehicle is restricted by the two road boundary lines and cannot drive outside the road. Therefore, the two road boundary lines are the repulsion point function, and the attraction point function is the road centerline function.

2) When there are obstacles in the road

When there are obstacles in the road, the widest passable road is selected first, so as to determine the road boundary point and the deviation coefficient.

The gravitational point function when there are obstacles in the road is divided into two cases. In one case, the obstacle is used as the boundary line on one side, and the road boundary line is used as the boundary line on the other side, that is, the repulsion point function is the road boundary line and the most dangerous boundary point of the obstacle, another situation is that two obstacles are used as the boundary line, and the first situation is now described in detail.

In this case, the repulsion point function is the real-time coordinate point (X _ob , Y _ob ) of the most dangerous point of the road boundary line and the obstacle.

Obstacles and one side of the road line form the boundary of the passable path, resulting in a new road centerline, then the maximum offset required by the original road centerline function

Δs＝|(a ₁ x _ob ³ +b ₁ x _ob ² +c ₁ x _ob +(d ₁ +d ₂ )/2)-(y _ob +a ₂ x _ob ³ +b ₂ x _ob ² +c ₂ x _ob +d ₂ )/2| (3)

That is, when the vehicle is in the boundary constraint condition where one side is an obstacle boundary, the gravitational point function is

y _goal ＝a ₁ x ³ +b ₁ x ² +c ₁ x+(d ₁ +d ₂ )/2+Δs (4)

However, directly adding an offset will cause the discontinuity of the path curvature, which does not meet the kinematic constraints of the vehicle. Based on the above analysis and considering the vehicle constraints, the Gaussian membership function is used to smooth the two objective functions, so the gravitational point function is corrected. for

y _goal ＝a ₁ x ³ +b ₁ x ² +c ₁ x+(d ₁ +d ₂ )/2+Δs*exp(-(XX _ob ) ² /2*σ ² ) (5)

Among them, σ is a coefficient related to the curvature of the Gaussian membership function curve. In the gravitational point function of gravity, by adjusting its value, the dynamic constraints of the vehicle can be satisfied, that is, the path curvature is bounded and the path curvature reciprocal is bounded, as shown in Figure 4. Value road track. From the above analysis, it can be seen that the situation without obstacles in the road is a special case of the situation with obstacles in the road, that is, in the case of no obstacles, Δs*exp(-(XX _ob ) ² /2*σ ² will tend to zero .

To sum up, the establishment of the gravitational point function is

y _goal ＝a ₁ x ³ +b ₁ x ² +c ₁ x+(d ₁ +d ₂ )/2+Δs*exp(-(XX _ob ) ² /2*σ ² ) (6)

3. Establishment of repulsion point function

repulsion point function

1) When there are no obstacles, the repulsion point function is the road boundary, that is, formula (1)

2) When there are obstacles in the road, according to the widest passable road boundary conditions, the introduced obstacles will be used as one side boundary

4. Gravity calculation

In this method, the unmanned vehicle (controlled object) is simplified as a particle, and its space is a two-dimensional Euclidean space. The position of the controlled object in space X=[xy] ^T , in the present invention, because it is in the vehicle body coordinate system, so X=[00] ^T . The gravitational field function U _att (X) suffered by the controlled object at X is defined as a function related to the target position X _g =[x _g y _g ] ^T :

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; \&kappa;</span>}{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

In the formula: κ is the gravitational field gain. The corresponding gravitational force F _att (X) is the negative gradient of the gravitational field:

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; \&dtri;</span>{\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&kappa;</span>}{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; G</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

In the formula: a _G is the unit vector of the controlled object pointing to the target; ρ _g = ||XX _g || is the distance between the controlled object and the gravitational point.

5. Calculation of repulsive force

The repulsion field function is

${\mathrm{<span\; class="notranslate">\; u</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; \&eta;</span>}{<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}}^{\mathrm{<span\; class="notranslate">\; no</span>}}& {\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; ></span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

where n is any real number greater than zero.

When the controlled object is not at the gravitational point, the corresponding repulsive force is:

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="f\; rep"\; filenames="HDA0000043738410000011.png,HDA0000043738410000052.png"\; state="\{\{state\}\}">f</figure-callout></span><figure-callout\; id="1"\; label="f\; rep"\; filenames="HDA0000043738410000011.png,HDA0000043738410000052.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; o</span>}}& {\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; ></span>{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$

in:

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="f\; rep"\; filenames="HDA0000043738410000011.png,HDA0000043738410000052.png"\; state="\{\{state\}\}">f</figure-callout></span><figure-callout\; id="1"\; label="f\; rep"\; filenames="HDA0000043738410000011.png,HDA0000043738410000052.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&eta;</span>}<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; )</span>\frac{{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}}^{\mathrm{<span\; class="notranslate">\; no</span>}}}{{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="ob"\; filenames="HDA0000043738410000011.png"\; state="\{\{state\}\}">ob</figure-callout></span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}$

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f\; rep"\; filenames="HDA0000043738410000011.png"\; state="\{\{state\}\}">f</figure-callout></span><figure-callout\; id="2"\; label="f\; rep"\; filenames="HDA0000043738410000011.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; no</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; \&eta;</span>}{<span\; class="notranslate">\; (</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{{\mathrm{<span\; class="notranslate">\; \&rho;</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}}^{\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}$

In the formula: η is the gain of the repulsion field function, ρ _ob =||XX _ob || is the shortest distance between the controlled object and the obstacle, and the constant ρ ₀ is the influence distance of the obstacle set according to the vehicle speed.

Among them, a _O is the unit vector of the obstacle pointing to the controlled object.

6. Calculation of resultant force direction

The direction of the resultant force determines the direction of motion of the controlled object. In the car body coordinate system, the attractive and repulsive forces are decomposed into component forces on the two coordinate axes respectively. The schematic diagram of the resultant force direction angle calculation device is shown in Figure 5.

On the gravitational point function established with the car body coordinate system as the coordinate axis, select the gravitational point X _g = [x _g , y _g ], then the angle between the unmanned vehicle and the gravitational point

α＝arctan(y _g /x _g ) (11)

The components of gravitational force on the abscissa and ordinate are

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; cos</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&alpha;</span>}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; g</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; att</span>}}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; sin</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&alpha;</span>}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$

On the car body coordinate system, the repulsion point X _ob (i) = [x _ob (i), y _ob (i)], then the angle between the unmanned vehicle and the obstacle

β _i =arctan(y _ob (i)/x _ob (i)) (13)

The components of the repulsive force on the abscissa and ordinate are

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; cos</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; rep</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; sin</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 14</span>}<span\; class="notranslate">\; )</span>$

Then the angle between the driverless car and the resultant force is the expected heading angle

θ=arctan((F _att (y)+F _rep (y(i)))/(F _att (x)+F _rep (x(i)))) (15)

steering wheel angle

δ _sw = θ*i _s (16)

where i _s is the transmission ratio of the steering system.

The above inventions are simulated and verified in different environments, and the parameters of the system adopt the following values:

κ=6, ρ ₀ =10, η=0.7, n=2, σ=4.5, vehicle speed v=18km/h

When there is no obstacle in the road or the obstacle is not within the effective distance range (ie ρ _ob > ρ ₀ ), the improved artificial potential field method is used to use the road boundary line as the repulsive force and the road centerline as the attractive force. At this time, the correction term is zero. Figure 6 shows the driving trajectory of the driverless car. It can be seen from the driving trajectory that the driverless car can follow the centerline of the road very well. Figure 7 shows the steering wheel angle. From the given steering wheel angle commands, it can be seen that the expected steering wheel angle is smooth and satisfies the kinematics and dynamics constraints of the vehicle (for example, limited by the steering actuator, the steering wheel angle of an unmanned vehicle Constraint is |δ _sw |<=500 °, and the constraint of steering wheel angular velocity is |V _δ |<=200°/s.As can be seen from Fig. 7, under the obstacle-free environment, utilize unmanned vehicle of the present invention to be in When driving on a road with a certain curvature, the steering wheel angle can be kept within 200 degrees, and the steering wheel angular velocity can also be kept within the constraint range of 200 degrees per second).

When there is an obstacle in the road ahead, and the distance between the unmanned vehicle and the obstacle is within the effective range (ρ _ob ≤ ρ ₀ ), the obstacle and the road boundary line on one side are used as the repulsion point function. At this time, the correction item and the maximum deviation It is related to the displacement, and the center line of the obstacle boundary line and the road boundary line is the gravitational force. Figure 8 shows the driving trajectory of the driverless car. It can be seen from the driving trajectory that when there is an obstacle in the road, the driverless car can smoothly bypass the obstacle and return to the road smoothly after bypassing the obstacle center line. Fig. 9 is the steering wheel angle. From the given steering wheel angle, it can be seen that the desired angle is smooth and satisfies the vehicle dynamics constraints (in an environment with obstacles, the steering wheel angle of the present invention can be kept within 120 degrees, and the steering wheel Angular velocity can also be kept within the constraint of 200 deg/s).

The descriptions of the above embodiments are for those of ordinary skill in the art to understand and apply the present invention. It is obvious that those skilled in the art can easily make various modifications to these embodiments, and apply the general principles described here to other embodiments without creative efforts. Therefore, the present invention is not limited to the embodiments herein, and improvements and modifications made by those skilled in the art according to the disclosure of the present invention should fall within the protection scope of the present invention.

Claims (10)

1. A device for local path planning of unmanned vehicles, characterized in that: it comprises: Environmental sensing device, used to detect obstacles, establish road boundary model and road centerline model; A repulsion calculation device, used to establish a repulsion point function and calculate the repulsion; A gravitational calculation device, used to establish the gravitational point function and calculate the gravitational force; The resultant force direction angle calculation device is used to calculate the direction angle of the resultant force of repulsion and attraction; The steering wheel angle calculation device is used to determine the steering wheel angle according to the direction angle of the resultant force and the transmission ratio of the steering system. 2. The device for local path planning of unmanned vehicles as claimed in claim 1, characterized in that: the environment perception device is a visual sensor and a radar, the visual sensor detects the road boundary and calculates the road centerline, and the radar detects Obstacle information, the road coordinate point information given by the visual sensor is fitted to a multiple curve, and the road boundary model and road centerline model are established. 3. The device for local path planning of unmanned vehicles as claimed in claim 1, characterized in that: the environment sensing device is a radar, the radar detects the road edge, fits the road boundary information, and calculates the road centerline information , to establish the road boundary model and the road centerline model. 4. The device for local path planning of unmanned vehicles as claimed in claim 1, characterized in that: said repulsion computing device determines the boundaries of the two roads on which the vehicle will travel according to the real-time information of the environment given by the environment sensing device To establish the repulsion point function; and according to the repulsion point function, calculate the position of the unmanned vehicle repulsion point and calculate the repulsion force of the repulsion point to the unmanned vehicle as the environment changes. 5. The device for local path planning of unmanned vehicles as claimed in claim 1, wherein the gravitational calculation device calculates the distance between the center line of the road and the current vehicle location with the new boundary line as the road center line by the deviation calculation device. At the same time, through the Gaussian membership combination function, the information of whether there are obstacles in the road and the distance between obstacles and unmanned vehicles is reflected in the gravitational point function in real time; Changes in the environment, the position of the gravitational point of the driverless car, and the calculation of the gravitational force of the gravitational point on the driverless car. 6. A method for local path planning of unmanned vehicles, characterized in that: it comprises: Environmental awareness step, detecting obstacles, establishing road boundary model and road centerline model; The repulsion calculation step is to establish the repulsion point function and calculate the repulsion; The gravitational calculation step is to establish the gravitational point function and calculate the gravitational force; The resultant force direction angle calculation step is to calculate the direction angle of the resultant force of the repulsive force and the gravitational force; In the step of calculating the steering wheel angle, the steering wheel angle is determined according to the direction angle of the resultant force and the transmission ratio of the steering system. 7. The method for local path planning of unmanned vehicles as claimed in claim 6, characterized in that: the environmental perception step is to detect road boundaries by visual sensors and calculate the center line of the road, and to detect obstacle information by radar. The road coordinate point information given by the visual sensor is fitted to multiple curves, and the road boundary model and road centerline model are established. 8. The method for local path planning of unmanned vehicles as claimed in claim 6, characterized in that: the environment perception step is to detect the roadside by radar, fit the road boundary information, calculate the road centerline information, and establish Road boundary model and road centerline model. 9. The method for local path planning of unmanned vehicles as claimed in claim 6, characterized in that: said repulsive force calculation step determines the boundaries of the two roads on which the vehicle will travel according to the real-time information of the environment provided by the environment sensing device To establish the repulsion point function; and according to the repulsion point function, calculate the position of the unmanned vehicle repulsion point and calculate the repulsion force of the repulsion point to the unmanned vehicle as the environment changes. 10. The method for local path planning of unmanned vehicles as claimed in claim 6, characterized in that: said gravitational calculation step calculates the difference between the center line of the road and the current vehicle location with the new boundary line as the road by means of a deviation calculation device. At the same time, through the Gaussian membership combination function, the information of whether there are obstacles in the road and the distance between obstacles and unmanned vehicles is reflected in the gravitational point function in real time; Changes in the environment, the position of the gravitational point of the driverless car, and the calculation of the gravitational force of the gravitational point on the driverless car.

Images