Method of Updating All-Attitude Angle of Agricultural Machine Based on Nine-Axis MEMS Sensor

Abstract

A method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor includes the following steps: establishing an error model of a gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model, and setting a parameter vector corresponding to a vehicle motional attitude (S1); acquiring data including an acceleration and an angular velocity of a motion of vehicle, and an geomagnetic field intensity in real time (S2); calculating an angle, a velocity, position information, and a course angle of the vehicle by established error model of the gyroscope and the electronic compass calibration ellipse model(S3); data-fusion processing the angle, the velocity, the position information and the course angle of the vehicle by the seven-dimensional EKF filtering model, and updating a motional attitude angle of the vehicle in real time. The steps of the method have a small error, high precision, and reliability.

Claims

1. A method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor, comprising the following steps: establishing an error model of a gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model, and setting a parameter vector corresponding to a vehicle motional attitude; acquiring data including an acceleration and an angular velocity of a motion of vehicle, and a geomagnetic field intensity in real time by a nine-axis MEMS sensor; calculating an angle, a velocity, position information, and a course angle of the vehicle by established error model of the gyroscope and the electronic compass calibration ellipse model, according to obtained data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity; and data-fusion processing the angle, the velocity, the position information and the course angle of the vehicle by the seven-dimensional EKF filtering model, and updating a motional attitude angle of the vehicle in real time; wherein the nine-axis MEMS sensor is composed of a three-axis gyroscope, a three-axis acceleration sensor, and a three-axis geomagnetic sensor.

2. The method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor of claim 1, wherein the step of establishing an error model of a gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model, and setting a parameter vector corresponding to a vehicle motional attitude further includes, calculating the angular velocity of the gyroscope in the error model of the gyroscope via an error calculation formula of the gyroscope; wherein the error calculation formula of the gyroscope is: ω=ω.sub.ib+b.sub.ωr+b.sub.ωg, wherein co is an angular velocity output by the gyroscope, ω.sub.ib is a real angular velocity of the gyroscope, b.sub.ωr is a zero drift of the gyroscope, and b.sub.ωg is a white noise output by the gyroscope; eliminating a magnetic field interference by the electronic compass calibration ellipse model; wherein the electronic compass calibration ellipse model is: $\frac{{(mx - Xoffset)}^{2}}{{Xsf}^{.Math. 2}} + \frac{{(my - Yoffset)}^{2}}{{Ysf}^{.Math. 2}} = 1,$ wherein mx, my are magnetic field intensities, Xoffset and Yoffset are hard magnetic interferences, and Xsf and Ysf are soft magnetic interferences; updating the vehicle attitude by the seven-dimensional EKF filtering model, wherein, the seven-dimensional EKF filtering model uses an extended Kalman filter for a seven-dimensional state vector, and EKF includes a state equation and an observation equation:
{dot over (x)}=f(x,ω)+w1
y=h(x)+v1 wherein the state matrix is x [q, b.sub.ωr], is quaternion vectors q.sub.0, q.sub.1, q.sub.2, q.sub.3, and b.sub.ωr is a zero drift of the three-axis gyroscope, wherein ω is the angular velocity output by the gyroscope, w1 is a process noise matrix, v1 is an observation noise matrix, y is an observation vector, y=[a ψ.sub.mag].sup.T, wherein a is a three-axis acceleration value, ψ.sub.nag is a course angle calculated by the electronic compass, $f (x, ω) = [\underset{.Math. 0^{3 \times 1}}{\frac{1}{2} [\begin{matrix} - q_{1} & - q_{2} & - q_{3} \\ q_{0} & - q_{3} & q_{2} \\ q_{3} & q_{0} & - q_{1} \\ - q_{2} & q_{1} & q_{0} \end{matrix}]} [\begin{matrix} ω_{x} - b_{ω .Math. .Math. rx} \\ ω_{y} - b_{ω .Math. .Math. ry} \\ ω_{2} - b_{ω .Math. .Math. rz} \end{matrix}]]$ $h = [\begin{matrix} 2 .Math. g (q_{1} .Math. q_{3} - q_{0} .Math. q_{2}) \\ 2 .Math. g (q_{2} .Math. q_{3} + q_{0} .Math. q_{1}) \\ g (q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) \\ \tan^{- 1} (\frac{2 .Math. (q_{1} .Math. q_{2} + q_{0} .Math. q_{3})}{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}}) \end{matrix}] .$

3. The method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor of claim 2, wherein the step of acquiring data including an acceleration and an angular velocity of a motion of vehicle, and an geomagnetic field intensity in real time by a nine-axis MEMS sensor further includes, obtaining the angular velocity of the vehicle through the gyroscope, and compensating the zero drift of the gyroscope; acquiring the acceleration of the vehicle by the acceleration sensor; and acquiring the geomagnetic field intensity of the vehicle by the geomagnetic sensor.

