CN103063218A - Vehicle remote monitoring and track reconstruction system and track reconstruction method thereof - Google Patents

Abstract

The invention discloses a vehicle remote monitoring and track reconstruction system, which comprises a mobile phone client, a vehicle system and a host computer; and the vehicle system comprises a first processor, a second processor, a GPS (Global Positioning System) module, a GPRS (General Packet Radio Service) module, a nine-axis attitude meter, an ethanol sensor, a temperature sensor and a bluetooth module. The invention further discloses a track reconstruction method of the system. Through the vehicle remote monitoring and track reconstruction system and the track reconstruction method thereof, real-time reconstruction of track is realized, the accuracy of track recovery is improved, and the disadvantage in the prior art that three-dimensional track reconstruction has just track but no attitude angle is overcome, so that the judgment efficiency is improved.

Description

Vehicle remote monitoring and track reproduction system and track reproducing method thereof

Technical field

The present invention relates to the vehicle monitoring technical field, particularly vehicle remote monitoring and track reproduction system and track reproducing method thereof.

Background technology

The domestic people of having proposes some vehicle management systems, the monitoring auto parts, whether vehicle gets into an accident or whether drunk driving of driver, but these management systems do not have the in real time function of reproduction of track of vehicle, and those onboard systems that are installed on the vehicle can not be revised its information simply from the outside simultaneously.

Now, much newly going out automobile can provide automatic alarm system, but these systems only rely on the acceleration transducer on the car, the instrument that tests the speed again in conjunction with certain algorithm, then directly report to the police when image data exceeds index.But for the false alarm reduction rate, these indexs are often higher, can not automatic alarm and cause getting into an accident.Existing " car accident track reproduction system " adopts acceleration transducer and gyroscope technology can reappear the three-dimensional track of vehicle, but this system can only process as traffic accident afterwards, can not the real-time exhibition track of vehicle, do not play shortening rescue time.Although and the picture that recovers of this technology is three-dimensional, car is not have attitude angle in the picture, can't observe out the state that car turns over side.

Summary of the invention

For the above-mentioned shortcoming and deficiency that overcomes prior art, the object of the present invention is to provide a kind of vehicle remote monitoring and track reproduction system, have vehicle 3D track and reappear in real time function.

Another object of the present invention is to provide the track reproducing method of said system.

Purpose of the present invention is achieved through the following technical solutions:

Vehicle remote monitoring and track reproduction system comprise cell-phone customer terminal, onboard system and host computer;

Described onboard system comprises first processor, the second processor, GPS module, GPRS module, gsm module, nine axle attitude instrument, alcohol sensor, temperature sensor, bluetooth module, and described first processor is connected with bluetooth module, nine axle attitude instrument respectively; Described the second processor is connected with GPS module, GPRS module, gsm module, alcohol sensor, temperature sensor respectively; Described first processor is connected by serial ports with the second processor;

Described onboard system carries out radio communication by bluetooth module and cell-phone customer terminal, carries out radio communication by GPRS module, gsm module and host computer.

Described nine axle attitude instrument comprise digital acceleration sensor and digital gyro instrument, and described digital acceleration sensor is connected with first processor with the digital gyro instrument and is connected.

The track reproducing method of above-mentioned vehicle remote monitoring and track reproduction system, the angular velocity that the acceleration that digital acceleration sensor is collected and digital gyro instrument collect carries out attitude algorithm, obtains the attitude angle of vehicle; Convert by the longitude and latitude that the GPS module is collected and to obtain the coordinate of vehicle; In conjunction with attitude angle and the coordinate of vehicle, realize that track reappears in real time.

Described attitude algorithm, be specially: adopt and find the solution quaternion differential equation, the data that the digital gyro instrument is gathered are with the formal representation of hypercomplex number, utilize and finish card algorithm renewal hypercomplex number, add simultaneously the 3-axis acceleration value that is obtained by digital acceleration sensor, utilize gradient descent method that digital gyroscope is corrected, the error that digital gyro instrument integration is produced trends towards 0, at last hypercomplex number is converted to attitude angle.

Described gradient descent method is specially:

The matrix that makes navigation coordinate system change to body axis system

$C_n^b=\left[\begin{array}{ccc}1-{2q}_2^2-{2q}_3^2& 2q_1{q}_2+{2q}_0{q}_3& {2q}_1{q}_3-{2q}_0{q}_2\\ {2q}_1{q}_2-{2q}_0{q}_3& 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_3^2& 2q_2{q}_3+{2q}_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\\ {2q}_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1& 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_2^2\end{array}\right]$

Wherein, q ₀, q ₁, q ₂, q ₃Be hypercomplex number;

If gravity acceleration g _nBe the amount under n system, utilize With g _nRotate in the b system g _n=[0,0,1], the definition error $\mathrm{\&Delta;a}=g_b-a_b=\left[\begin{array}{c}\mathrm{\&Delta;}{a}_x\\ \mathrm{\&Delta;}{a}_y\\ \mathrm{\&Delta;}{a}_z\end{array}\right]=\left[\begin{array}{c}{2q}_1{q}_3-{2q}_0{q}_2-{\mathrm{ax}}_b\\ {2q}_2{q}_3+{2q}_0{q}_1-{\mathrm{ay}}_b\\ 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_2^2-{\mathrm{az}}_b\end{array}\right],$ Constructor f (Δ a)=| Δ a| ², this function is asked gradient, respectively to q ₀, q ₁, q ₂, q ₃Ask local derviation to obtain

