PROCEDE ET DISPOSITIF D'AIDE A LA NAVIGATION BASEE SUR UN FILTRE DE KALMAN

Technique antérieure

[0001 ] L’invention se rapporte au domaine général des mesures réalisées à l’aide de capteurs, telles que la position et la vitesse d’un véhicule.

[0002] Elle concerne plus particulièrement le contrôle de la navigation d’un véhicule à l’aide d’un filtre de Kalman. Ce filtre de Kalman sert à estimer une grandeur nécessaire pour la mise en oeuvre d’une manoeuvre de navigation de ce véhicule. Cette grandeur représente par exemple une caractéristique du véhicule lui-même (ex. la position, la vitesse, l’orientation etc.), ou peut également représenter une caractéristique relative à l’environnement (la présence d’un objet à proximité du véhicule, la vitesse du vent etc.). Par la suite, de telles caractéristiques, qu’elles soient internes ou externes au véhicule, seront appelées « états ».

[0003] Notons qu’un filtre de Kalman estime également des incertitudes, sous forme de matrices de covariances, sur l’estimation des états. Ces incertitudes estimées peuvent aussi servir à la mise en oeuvre d’une manoeuvre. Le terme « grandeur » pourra donc également désigner de telles incertitudes estimées par le filtre de Kalman.

[0004] De façon connue, dans un système de navigation on se sert fréquemment d’un filtre de Kalman en tant qu’estimateur d’état. Le filtre de Kalman évalue l’état x _n récursivement, à chaque instant n, à partir d’un modèle numérique (notamment un modèle décrivant comment les différents états évoluent les uns en fonction des autres, et comment les mesures dépendent des états) et d’informations recueillies par des capteurs, ci-après appelées « mesures ».

[0005] Par exemple, dans un système de navigation qui génère une estimation de la position, de l’orientation et de la vitesse d’un objet cible, le modèle comprend notamment des équations cinématiques régissant la trajectoire d’un véhicule selon sa position, son orientation et sa vitesse. Le modèle peut impliquer certains paramètres dont les valeurs sont connues a priori (ex. l’accélération de la pesanteur) et/ou dont les valeurs peuvent varier selon le pas de temps n.

[0006] Dans la suite de la demande, pour évoquer la temporalisation n, les expressions « moment >>, « étape » et « instant » donné seront utilisées indifféremment.

[0007] L’intervalle de temps entre deux de ces instants successifs est généralement un paramètre ajustable par l’utilisateur.

[0008] Dans le domaine des systèmes de navigation, les capteurs sont souvent associés à une unité de mesure inertielle.

[0009] Par exemple, en navigation, il est courant d’utiliser une unité de mesure inertielle couplée à un récepteur GPS pour estimer la position et l’orientation d’un véhicule. L’unité de mesure inertielle, comportant des gyroscopes et des accéléromètres, évalue respectivement les changements d’orientation et les forces successives auxquels est soumis le porteur mobile. Par simple opération mathématique d’intégration, ces mesures permettent d’estimer l’état du véhicule en termes de position, vitesse et orientation. L’information communiquée par les GPS permet de calculer la position du véhicule. Un filtre de Kalman fusionne ces informations pour estimer l’état du véhicule. La qualité de la fusion se fait, entre autres, via un bon calibrage des paramètres du filtre. Les modèles et les méthodes de fusion de données sont connus et ne seront pas exposés plus en détail.

[0010] La ou les grandeurs estimées par le filtre de Kalman, et l’utilisation de ces estimations, varient selon l’application. Notamment :

- l’estimation porte par exemple sur l’état du véhicule et l’estimation peut servir pour mettre à jour une carte routière affichée au conducteur du véhicule et/ou pour décider quelles directions lui seront données pour parvenir au but de son voyage ;

- dans un système équipé de capteurs radar, l’estimation peut porter sur les états d’objets à proximité du véhicule et cette estimation peut servir d’indication de la position de ces objets afin que la trajectoire du véhicule soit adaptée ;

- dans un véhicule autonome, l’estimation porte par exemple sur le véhicule et peut renseigner un module de commande qui pilote le véhicule permettant à ce dernier de décider de la prochaine manoeuvre, etc.

[0011 ] Le filtre de Kalman détermine, récursivement, une estimation x _n | _n de l’état actuel de l’objet cible en se servant de l’estimation précédente ainsi que des mesures fournies par des capteurs jusqu’à l’instant n. Ce calcul récursif est illustré de manière schématique par la figure 1 .

[0012] Même avec un modèle mathématique approprié, la qualité des mesures fournies par le ou les capteurs a une influence importante sur la qualité des estimations par le filtre de Kalman.

[0013] Pour apprécier cette importance de la qualité des mesures, il convient tout d’abord de rappeler le fonctionnement classique d’un filtre de Kalman.

[0014] On notera x _n l’état qu’estime le filtre de Kalman à une étape n donnée. Dans beaucoup d’applications l’état s’exprime en termes d’une pluralité de variables et, donc, x _n constitue un vecteur.

[0015] Le système évolue selon le modèle numérique et, donc, on peut considérer que l’état x _n de l’objet cible à un moment donné n dépend de l’état x _n -i de l’instant précédent n-1 selon l’équation [Math. 1 ] ci-dessous :

[Math. 1 ]

Où F _n est une matrice carrée connue, un un vecteur d’entrée de commande (connu de l’utilisateur), c’est-à-dire une action appliquée au système, B _n la matrice représentant le modèle qui lie l’état x _n à cette entrée de commande, et w _n une variable aléatoire gaussienne centrée représentant un bruit de covariance représentée par une matrice Q _n .

[0016] Dans l’équation [Math.1 ] et les équations suivantes, la notation côte à côte de deux quantités correspond à :

- un simple produit si ces deux quantités sont des nombres,

- un produit d’une matrice avec un vecteur si l’une de ces quantité est une matrice et l’autre quantité est un vecteur (par exemple F _n x _n -i dans l’équation [Math. 1 ]), ou - un produit matricielle si ces deux quantités sont des matrices (par exemple F _n P _n -i|n-i dans l’équation [Math. 4] ci-après).

[0017] La ou les mesures, notamment celle(s) délivrée(s) par le(s) capteur(s), à l’instant n, seront notées y _n et dépendent de l’état x _n selon l’équation [Math. 2] ci- dessous :