4. The method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor of claim 3, wherein the step of calculating an angle, a velocity, position information, and a course angle of the vehicle by established error model of the gyroscope and the electronic compass calibration ellipse model, according to obtained data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity further includes, obtaining the angle by an integral calculation of the angular velocity by the error model of the gyroscope; calculating the velocity by integrating the acceleration, and the position information is calculated by further integrating the velocity; and calculating the course angle of the vehicle from the geomagnetic field intensity which is compensated by a calibration parameter and corrected by an oblique angle, and both the calibration parameter and the oblique angle being calculated by the elliptical model.

5. The method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor of claim 4, wherein the step of data-fusion processing the angle, the velocity, the position information and the course angle of the vehicle by the seven-dimensional EKF filtering model, and updating the motional attitude angle of the vehicle in real time further includes, calculating the motional attitude angle of the vehicle by the seven-dimensional EKF filtering model, through a quaternion attitude updating algorithm, wherein a calculation process of the EKF algorithm is as below: ${\hat{x}}_{k} (-) = Φ_{k - 1} .Math. {\hat{x}}_{k} (+)$ $Φ_{k} = e^{\frac{\partial f}{\partial x}}$ $P_{k} (-) = Φ_{k - 1} .Math. P_{k - 1} (+) .Math. Φ_{k - 1}^{T} + Q$ ${\hat{x}}_{k} (+) = {\hat{x}}_{k} (-) + K_{k} .Math. .Math. y_{k} - h ({\hat{x}}_{k} (-)) .Math.$ $P_{k} (+) = [I - K_{k} .Math. H_{k}] .Math. P_{k} (-)$ $H_{k} = \frac{\partial h}{\partial x} .Math. _{k} .Math. .Math. K_{k} = P_{k} (-) .Math. H_{k} [H_{k} .Math. P_{k} (-) .Math. H_{k}^{T} + R_{k}]$ wherein k is a sampling time point, {right arrow over (x)}.sub.k is a system state estimation, (−) is a previous time point, (+) is a later time point, Φ.sub.k is a state transition matrix, P.sub.k is a minimum mean square error matrix, Q is a covariance matrix corresponding to the state vector, K.sub.k is an error gain, y.sub.k is an observation vector, H.sub.k is a transition matrix for the observation equation, R.sub.k is a covariance matrix corresponding to the observation vector; $Q = \frac{q_{0} + q_{1} .Math. i + q_{2} .Math. j + q_{3} .Math. k}{\sqrt{q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2}}}$ wherein Q is a quaternion vector, q.sub.0, q.sub.1, q.sub.2, q.sub.3 are scalars forming the quaternion vector, i, j, k are unit vectors in a three-dimensional coordinate system, an updated attitude matrix is as below: $C_{b}^{n} = [\begin{matrix} q_{1}^{2} + q_{0}^{2} - q_{3}^{2} - q_{2}^{2} & 2 .Math. (q_{1} .Math. q_{2} - q_{0} .Math. q_{3}) & 2 .Math. (q_{1} .Math. q_{3} + q_{0} .Math. q_{2}) \\ 2 .Math. (q_{1} .Math. q_{2} + q_{0} .Math. q_{3}) & q_{2}^{2} - q_{3}^{2} + q_{0}^{2} - q_{1}^{2} & 2 .Math. (q_{2} .Math. q_{3} - q_{0} .Math. q_{1}) \\ 2 .Math. (q_{1} .Math. q_{3} + q_{0} .Math. q_{2}) & 2 .Math. (q_{2} .Math. q_{3} + q_{0} .Math. q_{1}) & q_{3}^{2} - q_{2}^{2} - q_{1}^{2} + q_{0}^{2} \end{matrix}]$ wherein C.sub.b.sup.n is a rotation matrix for transforming a carrier coordinate system to a navigation coordinate system, $C_{b}^{n} = (\begin{matrix} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{matrix}) = (\begin{matrix} \cos .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ + \sin .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ & \sin .Math. .Math. ψ .Math. .Math. \cos .Math. .Math. θ & \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ - \cos .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ \\ - \cos .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ + \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ & \cos .Math. .Math. ψ .Math. .Math. \cos .Math. .Math. θ & - \sin .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ - \cos .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ \\ - \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. θ & \sin .Math. .Math. θ & \cos .Math. .Math. γ .Math. .Math. \cos .Math. .Math. θ \end{matrix})$ wherein γ, θ, ψ are a rolling angle, a pitch angle, and the course angle respectively.

