阅读说明：本技术 用于使用不变卡尔曼滤波器辅助一队交通工具导航的方法和装置 (Method and apparatus for assisting fleet vehicle navigation using an invariant Kalman filter ) 是由 阿克塞尔·巴劳 于 2019-07-23 设计创作，主要内容包括：本发明涉及一种用于辅助一队交通工具导航的方法,这一队交通工具包括主交通工具(1)及相对于主交通工具(1)可移动的副交通工具(2),所述方法包括以下步骤：·接收由至少一个传感器(2,12)获取的、主交通工具(1)和副交通工具(2)之间的相对运动数据(Y-1,Y-2)；·通过不变卡尔曼滤波器使用所接收的数据(Y-1,Y-2)作为观测值,估计(100,200)这一队交通工具的导航状态,导航状态包括：·第一变量,表示将与主交通工具(1)相关联的位置标记关联到参考点的第一刚性变换；以及·第二变量,表示将与主交通工具(1)相关联的位置标记关联到与副交通工具(2)相关联的位置标记的第二刚性变换,不变卡尔曼滤波器使用包括第一刚性变换和第二刚性变换的逐项组合的法则作为内部组合法则。(The invention relates to a method for assisting the navigation of a fleet of vehicles comprising a primary vehicle (1) and a secondary vehicle (2) movable relative to the primary vehicle (1), said method comprising the steps of: receiving relative movement data (Y) between the primary vehicle (1) and the secondary vehicle (2) acquired by at least one sensor (2, 12) 1 ，Y 2 ) (ii) a Use of the received data (Y) by means of an invariant Kalman filter 1 ，Y 2 ) As observations, a navigational state of the fleet of vehicles is estimated (100, 200), the navigational state comprising: a first variable representing a first rigid transformation relating a position marker associated with the host vehicle (1) to a reference point; and a second variable indicating that the host vehicle will be involved in trafficA second rigid transformation having the position marker associated with (1) associated to the position marker associated with the secondary vehicle (2), the invariant kalman filter using as an internal combination law a law comprising a successive combination of the first rigid transformation and the second rigid transformation.)

1. a method for assisting navigation of a fleet of vehicles, the fleet of vehicles comprising a primary vehicle (1) and a secondary vehicle (2) movable relative to the primary vehicle (1), the method comprising:

receptionData (Y) acquired by at least one sensor (2, 12)₁，Y₂) -the received data comprises relative motion data between the primary vehicle (1) and the secondary vehicle (2);

use of the received data (Y) by means of an invariant Kalman filter₁，Y₂) Estimating (100, 200) a navigational state of the fleet of vehicles as an observation, wherein the navigational state comprises:

-a first variable representing a first rigid transformation relating a coordinate system attached to the host vehicle (1) to a reference coordinate system; and

a second variable representing a second rigid transformation relating a coordinate system attached to the primary vehicle (1) to a coordinate system attached to the secondary vehicle (2),

wherein the invariant Kalman filter uses an operation comprising a term-by-term combination of the first rigid transformation and the second rigid transformation as a binary operation.

2. The method according to the preceding claim, wherein the navigation state of the fleet of vehicles further comprises: at least one error state specific to the sensor (2, 12), and wherein the binary operation is an addition to the error state specific to the sensor.

3. The method according to any one of the preceding claims, comprising: -receiving proprioceptive movement data of the primary vehicle (1) and of the secondary vehicle (2), wherein the estimation comprises the phases of:

propagating (112, 212) the navigational state of the fleet of vehicles previously estimated by the invariant Kalman filter using proprioceptive movement data of the primary vehicle (1) and the secondary vehicle (2) to obtain a propagated state;

-updating (110, 210) the propagation state using the observations to obtain a new navigational state of the fleet of vehicles.

4. The method according to the preceding claim, wherein the proprioceptive movement data is acquired by respective proprioceptive sensors of the primary vehicle (1) and the secondary vehicle (2), and the navigation state of the fleet of vehicles further comprises: at least one error state specific to at least one of the proprioceptive sensors for which the binary operation is an addition.

5. The method of any one of the preceding claims, wherein:

the first variable comprises a rotation matrix representing the attitude of the host vehicle (1) and the position vector of the host vehicle (1), and

the second variable comprises a rotation matrix representing the attitude of the primary vehicle (1) relative to the secondary vehicle (2) and the position vector of the primary vehicle (1) relative to the secondary vehicle (2).

6. The method of any one of the preceding claims, wherein:

the navigational state of the fleet of vehicles further comprises: velocity vector v of the host vehicle in the reference coordinate system_pAnd is equal toA velocity vector of (a); wherein v is_SIs the speed of the secondary vehicle in the reference coordinate system, and R_SIs a rotation matrix representing the attitude of the secondary vehicle (2) relative to the reference coordinate system,

the binary operation applies the same transformation to one of the position vectors and one of the velocity vectors.

7. The method according to any one of the preceding claims, wherein the estimation is performed by the host vehicle (1) and the received data comprises position data of the secondary vehicle (2) in a coordinate system attached to the host vehicle (1), acquired at least by a first sensor (3) of the host vehicle (1), the first sensor comprising for example a lidar, a camera.

8. The method according to any one of the preceding claims, wherein the estimation is performed by the primary vehicle (1) and the received data comprise position data of the primary vehicle (1) in a coordinate system attached to the secondary vehicle (2) acquired at least by a second sensor (12) of the secondary vehicle (2), the second sensor comprising for example a lidar, a camera.

9. The method according to any one of the preceding claims, wherein the received data comprises relative motion data between the primary vehicle (1) and a third object different from the primary vehicle (1) and the secondary vehicle (2), the relative motion data between the primary vehicle (1) and the third object also being used as observations by the invariant Kalman filter.

10. The method of claim 6, wherein the third object is a heading.

11. The method according to either one of claims 9 and 10, wherein the relative motion data between the host vehicle (1) and the third object comprises data acquired by a GPS/GNSS receiver (8) of the host vehicle (1).

12. The method according to any one of claims 7 and 8 considered in combination, comprising:

-performing (100) said estimation step on the basis of position data of said secondary vehicle in a coordinate system fixed to said primary vehicle (1) to generate a first estimate of the navigational state of said fleet of vehicles

-performing (200) said estimation step on the basis of position data of said host vehicle (1) in a coordinate system fixed to said secondary vehicle, to generate a second estimation of the navigational state of said fleet of vehicles

Merging the first and second estimates of the navigational state of the fleet of vehiclesTo generate a third estimate of the navigational state of the fleet of vehiclesThe merging comprises, for example, calculating an average of the first and second estimates of the navigational state of the objects of the set.

13. The method of any one of the preceding claims, wherein:

the received data used as observations form a vector Y₁In the form of Y₁＝l(X₁，y₀) Where l (,) is the left group action of the binary operation, X₁Is the navigational state of the fleet of vehicles, and y₀Is with Y₁Vectors having the same dimensions, and wherein the invariant Kalman filter uses innovationsOr therein

The received data used as observations form a vector Y₂In the form ofWhere l (,) is the left group action of the binary operation, X₂Is the oneNavigational status of fleet vehicles, and y₀Is with Y₂Vectors having the same dimensions, and wherein the invariant Kalman filter uses innovations

14. A device for assisting navigation of a fleet of vehicles, the fleet of vehicles comprising a primary vehicle (1) and a secondary vehicle (2) movable relative to the primary vehicle (1), the device comprising:

a receiving interface (3, 4) for receiving data (Y) acquired by at least one sensor₁，Y₂) -said data comprising relative motion data between said primary vehicle (1) and said secondary vehicle (2);

-a processing unit (1) configured to implement an invariant kalman filter to estimate a navigation state of the fleet of vehicles, wherein the invariant kalman filter is configured to use the received data as observations, wherein the navigation state comprises: a first variable representing a first rigid transformation relating a coordinate system attached to the host vehicle (1) to a reference coordinate system; and a second variable representing a second rigid transformation relating the coordinate system attached to the primary vehicle (1) to the coordinate system attached to the secondary vehicle (2), and wherein the invariant kalman filter is configured to use a term-by-term combination of the first rigid transformation and the second rigid transformation as a binary operation.

Technical Field

The invention relates to a method for assisting the navigation of a fleet of vehicles and to a device suitable for implementing such a method.

Background

The problem of estimating the state of a physical system is generally as follows. The state of the system at time n is represented by vector X_nRepresented, and the observations available at time n are represented by another vector Y_nAnd (4) showing. The evolution of the system is written as follows:

X_n+1＝f(X_n)

where f is a known function (commonly referred to as a propagation function). Observed value Y_nThe state of the system is related by a known observation function h:

Y_n＝h(X_n)

from the sequence (Y)_n)_n≥0Establishing State X_nIs well estimatedIs often a challenge, but in some cases can be simplified.

"linear system" refers to the special case of a system having the form:

X_n+1＝FX_n+w_n

Y_n＝HX_n+V_n

where F is the propagation matrix, H is the observation matrix, w_nAnd V_nIs noise that interferes with the prediction and measurement.