[Math. 2] yn _n X _n + _n

[0018] Où H _n est une matrice d’observation connue, et v _n une variable aléatoire gaussienne centrée représentant du bruit de covariance R _n . Encore une fois, dans beaucoup d’applications, une pluralité de mesures est délivrée au filtre de Kalman à chaque instant n et, dans ce cas, y _n et v _n seront des vecteurs et R _n une matrice.

[0019] Lors de l’implémentation pratique d’un filtre de Kalman il faut choisir les matrices de covariance du bruit, Q _n et R _n . Souvent cela se fait en étudiant au préalable comment ces covariances se rapportent aux données obtenues dans le domaine en question. Une approche typique pour estimer les covariances consiste en l’application de l’algorithme ALS (pour « autocovariance leastsquares » en anglais).

[0020] Les matrices de covariance du bruit Q _n et R _n correspondent à des incertitudes sur les mesures effectuées par les capteurs. Ainsi, dans la présente demande, et par souci de concision, nous nous référerons à ces quantités sous le terme d’incertitude sur la mesure y _n .

[0021 ] Le filtre de Kalman maintient une estimation de l’état x _n à un moment donné n, et cette estimation sera notée x _n |n-i lorsqu’elle est calculée (prédite) à partir des mesures y _n -i disponibles au moment n-1 , et x _n | _n lorsque le calcul intègre les mesures y _n obtenues au moment n. Le filtre de Kalman estime aussi une matrice de covariance P _n représentative de l’incertitude de l’estimation de l’état x _n et cette matrice sera notée P _n |n-i lorsqu’elle s’applique à l’estimation x _n | _n -i, et P _n | _n lorsqu’elle s’applique à l’estimation x _n |n-

[0022] A chaque étape n, le filtre fonctionne en deux temps. [0023] Dans un premier temps, le filtre de Kalman prédit l’estimation de l’état a priori x _n |n-i de l’objet cible, ainsi que la matrice de covariance P _n |n-i , à partir de l’estimation de l’état x _n -i|n-i et de la matrice de covariance P _n -i |n-i , selon les équations [Math. 3] et [Math. 4] ci-dessous :

[0024] [Math. 3]

[0028] Où F _n ^T est la transposée de la matrice F _n .

[0029] Dans un second temps, le filtre de Kalman met à jour l’estimation a priori de l’état de l’objet cible en se servant de la prédiction faite à l’étape précédente et prenant en compte les mesures y _n actuelles avec une certaine pondération représentée par un coefficient K _n nommé « le gain de Kalman ». La mise à jour se fait selon les équations [Math. 5] et [Math. 6] ci-dessous :

[0030] [Math. 5]

[0034] où K _n est calculé selon l’équation [Math. 7] ci-dessous :

[0035] [Math. 7]

[0037] où S _n , qui représente la covariance des mesures y _n est calculée selon l’équation [Math. 8] ci-dessous :

[0038] [Math. 8]

[0040] Sn' ¹ est la matrice inverse de S _n , et z _n est définie selon l’équation [Math. 9] ci- dessous : [0041 ] [Math. 9]

[0043] Le filtre de Kalman émet en sortie l’estimation actuelle x _n | _n de l’état courant et l’incertitude P _n |n associée à cette estimation, ces grandeurs étant délivrées, selon l’application, à un système de commande ou autre.

[0044] Tant que le filtre de Kalman présume que les mesures issues du GPS sont fiables, une erreur de mesure, même faible, fera écarter l’estimation plus ou moins fortement de la situation réelle.

[0045] Ainsi, la donnée des valeurs d’incertitude associée à une estimation ne suffit pas pour évaluer la fiabilité des mesures et des estimations par le filtre de Kalman, ni ne permet de détecter quelle mesure ou quel capteur est responsable d’une mauvaise robustesse de cette estimation.

[0046] L’invention vise à pallier ces inconvénients.

Exposé de l’invention

[0047] Ainsi, et selon un premier aspect, l’invention propose un procédé de contrôle de navigation d’un véhicule pour la mise en oeuvre d’une manoeuvre de navigation, le procédé comprenant les étapes :

- d’estimation d’une grandeur par un filtre de Kalman à partir de paramètres, d’un modèle numérique du véhicule dans son environnement, et d’au moins une mesure effectuée par au moins un capteur et associée à une incertitude sur cette mesure, ladite grandeur servant à la mise en oeuvre de ladite manoeuvre de navigation ;

- de détermination d’au moins une valeur de dérivée partielle de ladite grandeur par rapport à ladite au moins une mesure. ;

- d’obtention d’au moins une valeur de criticité de ladite au moins une mesure pour ladite estimation, à partir de ladite au moins une valeur de dérivée partielle ;

- d’envoi d’au moins un signal de criticité représentatif de ladite au moins une valeur de criticité ;

- de réception d’au moins une instruction en réponse à ladite étape d’envoi ; et

- d’application de ladite au moins une instruction ; une dite instruction indiquant une commande de contrôle pour la mise en oeuvre de ladite manoeuvre de navigation.

[0048] Corrélativement l’invention propose un système de contrôle de navigation d’un véhicule pour la mise en oeuvre d’une manoeuvre de navigation, le système comprenant :

- un module d’estimation d’une grandeur par un filtre de Kalman à partir de paramètres, d’un modèle numérique du véhicule dans son environnement, et d’au moins une mesure effectuée par au moins un capteur et associée à une incertitude sur cette mesure, ladite grandeur servant à la mise en oeuvre de ladite manoeuvre de navigation ;

- un module de détermination d’au moins une valeur de dérivée partielle de ladite grandeur par rapport à ladite au moins une mesure ;

- un module d’obtention d’au moins une valeur de criticité de ladite au moins une mesure pour ladite estimation, à partir de ladite au moins une valeur de dérivée partielle ;

- un module d’envoi d’au moins un signal de criticité représentatif de ladite au moins une valeur de criticité ;

- un module de réception d’au moins une instruction en réponse à ladite étape d’envoi ; et

- un module d’application de ladite instruction ; une dite instruction indiquant une commande de contrôle pour la mise en oeuvre de ladite manoeuvre de navigation.

[0049] Selon un mode de mise en oeuvre du procédé de contrôle une dite instruction indique d’ajouter des mesures pour une estimation ultérieure de ladite grandeur.