$\frac{\&PartialD;f}{\&PartialD;q_0}=-4\mathrm{\&Delta;}{a}_x{q}_2+4\mathrm{\&Delta;}{a}_y{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1$

$\frac{\&PartialD;f}{\&PartialD;{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}=4\mathrm{\&Delta;}{a}_x{q}_3+4\mathrm{\&Delta;}{a}_y{q}_0-4\mathrm{\&Delta;}{a}_{\mathrm{<figure-callout\; id="1"\; label="z\; q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">z</figure-callout>}}{q}_{}{}_1$

$\frac{\&PartialD;f}{\&PartialD;q_2}=-4\mathrm{\&Delta;}{a}_x{q}_0+4\mathrm{\&Delta;}{a}_y{q}_3-4\mathrm{\&Delta;}{a}_z{q}_2$

$\frac{\&PartialD;f}{\&PartialD;q_3}=4\mathrm{\&Delta;}{a}_{\mathrm{<figure-callout\; id="1"\; label="x\; q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">x</figure-callout>}}{q}_{}{}_1+4\mathrm{\&Delta;}{a}_y{q}_2$

Therefore new hypercomplex number is:

$q_{0(i+1)}=q_{0\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_0}$

$q_{1(i+1)}=q_{1\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}$

$q_{2(i+1)}=q_{2\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_2}$

$q_{3(i+1)}=q_{3\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_3}$

Carry out again the standardization of hypercomplex number by following formula

$q_{(i+1)}=\frac{q_{(i+1)}}{\sqrt[2]{q_{0(i+1)}^2+q_{1(i+1)}^2+q_{2(i+1)}^2+q_{3(i+1)}^2}}.$

The described conversion by the longitude and latitude that the GPS module is collected obtains the coordinate of vehicle, is specially:

The information that the GPS module the is collected processing that converts obtains latitude and longitude information, and pair warp and weft degree information adopts least square method to carry out match, obtains the coordinate of vehicle.

Compared with prior art, the present invention has the following advantages and beneficial effect:

The present invention carries out attitude algorithm by the angular velocity that acceleration that digital acceleration sensor is collected and digital gyro instrument collect, and obtains the attitude angle of vehicle; The coordinate of the vehicle that collects by the GPS module; In conjunction with attitude angle and the coordinate of vehicle, realize that track reappears in real time, improve the accuracy that track recovers, overcome the reproduction of prior art three-dimensional track and only had track not have the deficiency of attitude angle, thereby improved judging efficiency.

Description of drawings

Fig. 1 is the vehicle remote monitoring of embodiments of the invention and the schematic diagram of track reproduction system.

Fig. 2 is the composition schematic diagram of the onboard system of embodiments of the invention.

Fig. 3 is the course of work schematic diagram of the onboard system of embodiments of the invention.

Fig. 4 is the workflow diagram of the host computer of embodiments of the invention.

Fig. 5 is the workflow diagram of the cell-phone customer terminal of embodiments of the invention.

Fig. 6 is the process flow diagram of the attitude algorithm step of embodiments of the invention.

Fig. 7 is the process flow diagram of the long-term fusion steps of embodiments of the invention.

Fig. 8 is the process flow diagram of the rapid fusion step of embodiments of the invention.

Fig. 9 is the workflow diagram of the GPS module of embodiments of the invention.

Embodiment

Below in conjunction with embodiment and accompanying drawing, the present invention is described in further detail, but embodiments of the present invention are not limited to this.

Embodiment

As shown in Figure 1, the present embodiment vehicle remote monitoring and track reproduction system comprise cell-phone customer terminal, onboard system and host computer.

As shown in Figure 2, described onboard system comprises STM32 processor 1, STM32 processor 2, GPS module, GPRS module, gsm module, nine axle attitude instrument, alcohol sensor MQ303, temperature sensor 18B20, bluetooth module, and described first processor is connected with bluetooth module, nine axle attitude instrument respectively; Described the second processor is connected with GPS module, GPRS module, gsm module, alcohol sensor, temperature sensor respectively; Described first processor is connected by serial ports with the second processor; Described nine axle attitude instrument comprise digital acceleration sensor and digital gyro instrument, and described digital acceleration sensor is connected with first processor with the digital gyro instrument and is connected.

Described onboard system carries out radio communication by bluetooth module and cell-phone customer terminal, carries out radio communication by GPRS module, gsm module and host computer.

Cell-phone customer terminal arranges the essential information (driver, car plate etc.) of onboard system by blue teeth wireless.When travelling, onboard system adopts nine axle attitude instrument and GPS module to gather driving information, utilizes the strapdown inertial navigation algorithm to carry out coordinate data and resolves.When host computer inquiry or onboard system initiative alarming, by GPRS information is reached host computer operating system; Host computer operating system is with the information write into Databasce and be presented in the information report usefulness coordinate data drafting 3 D trajectory diagram.If have an accident, the make a telephone call to GSM environment inquiry field condition of onboard system of operator.

The below describes in detail to system's ingredient:

1, the course of work of onboard system is specifically seen Fig. 3, below forms the introduction of module for each:

(1) GPS module:

Can obtain satellite location data after this module initialization, module can be exported once by per second: the locator data of $ GPGGA $ GPGSA $ GPGSV $ GPRMC, these information outputs are to processor, treated device filters out our required data among the $ GPRMC, obtains real-time time, longitude and latitude, these real-time information of speed from this information.Wherein longitude and latitude algorithm process postmenstruation can draw the track of vehicle.

(2) alcohol sensor and temperature sensor

Temperature sensor can be monitored out the temperature of vehicle interior, judges whether vehicle the situations such as burning occur, more can monitor the temperature of automobile all parts after the later stage expansion, judges the state of vehicle with this.Whether alcohol sensor then is to have for the monitoring driver to drink before driving, avoids drunk driving.In case Monitoring Data occurs unusual, module can be reported to the police to processor, causes processor and responds accordingly.The most monitoring vehicle of present vehicle monitoring itself are not monitored this a part of gaps and omissions that the present invention is perfect to Che Nei and driver's situation.

(3) nine axle attitude instrument modules:

Nine axle attitude instrument comprise the ADXL345 digital acceleration sensor, L3G4200D digital gyro instrument.The angular velocity that the acceleration that collects by digital acceleration sensor and digital gyro instrument collect intactly resolves the attitude of body.

(4) GPRS communication module:

This module is used for communicating with far-end server, module do not occur when unusual and only is attached to and does not carry out data transmission on the GPRS network, only has when appearance is unusual and just can activate fully, and abnormal data is transferred to far-end server.

(5) gsm module:

This module is used for occurring in the vehicle monitoring data unusual, carried out the affirmation of accident situation under the alarm condition with the technician, after terminal was reported to the police to far-end server, the technician can pass through the gsm module of server to the gsm module communication of terminal, the inquiry accident situation.Avoid erroneous judgement with this, this is the manual feedback mechanism of our system, compares the waste that has reduced manpower and materials with other system, makes warning more accurate.

(6) bluetooth module:

This module is used for the Data Enter of terminal and mobile phone terminal, mobile phone can communicate by the bluetooth on self bluetooth and the terminal, owner information, information of vehicles are updated in the terminal and go, and the situations such as car owner's change, car plate change make more hommization of native system when being mainly used in the vehicle dealing.

2, host computer

Master system is comprised of six modules, its course of work as shown in Figure 4,

(1) tcp transmission control protocol: by the GPRS channel, communicate by letter with vehicle-mounted STM32;

(2) ACCESS database: memory of driving person, traffic police's information, and store accident information and send the coordinate data that the alerting signal vehicle transmits in real time;

(3) MFC class libraries: make accident information form and user interface;

(4) OPENGL three-dimensional picture API: drafting 3 D track of vehicle reproduction figure;

(5) gsm communication module: can with the conversations such as driver, traffic police;

(6) user's operational module: with the communication bridge of upper module, realize driver, the increase of traffic police's information, modification, realize the visual angle conversion of Track View, by track diagram decision event processing mode and write into Databasce.

3, cell-phone customer terminal

Cell-phone customer terminal adopts the rexsee exploitation, when mobile unit is installed onboard, with it the information (driver information and information of vehicles) of mobile unit is set; During the different driver of each replacing, come the information to equipment input driver with it, in order to have an accident rear contact driver.The workflow of cell-phone customer terminal as shown in Figure 5.

The track reproducing method of the vehicle remote monitoring of the present embodiment and track reproduction system: the angular velocity that the acceleration that digital acceleration sensor is collected and digital gyro instrument collect, carry out attitude algorithm, obtain the attitude angle of vehicle; Convert by the longitude and latitude that the GPS module is collected and to obtain the coordinate of vehicle; In conjunction with attitude angle and the coordinate of vehicle, realize that track reappears in real time:

Wherein attitude algorithm is: adopt and find the solution quaternion differential equation, the data that the digital gyro instrument is gathered are with the formal representation of hypercomplex number, utilize and finish card algorithm renewal hypercomplex number, add simultaneously the 3-axis acceleration value that is obtained by digital acceleration sensor, utilize gradient descent method that digital gyroscope is corrected, the error that digital gyro instrument integration is produced trends towards 0, at last hypercomplex number is converted to attitude angle, as shown in Figure 6, may further comprise the steps:

(1) makes i=0, the angle increment of asking i to order;

(2) attitude matrix of asking i to order;

(3) find the solution the hypercomplex number that i is ordered;

(4) make i=i+1;

(5) angle increment of asking i to order;

(6) judge whether to merge for a long time, if, carry out step (7), if not, carry out step (8);

(7) utilize gradient descent method that digital gyroscope is corrected, the error that digital gyro instrument integration is produced trends towards 0, at last hypercomplex number is converted to attitude angle; Carry out step (9);

(8) carry out rapid fusion, carry out step (9);

(9) hypercomplex number of asking i to order;

(10) i point attitude algorithms;

(11) judge whether to finish, if finish; If not, repeating step (4) ~ (11).

Wherein, the long-term steps flow chart that merges is seen Fig. 7, and the steps flow chart of rapid fusion is seen Fig. 8.

The below describes in detail to attitude algorithm:

(1) Quaternion Method of attitude algorithm general introduction

Quaternion Method (four parametric methods), for the kinetic measurement problem, body has displacement also to rotatablely move, and utilizes Quaternion Method to describe comparatively convenient, calculates also comparatively simple, the orthogonality of attitude matrix, only need to standardize to hypercomplex number get final product.Native system adopts the method to upgrade attitude matrix.

Hypercomplex number is made of four units, and it has complex expression, trigonometric expression, the representations such as matrix form and vector expression.

1) complex expression

Q=q ₀+q ₁i+q ₂j+q ₃k 4.1

q ₀, q ₁, q ₂, q ₃Be real number, i, j, k are mutually orthogonal empty vector of unit length.

2) trigonometric expression