In this linear case, the known method consists in building an estimator called "kalman filter". The kalman filter implements the following calculations:

wherein the indices n +1| n and n +1| n +1 are expressed without considering the observed value Y, respectively_n+1In the case of the estimated value calculated at the time n +1, and in consideration of the observed value Y_n+1In the case of (2), the estimated value calculated at the time n + 1. Matrix K_nReferred to as a "gain matrix," which may be calculated using the Riccati equation. The estimation error is then defined as:

(taking into account the observed value Y_nAfter)

(taking into account the observed value Y_n+1Front)

It can be easily verified that the error follows the following evolution:

e_n+1|n＝Fe_n|n(taking into account the observed value Y_n+1Front)

e_n+1|n+1＝(I-K_n+1H)e_n+1|n(taking into account the observed value Y_n+1After)

Wherein I represents an identity matrix.

The above equation is independent of X_nThus, an estimator can be built that is valid for any actual trajectory of the system, which is not the case for any non-linear system.

In the case of a nonlinear system, an ordinary kalman filter cannot be realized. Therefore, a variation of the kalman filter, called "extended" kalman filter, is proposed, suitable for non-linear systems. However, when using the extended kalman filter, the simplification of the observation in the linear case no longer occurs, thus yielding a reference to X_nAndthe error equation of (2). This problem is the root of most divergent situations encountered when using the extended kalman filter.

However, a second special case that makes the estimation problem simpler is that of an "affine group" observation system, that is to say for which the group operations (i.e. the binary operations that will be denoted in the following by an asterisk) are defined on the state space under consideration, so that the following two characteristics are verified:

a. the propagation function f verifies the relationship of any pair of elements a, b of the state space:

f(a*b)＝f(a)*f(Id)^-1*f(b)

where Id is the unit element of the group caused by operation.

b. The observation function h is of the form h (x) l (x, y)₀) Wherein y is₀Is (Y)_nBelonging) elements in the observation space, and l (,) is a group action, i.e. a function, which is used to verify:

l(a*b，y)＝l(a，l(b，y))

under these two conditions, an extended kalman filter (often more simply referred to as an "invariant kalman filter") called "invariant" can be defined, the invariant kalman filter being controlled by the following equation:

where exp (·) is an exponential mapping (if a Lie group is defined, then the function is known when the group operation is known), K_nIs the "gain matrix" as in the linear case. This may indicate that the estimation error may also evolve autonomously as in the linear case. Thus, the problem of estimating the state is simplified even if the system under consideration is not linear.

When condition b is not verified, a filter of the form:

therefore, the invariant kalman filter is used for navigation assistance of the carrier. The invariant kalman filter used in such navigation-assisted environments estimates the navigation state representing the motion of the considered carrier.

As explained earlier, the use of an invariant kalman filter requires finding an operation (or binary operation), the conditions a and b of which have been verified, to make the estimation problem simpler. There is no general method of finding such operations, and various publications aim to provide the correct operations for a particular system. For example:

the following documents describe the operations for the attitude and velocity estimation problem: bonnabel, s., Martin, p., and Sala un, E. (12 months 2009), invariant extended kalman filter: theory and application for velocity-assisted attitude estimation problem. Published in 2009, a decision and control conference held in association with the 28 th chinese control conference in 2009, CDC/CCC 2009. Recording of the 48 th IEEE conference is carried out (page 1297-1304). IEEE (institute of Electrical and electronics Engineers).

The following documents describe operations for inertial navigation, estimating attitude, velocity and position: barrau, a. and Bonnabel, S. (2017). An invariant extended kalman filter as a stable observer. IEEE automatic control bulletin, 62(4), 1797 and 1812.

Barrau, a. and Bonnabel, s. the operation for SLAM (instantaneous localization and mapping) is described in "an EKF-SLAM algorithm with consistent properties" published in 2015, that is, navigating using a fixed reference point in the vehicle environment. arXiv preprint arXiv: 1510.06263.

barrau, a. and Bonnabel, s. a list of actual systems whose operation is known is further described in "group linear observation system (I)" published in 2017.

In each of these applications, an invariant kalman filter is used to estimate the navigation state of a single carrier.

Zuo, d. and Tan, p. in 2013, published in IEEE model analysis and machine intelligence bulletins, document Coslam: collaborative visual slam in a dynamic environment. 35(2), 354 + 366 pursues another goal: instead of a single vehicle, a fleet of vehicles including a host vehicle and at least one secondary vehicle movable relative to the host vehicle is provided with navigation assistance.