[0050] Précisons ici que l’emploi du terme de dérivée partielle englobe ici la notion de gradient. En effet, une mesure ou une incertitude peut contenir plusieurs quantités (vitesse, position etc.) et peut donc être représentée sous la forme d’un vecteur.

[0051 ] Dans ce cas, par souci de concision, on emploiera également le terme de dérivée partielle de la grandeur par rapport à une quantité. Autrement dit, dans la présente demande, si une quantité est sous la forme d’un vecteur, la dérivée partielle de la grandeur par rapport à cette quantité désigne le gradient de cette grandeur selon le vecteur représentant la quantité.

[0052] Egalement par souci de concision, on appellera valeur d’une dérivée partielle de la grandeur par rapport à une mesure l’ensemble des valeurs que comprend le gradient de la grandeur selon le vecteur représentant cette mesure.

[0053] Précisons également que dans la présente demande, la valeur d’une dérivée partielle d’une grandeur par rapport à une première matrice (par exemple une matrice représentant des incertitudes sur une mesure) correspond à une deuxième matrice dont les composants sont égaux aux valeurs de dérivées partielles de cette grandeur par rapport à chacun des composants de la première matrice.

[0054] Dans la présente demande, si une somme entre deux vecteurs (par exemple deux dérivées partielles sous la forme de vecteurs) est invoquée, cette somme correspond à un vecteur égal à la somme élément par élément de ces deux vecteurs.

[0055] L’évaluation de ces dérivées partielles permet avantageusement de fournir des informations importantes sur la robustesse de l’estimation d’une grandeur nécessaire à la mise en oeuvre d’une manoeuvre de navigation, en particulier en permettant de quantifier la criticité des différentes mesures dans cette estimation.

[0056] Les informations obtenues sur la robustesse de l’estimation de la grandeur indiquent s’il est pertinent ou non de rajouter une mesure pour estimer la grandeur, parmi l’ensemble des mesures disponibles, ou d’appliquer une commande de contrôle, qui correspond par exemple à l’annulation de la manoeuvre envisagée.

[0057] En effet, d’une part, une ou des dérivées partielles d’une grandeur par rapport à une mesure effectuée à un instant donné, ou par rapport à des mesures effectuées lors d’une plage de temps donnée, permet d’évaluer l’influence des mesures sur l’estimation de la grandeur au cours du temps.

[0058] Par exemple, si une dérivée partielle par rapport à une mesure prend une grande valeur pour la mesure effectuée à l’instant précédent l’estimation de la grandeur, mais prend des valeurs proches de zéro pour la mesure effectuée aux instants antérieurs, cela signifie que seule la mesure effectuée à l’instant précédent l’estimation est prise en compte de manière significative dans cette estimation.

[0059] Ce cas de figure peut dénoter un manque de robustesse de l’estimation, dans le cas où une estimation robuste nécessite la prise en compte de plusieurs mesures successives.

[0060] Dans cet exemple, l’estimation peut être améliorée en augmentant la fréquence des mesures prises au cours du temps.

[0061 ] Ainsi, selon un mode de mise en oeuvre du procédé de contrôle :

- des mesures effectuées successivement sont obtenues, et

- au cours de ladite étape de détermination, pour chacune de ces mesures, une valeur de dérivée partielle de la grandeur est déterminée par rapport à cette mesure.

[0062] Dans ce mode de réalisation, le calcul de dérivées partielles est réalisé a posteriori. Autrement dit, la grandeur est obtenue après l’obtention des mesures par rapport auxquelles les valeurs de dérivée partielle de la grandeur sont calculées.

[0063] Par exemple, les dérivées partielles d’une estimation x _n | _n à l’instant n sont calculées par rapport à la série de mesures prise en compte entre les instants 0 et n ; ainsi, le calcul de ces dérivées partielles est réalisé suite à l’obtention de l’estimation x _n |n-

[0064] D’autre part une ou des dérivées partielles d’une grandeur par rapport à une mesure effectuée par un capteur donné, ou par rapport à des mesures effectuées par un ensemble de capteurs donnés, permettent d’évaluer l’influence du capteur ou de l’ensemble des capteurs sur l’estimation.

[0065] Par exemple, si une dérivée partielle par rapport à une mesure prend une valeur faible alors que les connaissances de l’homme du métier indiquent que la grandeur estimée est généralement très influencée par cette mesure (par exemple, l’estimation de la vitesse d’un véhicule est très influencée par la mesure de son accélération), alors cela indique que l’estimation n’est pas robuste. [0066] Selon un mode de mise en oeuvre du procédé de contrôle, pour au moins un instant donné, une dite valeur de dérivée partielle est déterminée au cours de ladite étape de détermination, par rapport à une mesure, cette mesure comportant au moins deux mesures effectuées chacune par des capteurs distincts à cet instant donné.

[0067] Rappelons que, comme expliqué ci-avant, une mesure peut correspondre à un vecteur dont les composantes sont des mesures de différents états. Par exemple, une dite mesure à un instant donné peut par exemple comporter une mesure de l’accélération du véhicule à cet instant donné ainsi qu’une mesure de la vitesse du véhicule à cet instant donné.

[0068] Rappelons également que dans ce mode de mise en oeuvre, une dérivée partielle de la grandeur par rapport à cette mesure est un vecteur dont les composants sont égaux aux dérivées partielles de la grandeur par rapport à chacune des mesures des différents états. Par exemple, la dérivée partielle de la grandeur par rapport à une mesure d’accélération et de vitesse comporte la dérivée partielle de la grandeur par rapport à la mesure de vitesse ainsi que la dérivée partielle de la grandeur par rapport à la mesure d’accélération.

[0069] Le calcul des dérivées partielles d’une grandeur estimée par un filtre de Kalman est, de façon avantageuse, peu coûteux en termes de puissance de calcul et de capacité de stockage.

[0070] Le calcul des dérivées partielles de la grandeur estimée exploite certaines valeurs qui sont déjà calculées lors du fonctionnement d’un filtre de Kalman en tant qu’estimateur d’états. De ce fait, la mise en oeuvre de l’invention ne nécessite que l’ajout de quelques moyens additionnels peu coûteux en termes de temps et de puissance de calcul.