6. The method of updating an all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor of claim 5, wherein after the step of data-fusion processing the angle, the velocity, the position information and the course angle of the vehicle by the seven-dimensional EKF filtering model, and updating the motional attitude angle of the vehicle in real time, the following step is performed: extracting the all-attitude angle of the vehicle from updated motional attitude angle of the vehicle, to determine a value of the all-attitude angle, the all-attitude angle of the vehicle including the pitch angle, the rolling angle and the course angle, wherein, ${\begin{matrix} ψ_{Principal} = \arctan .Math. .Math. (- \frac{C_{12}}{C_{22}}) \\ θ_{Principal} = \arcsin .Math. .Math. C_{32} \\ γ_{Principal} = \arctan .Math. .Math. (- \frac{C_{31}}{C_{33}}) \end{matrix}$ the course angle is: $ψ = {\begin{matrix} ψ_{principle} \\ ψ_{principle} + 360 .Math. ° & When .Math. .Math. C_{22} > 0 .Math. {\begin{matrix} ψ_{principle} > 0 \\ ψ_{principle} < 0 \end{matrix} \\ ψ_{principle} + 180 .Math. ° & When .Math. .Math. C_{22} < 0 \end{matrix}$ the pitch angle is: θ=θ.sub.principle the rolling angle is: $γ = {\begin{matrix} γ_{principle} + 180 .Math. ° \\ γ_{principle} - 180 .Math. ° & When .Math. .Math. C_{33} > 0 .Math. {\begin{matrix} γ_{principle} > 0 \\ γ_{principle} < 0 \end{matrix} \\ γ_{principle} & When .Math. .Math. C_{33} < 0 \end{matrix} .$

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0039] To illustrate the technical solutions of the embodiments of the invention more clearly, the accompanying drawings required for the embodiments are briefly introduced here. Apparently, the accompanying drawings in the following description are only some embodiments of the present invention. For a person of ordinary skill in the art, other accompanying drawings can be derived from these accompanying drawings, without making any creative efforts.

[0040] FIG. 1 is a flow chart of a method of updating the all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor according to Embodiment 1 of the invention;

[0041] FIG. 2 is a flow chart of a method of updating the all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor according to Embodiment 2 of the invention.

DETAILED DESCRIPTION OF THE INVENTION

[0042] The present invention will hereinafter be clearly and fully described, with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the embodiments to be described are only certain embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, the other embodiments made by a person of ordinary skill in the art without any creative efforts, all fall into the protection scope of the present invention.

Embodiment 1

[0043] As shown in FIG. 1, a method of updating the all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor includes the following steps:

[0044] Step S1: an error model of the gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model are established, and parameter vectors corresponding to vehicle motional attitudes are set.

[0045] The step S1 of establishing an error model of the gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model, and setting parameter vectors corresponding to vehicle motional attitudes is described in detail as below:

[0046] The angular velocity of the gyroscope is calculated in the error model of the gyroscope via an error calculation formula of the gyroscope, wherein the error calculation formula of the gyroscope is: ω=ω.sub.ib+b.sub.ωr+b.sub.ωg, wherein ω is an angular velocity output by the gyroscope, ω.sub.ib is a real angular velocity of the gyroscope, b.sub.ωr is a zero drift of the gyroscope, and b.sub.ωg is a white noise output by the gyroscope.

[0047] The magnetic field interference is eliminated by the electronic compass calibration ellipse model, wherein the electronic compass calibration ellipse model is:

[00010] $\frac{{(mx - Xoffset)}^{2}}{{Xsf}^{.Math. 2}} + \frac{{(my - Yoffset)}^{2}}{{Ysf}^{.Math. 2}} = 1,$

wherein mx, my are magnetic field intensities, Xoffset and Yoffset are hard magnetic interferences, and Xsf and Ysf are soft magnetic interferences.

[0048] The vehicle attitudes are updated by the seven-dimensional EKF filtering model, wherein the seven-dimensional EKF filtering model uses an extended Kalman filter for a seven-dimensional state vector, and EKF includes a state equation and an observation equation:

{dot over (x)}=f(x,ω)+w1

y=h(x)+v1

[0049] The state matrix is: x=[q b.sub.ωr], wherein q is quaternion vectors q0, q1, q2, q3, b.sub.ωr is a zero drift of the XYZ three-axis gyroscope; wherein ω is an angular velocity output by the gyroscope, w1 is a process noise matrix, v1 is an observation noise matrix, y is an observation vector, y=[a ψ.sub.mag].sup.T, wherein a is a three-axis acceleration value, ψ.sub.mag is a course angle calculated by the electronic compass,

[00011] $f (x, ω) = [\frac{1}{2} [\underset{0^{3 \times 1}}{\begin{matrix} - q_{1} & - q_{2} & - q_{3} \\ q_{0} & - q_{3} & q_{2} \\ q_{3} & q_{0} & - q_{1} \\ - q_{2} & q_{1} & q_{0} \end{matrix}}] [\begin{matrix} ω_{x} - b_{ω .Math. .Math. x} \\ ω_{y} - b_{ω .Math. .Math. y} \\ ω_{z} - b_{ω .Math. .Math. z} \end{matrix}]]$ $h = [\begin{matrix} 2 .Math. g (q_{1} .Math. q_{3} - q_{0} .Math. q_{2}) \\ 2 .Math. g (q_{2} .Math. q_{3} + q_{0} .Math. q_{1}) \\ g (q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) \\ \tan^{- 1} (\frac{2 .Math. (q_{1} .Math. q_{2} + q_{0} .Math. q_{3})}{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}}) \end{matrix}] .$

[0050] Since the geomagnetic field has a weak intensity, it is susceptible to the influence of surrounding ferrous material and electromagnetic field. Therefore, it is necessary to calibrate the gyroscope first. The electronic compass calibration ellipse model is established to eliminate the interference from the magnetic field. The acquired magnetic field intensity is fitted by the least square method in the actual calibration process, such that the above parameters are obtained.

[0051] Step S2: The data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity are acquired in real time by a nine-axis MEMS sensor.

[0052] The step S2 of acquiring the data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity in real time by a nine-axis MEMS sensor is described in detail as below:

[0053] The angular velocity of the vehicle is obtained through a gyroscope, and the zero drift of the gyroscope is compensated.

[0054] An acceleration sensor is used to acquire the acceleration data of the vehicle.

[0055] The geomagnetic field intensity of the vehicle is acquired by a geomagnetic sensor.

[0056] Step S3: According to the obtained data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity, the angle, the velocity, the position information, and the course angle of the vehicle are calculated by the established error model of the gyroscope and the electronic compass calibration ellipse model.

[0057] The step S3 of calculating the angle, the velocity, the position information, and the course angle of the vehicle by the established error model of the gyroscope and the electronic compass calibration ellipse model, according to the obtained data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity, is described in detail as below.

[0058] The angle data is obtained by an integral calculation of the angular velocity by the error model of the gyroscope.

[0059] The velocity is calculated by integrating the acceleration data, and the position information is calculated by further integrating the velocity.

[0060] The course angle of the vehicle is then calculated from the geomagnetic field intensity data which is compensated by calibration parameter and corrected by the oblique angle, and both the calibration parameter and the oblique angle are calculated by the elliptical model.

[0061] The motion information of the vehicle is acquired by the MEMS sensor in real time. The angular velocity of the vehicle acquired by the gyroscope is corrected by the state estimation and the zero offset of the gyroscope. The angular velocity of the vehicle is integrated to calculate the angle increment. The geomagnetic sensor is corrected and compensated by the soft magnetism, the hard magnetism, and the oblique angle to calculate the course angle.

[0062] Step S4: The angle, the velocity, the position information and the course angle of the vehicle are data-fusion processed by the seven-dimensional EKF filtering model, and the motional attitude angle of the vehicle is updated in real time.

[0063] The step S4 of data-fusion processing the angle, the velocity, the position information and the course angle of the vehicle by the seven-dimensional EKF filtering model, and updating the motional attitude angle of the vehicle in real time, is described in detail as below.

[0064] The attitude data of the vehicle is calculated by the seven-dimensional EKF filtering model, through the quaternion attitude updating algorithm, wherein the calculation process of the EKF algorithm is as below:

[00012] ${\hat{x}}_{k} (-) = Φ_{k - 1} .Math. {\hat{x}}_{k} (+)$ $Φ_{k} = e^{\frac{\partial f}{\partial x}}$ $P_{k} (-) = Φ_{k - 1} .Math. P_{k - 1} (+) .Math. Φ_{k - 1}^{T} + Q$ ${\hat{x}}_{k} (+) = {\hat{x}}_{k} (-) + K_{k} .Math. .Math. y_{k} - h ({\hat{x}}_{k} (-)) .Math.$ $P_{k} (+) = [I - K_{k} .Math. H_{k}] .Math. P_{k} (-) .Math. .Math. H_{k} = \frac{\partial h}{\partial x} .Math. _{k} .Math. .Math. K_{k} = P_{k} (-) .Math. H_{k} [H_{k} .Math. P_{k} (-) .Math. H_{k}^{T} + R_{k}]$

[0065] In the above formula, k is a sampling time point, {circumflex over (x)}.sub.k is a system state estimation, (−) is the previous time point, (+) is the later time point, Φ.sub.k is a state transition matrix, P.sub.k is a minimum mean square error matrix, Q is a covariance matrix corresponding to the state vector, K.sub.k is an error gain, y.sub.k is an observation vector, H.sub.k is a transition matrix for the observation equation, R.sub.k is a covariance matrix corresponding to the observation vector.

[00013] $Q = \frac{q_{0} + q_{1} .Math. i + q_{2} .Math. j + q_{3} .Math. k}{\sqrt{q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2}}}$

[0066] In the above formula, Q is a quaternion vector, q0, q1, q2, q3 are scalars forming a quaternion vector, i, j, k are unit vectors in the three-dimensional coordinate system. The updated attitude matrix is as below:

[00014] $C_{b}^{n} = [\begin{matrix} q_{1}^{2} + q_{0}^{2} - q_{3}^{2} - q_{2}^{2} & 2 .Math. (q_{1} .Math. q_{2} - q_{0} .Math. q_{3}) & 2 .Math. (q_{1} .Math. q_{3} + q_{0} .Math. q_{2}) \\ 2 .Math. (q_{1} .Math. q_{2} + q_{0} .Math. q_{3}) & q_{2}^{2} - q_{3}^{2} + q_{0}^{2} - q_{1}^{2} & 2 .Math. (q_{2} .Math. q_{3} - q_{0} .Math. q_{1}) \\ 2 .Math. (q_{1} .Math. q_{3} + q_{0} .Math. q_{2}) & 2 .Math. (q_{2} .Math. q_{3} + q_{0} .Math. q_{1}) & q_{3}^{2} - q_{2}^{2} - q_{1}^{2} + q_{0}^{2} \end{matrix}]$

[0067] In the above formula, C.sub.b.sup.n is a rotation matrix for transforming a carrier coordinate system to a navigation coordinate system.

[00015] $C_{b}^{n} = (\begin{matrix} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{matrix}) = (\begin{matrix} \begin{matrix} \cos .Math. .Math. γcosψ + \\ \sin .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψsin .Math. .Math. θ \end{matrix} & \sin .Math. .Math. ψcosθ & \begin{matrix} \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ - \\ \cos .Math. .Math. γsinψsinθ \end{matrix} \\ \begin{matrix} - \cos .Math. .Math. γsin .Math. .Math. ψ + \\ \sin .Math. .Math. γcosψsinθ \end{matrix} & \cos .Math. .Math. ψcosθ & \begin{matrix} - \sin .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ - \\ \cos .Math. .Math. γcosψsinθ \end{matrix} \\ - \sin .Math. .Math. γcosθ & \sin .Math. .Math. θ & \cos .Math. .Math. γcos .Math. .Math. θ \end{matrix})$

[0068] In the above formula, γ, θ, ψ are a rolling angle, a pitch angle and a course angle respectively.

[0069] In the above formula, the nine-axis MEMS sensor is composed of a three-axis gyroscope, a three-axis accelerometer, and a three-axis geomagnetic sensor.

Embodiment 2

[0070] As shown in FIG. 2, a method of updating the all-attitude angle of an agricultural machine based on a nine-axis MEMS sensor is provided. The method includes the following steps:

[0071] Step S1: an error model of the gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model are established, and parameter vectors corresponding to vehicle motional attitudes are set.

[0072] The step S1 of establishing an error model of the gyroscope, an electronic compass calibration ellipse model and a seven-dimensional EKF filtering model, and setting parameter vectors corresponding to vehicle motional attitudes is described in detail as below:

[0073] The angular velocity of the gyroscope is calculated in the error formula of the gyroscope via an error calculation formula of the gyroscope, wherein the error calculation formula of the gyroscope is: ω=ω.sub.ib+b.sub.ωr+b.sub.ωg, wherein w is an angular velocity output by the gyroscope, ω.sub.ib is a real angular velocity of the gyroscope, b.sub.ωr is a zero drift of the gyroscope, and b.sub.ωg is a white noise output by the gyroscope.

[0074] The magnetic field interference is eliminated by the electronic compass calibration ellipse model, wherein the electronic compass calibration ellipse model is:

[00016] $\frac{{(mx - Xoffset)}^{2}}{{Xsf}^{.Math. 2}} + \frac{{(my - Yoffset)}^{2}}{{Ysf}^{.Math. 2}} = 1,$

wherein mx, my are magnetic field intensities, Xoffset and Yoffset are hard magnetic interferences, and Xsf and Ysf are soft magnetic interferences.

[0075] The vehicle attitudes are updated by the seven-dimensional EKF filtering model, wherein the seven-dimensional EKF filtering model is an extended Kalman filter of a seven-dimensional state vector, and EKF includes a state equation and an observation equation:

{dot over (x)}=(x,ω)+w1

y=h(x)+v1

[0076] The state matrix is: x=[q b.sub.ωr], wherein q is quaternion vectors q0, q1, q2, q3, b.sub.ωr is a zero drift of the XYZ three-axis gyroscope. In the formula, ω is an angular velocity output by the gyroscope, w1 is a process noise matrix, v1 is an observation noise matrix, y is an observation vector, y=[a ψ.sub.mag].sup.T, wherein a is a three-axis acceleration value, ψ.sub.mag is a course angle calculated by the electronic compass,

[00017] $f (x, ω) = [\frac{1}{2} [\underset{0^{3 \times 1}}{\begin{matrix} - q_{1} & - q_{2} & - q_{3} \\ q_{0} & - q_{3} & q_{2} \\ q_{3} & q_{0} & - q_{1} \\ - q_{2} & q_{1} & q_{0} \end{matrix}}] [\begin{matrix} ω_{x} - b_{ω .Math. .Math. x} \\ ω_{y} - b_{ω .Math. .Math. y} \\ ω_{z} - b_{ω .Math. .Math. z} \end{matrix}]]$ $h = [\begin{matrix} 2 .Math. g (q_{1} .Math. q_{3} - q_{0} .Math. q_{2}) \\ 2 .Math. g (q_{2} .Math. q_{3} + q_{0} .Math. q_{1}) \\ g (q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) \\ \tan^{- 1} (\frac{2 .Math. (q_{1} .Math. q_{2} + q_{0} .Math. q_{3})}{q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2}}) \end{matrix}] .$

[0077] Since the geomagnetic field has a weak intensity, it is susceptible to the influence of surrounding ferrous material and electromagnetic fields. Therefore, it is necessary to calibrate the gyroscope first. The electronic compass calibration ellipse model is established to eliminate the interference from the magnetic field. The acquired magnetic field intensity is fitted by the least square method in the actual calibration process, such that the above parameters are obtained.

[0078] Step S2: The data including the acceleration, and the angular velocity of the motion of vehicle, and the geomagnetic field intensity are acquired in real time by a nine-axis MEMS sensor.

[0079] The step S2 of acquiring the data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity in real time by a nine-axis MEMS sensor is described in detail as below:

[0080] The angular velocity of the vehicle is obtained through a gyroscope, and the zero drift of the gyroscope is compensated.

[0081] An acceleration sensor is used to acquire the acceleration data of the vehicle.

[0082] The geomagnetic field intensity of the vehicle is acquired by a geomagnetic sensor.

[0083] Step S3: According to the obtained data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity, the angle, the velocity, the position information, and the course angle of the vehicle are calculated by the established error model of the gyroscope and the electronic compass calibration ellipse model.

[0084] The step S3 of calculating the angle, the velocity, the position information, and the course angle of the vehicle by the established error model of the gyroscope and the electronic compass calibration ellipse model, according to the obtained data including the acceleration and the angular velocity of the motion of vehicle, and the geomagnetic field intensity, is described in detail as below.

[0085] The angle data is obtained by an integral calculation of the angular velocity by the error model of the gyroscope.

[0086] The velocity is calculated by integrating the acceleration data, and the position information is calculated by further integrating the velocity.

[0087] The course angle of the vehicle is then calculated from the geomagnetic field intensity data which is compensated by calibration parameter and corrected by the oblique angle, and both the calibration parameter and the oblique angle are calculated by the elliptical model.

[0088] The motion information of the vehicle is acquired by the MEMS sensor in real time. The angular velocity of the vehicle acquired by the gyroscope is corrected by the state estimation and the zero offset of the gyroscope. The angular velocity of the vehicle is integrated to calculate the angle increment. The geomagnetic sensor is corrected and compensated by the soft magnetism, the hard magnetism, and the oblique angle to calculate the course angle.

[0089] Step S4: The angle, the velocity, the position information and the course angle of the vehicle are data-fusion processed by the seven-dimensional EKF filtering model, and the motional attitude angle of the vehicle is updated in real time.

[0090] The step S4 of data-fusion processing the angle, the velocity, the position information and the course angle of the vehicle by the seven-dimensional EKF filtering model, and updating the motional attitude angle of the vehicle in real time, is described in detail as below.

[0091] The attitude data of the vehicle is calculated by the seven-dimensional EKF filtering model, through the quaternion attitude updating algorithm, wherein the calculation process of the EKF algorithm is as below:

[00018] ${\hat{x}}_{k} (-) = Φ_{k - 1} .Math. {\hat{x}}_{k} (+)$ $Φ_{k} = e^{\frac{\partial f}{\partial x}}$ $P_{k} (-) = Φ_{k - 1} .Math. P_{k - 1} (+) .Math. Φ_{k - 1}^{T} + Q$ ${\hat{x}}_{k} (+) = {\hat{x}}_{k} (-) + K_{k} .Math. .Math. y_{k} - h ({\hat{x}}_{k} (-)) .Math.$ $P_{k} (+) = [I - K_{k} .Math. H_{k}] .Math. P_{k} (-)$ $H_{k} = \frac{\partial h}{\partial x} .Math. _{k} .Math. .Math. K_{k} = P_{k} (-) .Math. H_{k} [H_{k} .Math. P_{k} (-) .Math. H_{k}^{T} + R_{k}]$

[0092] In the above formula, k is a sampling time point, {circumflex over (x)}.sub.k is a system state estimation, (−) is the previous time point, (+) is the later time point, Φ.sub.k is a state transition matrix, Pk is a minimum mean square error matrix, Q is a covariance matrix corresponding to the state vector, Kk is an error gain, yk is an observation vector, Hk is a transition matrix for the observation equation, Rk is a covariance matrix corresponding to the observation vector.

[00019] $Q = \frac{q_{0} + q_{1} .Math. i + q_{2} .Math. j + q_{3} .Math. k}{\sqrt{q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2}}}$