In each of these applications, the kalman filter uses the proprioceptive data of the vehicle and is supplemented by measurements related to environmental objects (known or unknown visual reference points, the earth's magnetic field, the earth itself, etc.) that are considered stationary. The coordinates of these objects may or may not be known in the reference system, but in all cases they do not change over time. However, this assumption is no longer true if the carrier is surrounded by other potential moving vehicles.

Disclosure of Invention

It is an object of the invention to provide easy to implement navigation assistance for a fleet of multiple vehicles.

Thus, according to a first aspect of the invention, a method is proposed for assisting the navigation of a fleet of vehicles comprising a primary vehicle and a secondary vehicle movable with respect to the primary vehicle, the method comprising the steps of:

receiving data acquired by at least one sensor, the received data comprising relative motion data between the primary vehicle and the secondary vehicle;

estimating a navigational state of the fleet of vehicles by an invariant kalman filter using the received data as observations, wherein the navigational state comprises:

a first variable representing a first rigid transformation relating a coordinate system attached to the host vehicle to a reference coordinate system; and

a second variable representing a second rigid transformation relating the coordinate system attached to the master vehicle to the coordinate system attached to the slave vehicle,

wherein the invariant kalman filter uses an operation comprising a term-by-term combination of the first rigid transformation and the second rigid transformation as a binary operation.

The method proposed according to the first aspect of the invention advantageously makes use of the fact that: the carriers surrounding the main carrier also have movement sensors and these carriers transmit their measurements to the main carrier. Although the carriers are movable, these measurements enable the main carrier to use the carriers around them as reference points.

The proposed method also demonstrates that the binary operation used in the method enables to approximate the conditions a and b listed in the background. Therefore, the computation implemented by the invariant kalman filter is particularly easy to implement.

The method according to the first aspect of the present invention may further comprise the following features, which may be employed individually or in combination, when technically feasible.

Preferably, the navigation state of the fleet of vehicles further comprises: at least one error state specific to the sensor, wherein the binary operation is an addition to the error state specific to the sensor.

Preferably, the method comprises: receiving proprioceptive movement data for a primary vehicle and a secondary vehicle, wherein estimating comprises the following phases:

propagating the navigational state of the fleet of vehicles previously estimated by the invariant kalman filter using the proprioceptive movement data of the primary and secondary vehicles to obtain a propagated state;

update the propagation state using the observations to obtain a new navigational state for the fleet of vehicles.

Preferably, the proprioceptive movement data is acquired by respective proprioceptive sensors of the primary vehicle and the secondary vehicle, and the navigational state of the fleet of vehicles further comprises: at least one error state specific to at least one of the proprioceptive sensors, the binary operation being an addition to the at least one error state.

Preferably, the first and second electrodes are formed of a metal,

the first variable comprises a rotation matrix representing the attitude of the host vehicle and a position vector of the host vehicle, an

Wherein the second variable comprises a rotation matrix representing the attitude of the primary vehicle relative to the secondary vehicle and a position vector of the primary vehicle relative to the secondary vehicle.

Preferably, the first and second electrodes are formed of a metal,

the navigational state of a fleet of vehicles further comprises: velocity vector v of host vehicle in reference coordinate system_pAnd is equal toA velocity vector of (a); wherein v is_SIs the speed of the secondary vehicle in the reference coordinate system, and R_SIs a rotation matrix representing the attitude of the primary vehicle relative to the secondary vehicle;

the binary operation applies the same transformation to one of the position vectors and one of the velocity vectors.

The estimation may be performed by the primary vehicle and the received data may comprise position data of the secondary vehicle in a coordinate system attached to the primary vehicle, acquired at least by a first sensor of the primary vehicle, the first sensor comprising for example a lidar, a camera or an odometer. As a variant, this estimation may be performed by the primary vehicle and the received data may comprise position data of the primary vehicle in a coordinate system attached to the secondary vehicle, acquired at least by a second sensor of the secondary vehicle, comprising for example a lidar, a camera or an odometer.

The received data may include relative motion data between the primary vehicle and a third object different from the primary vehicle and the secondary vehicle, the relative motion data between the primary vehicle and the third object also being used as observations by the invariant kalman filter. The third object is for example a heading.

The relative motion data between the host vehicle and the third object may include data acquired by a GPS/GNSS receiver of the host vehicle.

Preferably, the method comprises the steps of:

-carrying out an estimation step based on position data of the secondary object in a coordinate system attached to the host vehicle to generate a first estimate of the navigational state of the fleet of vehicles;

-carrying out an estimation step based on position data of the host vehicle in a coordinate system attached to the secondary object, to generate a second estimate of the navigational state of the fleet of vehicles;

merging the first and second estimates of the navigational state of the fleet of vehicles to produce a third estimate of the navigational state of the fleet of vehicles, the merging comprising, for example, calculating an average of the first and second estimates of the navigational state of the objects of the set.

In order to make the evolution of the error independent of the system state, the following steps are preferably provided:

writing the received data as observations into a vector Y₁In the form of Y₁＝l(X₁，y₀) Where l (,) is the left group of actions for the binary operation, X₁Is the navigational state of a fleet of vehicles, and y₀Is with Y₁Vectors with the same dimensions, and wherein the invariant Kalman filter uses innovationsOr therein

Writing the received data as observations into a vector Y₂In the form of) Where l (,) is the left group of actions for the binary operation, X₂Is the navigational state of a fleet of vehicles, and y₀Is with Y₂Vectors with the same dimensions, and wherein the invariant Kalman filter uses innovations