[0071] A partir des valeurs de dérivées partielles, le procédé permet d’obtenir une valeur de criticité pour caractériser la criticité, pour l’estimation de la grandeur, d’une mesure. Autrement dit, une valeur de criticité quantifie l’influence d’une ou plusieurs mesures sur l’estimation de la grandeur. Dans un premier exemple, une ou des valeurs de criticité peuvent être égales aux valeurs de dérivées partielles, étant donné que ces valeurs permettent de caractériser la criticité de mesures pour l’estimation de la grandeur. [0072] Ainsi, selon un mode de mise en oeuvre du procédé de contrôle, la au moins une valeur de criticité est égale à la au moins une valeur de dérivée partielle déterminée au cours de ladite étape de détermination.

[0073] En particulier dans le cas d’un système muni d’un seul capteur qui ne donne qu’une mesure à la fois, les dérivées partielles de la grandeur par rapport aux mesures effectuées au cours du temps par ce seul capteur permettent d’estimer la robustesse de l’estimation de la grandeur.

[0074] Cependant, suivant la manoeuvre de navigation à effectuer, il peut être avantageux de définir autrement la ou les valeurs de criticité.

[0075] Par exemple, selon un mode particulier du procédé de contrôle, la ou les valeurs de criticité sont égales aux valeurs absolues des valeurs de dérivées partielles.

[0076] En effet, selon la manoeuvre de navigation à mettre en oeuvre, l’amplitude de la criticité d’une mesure sur l’estimation de la grandeur, quantifiée par la valeur absolue de la dérivée partielle par rapport à cette mesure, est pertinente.

[0077] Dans un autre exemple, les valeurs de criticité sont égales aux valeurs absolues des valeurs de dérivées partielles normalisées, de telle sorte que la somme de ces valeurs de criticité est égale à 1 .

[0078] La normalisation des valeurs absolues des valeurs dérivées partielles par rapport à différentes mesures permet notamment de comparer la criticité d’une mesure pour l’estimation d’une grandeur, relativement aux criticités des autres mesures utilisées pour cette même estimation.

[0079] Dans un autre exemple où sont déterminées plusieurs valeurs de dérivées partielles par rapport à des mesures prises successivement au cours du temps, les valeurs de criticité sont égales à des différences de deux valeurs de dérivées partielles par rapport à deux mesures effectuées successivement. De telles valeurs de criticité permettent de détecter une variation éventuellement trop grande, à un instant donnée, de la criticité d’une mesure pour l’estimation de la grandeur, et ainsi de détecter une mauvaise robustesse de l’estimation à cet instant donné. [0080] Il peut être avantageux de caractériser la criticité d’un groupe de mesures pour l’estimation de la grandeur, par exemple des mesures prises successivement au cours d’une plage de temps donnée, ou des mesures prises par un groupe de capteurs.

[0081] A cet effet, une valeur de criticité peut être égale à une somme sur les valeurs de dérivées partielles par rapport aux mesures parmi un groupe de mesures.

[0082] Ainsi, selon un mode de réalisation particulier du procédé de contrôle :

- plusieurs valeurs de dérivées partielles sont déterminées au cours de ladite étape de détermination,

- ladite au moins une valeur de criticité est égale à une somme desdites valeurs de dérivée partielle.

[0083] Selon un autre mode de réalisation du procédé de contrôle :

- plusieurs valeurs de dérivées partielles sont déterminées au cours de ladite étape de détermination ;

- ladite au moins une valeur de criticité est égale à une somme de produits entre :

(i) une dite valeur de dérivée partielle par rapport à une dite mesure, et

(ii) ladite incertitude sur cette mesure.

[0084] Autrement dit, dans ce mode de mise en oeuvre, une valeur de criticité est une somme de valeurs de dérivées partielles par rapport à des mesures, cette somme étant pondérée par les incertitudes sur ces mesures.

[0085] Ce mode de réalisation permet avantageusement, et de façon simple, de détecter si des mesures incertaines ont une criticité élevée pour l’estimation de la grandeur. Si une telle valeur de criticité est élevée, cela signifie donc que l’estimation de la grandeur n’est pas robuste.

[0086] L’invention permet également de déterminer une valeur de criticité similairement au mode de réalisation précédent, à partir d’une seule valeur de dérivée partielle.

[0087] Ainsi, selon un mode de réalisation du procédé de contrôle, ladite au moins une valeur de criticité est égale à un produit entre :

(i) une dite valeur de dérivée partielle par rapport à une dite mesure, et

(ii) ladite incertitude sur cette mesure. [0088] Afin de permettre l’utilisation de la ou des valeurs de criticité, le procédé de contrôle indique l’envoi d’un ou plusieurs signaux de criticité représentatifs de ces valeurs de criticité. Dans le système de contrôle de navigation permettant la mise en oeuvre du procédé décrit précédemment, ce ou ces signaux de criticité sont envoyés vers un module de réception qui permet de mettre en oeuvre l’étape de réception du procédé.

[0089] Par construction, ce ou ces signaux de criticité contiennent une ou plusieurs informations sur la robustesse de l’estimation de la grandeur, cette information permettant de prendre une décision par rapport à la mise en oeuvre de la manoeuvre de navigation.

[0090] Dans un mode de réalisation de l’invention, le ou les signaux de criticité comportent des valeurs égales aux valeurs de criticité.

[0091] Dans un autre mode de réalisation, le ou les signaux de criticité peuvent être définis à partir de la comparaison des valeurs de criticité avec un ou plusieurs seuils.

[0092] Ainsi, selon un mode de mise en oeuvre particulier du procédé de contrôle, l’étape d’envoi comporte les sous-étapes :

- de comparaison entre ladite au moins une valeur de criticité et au moins un seuil, ledit au moins un seuil étant défini par rapport à ladite manoeuvre ;

- de génération dudit au moins un signal de criticité en fonction d’un résultat de ladite comparaison.

[0093] Corrélativement, l’invention propose mode de réalisation particulier du système de contrôle dans lequel ledit module d’envoi comporte les sous- modules :

- de comparaison entre ladite au moins une valeur de criticité et au moins un seuil, ledit au moins un seuil étant défini par rapport ladite manoeuvre ;

- de génération dudit au moins un signal de criticité en fonction d’un résultat de la comparaison par ledit sous-module de comparaison.