$Q=\cos \frac{\mathrm{\&theta;}}{2}+u\sin \frac{\mathrm{\&theta;}}{2}---4.2$

θ is real number, and u is vector of unit length

3) matrix form

$Q=\left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]---4.3$

(2) relation of hypercomplex number and attitude matrix

The coordinate that vehicle adopts is b system, and what inertial navigation system adopted is n system, is transformed to the matrix of n system by b system

Be called attitude matrix, the renewal of attitude is the output according to inertia device, calculates in real time attitude matrix by computing.Because two coordinate systems are rectangular coordinate system, remain the right angle between each axle, coordinate system can be interpreted as rigid body, utilize coordinate translation, two coordinate origins are overlapped, and the at this moment shutdown between two coordinate systems is rigid body around the relation of fixed point rotary, draws thus following Quaternion Algorithm.

Hypercomplex number Q comprises by b system through forms the information that n is without the disposable equivalent rotary of pilot process, and u represents to rotate instantaneous axis and sense of rotation, and θ represents the angle that turns over.

Therefore can determine that b is tied to the attitude matrix of n system

For

$C_b^n=\left[\begin{array}{ccc}1-2q_2^2-{2q}_3^2& 2q_1{q}_2-{2q}_0{q}_3& 2q_1{q}_3+{2q}_0{q}_2\\ 2q_1{q}_2+2q_0{q}_2& 1-2{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_3^2& 2q_1{q}_3-2q_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\\ {2q}_1{q}_3-{2q}_0{q}_2& 2q_2{q}_3+2q_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1& 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_2^2\end{array}\right]---4.4$

Because

Be versor, so attitude matrix also can be expressed as

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2+q_3^2& 2q_1{q}_2-{2q}_0{q}_3& 2q_1{q}_3+{2q}_0{q}_2\\ 2q_1{q}_2+{2q}_0{q}_3& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2-q_3^2& 2q_2{q}_3-{2q}_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\\ {2q}_1{q}_3-2q_0{q}_2& 2q_2{q}_3+{2q}_0{q}_1& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2+q_3^2\end{array}\right]---4.5$