According to a second aspect of the present invention, there is also provided an apparatus for assisting navigation of a fleet of vehicles, the fleet of vehicles including a primary vehicle and a secondary vehicle movable relative to the primary vehicle, the apparatus comprising:

a receiving interface for receiving data acquired by at least one sensor, the data comprising relative motion data between the primary vehicle and the secondary vehicle;

a processing unit configured to implement an invariant kalman filter to estimate a navigation state of a fleet of vehicles, wherein the invariant kalman filter is configured to use the received data as observations, wherein the navigation state comprises: a first variable representing a first rigid transformation relating a coordinate system attached to the host vehicle to a reference coordinate system; and a second variable representing a second rigid transformation relating the coordinate system attached to the secondary vehicle to the coordinate system attached to the primary vehicle, wherein the invariant kalman filter is configured to use a itemized combination of the first rigid transformation and the second rigid transformation as a binary operation.

Drawings

Other features, objects and advantages of the invention will appear from the following description, which is intended to be illustrative only and not limiting, and which is to be read in connection with the accompanying drawings, wherein:

fig. 1 shows different objects, in particular a fleet of vehicles, that can be moved relative to each other.

Fig. 2 schematically shows the internal components of a primary vehicle and a secondary vehicle of a fleet of vehicles according to one embodiment of the invention.

FIG. 3 is a flow diagram of the steps of a navigation assistance method according to one embodiment of the invention.

Throughout the drawings, similar elements have the same reference numerals.

Detailed Description

1. Fleet of vehicles

Referring to fig. 1, a fleet of vehicles includes a host vehicle 1 and at least one secondary vehicle 2 that is movable relative to the host vehicle 1. Each vehicle 1, 2 may be of any of the following types: land vehicles, ships, airplanes, etc.

Objects outside a fleet of vehicles are also represented in fig. 1: guide signs (light pole and eiffel tower), and the earth itself. As will be seen below, these objects are the navigation reference points for a fleet of vehicles.

In the following, different coordinate systems are considered: a primary coordinate system attached to the primary vehicle 1, a secondary coordinate system attached to the secondary vehicle 2, and a reference coordinate system. The reference coordinate system is, for example, a celestial coordinate system attached to the star or the earth.

Referring to fig. 2, the host vehicle 1 includes a data receiving interface.

The receiving interface comprises at least one first sensor 3 on the host vehicle 1. The at least one first sensor 3 is configured to acquire motion data of the secondary vehicle 2 in the primary coordinate system. The at least one first sensor 3 comprises for example a lidar, a camera or an odometer.

In the present disclosure, the expression "motion data" is considered to cover in particular position, velocity or acceleration.

The receiving interface also comprises a communication interface 4 adapted to communicate with the secondary vehicle 2, in particular to receive from the secondary vehicle 2 motion data of the primary vehicle 1 in a coordinate system attached to the secondary vehicle 2. The communication interface 4 is of the radio type and comprises, for example, an antenna.

The host vehicle 1 further comprises at least one proprioceptive sensor 6.

The proprioceptive sensor 6 comprises, for example, an inertial unit. The inertial unit includes a plurality of inertial sensors, such as gyroscopes and accelerometers. As a variant, the proprioceptive sensor 6 comprises at least one odometer.

The host vehicle 1 further comprises a receiver 8 configured to acquire relative motion data between the host vehicle 1 and a third object different from the host vehicle 1 and the secondary vehicle 2. The receiver is for example a GPS/GNSS receiver, in which case the third object is one of the earth or the star to which the celestial coordinate system is attached. As a variant or in addition, the receiver comprises a lidar, a camera, in which case the third object may be a heading located in the vicinity of the host vehicle 1. In another variation, the receiver includes an odometer that obtains the relative velocity of the vehicle with respect to the earth.