[0094] Dans ce mode de mise en oeuvre particulier, la connaissance par l’homme du métier de la manoeuvre de navigation à mettre en oeuvre, notamment du fonctionnement des capteurs effectuant les mesures, permet de définir un ou plusieurs seuils. [0095] Le ou les signaux de criticité résultant de la comparaison de la ou des valeurs de criticité avec ce ou ces seuils ont l’avantage d’être des indicateurs facilement lisibles de la robustesse de l’estimation de la grandeur.

[0096] Notamment, un tel signal de criticité peut être un nombre binaire, indiquant la bonne ou la mauvaise robustesse de l’estimation.

[0097] Le ou les signaux de criticité peuvent être traités automatiquement ou bien traités par un utilisateur auquel est présenté, par l’intermédiaire de ce module de réception, le ou les signaux de criticité.

[0098] Par suite, une ou plusieurs instructions, par exemple déterminées automatiquement ou par l’utilisateur, sont reçues par le module de réception, puis appliquées. Par exemple, la ou les instructions sont déterminées en fonction du ou des signaux de criticité.

[0099] Une instruction peut concerner un contrôle du véhicule lui-même. Notamment, si une mauvaise robustesse de l’estimation de la grandeur est détectée, l’instruction peut indiquer un arrêt temporaire de la mise en oeuvre de la manoeuvre de navigation ou l’annulation de celle-ci.

[0100] Un tel arrêt temporaire de la manoeuvre de navigation permet notamment d’assurer la sécurité de la navigation, jusqu’à ce qu’une estimation plus robuste de la grandeur soit obtenue.

[0101] Si par contre une bonne robustesse de l’estimation de la grandeur est détectée, une instruction peut indiquer le prolongement de la manoeuvre.

[0102] Une instruction peut également indiquer, dans un mode de réalisation particulier, qu’il est souhaitable de prendre en compte une ou des mesures supplémentaires parmi l’ensemble des mesures disponibles fournies par les capteurs, afin d’effectuer une estimation plus robuste de la grandeur.

[0103] L’invention propose également un système de navigation d’un véhicule, ledit système de navigation étant adapté pour réaliser les fonctions du système de contrôle selon l’un des modes de réalisation décrits ci-avant, et comprenant au moins un capteur destiné à obtenir au moins une mesure servant à l’estimation d’une grandeur, ladite grandeur servant à la mise en oeuvre d’une manoeuvre de navigation. [0104] Selon un mode de réalisation de ce système de navigation, au moins un dit capteur est une unité de mesure inertielle.

[0105] Dans un tel système, le jeu de capteurs peut par exemple comporter au moins un capteur GPS associé à une unité de mesure inertielle. Il s’agit d’une configuration de capteurs que l’on retrouve habituellement embarqué sur un véhicule, drone ou autre.

[0106] L’invention propose un programme informatique comportant des instructions pour l’exécution des étapes d’un procédé de contrôle selon l’un quelconque des modes de mise en oeuvre décrits ci-avant.

[0107] Il est à noter que les programmes d’ordinateur mentionnés dans le présent exposé peuvent utiliser n’importe quel langage de programmation, et être sous la forme de code source, code objet, ou de code intermédiaire entre code source et code objet, tel que dans une forme partiellement compilée, ou dans n’importe quelle autre forme souhaitable.

[0108] L’invention propose un support d’enregistrement, lisible par un équipement informatique d’un programme informatique comportant des instructions pour l’exécution des étapes d’un procédé selon l’un des modes de réalisation décrits ci-avant.

[0109] Les supports d’enregistrement mentionnés dans le présent exposé peuvent être n'importe quelle entité ou dispositif capable de stocker le programme et d’être lu par un appareil de commande ou par tout équipement informatique, notamment un ordinateur.

[0110] Par exemple, le support peut comporter un moyen de stockage, ou encore un moyen d'enregistrement magnétique, par exemple un disque dur.

[0111] Alternativement, les supports d’enregistrement peuvent correspondre à un circuit intégré à un ordinateur ou à un système de navigation, circuit dans lequel le programme est incorporé, et adapté pour exécuter un procédé tel que décrit ci- avant ou pour être utilisé dans l'exécution de ce procédé.

Brève description des dessins [01 12] D’autres caractéristiques et avantages de la présente invention ressortiront de la description faite ci-dessous, en référence aux dessins annexés qui en illustrent des exemples de réalisation dépourvus de tout caractère limitatif. Sur les figures :

[01 13] [Fig. 1 ] la figure 1 représente de manière schématique le fonctionnement récursif d’un filtre de Kalman ;

[01 14] [Fig. 2] la figure 2 un procédé de contrôle de navigation conforme à un premier mode particulier de mise en oeuvre de l’invention ;

[01 15] [Fig. 3] la figure 3 représente un système de contrôle de navigation conforme à un premier mode particulier de mise en oeuvre de l’invention ;

[01 16] [Fig. 4] la figure 4 un procédé de contrôle de navigation conforme à un deuxième mode particulier de mise en oeuvre de l’invention ;

[01 17] [Fig. 5] la figure 5 représente un système de contrôle de navigation conforme à un deuxième mode particulier de mise en oeuvre de l’invention ;

[01 18] [Fig. 6] la figure 6 représente un système de navigation conforme à un mode particulier de mise en oeuvre de l’invention ;

[01 19] [Fig. 7] la figure 7 représente l’architecture matérielle d’un système de contrôle de navigation selon un mode particulier de mise en oeuvre de l’invention ;

[0120] [Fig. 8] la figure 8 représente des valeurs de dérivées partielles par rapport à des mesures effectuées successivement, pour deux intervalles de temps différents entre les mesures successives.

Description des modes de réalisation

[0121 ] Nous allons maintenant décrire plusieurs modes de réalisation de l’invention. D’une façon générale, et comme mentionné précédemment, l’invention propose un procédé de contrôle de navigation d’un véhicule Veh pour la mise en oeuvre d’une manoeuvre nécessitant l’estimation d’une grandeur G.

[0122] La figure 1 représente le processus itératif d’estimation d’une grandeur G à chaque instant n par un filtre de Kalman FK. [0123] Cette grandeur, à l’instant n, est par exemple la valeur x _n | _n d’un état estimé par le filtre de Kalman FK, ou bien d’une incertitude P _n | _n sur l’estimation de cet état. D’une façon générale, cette grandeur peut correspondre à toute quantité estimée par le filtre de Kalman FK, et requise pour la mise en oeuvre d’une manoeuvre de navigation Man,.

