Device and method to estimate the state of a moving vehicle

摘要

Device and method to estimate the state of a moving vehicle overflying a certain terrain. The device comprises a camera oriented toward the terrain, an inertial measurement unit, a device for the processing of images and a “navigation filter”. This filter uses an innovative method to obtain state estimates of the vehicle. Unlike the conventional art, only robust and flexible expressions are used here, producing accurate state estimates, with no possibility of divergence, with no need for initial state estimates or high computational power. The method calculates parameters describing geometrical relationships among points of the trajectory and others on the terrain. These parameters are combined with estimates of the accelerations to obtain estimates of the velocity at a given time and of the gravity acceleration vector. By integrating these estimates, velocity and position profiles are obtained. The state is expressed in a reference system fixed with respect to the terrain.

主权项

1. Device to estimate the state of a moving vehicle, composed of an inertial measurement unit (IMU), measuring the angular velocities and the accelerations of said vehicle, by a camera (CAM) and by a device (PROC) for the processing of the terrain photographic images taken at successive instants, identifying a set of characteristic points of the terrain, which are tracked on the different images, and by a navigation filter (NF) which using a suitable algorithm calculates the state of the vehicle in terms of velocity, position, attitude and angular velocity, in a reference frame fixed with respect to the ground, characterized in that:
the navigation filter, in order to get a unit vector (U), indicating the direction of the displacement of the vehicle between two time instants (T1) and (T2), performs the following two computing steps:
A) calculates at first a set of vectors, (E), which in case of no measurement errors would be all orthogonal to the said unit vector (U), by performing the following operations:
A.1) integrates the angular velocity measurements of the inertial measurement unit in order to obtain the profile of the attitude, i.e. the temporal behaviour of the attitude during a time interval, with respect to a reference frame fixed with respect to the ground,A.2) utilizes the said attitude profile in order to process the output of the image processing device, i.e. the set of pixel tracks where each pixel track describes the apparent motion of a characteristic point of the terrain in the sequence of the images, in order to obtain a set (TP) of tracks of unit vectors, where each unit vector track is a function of time and describes the motion of said characteristic point in a reference frame fixed with respect to the ground,A.3) the set (E) is constructed by performing the following computations on each member (K1) of a subset of (TP): A.3.1) the values of (KI) at (T1) and (T2), indicated respectively with (KI1) and (KI2), are extracted, A.3.2) the normalized vector cross product of (KI1) and (KI2), indicated with (KCI), is computed, A.3.3) (KCI) is inserted in the set (E),B) determines, through the method of the least squares, the vector closer to the perpendicular to the said set of vectors (E) and that in this way better approximates the actual unit vector (U); to obtain the ratio, (RHO), between the lengths of the displacements of the vehicle, respectively in the timeframe from (TA) to (TM) and from (TM) to (TB), where (TA), (TM) and (TB) are three successive time instants, the navigation filter performs the following computations:
A) computes, by using the procedure described above, the vectors (UA) and (UB), that indicate the directions of the displacements of the vehicle respectively between time instants (TA) and (TM), and (TM) and (TB),B) for each unit vector track (KI) that is a member of the said set (TP), two triangles, (TRA) and (TRB), are defined when possible, such that the vertices of (TRA) are: the position of the vehicle at time (TA), the position of the vehicle at time (TM), and the point on the terrain tracked by (KI) and the vertices of (TRB) are: the position of the vehicle at time (TM), the position of the vehicle at time (TB), and the said tracked point of the terrain,C) the relevant angles of the triangles (TRA) and (TRB) are computed by performing the following operations: a) three unit vectors, (KIA), (KIM) and (KIB), belonging to the said pixel track (KI), and associated respectively to the time instants (TA), (TM) and (TB), are extracted from the said pixel track b) the angle between vectors (KIA) and (UA), which is also the angle (ALPHA) of the triangle (TRA), is computed, c) the angle between vectors (KIA) and (KIM), which is also the angle (BETA) of the triangle (TRA), is computed, d) the angle between the opposite of the vector (UB), and the vector (KIB), is computed, which is also the angle (ALPHA) of the triangle (TRB), is computed, e) the angle between vectors (KIB) and (KIM), which is also the angle (BETA) of the triangle (TRB), is computed,D) (RHO) is then computed by multiplying the sine of the angle (ALPHA) of (TRA) with the sine of the angle (BETA) of (TRB), and then dividing that product with the product of the sine of the angle (ALPHA) of (TRB) with the sine of the angle (BETA) of (TRA); multiple estimates of (RHO) can be combined, to calculate the length of the displacement of the vehicle between two time instants, the navigation filter processes the said unit vectors (U), the said ratio (RHO), and the measurements of the inertial measurement unit, conveniently treated, together with the value of the gravity acceleration vector or the value of the norm of such vector.