(3) implementation procedure of attitude algorithm algorithm

1) asks first angle increment (i=0)

The digital gyro instrument that native system adopts is L3G4200D, and digital acceleration sensor is ADXL345, and the two output signal is digital signal, so image data only need to arrive to read in the register of correspondence and gets final product.

Enter the medium counter overflow for the treatment of of major cycle and interrupt through a series of initialization is laggard after system powers on, timing is 20ms, enters immediately image data ax, ay when interrupting arriving, az, gx, gy, gz, first three is for reading the value of digital acceleration sensor, and rear three is the value that reads the digital gyro instrument, gx, gy, gz are respectively the digital gyro instrument around x, y, the angular velocity that the rotation of z axle obtains, so angle increment wx=gx * t, wy=gy * t, wz=gz * t, t herein is the sampling time, equals 0.02.

Wx simultaneously, wy, wz be also as the Roll of first point, Pitch, Yaw Eulerian angle.

2) ask initial attitude matrix

If take navigation coordinate be n as reference frame, it is gone to body axis system b, need to through three times the rotation.

OX _nY _nZ _n---around Z _nAxle turns z degree → OX ₁Y ₁Z ₁---around X ₁Axle turns x degree → OX ₂Y ₂Z ₂---turn y degree → OX around the Y2 axle _bY _bZ _b

According to the rotation order that following formula is described, can write out the expression formula of corresponding rotation matrix:

${\mathrm{<figure-callout\; id="1"\; label="C\; n"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">C</figure-callout>}}_n^{}=\left[\begin{array}{ccc}\cos z& -\sin z& 0\\ \sin z& \cos z& 0\\ 0& 0& 1\end{array}\right]$

$C_1^b=C_2^{\mathrm{<figure-callout\; id="1"\; label="b\; C"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">b</figure-callout>}}{C}_{}^{}{}_1^2\left[\begin{array}{ccc}\cos y& 0& -\sin y\\ 0& 1& 0\\ \sin y& 0& \mathrm{<figure-callout\; id="1"\; label="cos\; y"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">cos</figure-callout>}y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos x& \sin x\\ 0& -\sin x& \cos x\end{array}\right]=\left[\begin{array}{ccc}\cos y& \sin x\sin y& -\cos x\sin y\\ 0& \cos x& \sin x\\ \sin y& -\sin x\cos y& \cos x\cos y\end{array}\right]$

$C_n^b=C_1^{\mathrm{<figure-callout\; id="1"\; label="b\; C\; n"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">b</figure-callout>}}{C}_n^{}{}_1^{}\left[\begin{array}{ccc}\cos y\cos z+\sin x\sin y\sin z& -\cos y\sin z+\sin x\sin y\cos z& -\cos x\sin y\\ \cos x\sin z& \cos x\cos z& \sin x\\ \sin y\cos z-\sin x\cos y\sin z& -\sin y\sin z-\sin x\cos y\cos z& \cos x\cos y\end{array}\right]$

Because

Orthogonal matrix, as can be known

$C_b^n={\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T$

$=\left[\begin{array}{ccc}\cos y\cos z+\sin x\sin y\sin z& \cos x\sin z& \sin y\cos z-\sin x\cos y\sin z\\ -\cos y\sin z+\sin x\sin y\cos z& \cos x\cos z& -\sin y\sin z-\sin x\cos y\cos z\\ -\cos x\sin y& \sin x& \cos x\cos y\end{array}\right]$

Be denoted as $C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---4.6$

3) ask first hypercomplex number

By initial attitude matrix

The pass of hypercomplex number and matrix is as can be known:

$\left\{\begin{array}{c}{q}_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2-q_3^2=T_{11}\\ q_0^2-q_0^2+q_2^2-q_3^2=T_{22}\\ q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2+q_3^2=T_{33}\\ 2(q_1{q}_2-q_0{q}_3)=T_{12}\\ 2(q_1{q}_3+q_0{q}_2)=T_{13}\\ 2(q_1{q}_2+q_0{q}_3)=T_{21}\\ 2(q_2{q}_3-q_0{q}_1)=T_{23}\\ 2(q_1{q}_3-q_0{q}_2)=T_{31}\\ 2(q_2{q}_3+q_0{q}_1)=T_{32}\\ q_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2+q_3^2=1\end{array}\right.---4.7$

Can solve hypercomplex number $\left\{\begin{array}{c}\left|q_0\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\\ \left|{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\end{array}\right.$ And $\left\{\begin{array}{c}4q_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1=T_{32}-T_{23}\\ {4q}_0{q}_2=T_{13}-T_{31}\\ {4q}_0{q}_3=T_{21}-T_{12}\end{array}\right.$

Q wherein ₀Symbol any, q ₁, q ₂, q ₃Symbol satisfy following formula:

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}(T_{32}-T_{23})\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}(T_{13}-T_{31})\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}(T_{21}-T_{12})\end{array}\right.$

4) ask the next angle increment (i=i+1) of putting

wx _(i+1)＝gx _(i+1)×t

wy _(i+1)＝gy _(i+1)×t

wz _(i+1)＝gz _(i+1)×t

5) calculate i+1 hypercomplex number constantly according to i hypercomplex number and i+1 angle increment constantly constantly.

Here adopt the timing sampling method of addition that finishes in the card algorithm to find the solution hypercomplex number.

$Q_{(i+1)}={\mathrm{<figure-callout\; id="1"\; label="e"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">e</figure-callout>}}^{\frac{1}{2}{\&Integral;}_i^{i+1}{M}^{\&prime;}\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)\mathrm{dt}}\&CenterDot;Q_{\left(i\right)}---4.8$

Wherein

The output (machinery is turned must be through moving from gyro, the compensation of static error) of digital gyro instrument,

Be up-to-date attitude matrix.

With

Respectively position speed and earth rate.

Order $\mathrm{\&Delta;\&Theta;}={\&Integral;}_i^{i+1}{M}^{\&prime;}\left({\mathrm{\&omega;}}_{\mathrm{nb}}^b\right)\mathrm{dt}={\&Integral;}_i^{i+1}\left(\begin{array}{cccc}0& -\mathrm{wx}& -\mathrm{wy}& -\mathrm{wz}\\ \mathrm{wx}& 0& \mathrm{wz}& -\mathrm{wy}\\ \mathrm{wy}& -\mathrm{wz}& 0& \mathrm{wx}\\ \mathrm{wz}& \mathrm{wy}& -\mathrm{wx}& 0\end{array}\right)\mathrm{dt}$

$\&ap;\left[\begin{array}{cccc}0& -\mathrm{\&Delta;\&theta;x}& -\mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}\\ \mathrm{\&theta;x}& 0& \mathrm{\&Delta;\&theta;z}& -\mathrm{\&Delta;\&theta;y}\\ \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}& 0& \mathrm{\&Delta;\&theta;x}\\ \mathrm{\&Delta;\&theta;z}& \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;x}& 0\end{array}\right]$

Formula 4.8 is made Taylor series expansion can be got

$Q_{(i+1)}={\mathrm{<figure-callout\; id="1"\; label="e"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">e</figure-callout>}}^{\frac{1}{2}\mathrm{\&Delta;\&Theta;}}\&CenterDot;Q_{\left(i\right)}=[I+\frac{0.5\mathrm{\&Delta;\&Theta;}}{1!}+\frac{{\left(0.5\mathrm{\&Delta;\&Theta;}\right)}^2}{2!}+...]Q_{\left(i\right)}---4.9$

Because $\mathrm{\&Delta;}{\mathrm{\&Theta;}}^2=\left[\begin{array}{cccc}0& -\mathrm{\&Delta;\&theta;x}& -\mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}\\ \mathrm{\&Delta;\&theta;x}& 0& \mathrm{\&Delta;\&theta;z}& -\mathrm{\&Delta;\&theta;y}\\ \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}& 0& \mathrm{\&Delta;\&theta;x}\\ \mathrm{\&Delta;\&theta;z}& \mathrm{\&Delta;\&theta;y}& -\mathrm{x\&Delta;\&theta;}& 0\end{array}\right]\left[\begin{array}{cccc}0& -\mathrm{\&Delta;\&theta;x}& -\mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}\\ \mathrm{\&Delta;\&theta;x}& 0& \mathrm{\&Delta;\&theta;z}& -\mathrm{\&Delta;\&theta;y}\\ \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}& 0& \mathrm{\&Delta;\&theta;x}\\ \mathrm{\&Delta;\&theta;z}& \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;x}& 0\end{array}\right]$

$=\left[\begin{array}{cccc}0& -\mathrm{\&Delta;\&theta;x}& -\mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}\\ \mathrm{\&Delta;\&theta;x}& 0& \mathrm{\&Delta;\&theta;z}& -\mathrm{\&Delta;\&theta;y}\\ \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;z}& 0& \mathrm{\&Delta;\&theta;x}\\ \mathrm{\&Delta;\&theta;z}& \mathrm{\&Delta;\&theta;y}& -\mathrm{\&Delta;\&theta;x}& 0\end{array}\right]=-\mathrm{\&Delta;}{\mathrm{\&theta;}}^{2}I$

I is unit matrix, So have

ΔΘ ³＝ΔΘ ²·ΔΘ=-Δθ ²ΔΘ

ΔΘ ⁴＝ΔΘ ²·ΔΘ ²=Δθ ⁴I

ΔΘ ⁵＝ΔΘ ⁴·ΔΘ=Δθ ⁴ΔΘ

ΔΘ ⁶＝ΔΘ ⁴·ΔΘ ²=-Δθ ⁶I

So 4.9 formula abbreviations get

$Q_{i+1}=\{I+I[\frac{\frac{\mathrm{\&Delta;\&Theta;}}{2}}{1!}+\frac{-{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^2}{2!}+\frac{-{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^2\frac{\mathrm{\&Delta;\&Theta;}}{2}}{3!}+\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^4}{4!}+\frac{\left({\frac{\mathrm{\&Delta;\&theta;}}{2}}^2\right)}{5!}+\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^2\frac{\mathrm{\&Delta;\&Theta;}}{2}}{5!}+\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^6}{6!}+...\left]\right\}{Q}_i$

$Q_{i+1}=\left\{I\right[1-\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^2}{2!}+\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^4}{4!}-\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^6}{6!}+...]+\frac{\mathrm{\&Delta;\&Theta;}}{2}[\frac{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}{1!}-\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^3}{3!}+\frac{{\left(\frac{\mathrm{\&Delta;\&theta;}}{2}\right)}^5}{5!}-...\left]\frac{1}{\frac{\mathrm{\&Delta;\&theta;}}{2}}\right\}{Q}_i$

$=[I\cos \frac{\mathrm{\&Delta;\&theta;}}{2}+\mathrm{\&Delta;\&Theta;}\frac{\sin \frac{\mathrm{\&Delta;\&theta;}}{2}}{\mathrm{\&Delta;\&theta;}}]Q_i$

Wherein

With

Must calculate by the finite term of series expansion, native system adopts three rank approximate datas of hypercomplex number to calculate i+1 hypercomplex number constantly $Q_{i+1}=[I(1-\frac{{\mathrm{\&Delta;\&theta;}}^2}{8})+(\frac{1}{2}-\frac{\mathrm{\&Delta;}{\mathrm{\&theta;}}^2}{48})\mathrm{\&Delta;\&Theta;}]Q_i.$

6) standardization processing of hypercomplex number

The hypercomplex number that is used for the rotation of sign rigid body is versor, namely

But owing to the error of the reasons such as calculating, hypercomplex number can be lost standardization gradually in the computation process, therefore just must carry out standardization processing to it, adopts three rank method of approximation owing to upgrading hypercomplex number, and therefore each computing all must standardize to hypercomplex number.

$q_i=\frac{{\hat{q}}_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}},$ i=0,1,2,3 4.10

7) the i+1 attitude reproduction of ordering

θ=Pitch, Ψ=Yaw is respectively roll angle, the angle of pitch, course angle

w＝q ₀，x＝q ₁，y＝q ₂，z＝q ₃

8) ask i+1 posture renewal matrix constantly

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

The attitude of point afterwards and track can use the same method and calculate reproduction, thereby obtain vehicle attitude and track under navigation coordinate system in setting-up time.

(4) error of utilizing gradient descent method correcting digital gyroscope integration to produce