[0124] A un instant n donné, le filtre de Kalman utilise les paramètres Par _n correspondant à un modèle numérique du véhicule dans son environnement, pour estimer des quantités dont la grandeur, à partir de quantités estimées par ce filtre de Kalman à l’instant précédent n-1 .

[0125] Notons que sur la figure 1 , l’estimation de la grandeur à l’instant n utilise également des mesures y _n (et les incertitudes R _n et Q _n sur ces mesures) effectuées à l’instant n. En particulier, l’estimation de cette grandeur prend en compte : une première estimation effectuée sans prendre en compte les mesures à l’instant n, puis une mise à jour de cette estimation avec les mesures y _n et leurs incertitudes R _n .

[0126] La première estimation est une prédiction à partir de l’instant n-1 , puisqu’elle ne nécessite pas d’observation du véhicule et/ou de son environnement à l’instant n.

[0127] Si la grandeur correspond à un état, la première estimation correspond à l’équation [Math.3] qui donne une première valeur x _n in-i estimée de l’état, et la mise à jour correspond à l’équation [Math.5] qui donne une valeur x _nin de l’état.

[0128] Dans un autre exemple, la grandeur G à l’instant n ne prend pas en compte les mesures effectuées à l’instant n, et est donc prédite à partir des mesures n-1 . Dans cet exemple, si la grandeur G correspond à un état, son estimation correspond à la première estimation de la valeur x _nin -i -

[0129] Une telle estimation est utile dans un cas où la manoeuvre de navigation Man, nécessite la prédiction de la grandeur G à l’instant n avant même d’obtenir les mesures à l’instant n.

[0130] Notons que les paramètres Par _n -i, Par _n , Par _n+i du modèle numérique peuvent être modifiés entre deux instants successifs. [0131] La figure 2 représente les principales étapes d’un procédé de contrôle de navigation destiné à améliorer la sécurité de la mise en oeuvre d’une manoeuvre de navigation Man, d’un véhicule Veh, conformément à un mode particulier de réalisation de l’invention.

[0132] Lors d’une étape ECap, des mesures y-i-y _n et leur incertitudes associées R _r R _n sont obtenues jusqu’à l’instant n via un ou plusieurs capteurs Cap ¹ -Cap ^k

[0133] Lors d’une étape d’estimation EO, une grandeur G est estimée par le filtre de Kalman à partir de ces mesures y-i-y _n et incertitudes RrR _n . L’estimation de cette grandeur est effectuée de manière itérative, comme représenté sur la figure 1 et expliqué ci-avant.

[0134] Dans un mode de mise en oeuvre particulier, cette estimation de la grandeur correspond à une première estimation de la grandeur sans prendre en compte les mesures à l’instant n suivie d’une mise à jour de cette première estimation avec les mesures à l’instant n. Dans ce mode, l’étape d’estimation correspond à l’estimation de la grandeur à l’instant n.

[0135] Dans un autre mode de mise en oeuvre, l’étape d’estimation de la grandeur comprend la première estimation de la grandeur, suivie de sa mise à jour par les mesures y _n et incertitudes R _n , puis d’une estimation de la grandeur à l’instant n+1 , à partir de l’estimation mise à jour de la grandeur à l’instant n. Dans ce mode, l’étape d’estimation correspond à la prédiction de la grandeur à l’instant n+1 .

[0136] Lors d’une étape EDP, des valeurs de dérivées partielles de la grandeur G par rapport aux mesures sont calculées.

[0137] Dans un mode de réalisation, pour calculer les valeurs de dérivées partielles par rapport aux mesures effectuées aux instants qui précédent n, la technique de rétropropagation est utilisée. Cette technique permet de calculer les dérivées partielles de façon itérative.

[0138] En particulier, elle permet de calculer une dérivée partielle de la grandeur par rapport à une mesure y _n -i à l’instant n-1 en fonction de la dérivée partielle de la grandeur par rapport à une mesure à l’instant n, puis de calculer une dérivée partielle de la grandeur par rapport à une mesure à l’instant n-2 en fonction de la dérivée partielle de la grandeur par rapport à la mesure à l’instant n-1 , et ainsi de suite.

[0139] Afin de calculer la dérivée partielle de la grandeur G par rapport à une mesure effectuée à un instant quelconque, la rétropropagation nécessite de combiner :

[0140] - d’une part les formules des dérivées partielles de la grandeur G par rapport aux mesures de l’instant n :

[0141 ] [Math.10]

[0143] - d’autre part les formules des dérivées partielles par rapport aux quantités estimées par le filtre de Kalman à l’instant précédent n :

[0144] [Math.11]

[0152] Où les quantités apparaissant dans ces équations ont été décrites précédemment.

[0153] Ainsi, à titre d’exemple, pour calculer la dérivée partielle par rapport aux mesures y _n -i effectuées à l’instant n-1 , on obtient d’une part, d’après l’équation [Math.10] :

[0154] [Math.15] [0156] Et d’autre part, d’après les équations [Math.14] et [Math.12] :

[0157] [Math.16]

[0159] Si par exemple la grandeur G est l’état x _n | _n estimé à l’instant n, alors on obtient, en combinant [Math.15] et [Math.16] :

[0160] [Math.17]

[0162] Pour calculer la dérivée partielle par rapport aux mesures y _n -2 à l’instant n-2, on utilise le même procédé, mais en remplaçant les n des équations précédentes par n-1 :

[0163] [Math.18]

[0165] Où la quantité d G/ d x _n -i |n-i a été calculée précédemment et n’a donc pas besoin d’être recalculée.

[0166] Si l’on poursuit l’exemple dans lequel G correspond à l’état x _n | _n estimé à l’instant n, on obtient finalement, en utilisant le résultat de la précédente itération :

[0167] [Math.21 ]

[0169] Le calcul de dérivées partielles de la grandeur G par rapport aux mesures y _r y _n est assez facile du point de vue de la charge de calcul et du stockage des grandeurs pendant le processus d’estimation, puisque :

- Les opérations matricielles ne sont pas nombreuses ;

- Les dimensions restent limitées aux dimensions de matrices dont les dimensions maximales sont égales aux nombre d’éléments dans les vecteurs d’état x _n |n ou aux nombre d’éléments dans les vecteurs de mesures y _n ;