The host vehicle 1 further comprises a data processing unit 10. The processing unit 10 is arranged to process data received by the receiving interface (thus received by the first sensor 3 or by the communication interface 4), data received by the inertial unit 6 or data received by the receiver 8.

The data processing unit 10 generally includes at least one processor configured to implement a navigation assistance method, which will be described below, by means of an invariant kalman filter. The invariant kalman filter is typically in the form of a computer program executable by a processor of the data processing unit. The general operation of an invariant kalman filter is known per se. However, as will be seen below, the binary operation implemented by the processing unit 10 for configuring the invariant kalman filter is selected in a particular way to suit the environment assisting the navigation of a fleet of vehicles comprising vehicles 1 and 2.

Preferably, the processing unit 10 comprises at least two processors to implement two kalman filters in parallel. As will be seen below, the two kalman filters do not use exactly the same input data.

Furthermore, the secondary vehicle 2 comprises at least one second sensor 12 and a communication interface 14 for transmitting data acquired by the at least one second sensor 12 to the communication interface 4 of the primary vehicle 1.

The second sensor 12 is configured to acquire movement data of the host vehicle 1 in a coordinate system attached to the secondary vehicle 2. The at least one second sensor 12 comprises for example a lidar, a camera or an odometer.

The secondary vehicle 2 comprises means for providing proprioceptive movement data of the secondary vehicle.

These providing means comprise for example at least one proprioceptive sensor 16. The proprioceptive sensor is for example one or more types of sensors envisaged for the proprioceptive sensor 6.

As a variant, these providing means comprise a memory storing an a priori model of the evolution of the secondary vehicle 2. The memory may also be integrated into the secondary vehicle 2 as well as the primary vehicle 1.

2. Configuration of invariant Kalman filter

The invariant kalman filter implemented by the processing unit 10 is configured to estimate the navigational state of a fleet of vehicles comprising the primary vehicle 1 and the secondary vehicle 2.

The navigation states include: a first variable representing a first rigid transformation relating the master coordinate system (attached to the master vehicle 1) to the reference coordinate system; and a second variable representing a second rigid transformation relating the secondary coordinate system (attached to the secondary vehicle 2) to the primary coordinate system.

The first rigid transformation enables, for example, switching from a coordinate system associated with the host vehicle 1 to a reference coordinate system, and the second rigid transformation enables switching from a coordinate system associated with the host vehicle 1 to a coordinate system associated with the secondary vehicle 2.

In a well-known manner, a rigid transformation (also called affine equidistant) is a transformation that preserves the distance between each pair of points of the entity. Thus, each of the first rigid transformation and the second rigid transformation may be characterized by a combination of rotation and translation.

In the following, an embodiment will be described in detail, wherein the navigation state denoted X comprises the following elements:

X＝(R_p，x_p，R_sp，x_sp)

wherein R is_p，x_p，R_sp，x_spThe definition is as follows:

·R_pand x_pThe vector, which is the rotation matrix and dimension 3, respectively, represents the pose and position of the host vehicle: the vector u written in the coordinate system of the host vehicle 1 becomes the vector R in the fixed coordinate system_pu+x_p。

·R_spAnd x_spThe vector, which is the rotation matrix and dimension 3, respectively, represents the attitude and relative position of the primary vehicle with respect to the secondary vehicle: the vector u written in the coordinate system of the host vehicle 1 becomes the vector R in the coordinate system of the sub-vehicle_spu+x_sp。

In this particular embodiment, the first variable is R_p，x_pAnd the second variable is R_sp，x_sp。

The expression "object X 'has the same properties as the state vector" as used below means that X' is a series of matrices and vectors similar to X.

The number 3X (r + v) 12 will also be referred to as the "dimension of state X", where r is the number of rotation matrices present in X and v is the number of vectors present in X. In other embodiments, the number may be different.

Furthermore, the invariant kalman filter is further configured to use the relative motion data between the primary vehicle 1 and the secondary vehicle 2 received by the receiving interface from the first sensor 3 of the primary vehicle 1 as an observation.

The observation here is the relative position of the secondary vehicle expressed in the coordinate system of the primary vehicle:

the invariant kalman filter is configured to use an operation comprising a term-by-term combination of the first rigid transformation and the second rigid transformation as a binary operation.

