CN104132664A - Method for estimation of slippage of agricultural tracked robot - Google Patents

Abstract

The invention discloses a method for the estimation of slippage of an agricultural tracked robot. Through agricultural tracked robot modeling analysis, a motion equation is established; according to the position information and velocity information received by a GPS receiver mounted on the agricultural tracked robot, direction information collected by gyroscope and acceleration information received by an acceleration sensor, an agricultural tracked robot observed quantity Zk=[zx, k, zy, k, zv, x, zv, y, z theta, k] T decided by the previous information is determined; and a state equation and an observation equation of the agricultural tracked robot are established. UKF is used to fuse the state and observation equations of the tracked robot to obtain accurate attitude information of the agricultural tracked robot, and then sliding parameters are calculated according to the sliding parameter calculation equation. The invention provides guarantee for normal working of the agricultural tracked robot along its track.

Description

A kind of method of estimation of agricultural caterpillar robot slippage

Technical field

The invention belongs to physical quantity and estimate fields of measurement, be specifically related to a kind of method of estimation of the agricultural caterpillar robot slippage for Internet of Things mobile node.

Background technology

Continuous expansion along with mobile robot's research and application, robot obtains people and more and more pays close attention to, the great achievement that particularly Sojourner of NASA (" Suo Jiena "), Spirit (" courage number ") and Opportunity (" Opportunity Rover ") mars exploration car obtain, expanded the mankind's the science visual field, in worldwide, started and utilized robot to carry out scientific exploration and practical upsurge.

Agricultural caterpillar robot, owing to having adopted creeper undercarriage, is embodying stronger landform adaptive faculty and locomotor activity aspect trafficability characteristic and maneuverability.Compare with indoor peddrail mobile robot, the working environment of field caterpillar robot is more complicated, has more not intellectual.Certainly will need to carry out online awareness and modeling to the residing farm environment of caterpillar robot, be robot navigation and the control basis of submitting necessary information.Wherein the slip between robot and kiss the earth is a very important factor that affects robot self capacity, especially for caterpillar robot, carrying out farmland detection and field study task, while particularly bearing Mobile routing and information acquisition task as Internet of Things mobile node, slippage is robot inevitable problem in craspedodrome and turning process especially.

In operational process, the motion of caterpillar robot is determined jointly by crawler belt radial drive power and crawler belt and ground side-friction force.Friction force determines by linear velocity and the angular velocity of caterpillar robot, and the side force balance equation of creeper truck shows as the differential equation that can not be long-pending, causes the path planning of caterpillar robot and path trace to occur coupling between controlling, i.e. nonholonomic restriction.In addition, due to the complicacy acting between crawler belt-ground, the uncertainty of path soil parameters, the ground force of caterpillar robot is difficult to accurately be estimated.When straight line or turning operation are carried out in non-smooth farmland, the variation due to caterpillar robot load and soil mechanics parameter, there will be again serious and strong disturbing phenomenon, makes caterpillar robot easily depart from predetermined travel route, has a strong impact on tracking accuracy.Therefore,, for the attitude estimation of robot and the reconstruction research of land slide parameter, the navigation of agricultural caterpillar robot and accurate control are had to deep theory significance and actual application value.

Summary of the invention

While the present invention is directed to the work for the agricultural caterpillar robot of internet node, although, GPSNeng Wei robot provides position and the velocity information of real-time high-precision, but gps signal is often blocked and is lost, be difficult to realize location exactly, inertial navigation system can utilize inertial sensor information independence and extrapolate robot with respect to the position of starting point, but there is Random Drift Error in inertial sensor, and the problem that error accumulates in time, the caterpillar robot attitude of design based on UKF and the estimating system of slip, utilize UKF that caterpillar robot kinematical equation and observation equation are merged, obtain robot pose information accurately, again according to slip calculation of parameter equation, calculate slip parameter, for guaranteeing that it provides safeguard by the normal work of track.

The technical solution adopted in the present invention is as follows:

A kind of method of estimation of agricultural caterpillar robot slippage, by agricultural caterpillar robot modeling analysis, set up its equation of motion, utilize position, the velocity information of the GPS receiver reception of installing on agricultural caterpillar robot, the acceleration information that the directional information that gyroscope gathers and acceleration transducer receive, the observed quantity Z of definite agricultural caterpillar robot being determined by these information _k=[z _x,k, z _y,k, z _v,x, z _v,y, z _{θ, k}] ^tand then set up state equation and the observation equation of agricultural caterpillar robot: utilize UKF that caterpillar robot state equation and observation equation are merged, obtain agricultural caterpillar robot attitude information accurately, again according to slip calculation of parameter equation, calculate slip parameter, it is characterized in that, specifically comprise the following steps:

(1), to agricultural caterpillar robot motion analysis:

(a), simplify agricultural caterpillar robot and define coordinate system: considering that agricultural caterpillar robot is mainly by mobile platform and composition of the control system, mobile platform is comprised of chassis (car body, 2 driving wheels, 4 bogie wheels, 2 inducers) and 2 crawler belts, and crawler belt is driven by 2 servomotors respectively; When to its motion analysis, only need to consider the vertical view of its geometry, so set up XOY, it is the agricultural caterpillar robot relative coordinate system of overall cartesian coordinate system and xoy, if the initial point of xoy is at agricultural caterpillar robot barycenter, ox is agricultural caterpillar robot working direction, be track length L, two crawler belt center distance are b, and the angle between two coordinate systems is θ;

(b), adopt the modeling of transient motion analytic approach, suppose: 1. crawler bearing length is fixed, and without relative sliding, exist between driving wheel and crawler belt; 2. crawler belt grounding pressure is even;

Ideally nonslipping, robot traffic direction remains consistent with reference direction.Under non-ideal condition, robot traffic direction departs from reference direction, and δ represents the slippage angle of crawler belt, and robot barycenter is comprised of translation and rotation, and translational velocity v is projected as v on xoy coordinate system _x, v _y, velocity of rotation ω is define left and right crawler belt longitudinal sliding motion ratio and be respectively (these two formula)

$\mathrm{il}=\frac{{\mathrm{r\ω}}_l-v_x}{{\mathrm{r\ω}}_l}---\left(1\right)$

$\mathrm{ir}=\frac{{\mathrm{r\ω}}_r-v_x}{{\mathrm{r\ω}}_r}---\left(2\right)$

ω in formula _l, ω _rfor the angular velocity of left and right wheels, r is crawler driving whell radius.

Definition [x, y] ^tfor initial point o coordinate, kinematical equation is

x ^&＝v _xcosθ-v _ysinθ (3)

y ^&＝v _xsinθ+v _ycosθ (4)

(2), set up agricultural caterpillar robot state equation and observation equation

(a), the foundation of state equation:

Use X _k=[x _k, y _k, v _x, v _y, θ _k] ^tthe attitude of describing caterpillar robot, the Robot equation of motion is

X _k+1＝f(X _k)+W _k (6)

Wherein

$f\left(X_k\right)=\left[\begin{array}{c}{x}_k+{\mathrm{tv}}_{x,k}\cos {\mathrm{\θ}}_k-{\mathrm{tv}}_{y,k}\sin {\mathrm{\θ}}_k\\ y_k+{\mathrm{tv}}_{x,k}\sin {\mathrm{\θ}}_k+{\mathrm{tv}}_{y,k}\cos {\mathrm{\θ}}_k\\ v_{x,k}+{\mathrm{tv}}_{y,k}{r}_k+{\mathrm{ta}}_{x,k}\\ v_{y,k}-{\mathrm{tv}}_{x,k}{r}_k+{\mathrm{ta}}_{y,k}\\ {\mathrm{\θ}}_k+{\mathrm{tr}}_{m,k}\end{array}\right];$

W _k＝[0 0 tw _a,x tw _a,y tw _r] ^T

W in formula _k-noise vector; T-be the sampling period;

X _k, y _kthe position of X, Y-axis in-global coordinate system;

V _x, v _ythe speed component of-robot in X, Y-axis;

θ _kthe deviation in direction of motion and course in-global coordinate system;

V _x,k, v _y,kspeed on-x and y axle;

A _x,k, a _y,kacceleration on-x and y axle;

R _m,kthe instantaneous yaw rate that-gyroscope is surveyed;

(b), the foundation of observation equation:

Observed quantity is the observable quantity directly obtaining by sensor, and observation equation has reflected the inner link between measurand, state variable; Agricultural caterpillar robot observed quantity Z _k=[z _x,k, z _y,k, z _v,x, z _v,y, z _{θ, k}] ^tby absolute position, speed and direction, formed; GPS receiver output device people's position and speed, gyro output relative rotation θ, acceleration transducer output acceleration, the observation equation between observed quantity and state variable is

Z _k＝h _k+v _k (7)

Wherein $h_k=\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x,k}\cos {\mathrm{\θ}}_k-v_{y,k}\sin {\mathrm{\θ}}_k\\ v_{x,k}\sin {\mathrm{\θ}}_k+v_{y,k}\cos {\mathrm{\θ}}_k\\ {\mathrm{\θ}}_k\end{array}\right]$

v _k＝[v _x,k v _y,k v _v,x v _v,y v _θ,k] ^T

V in formula _k-observation noise vector, other parameter-definition cotype (6).

(3), in conjunction with UKF, merge principle the discrete nonlinear state of caterpillar robot and observation equation equation (6) and (7) are carried out to recursion

UKF algorithm is to utilize UT conversion, with limited parameter approximation system state, by predicting and upgrading the estimation of carrying out system.This algorithm is first chosen one group of weights difference and can be characterized the Sigma point of random state statistics of variable characteristic, by in these substitution nonlinear equations, reconstruct the statistical property that comprises new average and variance, the average again conversion being obtained, variance and measurement variance, introduce in the recursive process of wave filter.

For the discrete nonlinear state of caterpillar robot and observation equation equation (6) and (7), UKF recursive algorithm is as follows:

(a), system state initialization:

Suppose robot initial state x ₀for the random vector of Gaussian distribution, original state and estimation variance are

$\left\{\begin{array}{c}{\hat{x}}_0=E\left(x_0\right)\\ P_0=E\left((x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right)\end{array}\right.---\left(8\right)$

(b), Sigma sampled point calculates:

State vector x for n dimension (n>=1), chooses 2n+1 sampled point x _i(i=0,1, L, 2n), the calculating formula that Sigma is ordered is [14]

$\left\{\begin{array}{cc}{x}_{0,k-1}={\hat{x}}_{k-1}& (i=0)\\ x_{i,k-1}={\hat{x}}_{k-1}+{\left(\sqrt{(n+\mathrm{\λ})P_{\mathrm{xx}}}\right)}_i& (i=\mathrm{1,2},L,n)\\ x_{i,k-1}={\hat{x}}_{k-1}-{\left(\sqrt{(n+\mathrm{\λ})P_{\mathrm{xx}}}\right)}_i& (i=n+1,n+2,L,2n)\end{array}\right.---\left(9\right)$

In formula and P _xxthe average of-vector x and real symmetric positive definite variance matrix;

the i row of-weighting matrix square root battle array; λ=α ²(n+k)-n-distribution yardstick;

α-determine the degree of scatter that sigma is ordered, regulates α can reduce the higher order term impact of nonlinear equation, is conventionally made as 10-3;

K-scale parameter, when state variable is single argument, k is 2, k=3-n during multivariate.By the average of state vector x with variance P _xxcan predict average and the variance of y.

(c), by state equation, propagate Sigma point:

System state, state average and error covariance predictor formula are

$\left\{\begin{array}{c}{x}_{i,k|k-1}=f(x_{i,k-1},k-1)\\ {\hat{x}}_k^{-}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(m\right)}{x}_{i,k|k-1}\\ P_{\mathrm{xx}}^{-}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(c\right)}(x_{i,k|k-1}-{\hat{x}}_k^{-}){(x_{i,k|k-1}-{\hat{x}}_k^{-})}^T+Q_k\end{array}\right.---\left(10\right)$

In formula the weight coefficient of-first-order statistics characteristic;

the weight coefficient of-second-order statistics;

Q-system noise covariance matrix;

with more new-typely be

$\left\{\begin{array}{cc}{\mathrm{\ω}}_0^{\left(m\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}& \\ {\mathrm{\ω}}_0^{\left(c\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β})& (\mathrm{\β}=2)\\ {\mathrm{\ω}}_i^{\left(m\right)}={\mathrm{\ω}}_i^{\left(c\right)}=\frac{0.5}{n+\mathrm{\λ}}& (i=\mathrm{1,2},L,2n)\end{array}\right.---\left(11\right)$

(d), observed quantity prediction, average and covariance calculating formula are

$\left\{\begin{array}{c}{y}_{i,k|k-1}=h(x_{i,k|k-1},k-1)\\ {\hat{y}}_k^{-}=\underset{0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(m\right)}{y}_{i,k|k-1}\\ P_{\mathrm{yy}}=\underset{0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(c\right)}[y_{i,k|k-1}-{\hat{y}}_k^{-}]{[y_{i,k|k-1}-{\hat{y}}_k^{-}]}^T+R_k\\ P_{\mathrm{xy}}=\underset{0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(c\right)}[x_{i,k|k-1}-{\hat{x}}_k^{-}]{[y_{i,k|k-1}-{\hat{y}}_k^{-}]}^T\end{array}\right.---\left(12\right)$

R-measurement noise covariance square.

(e), according to formula (12), upgrade filtering error variance battle array and system state:

$\left\{\begin{array}{c}{K}_k=P_{\mathrm{xy}}{P}_{\mathrm{yy}}^{-1}\\ P_k=P_k^{-}-K_k{P}_{\mathrm{yy}}{K}_k^T\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_k-{\hat{y}}_k^{-})\end{array}\right.---\left(13\right)$

K in formula _k-filter gain matrix

(4), the calculating of slippage

When robot moves, often slide effect can be produced, while rotating γ angle, slide angle δ can be produced, as shown in Figure 3 simultaneously.Utilize UKF to merge the robot speed's information after upgrading in conjunction with instantaneous yaw rate r _k, and the left and right crawler driving whell angular velocity omega of known caterpillar robot _land ω _r, left and right slip factor and slip angle calculating formula are

$\mathrm{il}=\frac{{\mathrm{r\ω}}_L-{\hat{v}}_x}{{\mathrm{r\ω}}_L}---\left(14\right)$

$\mathrm{ir}=\frac{{\mathrm{r\ω}}_R-{\hat{v}}_x}{{\mathrm{r\ω}}_R}---\left(15\right)$

$\mathrm{\δ}=\arctan \left(\frac{0.5{\mathrm{Lr}}_k+v_y}{v_x}\right)-\mathrm{\γ}---\left(16\right)$

Accompanying drawing explanation

Fig. 1 is system of the present invention;

Fig. 2 is caterpillar robot plane motion model of the present invention;

Fig. 3 is that slippage effect figure appears while turning in Agricultural Robot of the present invention.

Embodiment

A kind of method of estimation of agricultural caterpillar robot slippage, by agricultural caterpillar robot modeling analysis, set up its equation of motion, utilize position, the velocity information of the GPS receiver reception of installing on agricultural caterpillar robot, the acceleration information that the directional information that gyroscope gathers and acceleration transducer receive, the observed quantity Z of definite agricultural caterpillar robot being determined by these information _k=[z _x,k, z _y,k, z _v,x, z _v,y, z _{θ, k}] ^tand then set up state equation and the observation equation of agricultural caterpillar robot: utilize UKF that caterpillar robot state equation and observation equation are merged, obtain agricultural caterpillar robot attitude information accurately, again according to slip calculation of parameter equation, calculate slip parameter, specifically comprise the following steps:

(1), to agricultural caterpillar robot motion analysis:

(a), simplify agricultural caterpillar robot and define coordinate system: considering that agricultural caterpillar robot is mainly by mobile platform and composition of the control system, mobile platform is comprised of chassis (car body, 2 driving wheels, 4 bogie wheels, 2 inducers) and 2 crawler belts, and crawler belt is driven by 2 servomotors respectively; When to its motion analysis, only need to consider the vertical view of its geometry, so set up XOY, it is the agricultural caterpillar robot relative coordinate system of overall cartesian coordinate system and xoy, if the initial point of xoy is at agricultural caterpillar robot barycenter, ox is agricultural caterpillar robot working direction, be track length L, two crawler belt center distance are b, and the angle between two coordinate systems is θ;

(b), adopt the modeling of transient motion analytic approach, suppose: 1. crawler bearing length is fixed, and without relative sliding, exist between driving wheel and crawler belt; 2. crawler belt grounding pressure is even;

Ideally nonslipping, robot traffic direction remains consistent with reference direction.Under non-ideal condition, robot traffic direction departs from reference direction, and δ represents the slippage angle of crawler belt, and robot barycenter is comprised of translation and rotation, and translational velocity v is projected as v on xoy coordinate system _x, v _y, velocity of rotation ω is define left and right crawler belt longitudinal sliding motion ratio and be respectively (these two formula)

$\mathrm{il}=\frac{{\mathrm{r\ω}}_l-v_x}{{\mathrm{r\ω}}_l}---\left(1\right)$

$\mathrm{ir}=\frac{{\mathrm{r\ω}}_r-v_x}{{\mathrm{r\ω}}_r}---\left(2\right)$

ω in formula _l, ω _rfor the angular velocity of left and right wheels, r is crawler driving whell radius.

Definition [x, y] ^tfor initial point o coordinate, kinematical equation is

x ^&＝v _xcosθ-v _ysinθ (3)

y ^&＝v _xsinθ+v _ycosθ (4)

(2), set up agricultural caterpillar robot state equation and observation equation

(a), the foundation of state equation:

Use X _k=[x _k, y _k, v _x, v _y, θ _k] ^tthe attitude of describing caterpillar robot, the Robot equation of motion is

X _k+1＝f(X _k)+W _k (6)

Wherein

$f\left(X_k\right)=\left[\begin{array}{c}{x}_k+{\mathrm{tv}}_{x,k}\cos {\mathrm{\θ}}_k-{\mathrm{tv}}_{y,k}\sin {\mathrm{\θ}}_k\\ y_k+{\mathrm{tv}}_{x,k}\sin {\mathrm{\θ}}_k+{\mathrm{tv}}_{y,k}\cos {\mathrm{\θ}}_k\\ v_{x,k}+{\mathrm{tv}}_{y,k}{r}_k+{\mathrm{ta}}_{x,k}\\ v_{y,k}-{\mathrm{tv}}_{x,k}{r}_k+{\mathrm{ta}}_{y,k}\\ {\mathrm{\θ}}_k+{\mathrm{tr}}_{m,k}\end{array}\right];$

W _k＝[0 0 tw _a,x tw _a,y tw _r] ^T

W in formula _k-noise vector; T-be the sampling period;

X _k, y _kthe position of X, Y-axis in-global coordinate system;

V _x, v _ythe speed component of-robot in X, Y-axis;

θ _kthe deviation in direction of motion and course in-global coordinate system;

V _x,k, v _y,kspeed on-x and y axle;

A _x,k, a _y,kacceleration on-x and y axle;

R _m,kthe instantaneous yaw rate that-gyroscope is surveyed;

(b), the foundation of observation equation:

Observed quantity is the observable quantity directly obtaining by sensor, and observation equation has reflected the inner link between measurand, state variable; Agricultural caterpillar robot observed quantity Z _k=[z _x,k, z _y,k, z _v,x, z _v,y, z _{θ, k}] ^tby absolute position, speed and direction, formed; GPS receiver output device people's position and speed, gyro output relative rotation θ, acceleration transducer output acceleration, the observation equation between observed quantity and state variable is

Z _k＝h _k+v _k (7)

Wherein $h_k=\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x,k}\cos {\mathrm{\θ}}_k-v_{y,k}\sin {\mathrm{\θ}}_k\\ v_{x,k}\sin {\mathrm{\θ}}_k+v_{y,k}\cos {\mathrm{\θ}}_k\\ {\mathrm{\θ}}_k\end{array}\right]$

v _k＝[v _x,k v _y,k v _v,x v _v,y v _θ,k] ^T

V in formula _k-observation noise vector, other parameter-definition cotype (6).

(3), in conjunction with UKF, merge principle the discrete nonlinear state of caterpillar robot and observation equation equation (6) and (7) are carried out to recursion

UKF algorithm is to utilize UT conversion, with limited parameter approximation system state, by predicting and upgrading the estimation of carrying out system.This algorithm is first chosen one group of weights difference and can be characterized the Sigma point of random state statistics of variable characteristic, by in these substitution nonlinear equations, reconstruct the statistical property that comprises new average and variance, the average again conversion being obtained, variance and measurement variance, introduce in the recursive process of wave filter.

For the discrete nonlinear state of caterpillar robot and observation equation equation (6) and (7), UKF recursive algorithm is as follows:

(a), system state initialization:

Suppose robot initial state x ₀for the random vector of Gaussian distribution, original state and estimation variance are

$\left\{\begin{array}{c}{\hat{x}}_0=E\left(x_0\right)\\ P_0=E\left((x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right)\end{array}\right.---\left(8\right)$

(b), Sigma sampled point calculates:

State vector x for n dimension (n>=1), chooses 2n+1 sampled point x _i(i=0,1, L, 2n), the calculating formula that Sigma is ordered is [14]

$\left\{\begin{array}{cc}{x}_{0,k-1}={\hat{x}}_{k-1}& (i=0)\\ x_{i,k-1}={\hat{x}}_{k-1}+{\left(\sqrt{(n+\mathrm{\λ})P_{\mathrm{xx}}}\right)}_i& (i=\mathrm{1,2},L,n)\\ x_{i,k-1}={\hat{x}}_{k-1}-{\left(\sqrt{(n+\mathrm{\λ})P_{\mathrm{xx}}}\right)}_i& (i=n+1,n+2,L,2n)\end{array}\right.---\left(9\right)$

In formula and P _xxthe average of-vector x and real symmetric positive definite variance matrix;

the i row of-weighting matrix square root battle array; λ=α ²(n+k)-n-distribution yardstick;

α-determine the degree of scatter that sigma is ordered, regulates α can reduce the higher order term impact of nonlinear equation, is conventionally made as 10-3;

K-scale parameter, when state variable is single argument, k is 2, k=3-n during multivariate.By the average of state vector x with variance P _xxcan predict average and the variance of y.

(c), by state equation, propagate Sigma point:

System state, state average and error covariance predictor formula are

$\left\{\begin{array}{c}{x}_{i,k|k-1}=f(x_{i,k-1},k-1)\\ {\hat{x}}_k^{-}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(m\right)}{x}_{i,k|k-1}\\ P_{\mathrm{xx}}^{-}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(c\right)}(x_{i,k|k-1}-{\hat{x}}_k^{-}){(x_{i,k|k-1}-{\hat{x}}_k^{-})}^T+Q_k\end{array}\right.---\left(10\right)$

In formula the weight coefficient of-first-order statistics characteristic;

the weight coefficient of-second-order statistics;

Q-system noise covariance matrix;

with more new-typely be

$\left\{\begin{array}{cc}{\mathrm{\ω}}_0^{\left(m\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}& \\ {\mathrm{\ω}}_0^{\left(c\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β})& (\mathrm{\β}=2)\\ {\mathrm{\ω}}_i^{\left(m\right)}={\mathrm{\ω}}_i^{\left(c\right)}=\frac{0.5}{n+\mathrm{\λ}}& (i=\mathrm{1,2},L,2n)\end{array}\right.---\left(11\right)$

(d), observed quantity prediction, average and covariance calculating formula are

$\left\{\begin{array}{c}{y}_{i,k|k-1}=h(x_{i,k|k-1},k-1)\\ {\hat{y}}_k^{-}=\underset{0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(m\right)}{y}_{i,k|k-1}\\ P_{\mathrm{yy}}=\underset{0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(c\right)}[y_{i,k|k-1}-{\hat{y}}_k^{-}]{[y_{i,k|k-1}-{\hat{y}}_k^{-}]}^T+R_k\\ P_{\mathrm{xy}}=\underset{0}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i^{\left(c\right)}[x_{i,k|k-1}-{\hat{x}}_k^{-}]{[y_{i,k|k-1}-{\hat{y}}_k^{-}]}^T\end{array}\right.---\left(12\right)$

R-measurement noise covariance square.

(e), according to formula (12), upgrade filtering error variance battle array and system state:

$\left\{\begin{array}{c}{K}_k=P_{\mathrm{xy}}{P}_{\mathrm{yy}}^{-1}\\ P_k=P_k^{-}-K_k{P}_{\mathrm{yy}}{K}_k^T\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(y_k-{\hat{y}}_k^{-})\end{array}\right.---\left(13\right)$

K in formula _k-filter gain matrix

(4), the calculating of slippage

When robot moves, often slide effect can be produced, while rotating γ angle, slide angle δ can be produced, as shown in Figure 3 simultaneously.Utilize UKF to merge the robot speed's information after upgrading in conjunction with instantaneous yaw rate r _k, and the left and right crawler driving whell angular velocity omega of known caterpillar robot _land ω _r, left and right slip factor and slip angle calculating formula are

$\mathrm{il}=\frac{{\mathrm{r\ω}}_L-{\hat{v}}_x}{{\mathrm{r\ω}}_L}---\left(14\right)$

$\mathrm{ir}=\frac{{\mathrm{r\ω}}_R-{\hat{v}}_x}{{\mathrm{r\ω}}_R}---\left(15\right)$

$\mathrm{\δ}=\arctan \left(\frac{0.5{\mathrm{Lr}}_k+v_y}{v_x}\right)-\mathrm{\γ}---\left(16\right)$

Claims (1)

1. the method for estimation of an agricultural caterpillar robot slippage, by agricultural caterpillar robot modeling analysis is set up to its equation of motion, utilize position, the velocity information of the GPS receiver reception of installing on agricultural caterpillar robot, the acceleration information that the directional information that gyroscope gathers and acceleration transducer receive, the observed quantity Z of definite agricultural caterpillar robot being determined by these information _k=[z _x,k, z _y,k, z _v,x, z _v,y, z _{θ, k}] ^tand then set up state equation and the observation equation of agricultural caterpillar robot: utilize UKF that caterpillar robot state equation and observation equation are merged, obtain agricultural caterpillar robot attitude information accurately, again according to slip calculation of parameter equation, calculate slip parameter, it is characterized in that, specifically comprise the following steps:

(1), adopt transient motion analytic approach to agricultural caterpillar robot modeling, the position coordinates at selected place is xoy and overall Cartesian coordinates XOY, and the angle between two coordinate systems is θ, and obtains the kinematical equation of agricultural caterpillar robot:

x ^&＝v _xcosθ-v _ysinθ (3)

y ^&＝v _xsinθ+v _ycosθ (4)

Wherein, v _x, v _yfor the projection of agricultural caterpillar robot translational velocity v on xoy coordinate system; ω _l, ω _rfor the angular velocity of agricultural caterpillar robot left and right wheels, r is caterpillar robot crawler driving whell radius, and b is caterpillar robot two crawler belt center distance, and L is track length;

(2), utilize position, the velocity information of the GPS receiver reception of installing on agricultural caterpillar robot, the acceleration information that the directional information that gyroscope gathers and acceleration transducer receive, the observed quantity Z of definite agricultural caterpillar robot being determined by these information _k=[z _x,k, z _y,k, z _v,x, z _v,y, z _{θ, k}] ^t, and then set up state equation and the observation equation of agricultural caterpillar robot:

(a), the foundation of state equation:

Use X _k=[x _k, y _k, v _x, v _y, θ _k] ^tthe attitude of describing caterpillar robot, Robot nonlinear state equation is

X _k+1＝f(X _k)+W _k (6)

Wherein

$f\left(X_k\right)=\left[\begin{array}{c}{x}_k+{\mathrm{tv}}_{x,k}\cos {\mathrm{\θ}}_k-{\mathrm{tv}}_{y,k}\sin {\mathrm{\θ}}_k\\ y_k+{\mathrm{tv}}_{x,k}\sin {\mathrm{\θ}}_k+{\mathrm{tv}}_{y,k}\cos {\mathrm{\θ}}_k\\ v_{x,k}+{\mathrm{tv}}_{y,k}{r}_k+{\mathrm{ta}}_{x,k}\\ v_{y,k}-{\mathrm{tv}}_{x,k}{r}_k+{\mathrm{ta}}_{y,k}\\ {\mathrm{\θ}}_k+{\mathrm{tr}}_{m,k}\end{array}\right];$

W _k＝[0 0 tw _a,x tw _a,y tw _r] ^T

W in formula _k-noise vector; T-be the sampling period;

X _k, y _kthe position of X, Y-axis in-global coordinate system;

θ _kthe deviation in direction of motion and course in-Descartes global coordinate system;

V _x,k, v _y,kspeed on-x and y axle;

A _x,k, a _y,kacceleration on-x and y axle;

R _m,kthe instantaneous yaw rate that-gyroscope is surveyed;

(b), the foundation of observation equation:

Observation equation between observed quantity and state variable is

Z _k＝h _k+v _k (7)

Wherein $h_k=\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x,k}\cos {\mathrm{\θ}}_k-v_{y,k}\sin {\mathrm{\θ}}_k\\ v_{x,k}\sin {\mathrm{\θ}}_k+v_{y,k}\cos {\mathrm{\θ}}_k\\ {\mathrm{\θ}}_k\end{array}\right]$

v _k＝[v _x,k v _y,k v _v,x v _v,y v _θ,k] ^T

V in formula _k-observation noise vector, other parameter-definition cotype (6);

(3), utilize UFK recursive algorithm to process agricultural caterpillar robot discrete nonlinear state equation and observation equation, merge to upgrade agricultural caterpillar robot velocity information

(4), according to the agricultural caterpillar robot velocity information calculating through UFK algorithm the instantaneous yaw rate r that gyroscope survey obtains _k, and left and right crawler driving whell angular velocity omega _land ω _r, left and right tracks' slip ratio and the slip angle of calculating agricultural caterpillar robot are as follows:

$\mathrm{il}=\frac{{\mathrm{r\ω}}_L-{\hat{v}}_x}{{\mathrm{r\ω}}_L}---\left(14\right)$

$\mathrm{ir}=\frac{{\mathrm{r\ω}}_R-{\hat{v}}_x}{{\mathrm{r\ω}}_R}---\left(15\right)$

$\mathrm{\δ}=\arctan \left(\frac{0.5{\mathrm{Lr}}_k+v_y}{v_x}\right)-\mathrm{\γ}---\left(16\right)$