- les termes calculés pour une itération n, par exemple l-K _n H _n , peuvent être stockés et réutilisés pour le calcul d’une dérivée partielle par rapport à un élément d’une itération qui précède n ;

- les besoins de stockage lors du processus d’estimation par le filtre de Kalman FK peuvent se limiter aux trois quantités ci-dessous :

(i) (1-KnHn), ou K _n et H _n , ou K _n et H _n

(ii) R _n ou Sn

(iii) z _n , Hn et F _n .

[0170] Rappelons qu’une mesure effectuée à un instant n donné y _n peut être un vecteur. Chacun des composants y _n ^k de ce vecteur étant lui-même une mesure effectuée par un capteur Cap ^k à cet instant n donné.

[0171 ] Ainsi, selon un mode de réalisation conforme à l’invention, une mesure y _n comporte plusieurs mesures y _n ¹ -y _n ^k , chacune de ces mesures étant effectuée par un capteur distinct Cap ¹ -Cap ^k .

[0172] Lors d’une étape EOV, une ou plusieurs valeurs de criticité VC sont déterminées à partir de ces valeurs de dérivées partielles D ^G y-i-D ^G yn.

[0173] Dans un mode particulier de mise en oeuvre de l’invention, les valeurs de criticité VC sont égales, pour une manoeuvre Man, donnée, aux valeurs de dérivées partielles D ^G y-|-D ^G yn déterminées lors de l’étape EDP.

[0174] Un exemple particulier de manoeuvre Man, est de faire traverser un détroit maritime à un navire, et la grandeur nécessaire à la mise en oeuvre de cette manoeuvre est une position du navire par rapport aux côtes terrestres.

[0175] Les mesures y _n comportent par exemple des mesures d’accélération par un premier capteur (une unité de mesure i nertiel le par exemple), et des mesures de position par un second capteur (un GPS par exemple), à un instant n donné.

[0176] Les valeurs de dérivées partielles D ^G y _n , et donc les valeurs de criticité VC dans cet exemple, quantifient l’influence de chacune des mesures, à chaque instant n, sur l’estimation de la position du navire.

[0177] Un tel exemple est illustré sur la figure 8. Sur cette figure sont représentées les valeurs des dérivées partielles de la grandeur G, par rapport à des mesures effectuées successivement au cours du temps, sur une durée de 100 secondes (en abscisse). Dans cet exemple, les mesures sont des mesures de vitesse et la grandeur G est l’estimation de la vitesse à l’instant 100. [0178] En haut de la figure 8, l’intervalle de temps entre deux mesures est 20 secondes : on y voit donc la valeur d’une dérivée partielle toutes les 20 secondes. En bas de la figure 8, l’intervalle de temps est de 10 secondes.

[0179] La figure 8 montre que dans ce cas particulier, plus l’intervalle de temps entre deux mesures successives est grand, plus la dernière mesure est critique pour l’estimation de la grandeur.

[0180] En effet, en haut de la figure 8, on observe que la dernière valeur de dérivée partielle est trois fois plus grande en valeur absolue que l’avant-dernière valeur de dérivée partielle, et plus de vingt fois plus grande que les valeurs précédentes de dérivées partielles. A l’inverse, en bas de la figure, où l’intervalle de temps entre deux mesures est deux fois moindre, l’écart entre les valeurs de dérivées partielles par rapport à deux mesures successives est beaucoup plus faible.

[0181 ] Ainsi, sur le haut de la figure 8, seule la dernière mesure est critique pour l’estimation de ladite grandeur. Autrement dit, dans ce cas, l’estimation de la grandeur repose essentiellement sur la dernière mesure. Selon la manoeuvre à effectuer, cela peut signifier que l’estimation de la grandeur est dans ce cas peu robuste.

[0182] Comme le montre le bas de la figure 8, si l’intervalle de temps entre deux mesures successives est réduit, plusieurs mesures influencent sensiblement l’estimation de la grandeur, qui ne repose plus uniquement que sur une seule mesure. Selon la manoeuvre à effectuer, cela peut signifier que l’estimation de la grandeur est plus robuste si l’on diminue l’intervalle de temps entre deux mesures.

[0183] Dans un autre mode particulier de mise en oeuvre de l’invention, la ou les valeurs de criticité sont égales aux valeurs absolues des valeurs de dérivées partielles.

[0184] Dans un autre mode particulier de mise en oeuvre de l’invention, les valeurs de criticité sont égales aux valeurs absolues des valeurs de dérivées partielles normalisées, de telle sorte que la somme de ces valeurs de criticité est égale à 1 .

[0185] Dans un autre mode particulier de mise en oeuvre de l’invention dans lequel sont déterminées plusieurs valeurs de dérivées partielles par rapport à des mesures prises successivement au cours du temps, les valeurs de criticité sont égales à des différences de deux valeurs de dérivées partielles par rapport à deux mesures prises successivement. De telles valeurs de criticité permettent de détecter une variation éventuellement trop grande, à un instant donnée, de la criticité d’une mesure pour l’estimation de la grandeur, et ainsi de détecter une mauvaise robustesse de l’estimation à cet instant donné.

[0186] Dans un autre mode particulier de mise en oeuvre de l’invention, les valeurs de criticité sont des sommes sur des valeurs de dérivées partielles.

[0187] Par exemple, une valeur de criticité est la somme des mesures effectuées par un capteur sur un intervalle de temps donné. Cela quantifie l’influence des mesures de ce capteur sur l’estimation de la grandeur, lors de cet intervalle de temps.

[0188] Dans un autre exemple, une valeur de criticité est la somme des mesures effectuées par l’ensemble des capteurs, à un instant donné. Une telle valeur de criticité quantifie l’influence d’un ensemble de mesures sur l’estimation de la grandeur, à cet instant donné.

[0189] Dans un autre mode particulier de mise en oeuvre de l’invention, les valeurs de criticité sont des sommes sur des valeurs de dérivées partielles par rapport à des mesures, pondérées par des incertitudes sur ces mesures afin d’homogénéiser (rendre sans unité) les dérivées partielles. De telles valeurs de criticité peuvent être calculées avec la formule :

[0190] [Math.20]

[0195] où m est un instant précédent l’instant n, ou est l’instant n lui-même, auquel cas la somme de l’équation [Math.20] ou de l’équation [Math.21] ne comprend qu’un seul terme. [0196] De telles valeurs de criticité quantifient l’influence des mesures sur l’estimation de la grandeur G, relativement aux incertitudes sur ces mesures.