In one embodiment, this binary operation, denoted by x, applies the following transformation to two objects (R)_p，x_p，R_sp，x_sp) And (R'_p，x′_p，R′_sp，x′_sp)：

(R_p，x_p，R_sp，x_sp)*(R′_p′，x′_p，R′_sp，x′_sp)

＝(R_pR′_p，x_p+R_px′_p，R_spR′_sp，x_sp+R_spx′_sp)

3. Method for assisting the navigation of a fleet of vehicles

Referring to FIG. 3, a method 100 for assisting a fleet of vehicles in navigating and implementing an invariant Kalman filter configured as indicated in section 2 according to a first embodiment includes the following steps.

Assuming that an estimate of the navigational state of a fleet of vehicles has been estimated by an invariant Kalman filter

In an acquisition step 102, the first sensor 3 acquires a first set of movement data Y of an object outside the host vehicle 1 from the host coordinate system₁. These data may include:

position data of the secondary vehicle 2 in the primary coordinate system (then the corresponding external object is the secondary vehicle 2),

position data of at least one guide in the master coordinate system (then the corresponding external object is the guide). The guide is located at a known position in the reference coordinate system.

These data Y₁Is sent to the processing unit 10.

In step 104, the processing unit 10 calculates an observed measurement value Y₁Difference from the expected measurement (this difference is denoted as Z)₁Referred to as innovations in the literature relating to kalman filters). State X for which a measurement is expected to be estimated from a prior invariant Kalman filter₁And (5) deducing.

In a correction step 106, the data processing unit 10 will innovate Z₁With a matrix K called "gain" matrix₁Multiplication, in linear correction dx₁＝K₁Z₁In (A) represents Z₁To apply to the system state.

The choice of gain is a classical problem common to most estimation methods (see below).

In a retraction step 108, the processing unit 10 corrects the linearity by dx₁Is converted into andnon-linear correction C with identical properties₁(because of the stateInvolving rotation, thus stateNot a vector). The transformation used is any function that takes as parameters the vector of the dimension of state X (12 in this embodiment) and returns objects of the same nature as X, but a particularly efficient choice is the item-wise exponentiation of the lie group of rigid transformation pairs.

Then, a non-linear update step 110 is performed by the processing unit 10. In this step 110, the processing unit 10 estimates an estimated value X of the system state₁Nonlinear correction C₁Combined to establish a corrected estimate:

wherein the symbol is the binary operation defined above. Selecting a gain matrix K₁To stabilize the nonlinear estimation error e defined by the following equation:

wherein the symbols are.^-1Is a common inversion associated with operations:

this error can also be clearly written in the following way:

this error is used to construct a linearized system according to the usual procedure of invariant filtering, whereby the matrix K is derived, for example, by integrating the ricatt equation₁。

In a propagation step 112, known per se to the person skilled in the art, the processing unit 10 is slave-stateA propagated navigational state is generated. To this end, the processing unit 10 applies an evolution model, which may be, for example, a odometer, a prior model or a conventional integration of inertial measurements acquired by the proprioceptive sensors 6, 16 comprised in the vehicle 1, 2.

The above steps form an iteration of the invariant kalman filter.

Thanks to the invariant kalman filter, characteristics are obtained that can be obtained also in linear situations: the evolution of the estimation error is autonomous (neither dependent on X nor on X))。

The processing unit 10 repeats these same steps 102, 104, 106, 108, 110, 112 in a new iteration of the invariant kalman filter. The state estimated during the propagation step 112 of a given iteration is used as input data for the innovative calculation step 104 and the non-linear update step 110 of the next iteration.

Finally, thanks to the method 100, the primary vehicle 1 can obtain assistance not only with respect to its own navigation, but also with respect to the navigation of the secondary vehicle 2, based on the different data measured by the first sensor 3 and the proprioceptive sensors 6, 16.

The right-hand portion of fig. 3 also shows a method 200 for assisting the navigation of a fleet of vehicles and implementing an invariant kalman filter, also configured as indicated above, according to a second embodiment; the method 200 includes the following steps.

In an acquisition step 202, movement data Y of the host vehicle 1 in at least one coordinate system attached to an object external to the host vehicle is acquired₂. Data Y₂The method can comprise the following steps:

the motion data of the host vehicle 1 in the secondary coordinate system, acquired by the second sensor 12, for example the position data of the host vehicle 1 in the secondary coordinate system (in this case, the corresponding external object is the secondary vehicle);

data acquired by the receiver 8 (since these data enable the host vehicle to be geographically located with respect to the earth, the corresponding object may be considered to be the earth).

When data Y₂From the secondary vehicle 2, the data Y is transmitted, where appropriate, via the communication interfaces 14 and 4₂To the host vehicle 1. Data Y₂To the processing unit 10.