Because attitude matrix Orthogonal matrix, the matrix that therefore makes navigation coordinate system change to body axis system

$C_n^b=\left[\begin{array}{ccc}1-{2q}_2^2-{2q}_3^2& 2q_1{q}_2+{2q}_0{q}_3& {2q}_1{q}_3-{2q}_0{q}_2\\ {2q}_1{q}_2-{2q}_0{q}_3& 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_3^2& 2q_2{q}_3+{2q}_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1\\ {2q}_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1& 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_2^2\end{array}\right]$

Wherein, q ₀, q ₁, q ₂, q ₃Be hypercomplex number;

If gravity acceleration g _nBe the amount under n system, utilize

With g _nRotate in the b system g _n=[0,0,1], the definition error $\mathrm{\&Delta;a}=g_b-a_b=\left[\begin{array}{c}\mathrm{\&Delta;}{a}_x\\ \mathrm{\&Delta;}{a}_y\\ \mathrm{\&Delta;}{a}_z\end{array}\right]=\left[\begin{array}{c}{2q}_1{q}_3-{2q}_0{q}_2-{\mathrm{ax}}_b\\ {2q}_2{q}_3+{2q}_0{q}_1-{\mathrm{ay}}_b\\ 1-{2\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-{2q}_2^2-{\mathrm{az}}_b\end{array}\right],$ Constructor f (Δ a)=| Δ a| ², this function is asked gradient, respectively to q ₀, q ₁, q ₂, q ₃Ask local derviation to obtain

$\frac{\&PartialD;f}{\&PartialD;q_0}=-4\mathrm{\&Delta;}{a}_x{q}_2+4\mathrm{\&Delta;}{a}_{\mathrm{<figure-callout\; id="1"\; label="y\; q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">y</figure-callout>}}{q}_{}{}_1$

$\frac{\&PartialD;f}{\&PartialD;{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}=4\mathrm{\&Delta;}{a}_x{q}_3+4\mathrm{\&Delta;}{a}_y{q}_0-4\mathrm{\&Delta;}{a}_{\mathrm{<figure-callout\; id="1"\; label="z\; q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">z</figure-callout>}}{q}_{}{}_1$

$\frac{\&PartialD;f}{\&PartialD;q_2}=-4\mathrm{\&Delta;}{a}_x{q}_0+4\mathrm{\&Delta;}{a}_y{q}_3-4\mathrm{\&Delta;}{a}_z{q}_2$

$\frac{\&PartialD;f}{\&PartialD;q_3}=4\mathrm{\&Delta;}{a}_{\mathrm{<figure-callout\; id="1"\; label="x\; q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">x</figure-callout>}}{q}_{}{}_1+4\mathrm{\&Delta;}{a}_y{q}_2$

Therefore new hypercomplex number is:

$q_{0(i+1)}=q_{0\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_0}$

$q_{1(i+1)}=q_{1\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA00002609271900051.png,HDA00002609271900081.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}$

$q_{2(i+1)}=q_{2\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_2}$

$q_{3(i+1)}=q_{3\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_3}$

Carry out again the standardization of hypercomplex number by following formula

$q_{(i+1)}=\frac{q_{(i+1)}}{\sqrt[2]{q_{0(i+1)}^2+q_{1(i+1)}^2+q_{2(i+1)}^2+q_{3(i+1)}^2}}.$

The below is introduced the course of work of GPS module:

The information that the GPS module the is collected processing that converts obtains latitude and longitude information, and pair warp and weft degree information adopts least square method to carry out match, obtains the coordinate of vehicle, as shown in Figure 9, specifically may further comprise the steps:

The time interval that gathers gps signal is 1s, extracts the latitude and longitude information when vehicle in front when obtaining information of vehicles at every turn.Because the latitude and longitude information that collects also exists certain drift error, therefore at Data processing, adopted least square method to carry out match, so that the track that draws is relatively level and smooth.Owing to only considering latitude and longitude information, the track that therefore obtains just reappears on XOY plane, and the displacement on the Z axis must cooperate the Roll angle to calculate, and detailed process is as follows:

(1) obtains GPS information and do the longitude and latitude that corresponding processing obtains body.

(2) longitude and latitude is changed the variation that is converted in the displacement, obtain Δ x, Δ y, the m of unit.

(3) get suitable some number, the match of 5 points is adopted in this design, take time t as horizontal ordinate, and displacement x, y goes out corresponding straight line as ordinate with least square fitting.

(4) take out axial translation corresponding to corresponding time, the m of unit at the straight line that simulates.

(5) wait for the next time arrival of gps signal, obtain the latitude and longitude information of next point, repeat the work of (1) ~ (4), linear regression line is upgraded.

Above-described embodiment is the better embodiment of the present invention; but embodiments of the present invention are not limited by the examples; other any do not deviate from change, the modification done under Spirit Essence of the present invention and the principle, substitutes, combination, simplify; all should be the substitute mode of equivalence, be included within protection scope of the present invention.

Claims (6)

1. vehicle remote monitoring and track reproduction system is characterized in that, comprises cell-phone customer terminal, onboard system and host computer;

Described onboard system comprises first processor, the second processor, GPS module, GPRS module, gsm module, nine axle attitude instrument, alcohol sensor, temperature sensor, bluetooth module, and described first processor is connected with bluetooth module, nine axle attitude instrument respectively; Described the second processor is connected with GPS module, GPRS module, gsm module, alcohol sensor, temperature sensor respectively; Described first processor is connected by serial ports with the second processor;

Described onboard system carries out radio communication by bluetooth module and cell-phone customer terminal, carries out radio communication by GPRS module, gsm module and host computer.

2. vehicle remote monitoring according to claim 1 and track reappear system, it is characterized in that, described nine axle attitude instrument comprise digital acceleration sensor and digital gyro instrument, and described digital acceleration sensor is connected with first processor with the digital gyro instrument and is connected.

3. the track reproducing method of vehicle remote monitoring claimed in claim 2 and track reproduction system is characterized in that, the angular velocity that the acceleration that digital acceleration sensor is collected and digital gyro instrument collect carries out attitude algorithm, obtains the attitude angle of vehicle; Convert by the longitude and latitude that the GPS module is collected and to obtain the coordinate of vehicle; In conjunction with attitude angle and the coordinate of vehicle, realize that track reappears in real time.

4. track reproducing method according to claim 3, it is characterized in that, described attitude algorithm, be specially: adopt and to find the solution quaternion differential equation, the data that the digital gyro instrument is gathered are with the formal representation of hypercomplex number, utilize to finish the card algorithm and upgrade hypercomplex number, add simultaneously the 3-axis acceleration value that is obtained by digital acceleration sensor, utilize gradient descent method that digital gyroscope is corrected, the error that digital gyro instrument integration is produced trends towards 0, at last hypercomplex number is converted to attitude angle.

5. track reproducing method according to claim 4 is characterized in that, described gradient descent method is specially:

The matrix that makes navigation coordinate system change to body axis system

$C_n^b=\left[\begin{array}{ccc}1-{2q}_2^2-{2q}_3^2& 2q_1{q}_2+{2q}_0{q}_3& {2q}_1{q}_3-{2q}_0{q}_2\\ {2q}_1{q}_2-{2q}_0{q}_3& 1-{2q}_1^2-{2q}_3^2& 2q_2{q}_3+{2q}_0{q}_1\\ {2q}_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-{2q}_1^2-{2q}_2^2\end{array}\right]$

Wherein, q ₀, q ₁, q ₂, q ₃Be hypercomplex number;

If gravity acceleration g _nBe the amount under n system, utilize

With g _nRotate in the b system g _n=[0,0,1], the definition error $\mathrm{\&Delta;a}=g_b-a_b=\left[\begin{array}{c}\mathrm{\&Delta;}{a}_x\\ \mathrm{\&Delta;}{a}_y\\ \mathrm{\&Delta;}{a}_z\end{array}\right]=\left[\begin{array}{c}{2q}_1{q}_3-{2q}_0{q}_2-{\mathrm{ax}}_b\\ {2q}_2{q}_3+{2q}_0{q}_1-{\mathrm{ay}}_b\\ 1-{2q}_1^2-{2q}_2^2-{\mathrm{az}}_b\end{array}\right],$ Constructor f (Δ a)=| Δ a| ², this function is asked gradient, respectively to q ₀, q ₁, q ₂, q ₃Ask local derviation to obtain

$\frac{\&PartialD;f}{\&PartialD;q_0}=-4\mathrm{\&Delta;}{a}_x{q}_2+4\mathrm{\&Delta;}{a}_y{q}_1$

$\frac{\&PartialD;f}{\&PartialD;q_1}=4\mathrm{\&Delta;}{a}_x{q}_3+4\mathrm{\&Delta;}{a}_y{q}_0-4\mathrm{\&Delta;}{a}_z{q}_1$

$\frac{\&PartialD;f}{\&PartialD;q_2}=-4\mathrm{\&Delta;}{a}_x{q}_0+4\mathrm{\&Delta;}{a}_y{q}_3-4\mathrm{\&Delta;}{a}_z{q}_2$

$\frac{\&PartialD;f}{\&PartialD;q_3}=4\mathrm{\&Delta;}{a}_x{q}_1+4\mathrm{\&Delta;}{a}_y{q}_2$ Therefore new hypercomplex number is:

$q_{0(i+1)}=q_{0\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_0}$

$q_{1(i+1)}=q_{1\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_1}$

$q_{2(i+1)}=q_{2\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_2}$

$q_{3(i+1)}=q_{3\left(i\right)}-\mathrm{dt}\&times;\frac{\&PartialD;f}{\&PartialD;q_3}$

Carry out again the standardization of hypercomplex number by following formula

$q_{(i+1)}=\frac{q_{(i+1)}}{\sqrt[2]{q_{0(i+1)}^2+q_{1(i+1)}^2+q_{2(i+1)}^2+q_{3(i+1)}^2}}.$

6. track reproducing method according to claim 3 is characterized in that, the described conversion by the longitude and latitude that the GPS module is collected obtains the coordinate of vehicle, is specially:

The information that the GPS module the is collected processing that converts obtains latitude and longitude information, and pair warp and weft degree information adopts least square method to carry out match, obtains the coordinate of vehicle.

Images