[0197] Lors de l’étape d’envoi EE, un ou plusieurs signaux de criticité Sig sont obtenus à partir des valeurs de criticité déterminées lors de l’étape EDP. Ces signaux sont représentatifs des valeurs de criticité. Autrement dit, ils contiennent l’information sur l’influence des mesures que quantifient les valeurs de criticité.

[0198] Lors de cette même étape EE, ce ou ces signaux de criticité sont envoyés vers un module de réception.

[0199] Dans un mode particulier de réalisation de l’invention, les signaux de criticité sont des valeurs identiques aux valeurs de criticité.

[0200] Dans un autre mode particulier de réalisation de l’invention, représenté sur la figure 4, le ou les signaux de criticité sont obtenus par une comparaison entre les valeurs de criticité VC et des seuils Se, au cours d’une sous-étape EComp de l’étape d’envoi EE, puis généré à partir du résultat de cette comparaison au cours d’une sous-étape EGen de l’étape d’envoi.

[0201] Par exemple, si une valeur de criticité VC est supérieure à un seuil Se donné, le signal de criticité Sig indique une alerte de sécurité pour la mise en oeuvre de la manoeuvre Man, nécessitant l’estimation de la grandeur. Si à l’inverse, cette valeur de criticité VC est inférieure à ce seuil Se, le signal de criticité Sig indique que l’estimation est robuste.

[0202] La figure 5 représente un système de contrôle de navigation SCN conforme à un mode de réalisation de l’invention, ce système permettant de mettre en oeuvre le mode de réalisation du procédé de contrôle représenté sur la figure 4. Ainsi, le système SCN représenté sur la figure 5 ne diffère du système SCN représenté sur la figure 3 que par la présence des sous-modules de comparaison MComp et de génération MGen dans le module d’envoi ME, ces sous-modules permettant de mettre en oeuvre les sous-étapes de comparaison EComp et de génération EGen de signaux de criticité Sig représentées sur la figure 4.

[0203] Le signal de criticité Sig est ensuite envoyé à un module de réception MR, puis analysé par un utilisateur ayant accès au module de réception MR. Cet utilisateur envoie en retour une instruction Inst au module de réception MR. Cette instruction Inst est ensuite appliquée, lors d’une étape d’application EA, par un module d’application MA. Dans un autre mode de réalisation, l’analyse du signal de criticité et la détermination de l’instruction peuvent être traitées automatiquement.

[0204] L’instruction Inst peut correspondre à une commande de contrôle à appliquer au véhicule. Par exemple, si l’utilisateur déduit du ou des signaux de criticité Sig que l’estimation de la grandeur G n’est pas robuste, il peut envoyer l’instruction Inst d’arrêter le véhicule Veh ou de mettre en pause la manoeuvre Man, jusqu’à ce qu’une estimation robuste de la grandeur G soit obtenue.

[0205] L’instruction Inst peut également correspondre à une prise en compte d’au moins une mesure supplémentaire y-i-y _n parmi l’ensemble des mesures disponibles effectuées par au moins un capteur Cap ¹ -Cap ^k associée à une incertitude R _n ou Q _n afin d’améliorer la robustesse de l’estimation de la grandeur G de l’état du véhicule Veh.

[0206] Les modes de mise en oeuvre particuliers du procédé de contrôle de navigation représentés sur les figures 2 et figure 4 sont mis en oeuvre sur des systèmes de contrôle de navigation SCN représentés respectivement sur les figures 3 et figure 5.

[0207] Sur la figure 3, des capteurs Cap ^k prennent des mesures sur le véhicule Veh et son environnement Env. Par exemple, un GPS peut mesurer la position du véhicule Veh, une unité de mesure inertielle peut mesurer la vitesse et la position du véhicule, un radar peut mesurer la position d’objets à proximité du véhicule etc.

[0208] Ces mesures et les incertitudes qui leurs sont associées sont utilisées par un module d’estimation MO pour estimer une grandeur G avec un filtre de Kalman FK.

[0209] Un module MDP détermine ensuite les dérivées partielles de la grandeur par rapport aux mesures.

[0210] A partir des valeurs de dérivées partielles déterminées par le module MDP, un module MOV obtient une ou plusieurs valeurs de criticité telles que décrit précédemment. [0211 ] Un module ME envoie ensuite un ou plusieurs signaux de criticité Sig représentatifs de ces valeurs de criticité VC à un module de réception MR.

[0212] Ce module MR permet d’afficher le ou les signaux de criticité à un utilisateur qui décide d’une instruction telle que décrite précédemment.

[0213] Cette instruction est reçue ce module MR puis appliquée par un module d’application MA.

[0214] Si cette instruction indique une commande de contrôle de navigation, elle est appliquée au véhicule Veh. Si elle indique de prendre en compte plus de mesures, une nouvelle estimation de l’état cible est réalisée.

[0215] La figure 6 représente un système de navigation SNav intégrant un système de contrôle de navigation SCN et un jeu de capteurs Cap ¹ -Cap ^k .

[0216] Dans un mode particulier de réalisation de ce système Nav, ce jeu de capteurs Cap ¹ -Cap ^k comporte au moins une unité de mesure inertielle.

[0217] La figure 7 représente l’architecture matérielle d’un système de contrôle de navigation SCN conforme à un mode particulier de réalisation de l’invention.

[0218] Dans le mode de réalisation décrit ici, le système de contrôle de navigation SCN dispose d’une architecture matérielle d’un ordinateur. Il comprend notamment un processeur D1 , une mémoire morte D2, une mémoire vive D3, une mémoire non volatile réinscriptible D4 et des moyens de communication D5.

[0219] La mémoire morte D2 du système SCN constitue un support d'enregistrement conforme à l'invention, lisible par le processeur D1 et sur lequel est enregistré un programme d'ordinateur PGI conforme à l'invention, ce programme comportant des instructions pour l'exécution des étapes d'un procédé de contrôle selon l'invention décrit précédemment en référence à la figure 2 ou figure 4 dans deux modes de réalisation.

[0220] Le programme d’ordinateur PGI définit des modules fonctionnels des systèmes de contrôle SCN représentés sur la figure 3 ou figure 5.