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

The real-time location navigation of moving vehicle, motion control method and system based on bionical swarm intelligence

Technical field

The present invention relates to the real-time location navigation of a kind of moving vehicle based on bionical swarm intelligence, motion control method, also relate to the real-time location navigation of a kind of moving vehicle based on bionical swarm intelligence, kinetic control system.

Background technology

Vehicle automatic positioning technology is the gordian technique that in intelligent transportation system, various fields all relates to.At present, the vehicle positioning technology of main practical application is mainly GPS location, and the combined orientation technology based on GPS.

Document 1 " patent announcement number is the Chinese utility model patent of CN201741289U " discloses a kind of vehicle locating device, and this locating device is mainly that GPS module has been installed, and vehicle is located it in real time by receiving gps signal.

Document 2 " Li Guifang; the vehicle GPS/DR integrated navigation research based on UPF algorithm; < < science and technology and engineering > > 2012.11, p8143-8146. " discloses the Combinated navigation method of a kind of GPS and DR fusion.The method is usingd positioning signal that current statistical model obtains as the state equation of system, the GPS device of vehicle mounting of usining as the measuring value of system, and vehicle is located in real time.

In disclosed vehicle positioning method, its essence is to adopt to receive the gps satellite navigate mode that in space, a plurality of satellite-signals position above, is requisite ingredient in its positioning system from the satellite-signal in space, high-altitude, and its defect is:

(1) there is the susceptible problem of reliability in gps signal, is subject to the interference of external environment larger, and the environment such as the skyscraper in city, tunnel all can affect accuracy and the reliability of vehicle location; The satellite-signal transmission of long distance is very easily subject to the impact of physical environment and artificial interference, and while sailing long tunnel into as vehicle, dropout can cause the temporary transient inefficacy of location; Satellite became striking target of enemy and also can cause positioning system forever to paralyse wartime.

(2) vehicle only obtains the position coordinates of self, and do not have to consider and other neighbours' vehicles between position relationship, thereby the coordinating and unifying of travelling between lower shortage vehicle colony at the crowded road environment of complexity, make running efficiency and the deterioration of safety of vehicle in road.

As can be seen from the above the locating information that open source literature is difficult to provide accurately, reliability is high, also lacks the function of coordinating a plurality of vehicle movements simultaneously.

Summary of the invention

In order to overcome the lower deficiency of existing vehicle GPS and integrated positioning system thereof reliability in urban environment, the invention provides the real-time location navigation of a kind of moving vehicle based on bionical swarm intelligence, motion control method.First the method chooses the vehicle of known coordinate in complicated road and information node as position reference node, by the poor positioning equation group of setting up of the relative distance between measuring vehicle and reference mode, to solve again this positioning equation group problem and be converted into extremal optimization problem, and adopt bionical ant colony algorithm to solve the elements of a fix.For travel control method between many vehicles, by setting up biotic population behavior, set up the motion between bionical group movement model control vehicle, can improve the controllability of the real-time location navigation of moving vehicle, motion control.

The present invention also provides the real-time location navigation of a kind of moving vehicle based on bionical swarm intelligence, kinetic control system.This system is comprised of vehicle-mounted control terminal module, road information node module and road traffic control center module.Three module cooperative work, can realize the reliable location in real time of moving vehicle, and the control of travelling of the colony between vehicle.

The technical solution adopted for the present invention to solve the technical problems is: the real-time location navigation of a kind of moving vehicle based on bionical swarm intelligence, motion control method, be characterized in 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).

The real-time location navigation of moving vehicle based on bionical swarm intelligence, a kinetic control system, be characterized in: 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 discernible digital signal of processor.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.

The invention has the beneficial effects as follows: the real-time location navigation of moving vehicle based on bionical swarm intelligence, motion control method, first choose the vehicle of known coordinate in complicated road and information node as position reference node, by the poor positioning equation group of setting up of the relative distance between measuring vehicle and reference mode, to solve again this positioning equation group problem and be converted into extremal optimization problem, and adopt bionical ant colony algorithm to solve the elements of a fix.For travel control method between many vehicles, by setting up biotic population behavior, set up bionical group movement model and control the motion between vehicle, improved the controllability of the real-time location navigation of moving vehicle, motion control.The real-time location navigation of moving vehicle based on bionical swarm intelligence, kinetic control system.This system is comprised of vehicle-mounted control terminal module, road information node module and road traffic control center module.Three module cooperative work, have realized the reliable location in real time of moving vehicle, and the control of travelling of the colony between vehicle.