[0093] In the above formula, Q is a quaternion vector, q0, q1, q2, q3 are scalars forming a quaternion vector, i, j, k are unit vectors in the three-dimensional coordinate system, the updated attitude matrix is as below:

[00020] $C_{b}^{n} = [\begin{matrix} q_{1}^{2} + q_{0}^{2} - q_{3}^{2} - q_{2}^{2} & 2 .Math. (q_{1} .Math. q_{2} - q_{0} .Math. q_{3}) & 2 .Math. (q_{1} .Math. q_{3} + q_{0} .Math. q_{2}) \\ 2 .Math. (q_{1} .Math. q_{2} + q_{0} .Math. q_{3}) & q_{2}^{2} - q_{3}^{2} + q_{0}^{2} - q_{1}^{2} & 2 .Math. (q_{2} .Math. q_{3} - q_{0} .Math. q_{1}) \\ 2 .Math. (q_{1} .Math. q_{3} + q_{0} .Math. q_{2}) & 2 .Math. (q_{2} .Math. q_{3} + q_{0} .Math. q_{1}) & q_{3}^{2} - q_{2}^{2} - q_{1}^{2} + q_{0}^{2} \end{matrix}]$

[0094] In the above formula, C.sub.b.sup.n is a rotation matrix for transforming a carrier coordinate system to a navigation coordinate system.

[00021] $C_{b}^{n} = (\begin{matrix} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{matrix}) = (\begin{matrix} \cos .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ + \sin .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ & \sin .Math. .Math. ψ .Math. .Math. \cos .Math. .Math. θ & \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ - \cos .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ \\ - \cos .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ + \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ & \cos .Math. .Math. ψ .Math. .Math. \cos .Math. .Math. θ & - \sin .Math. .Math. γ .Math. .Math. \sin .Math. .Math. ψ - \cos .Math. .Math. γ .Math. .Math. \cos .Math. .Math. ψ .Math. .Math. \sin .Math. .Math. θ \\ - \sin .Math. .Math. γ .Math. .Math. \cos .Math. .Math. θ & \sin .Math. .Math. θ & \cos .Math. .Math. γ .Math. .Math. \cos .Math. .Math. θ \end{matrix})$

[0095] In the above formula, γ, θ, ψ are a rolling angle, a pitch angle, and a course angle respectively.

[0096] In the above formula, the nine-axis MEMS sensor is composed of a three-axis gyroscope, a three-axis accelerometer, and a three-axis geomagnetic sensor.

[0097] Step S5: The all-attitude angle data of the vehicle is extracted from the updated attitude data of the vehicle, to determine the value of the attitude angle data. The all-attitude angle of the vehicle includes a pitch angle, a rolling angle and a course angle, wherein,

