Navigation assistance method and equipment based on Kalman filter
By calculating the measured partial derivative value and critical signal processing, the problem that Kalman filter is difficult to evaluate measurement errors and uncertainties in the navigation system is solved, which improves the robustness and estimation quality of navigation control, and ensures the safety of the navigation system. sex.
Patent Information
- Application Number
- CN202380032203.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Priority Date
- 2022-03-04
- Filing Date
- 2023-02-27
- Publication Date
- 2024-11-19
AI Technical Summary
The existing Kalman filters are difficult to effectively detect measurement errors and uncertainties in navigation systems, making it difficult to evaluate the reliability and robustness of the estimates, and cannot accurately detect sensors that lead to poor estimation robustness.
By calculating the partial derivative value of the measurement, a critical value is obtained to evaluate the effect of the measurement on the magnitude estimate, and a critical signal is sent to implement monitoring control of navigation control, improving the robustness of the estimate.
Through the calculation of partial derivative values and critical signal processing, the criticality and uncertainty of measurement can be quantified, the robustness of navigation control can be improved, navigation safety can be ensured, and estimated quality can be optimized.
Smart Images

Figure CN118984925A_ABST
Abstract
Description
Background Art
[0001] The present invention relates to the general field of using sensors to make measurements, such as the position and velocity of a vehicle.
[0002] More specifically, the invention relates to monitoring the navigation of a vehicle using a Kalman filter. The Kalman filter is used to estimate the magnitudes required to perform navigation maneuvers of the vehicle. The magnitudes represent, for example, characteristics of the vehicle itself (e.g. position, speed, orientation, etc.), or they may also represent characteristics related to the environment (presence of objects near the vehicle, wind speed, etc.). These characteristics (whether internal or external to the vehicle) will subsequently be referred to as "states".
[0003] Note that the Kalman filter also estimates the uncertainty of the state estimate in the form of a covariance matrix. These estimated uncertainties can also be used to implement the control. Therefore, the term "magnitude" can also refer to this uncertainty estimated by the Kalman filter.
[0004] As we all know, Kalman filters are often used as state estimators in navigation systems. The Kalman filter recursively evaluates the state x at each time instant n. n , based on a numerical model (in particular, a model that describes how different states evolve depending on each other and how measurements depend on the states) and the information collected by sensors (hereafter referred to as "measurements").
[0005] For example, in a navigation system that generates estimates of the position, orientation, and velocity of a target object, the model specifically includes equations of motion that govern the trajectory of the vehicle as a function of the vehicle's position, orientation, and velocity. The model may involve some parameters whose values are known a priori (e.g., the acceleration due to gravity) and / or whose values may vary depending on the time step n.
[0006] In the rest of the application, in order to refer to a time instant, the given expressions "instant", "step" and "moment" will be used interchangeably.
[0007] The time interval between two of these consecutive moments is typically a user-adjustable parameter.
[0008] In the field of navigation systems, sensors are often associated with inertial measurement units.
[0009] For example, in navigation, an inertial measurement unit coupled to a GPS receiver is often used to estimate the position and orientation of a vehicle. An inertial measurement unit (including a gyroscope and an accelerometer) evaluates the orientation changes and the continuous forces to which the mobile carrier is subjected, respectively. Through simple mathematical integration operations, these measurements make it possible to estimate the state of the vehicle in terms of position, velocity and orientation. The information conveyed by the GPS makes it possible to calculate the position of the vehicle. A Kalman filter combines this information to estimate the state of the vehicle. The quality of the combination is achieved, among other things, by a good calibration of the filter parameters. Models and data merging methods are known and will therefore not be described in detail.
[0010] The magnitude(s) estimated by the Kalman filter and the use of these estimates vary depending on the application. In particular:
[0011] - the estimate is related, for example, to the state of the vehicle and can be used to update the road map displayed to the driver of the vehicle and / or to decide which directions to give him to reach the destination of his journey;
[0012] - In systems equipped with radar sensors, estimates can be related to the state of objects in the vicinity of the vehicle, and this estimate can be used as an indication of the location of these objects, thereby adjusting the trajectory of the vehicle;
[0013] -In an autonomous car, the estimate is, for example, relevant to the vehicle and can provide information to the control module driving the vehicle, allowing the vehicle to decide on the next maneuver, etc.
[0014] The Kalman filter recursively determines an estimate x of the current state of the target object by using previous estimates and measurements provided by sensors before time n. n|n The recursive calculation is Figure 1 is schematically illustrated in FIG.
[0015] Even with an appropriate mathematical model, the quality of the measurements provided by the sensor(s) can have a significant impact on the quality of the Kalman filter's estimates.
[0016] To understand the importance of measurement quality, it is first useful to review the classical operation of a Kalman filter.
[0017] The state estimated by the Kalman filter at a given step n will be denoted by x n In many applications, the state is expressed in multiple variables, and thus x n Make up the vector.
[0018] The system evolves according to the digital model, and therefore it can be considered that the state x of the target object at a given time n is according to the following equation [Formula 1] n Depends on the state x at the previous time n-1n-1 :
[0019] [Formula 1]
[0020] x n =F n x n-1 +B n u n +w n
[0021] where F n is a known square matrix, U n is the control input vector (known to the user), that is, the action applied to the system, B n It means that the state x n The matrix of the model associated with this control input, and w n is a centered Gaussian random variable represented by the matrix Q n Represents the covariance noise.
[0022] In equation [Formula 1] and the following equation, the parallel representation of the two quantities corresponds to:
[0023] - If the two quantities are numbers, then it is a simple product,
[0024] - If one of the quantities is a matrix and the other is a vector, then the product of the matrix and the vector (e.g., F in equation [Formula 1] n x n-1 ),or
[0025] - If the two quantities are matrices, then it is the matrix product (e.g., F in the following equation [Formula 4] n P n-1|n-1 ).
[0026] The measurement(s) (in particular the measurement provided by the sensor(s) at time n) will be denoted y n , and depends on the state x according to the following equation [Formula 2] n :
[0027] [Formula 2]
[0028] y n =H n x n +v n
[0029] Among them, H n is a known observation matrix, and v n is the covariance noise R nis a centered Gaussian random variable. Similarly, in many applications, multiple measurements are delivered to the Kalman filter at each time instant n, and in this case, y n and v n will be a vector, and R n is a matrix.
[0030] During the actual implementation of the Kalman filter, it is necessary to choose the noise covariance matrix Q n and R n This is usually done by studying beforehand the relationship between these covariances and data obtained in the relevant field. A typical method for estimating covariances consists in applying the ALS (Autocovariance Least Squares) algorithm.
[0031] Noise covariance matrix Q n and R n corresponds to the uncertainty about the measurement made by the sensor. Therefore, in this application, for the sake of brevity, these quantities will be referred to in terms: about the measurement y n uncertainty.
[0032] The Kalman filter maintains a state x at a given time n n and when this estimate is based on the measurement y available at time n-1 n-1 The calculated (predicted) value is recorded as x n|n-1 ; and when calculating the measurement y obtained at time n n When integrating, it is recorded as x n|n The Kalman filter also estimates the state x n The estimated uncertainty covariance matrix Pn, and when this matrix is applied to the estimate x n|n-1 When n|n-1 ; and when this matrix is suitable for estimating x n|n When n|n .
[0033] At each step n, the filter operates twice.
[0034] First, according to the following equations [Formula 3] and [Formula 4], the Kalman filter is based on the state x n-1|n-1 and the covariance matrix P n-1|n-1 The estimate of the prior state x of the target object is used to predict n|n-1 And the covariance matrix P n|n-1 Estimates:
[0035] [Formula 3]
[0036] x n|n-1 =F n x n-1|n-1 +un
[0037] [Formula 4]
[0038] P n|n-1 =F n P n-1|n-1 F n T +Q n
[0039] where F n T is the matrix F n The transpose of .
[0040] Second, the Kalman filter uses the prediction made in the previous step and takes into account the current measurement y n , with a coefficient K called the “Kalman gain” n The prior estimate of the target object state is updated by a certain weight represented by . The update is performed according to the following equations [Formula 5] and [Formula 6]:
[0041] [Formula 5]
[0042] x n|n =x n|n-1 +K n (y n -H n x n|n-1 )=x n|n-1 +K n z n
[0043] [Formula 6]
[0044] P n|n =(IK n H n ) n|n-1
[0045] Where K n Calculated according to the following equation [Formula 7]:
[0046] [Formula 7]
[0047]
[0048] Where Sn represents the measured y n The covariance of is calculated according to the following equation [Formula 8]:
[0049] [Formula 8]
[0050] S n =H n P n|n-1 H nT +R n
[0051] S n -1 YesS n The inverse matrix of z n It is defined according to the following equation [Formula 9]:
[0052] [Formula 9]
[0053] z n =y n -H n x n|n-1
[0054] The Kalman filter outputs the current estimate of the current state x n|n and the uncertainty associated with this estimate P n|n , these magnitudes are delivered to control systems etc. depending on the application.
[0055] As long as the Kalman filter assumes that the measurements from the GPS are reliable, measurement errors, even small ones, will cause the estimate to deviate more or less from the actual situation.
[0056] Therefore, data on the value of the uncertainty associated with the estimate are not sufficient to assess the reliability of the Kalman filter's measurements and estimates, nor to detect which measurement or which sensor is causing the estimate to be less robust.
[0057] The present invention aims to overcome these disadvantages. Summary of the invention
[0058] Therefore, according to a first aspect, the present invention provides a method for monitoring vehicle navigation to perform navigation control, the method comprising the following steps:
[0059] - estimating, by a Kalman filter, a magnitude from parameters, a digital model of the vehicle in its environment and at least one measurement made by at least one sensor and associated with an uncertainty about this measurement, said magnitude being used to implement said navigation maneuver;
[0060] - determining at least one partial derivative value of said magnitude with respect to said at least one measurement;
[0061] - obtaining from said at least one partial derivative value at least one critical value for said at least one measure of said estimation;
[0062] - sending at least one threshold signal representing said at least one threshold value;
[0063] - receiving at least one instruction in response to said sending step; and
[0064] - applying the at least one instruction;
[0065] The instructions instruct supervisory control for implementing the navigation maneuver.
[0066] Accordingly, the present invention proposes a system for monitoring vehicle navigation to implement navigation control, the system comprising:
[0067] - means for estimating, by a Kalman filter, a magnitude from parameters, a digital model of the vehicle in its environment and at least one measurement made by at least one sensor and associated with an uncertainty about this measurement, said magnitude being used to implement said navigation maneuver;
[0068] - means for determining at least one partial derivative value of said magnitude with respect to said at least one measurement;
[0069] - means for obtaining, from said at least one partial derivative value, at least one critical value for said at least one measurement of said estimation;
[0070] - means for transmitting at least one threshold signal representative of said at least one threshold value;
[0071] - means for receiving at least one instruction in response to said sending step; and
[0072] - a module for applying said instructions;
[0073] The instructions instruct supervisory control for implementing the navigation maneuver.
[0074] According to one mode of realization of the monitoring method, the instructions instruct to add the measurements for a subsequent estimation of the magnitude.
[0075] It is specified here that the term partial derivative used here encompasses the concept of gradient.In practice, a measurement or uncertainty may contain several quantities (velocity, position, etc.) and may therefore be represented in the form of a vector.
[0076] In this case, for the sake of brevity, the term partial derivative of magnitude with respect to magnitude will also be used. In other words, in this application, if the magnitude is in the form of a vector, the partial derivative of magnitude with respect to the magnitude indicates the gradient of the magnitude according to the vector representing the magnitude.
[0077] Likewise, for simplicity, the value of the partial derivative of the magnitude with respect to a measurement will refer to the set of values comprising the gradient of the magnitude with respect to the vector representing that measurement.
[0078] It is also stipulated that in the present application, the value of the partial derivative of the magnitude with respect to a first matrix (for example, a matrix representing uncertainty about measurement) corresponds to a second matrix, the components of which are equal to the value of the partial derivative of the magnitude with respect to each component of the first matrix.
[0079] In this application, if we call the sum between two vectors (eg, partial derivatives in the form of two vectors), then the sum corresponds to a vector that is equal to the element-by-element sum of the two vectors.
[0080] The evaluation of these partial derivatives advantageously makes it possible to provide important information on the robustness of the estimate of the magnitude required to implement the navigation maneuver, in particular by making it possible to quantify the criticality of the different measures in this estimate.
[0081] The information obtained about the robustness of the magnitude estimation indicates whether it is relevant to measure the camera among all available measurements to estimate the magnitude, or whether it is relevant to apply a monitoring control, which corresponds, for example, to cancelling the envisaged maneuver.
[0082] Indeed, on the one hand, one or more partial derivatives of the magnitude with respect to a measurement made at a given moment or with respect to a measurement made during a given time range make it possible to assess the influence of the measurement on the magnitude estimate made over time.
[0083] For example, if the partial derivative with respect to the measurement takes a large value for measurements made at times prior to the estimate of the magnitude, but takes a value close to zero for measurements made at earlier times, this means that only measurements made at times prior to the estimate are significantly considered in the estimate.
[0084] In cases where a robust estimation requires consideration of several consecutive measurements, such a scenario may indicate a lack of robustness in the estimation.
[0085] In this example, the estimate can be improved by increasing the frequency of measurements taken over time.
[0086] Therefore, according to one implementation mode of the monitoring method:
[0087] - obtain continuous measurements, and
[0088] - During said determination step, for each of these measurements, a partial derivative value related to the magnitude of this measurement is determined.
[0089] In this embodiment, the partial derivatives are calculated a posteriori. In other words, the magnitude is obtained after a measurement is obtained with respect to which the partial derivative values of the magnitude are calculated.
[0090] For example, the estimate x at time n is n|nThe partial derivatives of are calculated with respect to a series of measurements considered between the times 0 and n; thus, in obtaining an estimate x n|n These partial derivatives are then calculated.
[0091] On the other hand, one or more partial derivatives with respect to the measurement made by a given sensor or with respect to the magnitude of the measurements made by a given set of sensors make it possible to assess the influence of the sensor or sensor group on the estimate.
[0092] For example, if the partial derivative with respect to a measurement takes a low value, and knowledge of a person skilled in the art indicates that the estimated magnitude is typically strongly influenced by that measurement (e.g., the estimate of a vehicle's velocity is strongly influenced by the measurement of its acceleration), then this indicates that the estimate is not robust.
[0093] According to one mode of realization of the monitoring method, for at least one given moment, said partial derivative value is determined during said determination step with respect to a measurement comprising at least two measurements carried out at this given moment by separate sensors.
[0094] Recall that, as explained above, a measurement may correspond to a vector whose components are measurements of different states. For example, the measurements at a given moment may include, for example, a measurement of the vehicle acceleration at that given moment and a measurement of the vehicle velocity at that given moment.
[0095] It should also be noted that in this implementation mode, the partial derivative of the magnitude with respect to the measurement is a vector whose components are equal to the partial derivative of the magnitude with respect to each measurement of the different states. For example, the partial derivative of the magnitude with respect to the acceleration and velocity measurements includes the partial derivative of the magnitude with respect to the velocity measurement and the partial derivative of the magnitude with respect to the acceleration measurement.
[0096] Advantageously, the computation of the partial derivatives of the magnitude estimated by the Kalman filter is cheap in terms of computing power and storage capacity.
[0097] The calculation of the estimated partial derivatives of the magnitude exploits some values already calculated during the operation of the Kalman filter as a state estimator. As a result, the implementation of the invention requires only the addition of some additional means which are cheap in terms of time and computing power.
[0098] From the partial derivative values, the method makes it possible to obtain critical values for characterizing the criticality of the measurements for use in the estimation of the magnitude. In other words, the critical values quantify the influence of one or more measurements on the estimation of the magnitude. In a first example, one or more critical values may be equal to the partial derivative values, since these values make it possible to characterize the criticality of the measurements for use in the estimation of the magnitude.
[0099] Thus, according to one mode of realization of the monitoring method, at least one critical value is equal to at least one partial derivative value determined during said determination step.
[0100] In particular in the case of systems equipped with a single sensor which gives only one measurement at a time, the partial derivative of the magnitude with respect to the measurement made by this single sensor over time makes it possible to estimate the robustness of the magnitude estimate.
[0101] However, depending on the navigation maneuver to be performed, it may be advantageous to define the threshold value(s) in a different manner.
[0102] For example, according to one particular mode of the monitoring method, the critical value(s) are equal to the absolute value of the partial derivative value.
[0103] In fact, depending on the navigation maneuver to be implemented, the magnitude of the criticality of a measurement is relevant to the magnitude estimate, which is quantified by the absolute value of the partial derivative with respect to this measurement.
[0104] In another example, the critical values are equal to the absolute values of the normalized partial derivative values such that the sum of the critical values is equal to 1.
[0105] The normalization of the absolute values of the partial derivative values with respect to the different measures makes it possible to compare the criticality of a measure used for magnitude estimation in particular with respect to the criticality of other measures used for the same estimation.
[0106] In another example, in which several partial derivative values are determined with respect to measurements performed successively over time, a critical value is equal to the difference between two partial derivative values with respect to two measurements performed successively. Such a critical value makes it possible to detect a possibly excessive change in the criticality of the measurement used for the magnitude estimation at a given moment, and therefore to detect a poor robustness of the estimation at this given moment.
[0107] It may be advantageous to characterize the criticality of a set of measurements for magnitude estimation, such as measurements taken continuously during a given time frame, or measurements taken by a set of sensors.
[0108] For this purpose, the critical value may be equal to the sum of the values of the partial derivatives with respect to the measurement among a set of measurements.
[0109] Therefore, according to a specific embodiment of the monitoring method:
[0110] - determining a number of partial derivative values during said determination step,
[0111] - said at least one critical value is equal to the sum of said partial derivative values.
[0112] According to another embodiment of the monitoring method:
[0113] - determining a number of partial derivative values during said determining step;
[0114] - the at least one critical value is equal to the sum of the products of:
[0115] (i) the value of the partial derivative with respect to the measurement, and
[0116] (ii) the uncertainty about the measurement.
[0117] In other words, in this implementation mode, the critical value is the sum of the partial derivative values with respect to the measurements, weighted by the uncertainty about these measurements.
[0118] This embodiment advantageously enables to detect in a simple manner whether an uncertain measurement has a high criticality for the estimate of the magnitude. If such a critical value is high, it means that the estimate of the magnitude is not robust.
[0119] The present invention also allows the critical value to be determined based on a single partial derivative value, similar to the previous embodiment.
[0120] Therefore, according to one embodiment of the monitoring method, the at least one critical value is equal to the product of:
[0121] (i) the value of the partial derivative with respect to the measurement, and
[0122] (ii) the uncertainty about the measurement.
[0123] In order to allow the use of (one or more) critical values, the monitoring method indicates sending one or more critical signals representing these critical values. In a navigation monitoring system that allows the implementation of the above method, this / these critical signals are sent to a receiving module, thereby making it possible to implement the receiving step of the method.
[0124] By design, the critical signal(s) contain one or more information about the robustness of the estimate of the magnitude, which information enables a decision to be made regarding the implementation of the navigation maneuver.
[0125] In one embodiment of the invention, the threshold signal(s) comprise a value equal to a threshold value.
[0126] In another embodiment, the critical signal(s) may be determined by comparing the critical value to one or more threshold values.
[0127] Therefore, according to a specific implementation mode of the monitoring method, the sending step includes the following sub-steps:
[0128] - a comparison between the at least one critical value and at least one threshold value, the at least one threshold value being defined in relation to the manipulation;
[0129] - generating said at least one threshold signal as a function of a result of said comparison.
[0130] Relatedly, the present invention proposes a specific embodiment of the monitoring system, wherein the sending module includes submodules for the following operations:
[0131] - a comparison between the at least one critical value and at least one threshold value, the at least one threshold value being defined in relation to the manipulation;
[0132] - generating the at least one critical signal according to the result of the comparison of the comparison submodule.
[0133] In this particular mode of realization, a person skilled in the art exploits the knowledge of the navigation maneuver to be implemented, and in particular of the operation of the sensors making the measurements, making it possible to define one or more threshold values.
[0134] The critical signal(s) generated by comparing the critical value(s) to the threshold(s) has the advantage of being an easily readable indicator of the robustness of the magnitude estimate.
[0135] In particular, such a critical signal may be a binary number indicating how robust the estimate is.
[0136] The critical signal(s) may be processed automatically or by a user to whom the critical signal(s) are presented via the receiving module.
[0137] Subsequently, the receiving module receives one or more instructions, for example, determined automatically or by a user, and then applies these instructions. For example, the instruction(s) are determined based on the critical signal.
[0138] The instructions may involve monitoring of the vehicle itself. In particular, if poor robustness of the magnitude estimate is detected, the instructions may indicate temporarily stopping the implementation of the navigation maneuver or canceling the navigation maneuver.
[0139] This temporary suspension of the navigation maneuver makes it possible in particular to ensure navigation safety until a more robust magnitude estimate is obtained.
[0140] On the other hand, if good robustness of the magnitude estimate is detected, the instructions may indicate a prolongation of the maneuver.
[0141] In one particular embodiment, the instructions may also indicate that, in order to perform a more robust magnitude estimation, it is desirable to consider one or more additional measurements among all available measurements provided by the sensor.
[0142] The invention also proposes a navigation system for a vehicle, which is suitable for performing the functions of a monitoring system according to one of the above-mentioned embodiments and comprises at least one sensor, which is intended to obtain at least one measurement for estimating a magnitude used to implement navigation maneuvers.
[0143] According to an embodiment of the navigation system, at least one of said sensors is an inertial measurement unit.
[0144] In such a system, the sensor set may include, for example, at least one GPS sensor associated with an inertial measurement unit. This is the configuration of sensors typically mounted on vehicles, drones, and the like.
[0145] The present invention proposes a computer program comprising instructions for executing the steps of the monitoring method according to any one of the above implementation modes.
[0146] It should be noted that the computer programs mentioned in the present disclosure may use any programming language, and may be in the form of source code, object code, or intermediate code between source code and object code, such as partially compiled form, or any other desired form.
[0147] The present invention proposes a computer program recording medium readable by a computer device, wherein the computer program includes instructions for executing the steps of the method according to any one of the above embodiments.
[0148] The recording medium mentioned in the present disclosure may be any entity or device that can store the program and can be read by a control device or any computer device (especially a computer).
[0149] For example, the medium may include a storage component or a magnetic recording component, such as a hard disk.
[0150] Alternatively, the recording medium may correspond to a circuit integrated into a computer or a navigation system, the circuit having the program incorporated therein, and the program being adapted to execute the method as described above or to be used in executing the method. BRIEF DESCRIPTION OF THE DRAWINGS
[0151] Other features and advantages of the invention will emerge from the following description, with reference to the accompanying drawings, which illustrate exemplary embodiments without any limitation. In the drawings:
[0152] [ Figure 1 ] Figure 1 Schematically represents the recursive operation of the Kalman filter;
[0153] [ Figure 2 ] Figure 2 Represents a navigation monitoring method according to a first specific mode of realisation of the invention;
[0154] [ Figure 3 ] Figure 3 Represents a navigation monitoring system according to a first particular mode of realisation of the invention;
[0155] [ Figure 4 ] Figure 4 Represents a navigation monitoring method according to a second specific implementation mode of the present invention;
[0156] [ Figure 5 ] Figure 5 Represents a navigation monitoring system according to a second particular mode of realisation of the invention;
[0157] [ Figure 6 ] Figure 6 represents a navigation system according to a specific implementation mode of the present invention;
[0158] [ Figure 7 ] Figure 7 It represents the hardware structure of a navigation monitoring system according to a specific implementation mode of the present invention;
[0159] [ Figure 8 ] Figure 8 Represents the value of the partial derivative with respect to consecutive measurements made at two different time intervals between the consecutive measurements. DETAILED DESCRIPTION
[0160] Several embodiments of the invention will now be described.Generally, as previously mentioned, the invention proposes a method for monitoring navigation of a vehicle Veh for implementing maneuvers requiring an estimated magnitude G.
[0161] Figure 1 represents the iterative process for estimating the magnitude G at each time instant n by the Kalman filter FK.
[0162] At time n, this magnitude is, for example, the value x of the state estimated by the Kalman filter FK n|n , or the uncertainty P about the estimate of the state n|n In general, this magnitude can correspond to any magnitude estimated by the Kalman filter FK and is a function of the navigation control Man j The value required for the realization of .
[0163] At a given time n, the Kalman filter uses the parameters Par corresponding to the digital model of the vehicle in its environment n , to estimate the magnitude, including the magnitude, based on the magnitude estimated by the Kalman filter at the previous time n-1.
[0164] Note that Figure 1 The estimate of the magnitude at time n also uses the measurement y made at time n.n (and the uncertainty R about these measurements n and Q n ). Specifically, the estimation of this magnitude considers: a first estimation without taking into account the measurement at time n, and then using the measurement y n and its uncertainty R n Update the estimate.
[0165] The first estimate is a prediction from time n-1, since it does not require observation of the vehicle and / or its environment at time n.
[0166] If the magnitude corresponds to the state, then the first estimate corresponds to the equation [Formula 3], which gives the first estimate of the state x nln-1 , and the update corresponds to the equation [Formula 5], which gives the value of the state x nln .
[0167] In another example, the magnitude G at time n does not take into account the measurement made at time n and is therefore predicted from measurement n-1. In this example, if the magnitude G corresponds to the state, then its estimate corresponds to the value x nln-1 First estimate.
[0168] This estimate is in the navigation control Man j This is useful in cases where the magnitude G at time n needs to be predicted even before a measurement at time n is obtained.
[0169] Note that the parameters Par of the digital model n-1 、Par n 、Par n+1 Can be modified between two consecutive moments.
[0170] Figure 2 A navigation control method for improving the vehicle Veh according to a specific embodiment of the present invention is shown. j The main steps of the safety navigation monitoring method.
[0171] During step ECap, via one or more sensors Cap 1 -Cap k Get the measurement y1-y up to time n n and its associated uncertainty R1-R n .
[0172] During the estimation step E0, the Kalman filter is used to estimate these measurements y1-y n and uncertainty R1-R n To estimate the magnitude G. The magnitude estimation is performed iteratively, such as Figure 1As indicated in and explained above.
[0173] In a particular mode of realization, this estimation of the magnitude corresponds to a first estimation of the magnitude without taking into account the measurement at instant n, which first estimation is subsequently updated with the measurement at instant n. In this mode, the estimation step corresponds to an estimation of the magnitude at magnitude n.
[0174] In another implementation mode, the step of estimating the magnitude comprises first estimating the magnitude and then calculating the magnitude based on the measured y n and uncertainty R n Update its value, and then estimate the magnitude at time n + 1 based on the updated estimate of the magnitude at time n. In this mode, the estimation step corresponds to a prediction of the magnitude at time n + 1.
[0175] During step EDP, the value of the partial derivative of the magnitude G with respect to the measurement is calculated.
[0176] In one embodiment, to calculate the partial derivative values with respect to the measurements made at time n before, a back-propagation technique is used. This technique allows the partial derivatives to be calculated iteratively.
[0177] Specifically, it allows the calculation of the magnitude of the measurement y at time n-1 as a function of the partial derivative of the magnitude with respect to the measurement at time n n-1 The partial derivative of is then calculated as a function of the partial derivative of the magnitude with respect to the measurement at time n-2, and so on.
[0178] To compute the partial derivatives of the magnitude G with respect to measurements made at any time, backpropagation requires combining:
[0179] - On the one hand, the partial derivative equation of the magnitude G with respect to the measurement at time n is:
[0180] [Formula 10]
[0181]
[0182] - On the other hand, the equation related to the partial derivative of the magnitude estimated by the Kalman filter at the previous time n:
[0183] [Formula 11]
[0184]
[0185] [Formula 12]
[0186]
[0187] [Formula 13]
[0188]
[0189] [Formula 14]
[0190]
[0191] The quantities appearing in these equations have been described previously.
[0192] Thus, for example, to calculate the value of the measurement y at time n-1 n-1 The partial derivative of, on the one hand, according to equation [Formula 10], we get:
[0193] [Formula 15]
[0194]
[0195] On the other hand, according to equations [Formula 14] and [Formula 12], we obtain:
[0196] [Formula 16]
[0197]
[0198] For example, if the magnitude G is the estimated state x at time n n|n , then we combine [Formula 15] and [Formula 16] to obtain:
[0199] [Formula 17]
[0200]
[0201] To calculate the value of the measurement y at time n-2 n The partial derivative with respect to -2 uses the same method, but replaces n in the previous equation with n-1:
[0202] [Formula 18]
[0203]
[0204] Among them, the value has already been calculated before and therefore does not need to be recalculated.
[0205] If we continue G corresponds to the estimated state x at time n n|n , we finally get by using the result of the previous iteration:
[0206] [Formula 21]
[0207]
[0208] From the perspective of computational load and storage of magnitudes during the estimation process, the computational magnitude G is related to the measurement y1-y n The partial derivatives of are fairly easy to find, since:
[0209] -Not many matrix operations;
[0210] - The dimensions are still restricted to the dimensions of the matrix, and the maximum dimension of the matrix is equal to the state vector x n|n The number of elements in or measurement vector y n The number of elements in ;
[0211] - Items calculated for iteration n (e.g. IK n H n ) can be stored and reused to compute partial derivatives of elements with respect to iterations before n;
[0212] The storage requirements during the estimation process using the Kalman filter FK can be limited to the following three quantities:
[0213] (i)(IK n H n ), or K n and H n , or K n and H n
[0214] (ii) R n or Sn
[0215] (iii) z n , H n and F n .
[0216] Recall that a measurement y made at a given time n is n Can be a vector. Each component y of this vector n k The sensor itself is Cap k The measurement is made at this given time n.
[0217] Therefore, according to one embodiment of the present invention, measuring y n Including several measurements n 1 -y n k Each of these measurements is performed by a separate sensor Cap 1 -Cap k conduct.
[0218] During step EOV, according to these partial derivative values D G y1-D G yn One or more critical values VC are determined.
[0219] In a specific implementation mode of the present invention, for a given control Man j , the critical value VC is equal to the partial derivative value D determined during step EDP G y1-D G y n .
[0220] Manipulation j A specific example of this is getting a ship through a strait, and the magnitude required to perform this maneuver is the position of the ship relative to the land coast.
[0221] Measure y n This includes, for example, acceleration measurements made by a first sensor (eg, an inertial measurement unit) and position measurements made by a second sensor (eg, a GPS) at a given time n.
[0222] Partial derivative value D G y n , and therefore the critical value VC in this example, quantifies the impact of each measurement at each time instant n on the estimate of the vessel's position.
[0223] Figure 8 Such an example is illustrated in . The figure represents the partial derivative value of the magnitude G with respect to a measurement made continuously over time (on the abscissa) over a period of 100 seconds. In this example, the measurement is a speed measurement, and the magnitude G is an estimate of the speed at time 100.
[0224] exist Figure 8 In the upper part, the time interval between the two measurements is 20 seconds: therefore, we see the value of the partial derivative every 20 seconds. Figure 8 In the lower part, the time interval is 10 seconds.
[0225] Figure 8 It is shown that, in this particular case, the larger the time interval between two consecutive measurements, the more critical the last measurement is for the magnitude estimation.
[0226] In fact, Figure 8 In the upper part of the figure, it is observed that the absolute value of the last partial derivative value is three times larger than the second-to-last partial derivative value and more than twenty times larger than the previous partial derivative value. In contrast, in the lower part of the figure, the time interval between two measurements is half that of the previous ones, which is associated with a much smaller gap between the partial derivative values of two consecutive measurements.
[0227] Therefore, in Figure 8In the upper part of , only the last measurement is crucial for the estimation of the magnitude. In other words, in this case, the estimation of the magnitude is essentially based on the last measurement. Depending on the maneuver to be performed, this may mean that the estimation of the magnitude in this case is not very robust.
[0228] like Figure 8 As shown in the lower part of , if the time interval between two consecutive measurements is reduced, several measurements can significantly affect the estimate of the magnitude, which no longer relies solely on a single measurement. Depending on the manipulation to be performed, this may mean that the estimate of the magnitude is more robust if the time interval between two measurements is reduced.
[0229] In another particular implementation mode of the invention, the critical value(s) are equal to the absolute value of the partial derivative value.
[0230] In another particular implementation mode of the invention, the critical values are equal to the absolute values of the normalized partial derivative values, so that the sum of these critical values is equal to 1.
[0231] In another particular mode of realisation of the invention, in which several partial derivative values are determined in relation to measurements carried out successively over time, a critical value is equal to the difference between two partial derivative values in relation to two measurements carried out successively. Such a critical value makes it possible to detect a possibly excessive variation in the criticality of the measurement used for the magnitude estimation at a given moment and therefore to detect a poor robustness of the estimation at this given moment.
[0232] In another particular implementation mode of the invention, the critical value is the sum of the partial derivative values.
[0233] For example, the threshold is the sum of the measurements made by a sensor in a given time interval. This quantifies the impact of the measurements of that sensor on the estimate of the magnitude during that time interval.
[0234] In another example, the threshold is the sum of the measurements made by all sensors at a given moment in time. Such a threshold quantifies the impact of a set of measurements on the estimate of the magnitude at a given moment in time.
[0235] In another specific implementation mode of the invention, the critical value is the sum of the partial derivative values with respect to the measurements, weighted by the uncertainty about these measurements to make the partial derivatives homogenous (unitless). Such a critical value can be calculated using the following equation:
[0236] [Formula 20]
[0237]
[0238] Or the formula:
[0239] [Formula 21]
[0240]
[0241] Wherein m is the time before time n, or is time n itself, in which case the sum of equation [Formula 20] or equation [Formula 21] contains only a single term.
[0242] Such a critical value quantifies the impact of the measurements on the estimate of the magnitude G relative to the uncertainty about these measurements.
[0243] During the sending step EE, one or more threshold signals Sig are obtained from the threshold values determined during step EDP. These signals represent the threshold values. In other words, they contain information about the influence of the measurement quantified by the threshold values.
[0244] During this same step EE, the critical signal(s) are sent to the receiving module.
[0245] In a specific embodiment of the present invention, the threshold signal is the same value as the threshold value.
[0246] In another specific embodiment of the present invention, Figure 4 As indicated in , (one or more) critical signals are obtained during the sub-step EComp of the sending step EE by comparing the critical value VC with the threshold value Se, and then generated during the sub-step EGen of the sending step according to the result of the comparison.
[0247] For example, if the critical value VC is greater than a given threshold Se, the critical signal Sig indicates that the control Man for the required magnitude estimation is j On the contrary, if the critical value VC is lower than the threshold value Se, the critical signal Sig indicates that the estimation is robust.
[0248] Figure 5 A navigation monitoring system SCN according to an embodiment of the present invention is shown, which makes it possible to implement Figure 4 An embodiment of the monitoring method represented in . Therefore, Figure 5 The system SCN represented in Figure 3 The system SCN represented in FIG. 1 differs only in the presence of the comparison MComp and generation MGen submodules in the sending module ME, which make it possible to implement Figure 4 The sub-steps of comparing EComp and generating EGen of the critical signal Sig are represented.
[0249] The critical signal Sig is then sent to the receiving module MR and then analyzed by a user who has access to the receiving module MR. The user in turn sends an instruction Inst to the receiving module MR. The instruction Inst is then applied by the application module MA during an application step EA. In another embodiment, the analysis of the critical signal and the determination of the instruction can be processed automatically.
[0250] The instruction Inst may correspond to a monitoring control to be applied to the vehicle. For example, if the user infers from the critical signal(s) Sig that the estimate of the magnitude G is not robust, he may send an instruction Inst to stop the vehicle Veh or to suspend the maneuver Man j , until a robust estimate of the magnitude of G is obtained.
[0251] The instruction Inst can also correspond to considering the uncertainty R n or Q n Associated with at least one sensor Cap 1 -Cap k At least one additional measurement y1-y among all available measurements n , in order to improve the robustness of the estimate of the magnitude G of the vehicle Veh state.
[0252] Figure 2 and Figure 4 The specific implementation modes of the navigation monitoring method represented in Figure 3 and Figure 5 It is implemented on the navigation monitoring system SCN represented in.
[0253] exist Figure 3 In the sensor Cap k The vehicle Veh and its environment Env are measured. For example, GPS can measure the position of the vehicle Veh, an inertial measurement unit can measure the speed and position of the vehicle, and a radar can measure the position of objects near the vehicle.
[0254] These measurements and their associated uncertainties are used by the estimation module M0 in order to estimate the magnitude G using a Kalman filter FK.
[0255] The module MDP then determines the partial derivatives of the magnitude with respect to the measurements.
[0256] Based on the values of the partial derivatives determined by the module MDP, the module MOV obtains one or more critical values, as described above.
[0257] The module ME then sends one or more threshold signals Sig representative of these threshold values VC to the receiving module MR.
[0258] This module MR makes it possible to display the critical signal(s) to the user as determined on instruction, as previously described.
[0259] The instruction is received by the module MR and then applied by the application module MA.
[0260] If the instruction indicates navigation monitoring control, it is applied to the vehicle Veh. If it indicates to take more measurements into account, a new estimate of the target state is made.
[0261] Figure 6 It integrates the navigation monitoring system SCN and a set of sensors Cap 1 -Cap k Navigation system SNav.
[0262] In a particular embodiment of the system Nav, the set of sensors Cap 1 -Cap k Includes at least one inertial measurement unit.
[0263] Figure 7 The figure shows the hardware architecture of the navigation monitoring system SCN according to a specific embodiment of the present invention.
[0264] In the embodiment described here, the navigation monitoring system SCN has the hardware architecture of a computer, and particularly comprises a processor D1, a read-only memory D2, a random access memory D3, a rewritable non-volatile memory D4 and a communication component D5.
[0265] The read-only memory D2 of the system SCN constitutes a recording medium according to the invention, which can be read by the processor D1 and on which is recorded a computer program PGI according to the invention, the program comprising instructions for executing the previously referenced Figure 2 or Figure 4 The steps of the monitoring method according to the invention are described in two exemplary embodiments.
[0266] The computer program PGI defines Figure 3 or Figure 5 The functional modules of the monitoring system SCN are represented in FIG.
Claims
1. A method for monitoring navigation of a vehicle (Veh) to perform navigation control (MAN j ), the method comprising the following steps: - Kalman filter (FK) according to the parameters (Par1-Par n ), a digital model of the vehicle in its environment, and at least one measurement (y1-y n ) to estimate (E0) the magnitude (G), said at least one measurement (y1-y n ) is composed of at least one sensor (Cap 1 -Cap k ) and with respect to the measurement (y1-y n ) uncertainty (R1-R n 、Q1-Q n ), the magnitude (G) is used to implement the navigation control (MAN j ); - determining (EDP) said magnitude (G) relative to said at least one measurement (y1-y n ) of at least one partial derivative value (D G y1-D G y n ); - from the at least one partial derivative value (D G y1-D G y n ) obtains (EOV) said at least one measurement (y1-y2) for said estimation (E0) n ), said critical value quantifying the influence of said at least one measurement on the estimate of the magnitude; - applying (EA) at least one instruction (Inst) determined as a function of at least one information on the robustness of the estimate of the magnitude, said information being contained in at least one critical signal (Sig) representative of said at least one critical value (VC); The instruction indicates the operation for implementing the navigation control (MAN j ) monitoring and control.
2. Method according to claim 1, wherein the instruction (Inst) indicates that the measurements are added for a subsequent estimation of the magnitude (G).
3. The method according to claim 1 or 2, wherein: - Get the measurements (y1-y n ); as well as - During said determination step (EDP), for each of these measurements, a value of the partial derivative of said magnitude (D G y1-D G y n ).
4. A method according to any one of claims 1 to 3, wherein for at least one given moment (n), during said determination step (EDP), n ) determines the partial derivative value (D G y n ), which measurement includes at least two measurements (y n 1 -y n k ), each measurement is performed by a separate sensor (Cap 1 -Cap k ) is performed at that given moment (n).
5. The method according to claim 1 , wherein the at least one critical signal (Sig) is generated (EGen) as a result of a comparison (EComp) between the at least one critical value (VC) and at least one threshold value (Se), the at least one threshold value (Se) being related to the control (MAN) j ) is defined by 6. The method according to any one of claims 1 to 5, wherein said at least one critical value (VC) is equal to said at least one partial derivative value (D G y1-D G y n ).
7. The method according to any one of claims 1 to 5, wherein: - During said determination step (EDP) a number of partial derivative values (D G y1-D G y n );as well as - said at least one critical value (VC) is equal to said partial derivative value (D G y1-D G y n ) is the weighted sum of .
8. The method according to any one of claims 1 to 5, wherein: - During said determination step (EDP) a number of partial derivative values (D G y1-D G y n ); - said at least one critical value (VC) is equal to the sum of the products between: (i) related to the measurement (y1-y n ) of the partial derivative value (D G y1-D G y n );as well as (ii) With respect to the measurement (y1-y n ) of the uncertainty (R1-R n 、Q1-Q n ).
9. The method according to any one of claims 1 to 5, wherein the at least one critical value (VC) is equal to the product between: (i) related to the measurement (y1-y n ) of the partial derivative value (D G y1-D G y n );as well as (ii) With respect to the measurement (y1-y n ) of the uncertainty (R1-R n 、Q1-Q n ).
10. A method for monitoring navigation of a vehicle (Veh) to perform navigation control (MAN j ) system, the system comprising: - used by the Kalman filter (FK) according to the parameters (Par1-Par n ), a digital model of the vehicle in its environment, and at least one measurement (y1-y n ) to estimate (M0) the magnitude (G) of the module, the at least one measurement (y1-y n ) is composed of at least one sensor (Cap 1 -Cap k ) and with respect to the measurement (y1-y n ) uncertainty (R n ,Q n ), the magnitude (G) is used to implement the navigation control (MAN j ); - for determining (MDP) said magnitude (G) relative to said at least one measurement (y1-y n ) of at least one partial derivative value (D G y1-D G y n ) module; - for obtaining from said at least one partial derivative value (D G y1-D G y n ) obtain (MOV) said at least one measurement (y1-y2) for said estimation (E0) n ) of the at least one measurement, the at least one critical value (VC) quantifying the influence of the at least one measurement on the estimate of the magnitude; - a module for applying (MA) at least one instruction (Inst) determined as a function of at least one information on the robustness of the estimate of the magnitude, said information being contained in at least one critical signal (Sig) representative of said at least one critical value (VC); The instruction indicates the operation for implementing the navigation control (MAN j ) monitoring and control.
11. The monitoring system (SCN) according to claim 10, wherein the sending module (ME) comprises submodules for the following operations: - a comparison (MComp) between said at least one critical value (VC) and at least one threshold value (Se), said at least one threshold value (Se) being related to said control (MAN j ) - generating (MGen) the at least one limit signal (Sig) as a function of the result of the comparison of the comparison submodule (MComp).
12. A navigation system (SNav) for a vehicle (Veh), the navigation system (SNav) being adapted to perform the functions of a monitoring system (SCN) according to claim 10 or 11 and comprising at least one measurement (y1-y2) for estimating a magnitude (G) n ) of at least one sensor (Cap 1 -Cap k ), the magnitude (G) is used to implement navigation control (MAN j ).
13. The navigation system (SNav) according to claim 12, wherein at least one of the sensors (Cap 1 -Cap k ) is an inertial measurement unit.
14. Computer program (PGI) comprising instructions for executing the steps of the method according to at least one of claims 1 to 9.
15. A computer-readable recording medium (D2) of a computer program (PGI), the computer program (PGI) comprising instructions for executing the steps of the method according to at least one of claims 1 to 9.