Below in conjunction with the drawings and specific embodiments, describe the present invention in detail.

Accompanying drawing explanation

Fig. 1 is the real-time location navigation of moving vehicle that the present invention is based on bionical swarm intelligence, the process flow diagram of motion control method.

Fig. 2 is the real-time location navigation of moving vehicle that the present invention is based on bionical swarm intelligence, the block scheme of kinetic control system.

Fig. 3 is that in Fig. 2, vehicle-mounted control terminal module forms structural drawing.

Fig. 4 is that in Fig. 2, road information node module forms structural drawing.

Fig. 5 is that in Fig. 2, road traffic control center module forms structural drawing.

Fig. 6 is the schematic diagram of the first-selected embodiment of the present invention.

Embodiment

Following examples are with reference to Fig. 1-6.

Embodiment 1.The present embodiment is described the real-time location navigation of moving vehicle based on bionical swarm intelligence, the step of motion control method in detail:

Step 1: at moment t, vehicle V to be positioned vehicle V1 adjacent thereto, V2, V3, V4, V5, V6 and information node N1 form cordless communication network, the coordinate of vehicle V1, V2, V3, V4, V5, V6 and information node N1 is known, and the position coordinates that sends self in the mode of broadcast transmission by wireless network is to vehicle V.

Step 2: vehicle V to be positioned receives the location coordinate information that neighbours' vehicle V1, V2, V3, V4, V5, V6 and information node N1 send is passed through the distance d between electromagnetic wave signal principle time of arrival measurement self and neighbours' vehicle V1, V2, V3, V4, V5, V6, information node N1 simultaneously _v1, d _v2, d _v3, d _v4, d _v5, d _v6, d _n1.

d _i＝ct _i，i＝v1,v2,v3,v4,v5,v6,N1 （1）

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

From d _v1, d _v2, d _v3, d _v4, d _v5, d _v6, d _n1the m=4 of a middle selected value minimum vehicle or information node are as the position reference node of vehicle to be positioned.In this example, choose V1, V2, V4, N1 as location reference point.

Step 3: according to the relative distance d between vehicle V to be positioned and reference point V1, V2, V4, N1 _v1, d _v2, d _v4, d _n1position coordinates (x with reference point _v1, y _v1), (x _v2, y _v2), (x _v4, y _v4), (x _n1, y _n1) set up positioning equation:

$\left\{\begin{array}{c}\sqrt{{(x_{v2t}-x_t)}^2+{(y_{v2t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}=d_{v2}-d_{v1}\\ \sqrt{{(x_{v3t}-x_t)}^2+{(y_{v3t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}=d_{v3}-d_{v1}\\ \sqrt{{(x_{v4t}-x_t)}^2+{(y_{v4t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}=d_{v4}-d_{v1}\\ \sqrt{{(x_{N1t}-x_t)}^2+{(y_{N1t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}=d_{N1}-d_{v1}\end{array}\right.---\left(2\right)$

In formula, (x _t, y _t) be that vehicle V to be positioned is at t moment coordinate.

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

$\left\{\begin{array}{c}{f}_1={[\sqrt{{(x_{v2t}-x_t)}^2+{(y_{v2t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}]}^2-{(d_{v2}-d_{v1})}^2=0\\ f_2={[\sqrt{{(x_{v3t}-x_t)}^2+{(y_{v3t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}]}^2-{(d_{v3}-d_{v1})}^2=0\\ f_3={[\sqrt{{(x_{v4t}-x_t)}^2+{(y_{v4t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}]}^2-{(d_{v4}-d_{v1})}^2=0\\ f_4={[\sqrt{{(x_{N1t}-x_t)}^2+{(y_{N1t}-y_t)}^2}-\sqrt{{(x_{v1t}-x_t)}^2+{(y_{v1t}-y_t)}^2}]}^2-{(d_{N1}-d_{v1})}^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 field, radius R=100, is chosen field in-group neighbours' vehicle, and its 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 coordinate figure on axle in the vertical direction.Establish vehicle herein in same level, z _ibe worth identical.

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.,i=v1,v2,v3,v4,v5,v6---\left(6\right)$

In formula, α _it, β _it, γ _itfor vehicle is at t direction of motion 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).

Embodiment 2.The present embodiment is described the real-time location navigation of moving vehicle based on bionical swarm intelligence, the structure of kinetic control system in detail:

The unified TMS320F2808 chip that adopts of data processing module in structure and path planning module; Wireless communication module adopts NRF905 chip; Memory module adopts W25X16AVSIG chip; Sensor assembly comprises ccd sensor TSL1401CL, accelerometer LSM303DLHC; Vehicle control module adopts MC9S12XS128MAA chip; Display module is LCD1602 display screen.

In the present invention, vehicle control terminal module comprises: wireless communication module, data processing module, data acquisition module, vehicle control module, path planning module, memory module and display module.Data processing module obtains the required parameter in location from wireless communication module, sets up positioning equation group, thereby and adopts ant colony algorithm positioning equation group to be solved to the position coordinates that obtains vehicle to be positioned.Data acquisition module receives the signal of measuring from vehicle self speed pickup, rotary angle transmitter, acceleration transducer, and is translated into digital quantity to obtain the motion state of self.Path planning module receives the signal from wireless communication module, data acquisition module, data processing module, to 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.

Vehicle control terminal module information flows to: vehicle in the process of moving, wireless communication module in vehicle-mounted control terminal is neighbours' vehicle and road information node transmission positioning request signal towards periphery, reception is from the positional information of neighbours' vehicle and road information node, and this information is sent to data processing module; Data acquisition module is measured locating required relative information, and is sent to data processing module; Data processing module sends respectively the positional information calculating to path planning module, vehicle control module, memory module and display module; Path planning module sends by data processing module the vehicle route of having planned to vehicle control module.

The bulk information node of road information node in being dispersed in road and around road forms, and each node is comprised of wireless communication module, data processing module and sensor assembly.Sensor assembly can measurement road environment in the road information such as vehicle fleet size, wagon flow speed and visibility, and by the analog signal transmission of measuring to data processing module.The simulating signal of data processing module autobiography sensor module in future is processed and is translated into digital signal.Wireless communication module sends to vehicle-mounted control terminal, road traffic control center by signals such as the node location from data processing module, road traffic, the speed of a motor vehicle.

Road information node module information flow direction is: sensor assembly is sent to data processing module by the relevant information measuring, and data processing module sends to wireless communication module after metrical information is processed.

Road traffic control center module is comprised of 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 relevant 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.

Road traffic control center module information flow direction is: wireless communication module sends to data processing module by the relevant information receiving, and information is specified and arranged to store in database module by data processing module after treatment; Data memory module also can send localized road geography information to data processing module simultaneously, and sends to required vehicle by wireless communication module.

The information flow direction of whole system is: in the process of moving, vehicle-mounted control terminal receives the positional information from neighbours' vehicle and road information node to vehicle, receives the localized road geography information from road traffic control center module simultaneously.In addition, the status information that vehicle-mounted control terminal also can send self is to road information node module and road traffic control center module.

Claims (2)

1. A mobile vehicle real-time positioning navigation, motion control method based on bionic swarm intelligence, is characterized in that comprising the following steps: Step 1: At time t, the vehicle forms a wireless communication network with its adjacent vehicles and information nodes, 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 swarm intelligence 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 neighbor vehicles; $\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 , γ _it are the moving direction of the vehicle along the three coordinate axes at time t, 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). 2. A mobile vehicle real-time positioning navigation and motion control system based on bionic swarm intelligence, characterized in that: it includes a vehicle-mounted 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 vehicle The parameters required for 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 exact position coordinates of the vehicle according to the algorithm; the data acquisition module receives information from the vehicle's own speed sensor , rotation angle sensor, and acceleration sensor, and convert it 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 to obtain real-time road environment and the vehicle itself Data, and the position coordinates of the vehicle itself, make reasonable and optimal planning for the driving path of the vehicle at the next moment; 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 The display module receives and displays the position and optimal path information from the data processing module and the path 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 transmits the information from the sensor module The analog signal is processed to convert it into a digital signal that the processor can recognize; 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 is preprocessed and stored in the database module, and at the same time, the relevant road geographic information is extracted from the database according to the needs of the vehicle control terminal module; the database module stores the traffic flow information and Road geographic information for navigation.

Images