[00022] ${\begin{matrix} ψ_{Principal} = \arctan .Math. .Math. (- \frac{C_{12}}{C_{22}}) \\ θ_{Principal} = \arcsin .Math. .Math. C_{32} \\ γ_{Principal} = \arctan .Math. .Math. (- \frac{C_{31}}{C_{33}}) \end{matrix}$ [0098] The course angle is:

[00023] $ψ = {\begin{matrix} ψ_{principle} \\ ψ_{principle} + 360 .Math. ° & When .Math. .Math. C_{22} > 0 .Math. {\begin{matrix} ψ_{principle} > 0 \\ ψ_{principle} < 0 \end{matrix} \\ ψ_{principle} + 180 .Math. ° & When .Math. .Math. C_{22} < 0 \end{matrix}$ [0099] The pitch angle is: [0100] θ=θ.sub.principle [0101] The rolling angle is:

[00024] $γ = {\begin{matrix} γ_{principle} + 180 .Math. ° \\ γ_{principle} - 180 .Math. ° & When .Math. .Math. C_{33} > 0 .Math. {\begin{matrix} γ_{principle} > 0 \\ γ_{principle} < 0 \end{matrix} \\ γ_{principle} & When .Math. .Math. C_{33} < 0 \end{matrix}$

[0102] The all-attitude attitude angle of the vehicle can be extracted from the updated and calculated attitude matrix C.sub.b.sup.n, and the all-attitude attitude angle includes the pitch angle, the rolling angle and the course angle. Since the pitch angle θ is defined within the interval of [−90°, +90° ], which is consistent with the principal value of an inverse sine function, there is no problem of multi-value. The rolling angle γ is defined within the interval of [180°, 180° ]. The course angle ψ is defined within the interval of [0°, 360°]. Hence, there are problems of multi-value for both γ and ψ. After the principle value is calculated, the specific quadrant can be determined by the elements in C.sub.b.sup.n.

[0103] The advantages of the invention are as below: the acceleration and the angular velocity of the motion of the object are acquired in real time by a MEMS sensor. The angular acceleration output by the gyroscope is integrated to obtain the angle. The acceleration is integrated to calculate the velocity, which is further integrated to calculate the position information. The geomagnetic field is obtained by the geomagnetic sensor, and the course angle is calculated by the compensation algorithm and the fusion of gyroscope. Next, the attitudes are converted into a transition matrix, so that the carrier coordinate system is transformed into a navigation coordinate system. This transition matrix functions as a “mathematical platform”. The SINS (Strapdown inertial navigation system) algorithm is applied to the agricultural machine, and the transition matrix is particularly important. Since the agricultural machine keeps moving, the attitudes thereof are also continuously changing. Thus, the transition matrix also needs to be continuously recalculated and updated. The conventional attitude updating algorithms include Euler angle algorithm, direction cosine algorithm, and quaternion algorithm. Compared with the Euler angle algorithm, the quaternion algorithm has no singular point. Compared with the direction cosine algorithm, the quaternion algorithm has a small calculation amount. Hence, the quaternion algorithm is very suitable for being used in the embedded product. The geomagnetic field and the error model of the gyroscope are established in the agricultural machine plane, and a seven-dimensional EKF (Extended Kalman Filter) is established to update the attitude matrix. The quaternion and the zero offset of the gyroscope are estimated. Next, an observation is performed from the course angle calculated through the acceleration and the magnetic field intensity, so that a high-precision three-dimensional attitude angle can be obtained. The error compensation algorithm and the correction algorithm greatly reduce the error interference of the SINS algorithm. The MEMS sensor and the SINS algorithm ensure that the present invention has high-performance parameters. As tested by a tractor, the error of the output course angle is less than 0.1°, and the pitch angle error and the rolling angle error are less than 0.01°. Since the quaternion is used as a Kalman filtering state vector, the calculation accuracy of the target parameters can be further improved.

[0104] The above description is merely illustrative of specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. The modifications or replacements easily conceived by a person of ordinary skill in the art, within the scope of the disclosure in the present invention, shall all fall into the protection scope of the present invention. Therefore, the protection scope of the present invention should be decided by the appended claims.

Method of Updating All-Attitude Angle of Agricultural Machine Based on Nine-Axis MEMS Sensor

Assignee

Inventors

Cpc classification

Classification Explorer

A01B79/005

HUMAN NECESSITIES

Classification Explorer

A01B69/008

HUMAN NECESSITIES

Classification Explorer

G01C23/00

PHYSICS

Classification Explorer

G01C25/00

PHYSICS

Classification Explorer

G01C21/1654

PHYSICS

Classification Explorer

G01C19/00

PHYSICS

Classification Explorer

G01C25/005

PHYSICS

International classification

Classification Explorer

G01C23/00

PHYSICS

Classification Explorer

A01B69/04

HUMAN NECESSITIES

Classification Explorer

G01C25/00

PHYSICS

Classification Explorer

A01B79/00

HUMAN NECESSITIES

Abstract

Claims

Description