CN103632558A - Bionic swarm intelligence-based real-time positioning navigation and motion control method and system for moving vehicle - Google Patents

Abstract

The invention discloses a bionic swarm intelligence-based mobile vehicle real-time positioning navigation and motion control method, which is used to solve the technical problem of low reliability of the existing mobile vehicle real-time positioning navigation and motion control methods. The technical solution is to firstly select vehicles and information nodes with known coordinates in complex roads as positioning reference nodes, transform the problem of solving positioning equations into an extreme value optimization problem, and use the bionic bee colony algorithm to solve the positioning coordinates. For the control of driving among multiple vehicles, the bionic group motion model is established to control the movement between vehicles by establishing the behavior of biological groups, which improves the controllability of real-time positioning, navigation and motion control of moving vehicles. The real-time positioning navigation and motion control system of mobile vehicles based on bionic swarm intelligence consists of a vehicle control terminal module, a road information node module and a road traffic control center module. The three modules work together to realize the real-time positioning navigation and motion control of the moving vehicle.

Description

Method and system for real-time positioning navigation and motion control of mobile vehicles based on bionic swarm intelligence

technical field

The invention relates to a method for real-time positioning, navigation, and motion control of a mobile vehicle based on bionic swarm intelligence, and also relates to a real-time positioning, navigation, and motion control system for a mobile vehicle based on bionic swarm intelligence.

Background technique

Vehicle automatic positioning technology is a key technology involved in many fields of intelligent transportation system. At present, the main practical application of vehicle positioning technology is mainly GPS positioning, and combined positioning technology based on GPS.

Document 1 "Chinese utility model patent whose patent announcement number is CN201741289U" discloses a vehicle positioning device. The positioning device is mainly equipped with a GPS module, and the vehicle performs real-time positioning by receiving GPS signals.

Document 2 "Li Guifang, Research on Vehicle GPS/DR Integrated Navigation Based on UPF Algorithm, "Science Technology and Engineering" 2012.11, p8143-8146." discloses an integrated navigation method of GPS and DR fusion. The method uses the current statistical model as the state equation of the system and the positioning signal obtained by the GPS device equipped on the vehicle as the measured value of the system to locate the vehicle in real time.

In the vehicle positioning method disclosed above, its essence is to adopt the GPS satellite navigation method for positioning by receiving multiple satellite signals in the space. The satellite signals from the high-altitude space are an indispensable part of its positioning system, and its defects are:

(1) The reliability of the GPS signal is easily affected, and it is greatly disturbed by the external environment. The environment such as high-rise buildings and tunnels in the city will affect the accuracy and reliability of vehicle positioning; long-distance satellite signal transmission is extremely easy Affected by the natural environment and human interference, for example, when the vehicle enters a long tunnel, the signal loss will cause the temporary failure of positioning; the satellite becoming the target of the enemy in wartime will also cause the permanent paralysis of the positioning system.

(2) The vehicle only obtains its own position coordinates, without considering the positional relationship with other neighboring vehicles. Therefore, it lacks coordination and unity among vehicle groups when driving in a complex and congested road environment, making the driving efficiency and safety of vehicles on the road Sexual deterioration.

It can be seen from the above that it is difficult for public documents to provide accurate and highly reliable positioning information, and it also lacks the function of coordinating the movement of multiple vehicles.

Contents of the invention

In order to overcome the low reliability of existing vehicle GPS and its combined positioning system in urban environments, the present invention provides a real-time positioning navigation and motion control method for mobile vehicles based on bionic swarm intelligence. This method first selects vehicles and information nodes with known coordinates in complex roads as positioning reference nodes, establishes a positioning equation set by measuring the relative distance difference between the vehicle and the reference node, and then transforms the problem of solving this positioning equation set into an extreme value optimization problem , and use the bionic bee colony algorithm to solve the positioning coordinates. For the multi-vehicle driving control method, the controllability of real-time positioning navigation and motion control of moving vehicles can be improved by establishing a bionic group motion model to control the motion between vehicles by establishing the behavior of biological groups.

The invention also provides a real-time positioning navigation and motion control system for a mobile vehicle based on bionic swarm intelligence. The system consists of vehicle control terminal module, road information node module and road traffic control center module. The three modules work together to realize reliable real-time positioning of moving vehicles and group driving control among vehicles.

The technical solution adopted by the present invention to solve the technical problems is: a method for real-time positioning navigation and motion control of mobile vehicles based on bionic swarm intelligence, which is characterized in that it comprises the following steps:

Step 1: At time t, the vehicle and its adjacent vehicles and information nodes form a wireless communication network, and each vehicle with known position coordinates sends its own position coordinates by broadcasting through the wireless network.

Step 2: The vehicle to be positioned receives the location coordinate information sent by the neighbor vehicle and the information node, and measures the relative distance d _i between itself and the neighbor vehicle and the information node through the arrival time principle of the electromagnetic wave signal

d _i =ct _i , i=1,2,...,n (1)

In the formula, c is the propagation speed of the electromagnetic wave signal in the air, t _i is the propagation time of the electromagnetic wave signal from the vehicle to be positioned to the neighbor vehicle i, and n is the number of received signals.

Select m vehicles or information nodes with the smallest value of d _i as the positioning reference nodes of the vehicle to be positioned.

Step 3: The vehicle to be positioned establishes a positioning equation according to the relative distance d _i from the reference node and the position coordinates of the reference node, and the positioning equation is expressed as

$\left\{\begin{array}{c}\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ \sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; mt</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; mt</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

In the formula, (x _t , y _t ) is the coordinates of the vehicle to be positioned at time t, and (x _it , y _it ) is the position coordinates of the reference node at time t.

Step 4: Transform the positioning equation of formula (2) into a minimum value problem, and its expression is

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; mt</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; mt</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</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">\; 3</span>}<span\; class="notranslate">\; )</span>$

${{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

For the minimum value equation of formula (4), the artificial bee colony intelligent calculation method is used to solve it, and the minimum value (x _t , y _t ) of the solution is the position coordinate of the vehicle to be positioned.

Step 5: The vehicle to be positioned is within its radius R field, select group neighbor vehicles in the field, and the selection standard is

N _it ＝{i:[x _t -x _it ] ² +[y _t -y _it ] ² +[z _t -z _it ] ² ≤R ² } (5)

In the formula, z _i is the position value of vehicle i on the vertical axis.

Step 6: The moving direction of vehicle k is the average value of the moving directions of its neighbors.

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; no</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; Nt</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; it</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; no</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; Nt</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; it</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; no</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; Nt</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; it</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

In the formula, α _it , β _it , and γ _it are the moving direction of the vehicle along the three coordinate axes at time t, and n _t is the number of neighbor vehicles at time t.

Then the position formula of vehicle k is:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}\end{array}\right.<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, v _k is the driving speed of vehicle k.

Each vehicle constantly adjusts its position according to the position equation of Equation (7).

A real-time positioning navigation and motion control system for mobile vehicles based on bionic swarm intelligence, which is characterized in that it includes a vehicle control terminal module, a road information node module and a road traffic control center module, and the three modules transmit signals in pairs.

The vehicle control terminal module includes a wireless communication module, a data processing module, a path planning module, a data acquisition module, a vehicle control module, a storage module and a display module. The wireless communication module communicates with road information nodes and other vehicles to receive and send information, and obtain parameters required for vehicle positioning and control. The data processing module obtains the parameters required for positioning from the wireless communication module, and converts the positioning information received by the vehicle terminal node into the accurate position coordinates of the vehicle according to the algorithm. The data acquisition module receives the signals measured by the vehicle's own speed sensor, rotation angle sensor, and acceleration sensor, and converts them into digital information to obtain its own motion state. The path planning module receives signals from the wireless communication module, data acquisition module and data processing module, obtains the real-time road environment and the vehicle's own data, and the vehicle's own position coordinates, and makes a reasonable and optimized driving path for the vehicle at the next moment planning. The vehicle control module receives the driving route planned by the path planning module, and controls the speed, steering and acceleration of the vehicle. The storage module and the display module receive and display the position and optimal route information from the data processing module and the route planning module in real time.

The road information node module includes a wireless communication module, a data processing module and a sensor module. The sensor module transmits the measured vehicle quantity, traffic speed and visibility information in the road environment to the data processing module as an analog signal. The data processing module processes the analog signal from the sensor module and converts it into a digital signal recognizable by the processor. The wireless communication module sends the node position, road traffic volume and vehicle speed signals from the data processing module to the road traffic control center module.

The road traffic control center module includes a wireless communication module, a data processing module and a database module. The wireless communication module communicates with the road information node module and the vehicle control terminal module to receive and send vehicle information, road traffic flow information and road geographic information. The data processing module receives the information from the wireless communication module, preprocesses it and stores it in the database module, and extracts the relevant road geographic information from the database according to the needs of the vehicle control terminal module. The database module stores road traffic flow information and road geographic information for navigation.

The beneficial effects of the present invention are: the real-time positioning navigation and motion control method of mobile vehicles based on bionic swarm intelligence, first select vehicles and information nodes with known coordinates in complex roads as positioning reference nodes, and measure the relative distance between the vehicle and the reference nodes Then, the problem of solving the positioning equations is transformed into an extreme value optimization problem, and the positioning coordinates are solved by using the bionic bee colony algorithm. For the multi-vehicle driving control method, the bionic group motion model is established to control the motion between vehicles by establishing the behavior of biological groups, which improves the controllability of real-time positioning, navigation and motion control of moving vehicles. Real-time positioning navigation and motion control system for mobile vehicles based on bionic swarm intelligence. The system consists of vehicle control terminal module, road information node module and road traffic control center module. The three modules work together to realize reliable real-time positioning of moving vehicles and group driving control among vehicles.

The present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments.

Description of drawings

Fig. 1 is a flowchart of the real-time positioning navigation and motion control method of a mobile vehicle based on bionic swarm intelligence in the present invention.

Fig. 2 is the block diagram of the mobile vehicle real-time positioning navigation and motion control system based on bionic swarm intelligence of the present invention.

Fig. 3 is a structural diagram of the vehicle-mounted control terminal module in Fig. 2 .

Fig. 4 is a structural diagram of the road information node module in Fig. 2 .

Fig. 5 is a structural diagram of the modules of the road traffic control center in Fig. 2 .

Figure 6 is a schematic diagram of a preferred embodiment of the present invention.

Detailed ways

The following embodiments refer to Figures 1-6.

Example 1. This embodiment describes in detail the steps of the mobile vehicle real-time positioning navigation and motion control method based on bionic swarm intelligence:

Step 1: At time t, the vehicle V to be positioned and its adjacent vehicles V1, V2, V3, V4, V5, V6 and the information node N1 form a wireless communication network, and the vehicles V1, V2, V3, V4, V5, V6 and the information node The coordinates of N1 are known, and the location coordinates of N1 are broadcast to the vehicle V through the wireless network.

Step 2: The vehicle V to be positioned receives the location coordinate information sent by the neighbor vehicles V1, V2, V3, V4, V5, V6 and the information node N1, and at the same time measures the distance between itself and the neighbor vehicles V1, V2, V3, V4, V5, V6, and the distances d _v1 , d _v2 , d _v3 , d _v4 , d _v5 , d _v6 , d _N1 between the information nodes N1.

d _i = ct _i , i = v1, v2, v3, v4, v5, v6, N1 (1)

In the formula, c is the propagation speed of the electromagnetic wave signal in the air, t _i is the propagation time of the electromagnetic wave signal from the vehicle to be positioned to the neighbor vehicle or information node i, and n is the number of received signals.

Select m=4 vehicles or information nodes with the smallest value from d _v1 , d _v2 , d _v3 , d _v4 , d _v5 , d _v6 , d _N1 as the positioning reference nodes of the vehicle to be positioned. In this example, V1, V2, V4, and N1 are selected as positioning reference points.

Step 3: According to the relative distances d _v1 , d v2 , d v4 , d N1 between the vehicle V to be positioned and the reference points V1, _V2 , _V4 , _N1 and the position coordinates (x _v1 , y _v1 ), (x _v2 ,y _v2 ), (x _v4 ,y _v4 ), (x _N1 ,y _N1 ) to establish positioning equations:

$\left\{\begin{array}{c}\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\\ \sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\\ \sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\\ \sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

In the formula, (x _t , y _t ) is the coordinates of the vehicle V to be positioned at time t.

Step 4: Transform the positioning equation of formula (2) into a minimum value problem:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span>\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</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">\; 3</span>}<span\; class="notranslate">\; )</span>$

${{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

For the minimum value equation of formula (4), the artificial bee colony intelligent calculation method is used to solve it, and the minimum value (x _t , y _t ) of the solution is the position coordinate of the vehicle to be positioned.

Step 5: The vehicle to be positioned is within its radius R = 100, select group neighbor vehicles in the area, and the selection criteria are

N _it ＝{i:[x _t -x _it ] ² +[y _t -y _it ] ² +[z _t -z _it ] ² ≤R ² } (5)

In the formula, z _i is the coordinate value of vehicle i on the vertical axis. Here it is assumed that the vehicles are in the same horizontal plane, then the value of _zi is the same.

Step 6: The moving direction of vehicle k is the average of the moving directions of its neighbors.

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; no</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; Nt</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; it</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; no</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; Nt</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; it</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; no</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&Element;</span>\mathrm{<span\; class="notranslate">\; Nt</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; it</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\end{array}\right.<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; v</span>}\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

In the formula, α _it , β _it , and γ _it are the moving directions of vehicles at time t, and n _t is the number of neighbor vehicles at time t.

Then the position formula of vehicle k is:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&alpha;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&beta;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}\\ {\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; kt</span>}}\end{array}\right.<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, v _k is the driving speed of vehicle k.

Each vehicle constantly adjusts its position according to the position equation of Equation (7).

Example 2. This embodiment describes in detail the structure of the mobile vehicle real-time positioning navigation and motion control system based on bionic swarm intelligence:

The data processing module and path planning module in the structure adopt TMS320F2808 chip; the wireless communication module adopts NRF905 chip; the storage module adopts W25X16AVSIG chip; the sensor module includes CCD sensor TSL1401CL, accelerometer LSM303DLHC; the vehicle control module adopts MC9S12XS128MAA chip; the display module is LCD1602 display screen.

The vehicle control terminal module in the present invention includes: a wireless communication module, a data processing module, a data acquisition module, a vehicle control module, a path planning module, a storage module and a display module. The data processing module obtains the parameters required for positioning from the wireless communication module, establishes a positioning equation group, and uses the bee colony algorithm to solve the positioning equation group to obtain the position coordinates of the vehicle to be positioned. The data acquisition module receives the signals measured by the vehicle's own speed sensor, rotation angle sensor, and acceleration sensor, and converts them into digital quantities to obtain its own motion state. The path planning module receives signals from the wireless communication module, data acquisition module, and data processing module to obtain real-time road environment and vehicle own data, as well as the vehicle's own position coordinates, and reasonably and optimize the driving path of the vehicle at the next moment planning. The vehicle control module receives the driving route planned by the path planning module, and controls the speed, steering and acceleration of the vehicle. The storage module and the display module receive and display the position and optimal route information from the data processing module and the route planning module in real time.

The information flow direction of the vehicle control terminal module is as follows: when the vehicle is driving, the wireless communication module in the vehicle control terminal sends a positioning request signal to the surrounding neighbor vehicles and road information nodes, receives the location information from the neighbor vehicles and road information nodes, and sends this The information is transmitted to the data processing module; the data acquisition module measures the relative information required for positioning and transmits it to the data processing module; the data processing module transmits the calculated position information to the path planning module, vehicle control module, storage module and display module; the path planning module transmits the planned vehicle path to the vehicle control module through the data processing module.

The road information node is composed of a large number of information nodes scattered in and around the road, and each node is composed of a wireless communication module, a data processing module and a sensor module. The sensor module can measure road information such as the number of vehicles, traffic speed and visibility in the road environment, and transmit the measured analog signal to the data processing module. The data processing module processes the analog signal from the sensor module and converts it into a digital signal. The wireless communication module sends the node position, road traffic flow, vehicle speed and other signals from the data processing module to the vehicle control terminal and road traffic control center.

The information flow direction of the road information node module is as follows: the sensor module transmits the measured information to the data processing module, and the data processing module processes the measured information and sends it to the wireless communication module.

The road traffic control center module is composed of a wireless communication module, a data processing module and a database module. The wireless communication module communicates with the road information node module and the vehicle control terminal module to receive and send vehicle information, road traffic flow information and road geographic information. The data processing module receives relevant information from the wireless communication module, preprocesses it and stores it in the database module, and extracts relevant road geographic information from the database according to the needs of the vehicle control terminal module. The database module stores road traffic flow information and road geographic information for navigation.

The information flow direction of the road traffic control center module is as follows: the wireless communication module sends the received relevant information to the data processing module, and after the information is processed, the data processing module arranges and stores it in the database module; at the same time, the data storage module can also store the local road geographic The information is sent to the data processing module and sent to the desired vehicle by the wireless communication module.

The information flow direction of the whole system is: when the vehicle is driving, the vehicle control terminal receives the location information from neighboring vehicles and road information nodes, and at the same time receives the local road geographic information from the road traffic control center module. In addition, the vehicle control terminal can also send its own status information to the road information node module and the road traffic control center module.

Claims (2)

1. the real-time location navigation of the moving vehicle based on bionical swarm intelligence, a motion control method, is characterized in that comprising the following steps:

Step 1: at moment t, vehicle vehicle adjacent thereto and information node form cordless communication network, and the vehicle of each known location coordinate sends the position coordinates of self in the mode of broadcast transmission by wireless network;

Step 2: vehicle to be positioned receives the location coordinate information of neighbours' vehicle and information node transmission is passed through the relative distance d between electromagnetic wave signal principle time of arrival measurement self and neighbours' vehicle, information node simultaneously _i

d _i＝ct _i，i＝1，2，…，n （1）

In formula, c is the aerial velocity of propagation of electromagnetic wave signal, t _ifor the travel-time of electromagnetic wave signal from vehicle to be positioned to neighbours' vehicle i, n is the number that receives signal;

Choose d _ibe worth minimum m vehicle or information node as the position reference node of vehicle to be positioned;

Step 3: the relative distance d between vehicle basis to be positioned and reference mode _iset up positioning equation with the position coordinates of reference mode, its positioning equation is expressed as

$\left\{\begin{array}{c}\sqrt{{(x_{1t}-x_t)}^2+{(y_{1t}-y_t)}^2}-\sqrt{{(x_{2t}-x_t)}^2+{(y_{2t}-y_t)}^2}=d_2-d_1\\ .\\ .\\ .\\ \sqrt{{(x_{1t}-x_t)}^2+{(y_{1t}-y_t)}^2}-\sqrt{{(x_{\mathrm{mt}}-x_t)}^2+{(y_{\mathrm{mt}}-y_t)}^2}=d_m-d_1\end{array}\right.---\left(2\right)$

In formula, (x _t, y _t) be that vehicle to be positioned is at t moment coordinate, (x _it, y _it) be that reference mode is at t moment position coordinates;

Step 4: the positioning equation of formula (2) is converted into minimizing problem, and its expression formula is

$\left\{\begin{array}{c}{f}_1={[\sqrt{{(x_{1t}-x_t)}^2+{(y_{1t}-y_t)}^2}-\sqrt{{(x_{2t}-x_t)}^2+{(y_{2t}-y_t)}^2}]}^2-{(d_2-d_1)}^2=0\\ .\\ .\\ .\\ f_m={[\sqrt{{(x_{1t}-x_t)}^2+{(y_{1t}-y_t)}^2}-\sqrt{{(x_{\mathrm{mt}}-x_t)}^2+{(y_{\mathrm{mt}}-y_t)}^2}]}^2-{(d_m-d_1)}^2=0\end{array}\right.---\left(3\right)$

${f_1}^2+{f_2}^2+...+f_m^2=0---\left(4\right)$

For formula (4) minimal value equation, adopt artificial bee colony intelligent computation method to solve it, the minimum value (x solving _t, y _t) be the position coordinates of vehicle to be positioned;

Step 5: vehicle to be positioned, in its radius R field, is chosen field in-group neighbours' vehicle, and selection standard is

N _it＝{i:[x _t-x _it] ²+[y _t-y _it] ²+[z _t-z _it] ²≤R ²} （5）

In formula, z _ifor the vehicle i positional value on axle in the vertical direction;

Step 6: the direction of motion of vehicle k is the mean value of its neighbours' direction of vehicle movement;

$\left\{\begin{array}{c}{\mathrm{\&alpha;}}_{\mathrm{kt}}=\frac{1}{n_{\mathrm{kt}-1}}\underset{i\&Element;\mathrm{Nt}}{\mathrm{\&Sigma;}}{\mathrm{\&alpha;}}_{\mathrm{it}-1}\\ {\mathrm{\&beta;}}_{\mathrm{kt}}=\frac{1}{n_{\mathrm{kt}-1}}\underset{i\&Element;\mathrm{Nt}}{\mathrm{\&Sigma;}}{\mathrm{\&beta;}}_{\mathrm{it}-1}\\ {\mathrm{\&gamma;}}_{\mathrm{kt}}=\frac{1}{n_{\mathrm{kt}-1}}\underset{i\&Element;\mathrm{Nt}}{\mathrm{\&Sigma;}}{\mathrm{\&gamma;}}_{\mathrm{it}-1}\end{array}\right.---\left(6\right)$

In formula, α _it, β _it, γ _itfor vehicle is at the t direction of motion along three axes constantly, n _tfor t moment neighbours' vehicle number;

The location formula of vehicle k is:

$\left\{\begin{array}{c}{x}_{\mathrm{kt}}=x_{\mathrm{kt}-1}+v_k\cos {\mathrm{\&alpha;}}_{\mathrm{kt}}\\ y_{\mathrm{kt}}=y_{\mathrm{kt}-1}+v_k\cos {\mathrm{\&beta;}}_{\mathrm{kt}}\\ z_{\mathrm{kt}}=z_{\mathrm{kt}-1}+v_k\cos {\mathrm{\&gamma;}}_{\mathrm{kt}}\end{array}\right.---\left(7\right)$

In formula, v _ktravel speed for vehicle k;

Each vehicle is constantly adjusted the position of oneself according to the position equation of formula (7).

2. the real-time location navigation of the moving vehicle based on bionical swarm intelligence, a kinetic control system, is characterized in that: comprise vehicle-mounted control terminal module, road information node module and road traffic control center module, transmission of signal between two between three modules;

Vehicle-mounted control terminal module comprises wireless communication module, data processing module, path planning module, data acquisition module, vehicle control module, memory module and display module; Wireless communication module and road information node, other vehicle communications, with reception, transmission information, obtain vehicle location and control required parameter; Data processing module obtains the required parameter in location from wireless communication module, and locating information car-mounted terminal node being received according to algorithm is converted into the accurate location coordinate of vehicle; Data acquisition module receives the signal of measuring from vehicle self speed pickup, rotary angle transmitter, acceleration transducer, and is translated into numerical information to obtain the motion state of self; Path planning module receives the signal from wireless communication module, data acquisition module and data processing module, obtain the position coordinates of Real-time Road environment and vehicle its data and vehicle self, to next driving path constantly of vehicle carry out reasonably, optimization plans; The travel route that vehicle control module RX path planning module is planned, control vehicle speed, turn to and acceleration; Memory module and display module receive and real-time position and the optimal path information showing from data processing module and path planning module;

Road information node module comprises wireless communication module, data processing module and sensor assembly; Sensor assembly is vehicle fleet size, wagon flow speed and visibility information in the road environment of measuring, with analog signal transmission to data processing module; The simulating signal of data processing module autobiography sensor module in future is processed and is translated into the digital signal that processor can be identified; Wireless communication module sends to road traffic control center module by the node location from data processing module, road traffic and vehicle speed signal;

Road traffic control center module comprises wireless communication module, data processing module and database module; Wireless communication module is communicated by letter with vehicle-mounted control terminal module with road information node module, receives, sends information of vehicles, terrain vehicle stream information and road geographic information; Data processing module receives the information from wireless communication module, and it is carried out after pre-service, its appointment being stored in database module, need to from database, extract relevant road geographic information according to vehicle-mounted control terminal module simultaneously; The information of vehicle flowrate of database module stores road and the road geographic information for navigating.

Images