In a step 204, similar to step 104, the processing unit 10 calculates an observed measurement value Y₂Difference from expected measurement (Innovation Z)₂). The state (denoted as the state) in which the measurement is expected to be estimated from the invariant Kalman filter) And (5) deducing.

In a correction step 206, the processing unit 10 will innovate Z₂And a gain matrix K₂Multiplication, in linear correction dx₂＝K₂Z₂In (A) represents Z₂To apply to the system state.

The calibration step 206 is similar to step 106, except that the gain matrix K is selected₂To stabilize a second nonlinear error variable e defined by the equation:

in a retraction step 208, which is identical to step 108, the processing unit 10 corrects the linearity by dx₂Is converted into andnon-linear correction C with identical properties₂(because of the stateInvolving rotation, thus stateNot a vector).

Then, a non-linear update step 210 similar to step 110 is performed by the processing unit 10. In this step 210, the processing unit 10 estimates the system stateNonlinear correction C₂In combination, to establish a corrected estimate by:

in a propagation step 212, which is the same as step 112, the processing unit 10 is in a slave stateA propagated state is generated.

The above steps form an iteration of the invariant kalman filter.

The processing unit 10 repeats these same steps 202, 204, 206, 208, 210, 212 in a new iteration of the invariant kalman filter. The state estimated during the propagation step 212 of a given iteration is used as input data for the innovative calculation step 204 and the non-linear update step 210 of the next iteration.

As in the method 100 according to the first embodiment, the dependency of the evolution of the error on the system state is reduced.

Either of the above-described methods 100, 200 may be implemented by the host vehicle 1.

The fundamental difference between the method 100 according to the first embodiment and the method 200 according to the second embodiment is that the relative motion data between the primary vehicle 1 and the secondary vehicle 2 is used as an observation by an invariant kalman filter: in the case of the method 100, these data are represented in a coordinate system attached to the host vehicle 1, whereas in the case of the method 200, these data are represented in an external coordinate system.

The processing unit 10 of the host vehicle 1 advantageously implements a method according to a third embodiment, which combines the two methods 100 and 200 described above in parallel.

Cause to obtain dataFor example, by a first processor of the processing unit 10, to cause data to be obtainedThe second method 200 is implemented by a second processor of the processing unit. In other words, the two processors implement two invariant kalman filters in parallel.

In a merging step 302, the processing unit 10 merges the dataAndto obtain an optimized estimate of the navigational state of a fleet of vehicles, denoted asFor example,is thatAndaverage value of (a). Due to the stateAndare not vectors and therefore their classical average is replaced by any average definition that fits the manifold. One skilled in the art can find a generalized average definition of manifold in Markley, F.L., Cheng, Y., Crassidis, J.L., and Oshman, Y., the "average quaternion" 30(4), 1193-1197 published in 2007 in the journal of guidance, control, and dynamics.

Where appropriate, by comparison with data representing the uncertainty of these estimatesAndthe associated covariance matrix weights the mean. These covariance matrices are also generated by the two invariant kalman filters of methods 100 and 200.

The present invention is not limited to the above-described embodiments.

Representing the landmark position q may be included in the navigational state of a fleet of vehicles_i(or heading) velocity or vector. The navigational state may be limited to include position and rotation data.

Further, the fleet of vehicles considered may include a plurality of secondary vehicles, and the navigational state may be expanded to include elements specific to each secondary vehicle of the fleet of vehicles.

In addition, during implementation of any of the above methods, it is not necessary to use inertial data acquired by the inertial unit. However, when such inertial data is used, the state of the system should include a state representing the speed of each vehicle equipped with an inertial unit. This would be:

speed v of the host vehicle in the reference coordinate system_p，

-a speed deviation v between the two vehicles with respect to a fixed coordinate system_spProjected into a coordinate system attached to the secondary vehicle. In other words, v_spDefined by the following equation:

wherein v is_pIs the velocity, v, of the main carrier in a fixed coordinate system_sIs the speed of the subcarrier in a fixed coordinate system, the rotation matrixSo that a point can switch from its coordinates in the secondary coordinate system to its coordinates in the fixed coordinate system. Only one of these two states may be added to the system.

It should be noted that in another embodiment, the navigational state may be represented by a natural variable (R)_p，x_p，R_s，x_s) Form, wherein a rotation matrix R_sSum vectorSo as to have coordinates in a coordinate system attached to the secondary vehicleWill have the coordinates R in a fixed coordinate system_su+x_s. In this case, the binary operation to be used is more complex: