This invention relates to methods and systems for detecting and identifying errors and/or failures in vehicle environment sensors.
Over the service life of a vehicle, many components and subsystems can experience wear, damage, and other degradation over time. While various approaches may be used to identify certain failed components, other types of components may be more difficult to diagnose. It may also be difficult to identify which component or aspect of a vehicle environment sensor is erroneous or failing.
According to one aspect of the invention, there is provided a method of carrying out a remedial vehicle action in response to determining an error in a monitored vehicle sensor the method including: determining a vehicle true velocity vector based on measurements from onboard vehicle sensors, determining a monitored sensor velocity vector based on measurements from the monitored vehicle sensor, the monitored sensor velocity vector including a plurality of measured vector components; when it is determined that the vehicle true velocity vector is different than the monitored sensor velocity vector, then testing for a first component error, the first component error being an error in a first measured vector component of the monitored sensor velocity vector, the first measured vector component being one of the plurality of measured vector components, and wherein the testing for the first component error includes: (i) calculating a first component error value based on a first component measured value and a first component corrected value, wherein the first component measured value is based on the measurements from the monitored vehicle sensor and the first component corrected value is derived from the vehicle true velocity vector, (ii) using the first component error value to obtain a first test vehicle velocity vector; (iii) comparing the first test vehicle velocity vector to a first component monitored sensor velocity vector, the first component monitored sensor velocity vector being derived based on the measurements from the monitored vehicle sensor; and (iv) when the first test vehicle velocity vector is within a predetermined threshold amount of the first component monitored sensor velocity vector, then determining that the monitored vehicle sensor is erroneous with respect to the first measured vector component of the monitored sensor velocity vector; and when one or more of the measured vector components of the monitored sensor velocity vector are determined to be erroneous, then carrying out a remedial vehicle action.
According to various embodiments, this method may further include any one of the following features or any technically-feasible combination of some or all of these features:
According to another aspect of the invention, there is provided a vehicle failure detection system, including: a system under monitoring (SUM) that includes one or more vehicle environment sensors; one or more onboard vehicle sensors, the onboard vehicle sensors being separate from the one or more vehicle environment sensors; a failure detection module that includes a failure detection system (FDS) signal processor, and a memory containing a computer program, and a communication link between the failure detection model and the SUM; wherein the failure detection module is configured to execute the computer program that is stored in the memory of the vehicle failure detection system using the failure detection signal processor, and wherein execution of the computer program by the failure detection signal processor causes the failure detection module to: (i) determine a vehicle true velocity vector based on measurements from the one or more onboard vehicle sensors; (ii) determine a monitored sensor velocity vector based on measurements from the one or more vehicle environment sensors, the monitored sensor velocity vector including a plurality of measured vector components; (iii) when it is determined that the vehicle true velocity vector is different than the monitored sensor velocity vector, then test for a first component error, the first component error being an error in a first measured vector component of the monitored sensor velocity vector, the first measured vector component being one of the plurality of measured vector components, wherein the testing for the first component error includes calculating a first component error value based on a first component measured value and a first component corrected value, and wherein the first component measured value is derived based on the measurements from the vehicle environment sensors and the first component corrected value is derived from the vehicle true velocity vector; and (iv) perform a remedial vehicle action in response to the detection of an error in the one or more vehicle environment sensors.
According to various embodiments, this system may further include any one of the following features or any technically-feasible combination of some or all of these features:
In other embodiments, the system and methods defined above can be utilized in other (non-vehicular) applications that involve an electronics-based system having sensors that provide the system state variable inputs needed to detect a failure of whatever component is being monitored.
One or more aspects of the disclosure will hereinafter be described in conjunction with the appended drawings, wherein like designations denote like elements, and wherein:
The system and method discussed below enables errors in a vehicle environment sensor tar other monitored vehicle sensor) to be detected and error values determined. In many embodiments, the errors in the vehicle environment sensor can be detected based on determining a vehicle true velocity vector and a monitored sensor velocity vector. The monitored sensor velocity vector refers to a velocity vector that is calculated based on measurements from the vehicle environment sensor, and the vehicle true velocity vector refers to a velocity vector that is calculated based on measurements from other vehicle sensors that are deemed to be more accurate than the vehicle environment sensor, such as one or more vehicle operations sensors (or other onboard vehicle sensors) that detect certain operation values for the vehicle. In an exemplary embodiment, the vehicle environment sensor can be (or include) a lidar unit (or lidar sensor) and/or a radar unit (or radar sensor). And, in the exemplary embodiment, the onboard vehicle sensors vehicle sensors can be a vehicle speed sensor (e.g., a wheel speed sensor) and a vehicle heading angle sensor, and can be separate from the lidar/radar unit (or other monitored vehicle sensor(s)).
The failure detection system (FDS) discussed below can be used to detect failures and/or degradation in vehicle electronic surveillance systems (e.g., a lidar/radar unit used for surveillance of the environment). In some scenarios, such surveillance systems are applicable in upper level autonomous vehicles for providing the data necessary for the onboard digital control system to react to conditions in the environment that affect the guidance system performance. In some embodiments, the FDS and the method below applies to certain lower level AVs by yielding a safety related environmental data that is often termed Bind Spot Detection (BSD).
The vehicle system to which the FDS can be applied can be an electronically scanned or an electromechanically scanned radar (radio detection and ranging) or lidar (light detection and ranging) unit. In one embodiment, a primary feature of the lidar/radar unit is the continues monitoring of objects within the field of view of the relevant surveillance system. This can involve providing the distance (range), the azimuth angle, and/or the elevation angle of an object relative to the vehicle coordinate system.
In some scenarios, as mentioned above, the vehicular surveillance systems are susceptible to errors in the range, azimuth, and elevation to objects within the field of view due to partial or complete component/part failure, software glitches, and changes in the environment around the vehicle, for example. Regardless of the source of error in lidar/radar data, there may be potentially hazardous consequences to the vehicle/occupant safety. Some embodiments below can be used to provide a means of precisely and accurately determining such errors. Moreover, depending upon the characteristics of the error, various embodiments can give guidance to maintenance personnel in repairing the lidar/radar unit, and/or for carrying out another remedial vehicle action in response to determining the source and/or magnitude of such errors.
With reference to
The other system sensors 14 are system sensors that are a part of the overall system 10, but are not a pan of the SUM 12. As an example, where the FDS 10 is being used on a vehicle to monitor one or more components of the vehicle, system sensors 13 and 14 would be a part of the OEM installed vehicle electronics that are used for vehicle operations, with sensors 13 being part of the component under monitoring (part of the SUM 12) and sensors 14 being part of the remaining vehicle electronics. In one embodiment, the sensors 13 can be a radar sensor and/or a lidar sensor, such as that which is installed on the front of a vehicle and facing in a forward direction.
The communication bus 18 is a hardwired electronic communication bus. However, it should be appreciated that the communication bus 18 can be any of a variety of electronic communication mechanisms, including those using wireless communications, as discussed more below. And, in some embodiments, the system 10 can include a hardwired communication bus 18, as well as a separate wireless communications module 30 that can be connected to the communication bus 18 and that can be used for communications within the system 10 and/or used with other external (o r remote) components (not shown).
The failure detection module (FDM) 16 is shown as including a failure detection system (PDS) signal processing module 20, a supplemental sensor interface 22, failure simulation electronics 24, a communication bus interface 26, a user interface 28, a wireless communications interface 30, and one or more supplemental sensors 32. These parts and/or devices of the failure detection module 16 can be included into a dedicated electronic module that is included in the system 10 for purposes of identifying failures of the SUM 12 and/or the method discussed herein. In other embodiments, the FDM 16 can be incorporated into one or more already existing portions or devices such as through configuring the one or more pre-existing individual components (e.g., components of the SUM 1.2 or another component separate from the SUM 12) so that these individual components carry out the method discussed herein. And, in yet another embodiment, various parts of the FDM 16 can be incorporated into a dedicated module and others can be incorporated (or configured) into other, pre-existing components of the system 10. Moreover, it should be appreciated that the dedicated module can be a single module or, in other embodiments, the FDM 16 can be incorporated into multiple dedicated modules.
The failure simulation electronics 24 can be used during development and testing of the failure detection system 10 and/or the failure detection module 16. The failure simulation electronics 24 provides a mechanism for assessing the FDS performance during the design and development phase. The failure simulation electronics 24 is not a part of the final FDS 10 when the FDS 10 (or the system into which the FDS 10 is incorporated) is in use by a consumer—that is, for example, when the IDS is used in the context of a vehicle system, the failure simulation electronics 24 would not be a part of the production vehicle. The failure simulation electronics 24 permits failures in components, including key components, to be simulated electronically and permits the performance of the FDS for detecting and/or identifying failures/errors to be measured and/or otherwise evaluated.
As mentioned, the SLAM 12 can include one or more individual components and, in some embodiments, can represent either a single component or a single subsystem. This direct connection 40 is depicted by a dashed line since it is not a communication link that normally would be included in the electronic system, unlike the communication bus typically would be, for example. In other embodiments, the FDM 16 and the SUM 12 can be connected via the communications bus 18 (or other communication link); however, a direct connection can be added (as shown by the dashed line) since, in some scenarios, there may be some exceptional conditions in which it may be desirable to avoid the latency of the communication bus 18. For example, for certain safety related failures, the message identifying the failure or error could conceivably be greatly improved through using this direct connection 40 by enabling the system to instantaneously (or, near-instantaneously) send the warning message concerning a component failure. In addition, in at least some embodiments, the direct connection 40 can simplify synchronization of sensor sampling between the supplemental sensors 32 and the existing system sensors, including the SUM sensors 13 and the other system sensors 14. Moreover, in some embodiments, the estimated error can restore system performance; an example of this is a calibration failure in a lidar/radar sensor or other surveillance sensors; such a failure in the sensor (when it is used for vehicle steering) can lead to possible accidents in some scenarios.
The wireless communications module 30 can be used to communicate data via short-range wireless communications (SRWC) and/or via cellular network communications through use of wireless communication circuitry (WCC). In one embodiment, the wireless communications module 30 includes WCC, an antenna, and a GNSS receiver, which includes a GNSS antenna; although in the depicted embodiment of
In some embodiments, the wireless communications module 30 can be configured to communicate wirelessly using one or more short-range wireless communications (SRWC) protocols, such as any of the Wi-Fi™, Wi-Fi Direct™, other IEEE 802.11 protocols, ZigBee™, Bluetooth™ (including any of the Bluetooth™ protocols, such as Bluetooth™ Low Energy), or near field communication (NFC). In such an embodiment, the WCC can be a short-range wireless communication (SRWC) circuit that enables the wireless communications module 30 to transmit and receive SRWC signals, such as Bluetooth signals. The SRWC circuit may allow the module 30 to connect to another SRWC device. Additionally, in some embodiments, the wireless communications module 30 may contain a cellular chipset that may be a part of the WCC also, or that may be separate WCC (or cellular chipset). Thus, in such embodiments, the wireless communications module enables the vehicle 100 to communicate via one or more cellular protocols, such as those used by various cellular carrier systems.
Wireless communications module 30 may enable vehicle 100 to be in communication with one or more remote networks via TCP/IP or other packet-switched data communication. Packet-switched data communications may also be carried out via use of a cellular network that may be accessible by the wireless communications module 30. The wireless communications module 30 may, via the WCC, communicate data over various wireless carrier systems, including cellular carrier networks. In such an embodiment, radio transmissions may be used to establish a communications channel, such as a voice channel and/or a data channel, with a wireless carrier system so that voice and/or data transmissions can be sent and received over the channel. Data can be sent either via a data connection, such as via packet data transmission over a data channel, or via a voice channel using techniques known in the art. For combined services that involve both voice communication and data communication, the system can utilize a single call over a voice channel and switch as needed between voice and data transmission over the voice channel, and this can be done using techniques known to those skilled in the art.
The user interface 28 can include various device-user interfaces that are capable of communicating information between the vehicle electronics and a user or operator of the vehicle. Non-limiting examples can include a display, an audio speaker, a visual display, a computer monitor, a television, a tactile actuator or other tactile/haptic mechanism, lights (e.g., LEDs), and/or various other device-user interfaces. Any of these device-user interfaces can be used to provide the notification or warnings discussed herein. In addition, the user interface can be designed to have user inputs to the system using standard touch screen technology.
With reference to
The FDSLR 110 includes a lidar and/or radar (lidar/radar) unit 112, which corresponds to the SUM 12 (
The failure detection module (FDM) 116 is analogous to that of the failure detection module (FDM) 16 (
The lidar/radar unit 112 is part of the electronics of the vehicle and can include a lidar or radar emitter and a lidar or radar receiver. In one embodiment the lidar/radar unit 112 is a lidar unit that can emit non visible light waves for purposes of detecting spatial information of an area surrounding the vehicle 100. And, in some embodiments, the lidar/radar unit 112 emits a plurality of light pulses (e.g., laser light pulses) and receives the reflected light pulses using a lidar receiver. In another embodiment, the lidar/radar unit 112 is a radar unit that transmits radar signals for purposes of detecting spatial information of an area surrounding the vehicle 100, and, in at least one embodiment, the lidar/radar unit 112 emits a plurality of radar signals (e.g., radar pulses) and receives the reflected radar pulses using a radar receiver. And, in some embodiments, the lidar/radar unit 112 can include both a radar unit and a lidar unit, and/or multiple radar units and/or multiple lidar units. The lidar/radar unit 112 can be mounted (or installed) on the front, the side, and/or the rear of the vehicle 100. Both the radar unit and the lidar unit are considered environment sensors as each detects information concerning an area surrounding the vehicle 100.
The GNSS receiver 114 includes an antenna that enables the receiver 114 to receive GNSS radio signals from GNSS satellites (not shown). GNSS receiver 114 may be any suitable commercially available GNSS receiver and may receive GNSS information, which may then be sent over the IVN 18 to other vehicle modules. Although the GNSS receiver 114 is shown as being separate from the FDMLR 116, in other embodiments, the GNSS receiver 114 can be included as a part of the FDMLR 116, such as one of the plurality of supplemental sensors 132.
Speed sensor 1321 is an onboard vehicle sensor that is installed as a part of the vehicle electronics. The speed sensor 1321 determines a speed of the vehicle (e.g., vehicle speed u0) and, in many embodiments, the speed sensor 1321 can be said to be an accurate vehicle vector sensor. As used herein, an accurate vehicle vector sensor is an onboard vehicle sensor that accurately determines a vector component of the vehicle. And, as used herein, the term “accurate” (including its various forms) when used in conjunction to describe sensors or sensor readings, refers to sensors or readings/measurements that are considered to be reliable for determining error(s) in environment sensors (e.g., the lidar/radar unit 112), such as that as discussed in the embodiments of the method(s) below. The speed sensor 1321 is an onboard vehicle sensor and, as used herein, an onboard vehicle sensor is a vehicle sensor that is installed as a part of the vehicle electronics and that determines or detects information concerning the operation of the vehicle, including vehicle speed and heading. In one embodiment, the speed sensor 1321 is a wheel speed sensor, a vehicle speed sensor (VSS), a radar speed sensor that can provide accurate speed reading/measurements for the vehicle, or other sensor that can provide a vehicle speed reading (or an accurate vehicle speed reading). In many embodiments, the radar speed sensor is separate from the lidar/radar unit under monitoring.
Heading angle sensor 1322 is an onboard vehicle sensor that is installed as a part of the vehicle electronics. The heading angle sensor 1322 determines a heading angle of the vehicle, such as the heading angle α0 as discussed below. In many embodiments, the heading angle sensor 1322 can be said to be an accurate vehicle vector sensor. In one embodiment, the heading angle sensor 1322 can use the current vehicle location (as determined using the GNSS receiver 114) and various other vehicle sensors (e.g., accelerometers) to determine the heading angle α0. In another embodiment, the heading angle sensor 1322 can be (or use) a digital compass. In other embodiments, the vehicle 100 can include a speed sensor and a heading angle sensor that are other system sensors 14 instead of being supplemental sensors 32. The electronic map 150 can be geographical map data that includes geographical information pertaining to one or more roadways and/or traffic signals (or other traffic-related objects) of an area surrounding the vehicle 100. The electronic map 150 can be stored in the memory of the wireless communications module 30, or as a pan of memory of another vehicle module, as shown in
The FDS for ruler or radar surveillance systems (FDSLR) (e.g., FDSLR 110) is based on independent computation and measurement of the vehicle velocity vector. The exemplary vehicle system involves the vehicle moving in a plane that is tangent to the earth and whose instantaneous location in this Earth Fixed (EF) coordinate system is given by coordinates x and y as depicted in
The actual range (i.e., straight line distance) to the fixed object from the vehicle instantaneous position is denoted r in
The model for vehicle position relative to r, ϕ, and a0 is given by:
(a)x=x0−r cos(θ)
(b)y=r sin(θ)
(c)r=[(x0−x)2+y2]1/2
(d)tan(θ)=y/(x0−x)
(e)θ=(ϕ−α0) (Equation 1)
where x is the location of the vehicle 100 along the x-coordinate, y is the location of the vehicle 100 along the y-coordinate, x0 is the location of the fixed object 200 along the x-coordinate, r is the range (or linear distance) between the vehicle 100 and the fixed object 200, a0 is the heading angle of the vehicle 100 relative to the x-coordinate, ϕ is the azimuth angle (or vehicle-object angle), and θ is the angle between the x-coordinate and the fixed object 200 as taken at the location of the vehicle 100 (x,y), and can be referred to as the fixed object angle θ.
The vehicle true velocity vector in relationship to the earth fixed (EF) coordinate system is denoted S and is given by or determined using the fallowing:
where Sx is the x component of the vehicle true velocity vector S, Sy is they component of the vehicle true velocity vector S. The Equation 2(d) through 2(f) represent the vehicle true velocity vector S in matrix equation form. In at least some embodiments, the vehicle true velocity vector S represents the actual (or accurate) vehicle velocity vector as determined through using sensors 132.
In some embodiments, computations are performed with a digital electronic system (e.g., special purpose computer), such as the DSP 120. Such computations for a dynamic system can be performed on discrete time samples of any continuous time variable. It can be assumed that sampling is periodic with sample period denoted TS in which the k-th sample time is denoted tk=kTS, where each k is an integer. The discrete time velocity vector at sample time tk is denoted S(k). The discrete time model for S(k) for the actual position of the vehicle 100 at sample time tk is denoted by xk, yk and is provided by the following:
(a)S(k)=[Sx(k),Sy(k)]T
(b)Sx(k)=−{dot over (r)}k cos(ϕk−α0)+rkϕk sin(ϕk−α0)
(c)Sy(k)={dot over (r)}k sin(ϕk−α0)+rk{dot over (ϕ)}k cos(ϕk−α0)
Where a sampled estimate at a specific sample time t(k1)=k/TS is given by:
where rl=r(ki) and ϕi=ϕ(kl) (e.g., r1=r(k1) and ϕ1=ϕ(k1)).
After the actual (or accurately-estimated) position of the vehicle is modelled using the vehicle modelling methodology provided above, a lidar/radar failure detection process can be carried out. Below there is provided an exemplary model for errors in the lidar/radar and for the failure detection algorithms that are used to calculate these errors. This failure detection is based on measurements of range rm and azimuth angle ϕm made by the lidar/radar system with the notation rm(k) denoting the measured range r at sample time tk and ϕm(k) denoting the measured value oldie azimuth angle ϕ at tk. For convenience, rm=(1+∈r)r(k1), ϕm=ϕ(k1)+∈ϕ, where ∈r is the range error and ∈ϕ is the azimuth error.
The sample of vehicle velocity that is calculated from these lidar/radar measurements (rm and ϕm) at time tk for a specific k1 is defined by:
where Sm is the measured vehicle velocity vector, Sxm is x-component of the measured vehicle velocity vector Sm, and Sym is y-component of the measured vehicle velocity vector Sm. The measured vehicle velocity vector Sm can be derived based on the measured range rm and the measured azimuth angle ϕm. The measured range rm and the measured azimuth angle ϕm are measured using the lidar/radar unit 112. In many embodiments, it can be determined that there is an error in the lidar/radar unit 112 when it is determined that Sm≠S(k1). The measured vehicle velocity vector Sm is based on calculations from the lidar/radar measurement and can be written in matrix form.
Once a lidar/radar error is detected (or it is determined that a lidar/error is present), then the source of the error can be determined—for example, whether the lidar/radar error is a range error and/or an azimuth error. The example below presents two independent error sources, (I) azimuth angle, and (II) range:
(I)ϕ1m=ϕm(k1)=ϕ(k1)+∈ϕ, assuming that ∈r=0
(II)r1m=rm(k1)=(1+∈r)r(k1), assuming that ∈ϕ=0 (Equation 5)
where ∈r is the tidal-Nadu range error and ∈ϕ is the lidar/radar azimuth error. In other in embodiments, other sources of error (e.g., elevation error) can be detected using similar methods as is apparent to one skilled in the art from the discussion below.
For the azimuth error, it can be assumed that the range error ∈r is zero, as shown above in Equation 5(I). In this sense, the hypothesis used below assumes that there is an error in the ϕ1 measurement-based calculations:
That is, S1m(k) is the estimated first component monitored sensor velocity vector based on the assumption that there is only an error in the azimuth angle. In at least one embodiment, A1m can be referred to as a first component estimate matrix. This first component monitored sensor velocity vector is a monitored sensor velocity vector that is determined based on measurements from the lidar/radar unit (or other monitored sensor) and that assumes there only exists an error in the first component (e.g., azimuth angle). In many embodiments, if there was no error in ϕ or r measurements, the product given in Equation 6(a) would yield the exact value of the velocity S(k1).
As in the case of the application of the FDS system as depicted in
S=[Sx,Sy]T
where Sx=u0 cos(α0)
Sy=u0 sin(α0) (Equation 7)
The estimate of a matrix A(k1) yields matrix coefficients from which an estimate of the correct (i.e., error free) azimuth angle ϕ1 is obtained using the following equations:
(a)Â(k1)=X1m−1S(k1)
(b)Â(k1)=[cos({circumflex over (ϕ)}(k1)−α0), sin({circumflex over (ϕ)}(k1)−α0)]T
(c){circumflex over (ϕ)}(k1)=sin−1(Â{2})+α0 (Equation 8)
where Â(k1) is an estimate of the correct or highly-accurate matrix A(k1) and {circumflex over (ϕ)}(k1) is the corrected azimuth angle, which can be considered a (highly) accurate estimate of the azimuth angle (at least in some embodiments). As used herein, A{n} is the n-th element of the matrix A and, for example, Â{2}=sin({circumflex over (ϕ)}(k1)−α0) as used in Equation 8. In some embodiments, the corrected azimuth angle (denoted {circumflex over (ϕ)}(k1)) is an estimate of the angle ϕ(k1) based on lidar/radar measurements, vehicle speed u0, and/or heading angle α0, and is derived using Equation 8. This corrected azimuth angle is an estimate of the azimuth angle assuming that the error in the lidar/radar unit 112 is only due to error(s) in the azimuth angle measurements (i.e., not in errors in other components of the monitored sensor velocity vector). The monitored sensor velocity vector is the vehicle velocity vector as determined using the SUM 12 (e.g., the lidar/radar unit 112). As mentioned above, the vehicle speed u0 can be measured using the vehicle speed sensor 1321 and the heading angle α0 can be measured using the heading sensor 1322. The calculations of Equation 8 yield an accurate estimate of azimuth angle ϕ(k1) (or corrected azimuth angle {circumflex over (ϕ)}(k1)) assuming that the error in the monitored sensor velocity vector is only with respect to the azimuth angle (e.g., with the accuracy determined by the accuracy of the sensors and the assumption that ∈r=0). The implementation of accurate speed sensors and heading sensors can be used to yield a (highly) accurate estimate of the azimuth angle ϕ(k1) (i.e., corrected azimuth angle {circumflex over (ϕ)}(k1)). The estimate of the azimuth angle error {circumflex over (∈)}ϕ to can now be given by {circumflex over (∈)}ϕ=ϕm(k1)−{circumflex over (ϕ)}(k1).
In some embodiments, the hypothesis that the lidar/radar error is limited only to the azimuth angle ϕ (hypothesis (I)) can be confirmed to assure or better predict that there is no error in the range measurement cement rm. In one embodiment, this confirmation or verification can be performed using the following equations:
(a)θT={circumflex over (ϕ)}(k1)+∈ϕ−α0
(b)A1T=[cos(θT), sin(θT)]T
(c)S1T=X1mA1T (Equation 9)
where θT is the tested fixed object angle, A1T is a first hypothesis verification vector, and S1T is the first test vehicle velocity vector. In at least one embodiment, A1T is used to confirm the validity of the corresponding hypothesis (e.g., hypothesis (I), hypothesis (II)). These tested values are based on the vehicle heading α0 as determined by the heading angle sensor 1322 and the estimated azimuth angle error {circumflex over (∈)}ϕ. This confirmation is accomplished by taking the estimated azimuth angle error {circumflex over (∈)}ϕ and using it with a matrix equation, as shown in Equation 9 (note, that this is analogous to that of Equation 6). When it is determined that S1T=S1m (or that the norm of the difference between S1T and S1m is less than a threshold amount), then it can be determined that the hypothesis (I) is confirmed.
As an illustration, a simulation was conducted with the vehicle travelling at a speed u0=10 km/sec (kilometers per second) at an angle α0=0.1745 radians. The errors for the first simulation are ∈r=0, ∈ϕ=0.0500. The simulation yields an estimate {circumflex over (∈)}ϕ=0.0500. The velocity vectors are as follows.
S1m=[9.9226,1.2417]T
S1T=[9.9226,1.2417]T
In some embodiments, the agreement between S1T and S1m are sufficient to confirm the hypothesis of an error only in the azimuth angle ϕ measurements. Another simulation was run with ∈r=0.01 in addition to the ∈ϕ error above of ∈ϕ=0.0500. In this simulation, the estimated error {circumflex over (∈)}ϕ and the two velocity vectors are.
{circumflex over (∈)}ϕ=0.0389
S1m=[10.0219,1.2541]T
S1T=[10.0351,1.1432]T
In this simulation, it can be determined that S1T≠S1m and, thus, since S1T≠S1m, the hypothesis (I) is not valid and is not confirmed.
In at least one embodiment, when the hypothesis (I) is not valid, then the hypothesis (II) can be tested. The hypothesis (II) can be for determining whether there is a lidar/radar error in the range and this error can be found using a similar procedure to that given above with respect to the hypothesis (I). In this case, the speed vector obtained by calculations on lidar/radar measurements is denoted S2m and has components Sx2m, Sy2m. That is, S2m is the second component monitored sensor velocity vector that is determined based on the assumption that there is only an error in the range. This second component monitored sensor velocity vector is a type of a monitored sensor velocity vector that is determined based on measurements from the lidar/radar unit (or other monitored sensor) and that assumes there only exists an error in the second component (e.g., range). And, in this case, the matrix to be estimated is denoted A2m and is a two-dimensional vector and, in some embodiments, can be referred to as a second component estimate matrix. These variables can be given by the following:
The correct values (or the estimated correct values) of the components that in correspond to those of the matrix A2m are obtained using the following equation:
Â2=X2m−1S(k1) (Equation 11)
where the speed vector S(k1) is the same as that of Equation 8(a) and is obtained as explained above using the supplemental speed and heading sensors 1322 of the FDS. The estimate of the correct (i.e., error free) range r(k1) is denoted {circumflex over (r)}(k1) and can be referred to as the corrected range {circumflex over (r)}(k1). The corrected range {circumflex over (r)}(k1) is the range that is determined assuming that there are no errors in the other vector components (e.g., the azimuth angle) and is obtained using the following equation:
{circumflex over (r)}(k1)=Â2{2} (Equation 12)
And, thus, the hypothesized range error {circumflex over (∈)}r is computed using the following:
Once the hypothesized range error {circumflex over (∈)}r is computed, then the hypothesis (II) can be tested.
A test of the validity of the hypothesis (II) in the lidar/radar system error is given by:
(a)rT=(1+{circumflex over (∈)}r){circumflex over (r)}(k1)
(b){dot over (r)}T=Â2{1}(1+{circumflex over (∈)}r)
(c)A2T=[{dot over (r)}T,rT]T
(d)S2T=X2mA2T (Equation 14)
Using the estimated range error {circumflex over (∈)}r, the two components of the range rT and time derivative (of the range) {dot over (r)}T are the components of the A2T vector, which is the second hypothesis verification vector. The second test velocity vector S2T is computed using Equation 14(d) and is the second test vehicle velocity vector. When it is determined that the test speed vector S2T is equal to the speed vector S2m that is computed from lidar/radar measurements, then it is (or can be) determined that the error hypothesis (II) and the range error estimate are correct. When it is determined that S2T=S2m (or that the absolute value of the difference between S2T and S2m is less than a threshold amount), then it can be determined that the hypothesis (II) is confirmed.
The simulation described above for the demonstration of estimating {circumflex over (∈)}ϕ was conducted for errors of hypothesis (II) for the same vehicle speed and heading. In this simulation, {circumflex over (∈)}ϕ=0, ∈r=0.1000. The estimate of ∈r is {circumflex over (∈)}r=0.1000. The test and measured speed vectors are:
S2m=[10.8330,1.9097]T
S2T=[10.8331,1.9098]T
These two vectors are sufficiently close to confirm hypothesis (II). In at least some embodiments, the threshold for determining when S2T is sufficiently close to S2m (also for S1T and Sm) is based on the tolerances of the lidar/radar measurements for their use in the vehicle control system. For example, these tolerances are upper bound limits to the magnitudes of ∈ϕ and ∈r for which the vehicle system using lidar/radar measurements is within acceptable performance. In at least some embodiments, the actual magnitude of these tolerances can and should be established during the design of the associated system and the determination of these values can readily be established via simulations and later confirmed in vehicle test track measurements of performance.
In one embodiment, if there are multiple vehicle systems that employ lidar/radar measurements, the tolerance limits are those that are the smallest of the set for all systems that employ lidar/radar data. For example, these tolerance limits can be readily used to compute the difference S1T−S1m or S2T−S2m. Whenever these differences are less than the corresponding tolerance limited calculations, the associated hypothesis is confirmed. That is, the two vectors are considered to be sufficiently close.
In the event that the test velocity vectors are unequal to the measured speed vector, both ∈ϕ and ∈r are nonzero. Thus, a technique for estimating the combined errors is based on iterations of applying both methods. The procedure for estimating the two errors is conducted as discussed below and can be referred to as a multi-component error determination process.
First, the range r and the azimuth angle ϕ are estimated through using Equations 8 and 12 above at times k1+1 and k1−1. That is, a range estimate and an azimuth angle estimate for each k1+1 and k1−1 can be determined using the Equations 8 and 12. Then, once these estimates {circumflex over (r)}k+1, {circumflex over (r)}k−1, {circumflex over (ϕ)}k+1, and {circumflex over (ϕ)}k−1, are determined, the derivative range estimate {dot over (r)} and the derivate azimuth estimate {dot over (ϕ)} are determined using Equations 3(d) and 3(e). Then, the derivative range estimate {dot over (r)} and the derivate azimuth estimate ϕ can be used to create a new matrix X1m using Equation 6(c).
Next, using Equation 8, a new estimate of ϕ(k1) and ϕ(k1) is determined using this new matrix X1m. Then, a new matrix X2m is determined using Equation 10, but where ϕ(k1) is used in place of ϕm and where ϕ(k1) is used in place of ϕm. Then, a new estimate of r(k1) and {dot over (r)}(k1) can be calculated using Equations 11 and 12. Once these estimates are obtained, then these estimations can be tested to determine whether they represent accurately estimated error values.
An estimate of the velocity vector Sm(k1) is obtained using the estimated values that were determined above: ϕ(k1), ϕ(k1), r(k1), and r(k1). Then, the difference between the estimated velocity vector Sm(k1) and the correct velocity vector S(k1) (derived from Equation 7) can be obtained using S(k1)−Sm(k1)=eS. Once the velocity vector error eS is calculated, then this value can be compared with a vector velocity tolerance limit e1L. The steps above can be repeated continuously until eS<e1L. In other embodiments, the steps above can be repeated continuously until eS≤e1L. Once this stopping condition is met, then the latest calculations of the estimates for r1 and ϕ1 are now considered to be sufficiently accurate estimates of the correct r1 and ϕ1. These sufficiently accurate estimates can be termed {circumflex over (r)}1L for the sufficiently accurate range estimate and {circumflex over (ϕ)}1L for the sufficiently accurate heading angle estimate. The error estimates can now be obtained using the following equation:
{circumflex over (∈)}ϕ={circumflex over (ϕ)}1L−ϕ1{circumflex over (∈)}r={circumflex over (r)}1L−r1 (Equation 15)
Once these sufficiently accurate range and heading angle estimates are obtained, then they can be provided to one or more vehicle system modules (VSMs).
With respect to
The method 300 begins with step 305, wherein a vehicle true velocity vector is determined. The vehicle true velocity vector is a velocity vector of the vehicle that is determined through use of onboard vehicle sensors and, in many cases, these onboard vehicle sensors are separate from the monitored vehicle sensor. In many embodiments, the vehicle speed sensor 1321 and the vehicle heading angle sensor 1322 can be used to determine the vehicle true velocity vector. This vehicle true velocity vector can be considered an accurate (or a highly accurate) determination of the actual vehicle velocity vector with respect to an external object, such as the fixed object 200 (
In step 310, a monitored sensor velocity vector is determined. This monitored sensor in velocity vector is derived using SUM measurements (e.g., range measurement rm and azimuth angle measurement ϕm from the lidar/radar unit 112). In many embodiments, the monitored sensor velocity vector is based on sensor values from environment sensors, such as a lidar unit and/or a radar unit of the lidar/radar unit 112. In one embodiment, the monitored sensor velocity vector can corresponds to Sm as used above, such as in Equation 4. The method 300 continues to step 315.
In step 315, it is determined whether the monitored sensor velocity vector is different from the vehicle true velocity vector. In one embodiment, it is determined that the monitored sensor velocity vector is different from the vehicle true velocity vector when a difference between at least one corresponding component of the respective vectors is not within a particular tolerance value. For example, when it is determined that Sx−Sxm>a threshold time derivative x component value, then it can be determined that the monitored sensor velocity vector is different from the vehicle true velocity vector. As another example, when it is determined that Sy−Sym>a threshold time derivative y component value, then it can be determined that the monitored sensor velocity vector is different from the vehicle 23 true velocity vector. The method 300 continues to step 320 when it is when it is determined that the monitored sensor velocity vector is different from the vehicle true velocity vector; otherwise, the method continues to step 350. In step 350, it is determined that there is no error (or the error is not above a threshold value) and, thus, the method 300 ends.
In step 320, a first component error is tested. The first vector component error refers to an error (or a potential error) in a first one of the vector components of the vehicle velocity vector. In the illustrated example of
With reference to
In step 420, a first test vehicle velocity vector is obtained using the first component error value. In one embodiment, the first test vehicle velocity vector is a vehicle velocity vector that is calculated using the first component error value and assuming that the error in the other components (e.g., ranger) is zero (or negligible). For example, the first test vehicle velocity vector can correspond to S1T of Equation 9. The method 400 continues to step 430.
In step 430, the first test vehicle velocity vector is compared to the first component monitored sensor velocity vector. The first component monitored sensor velocity vector is a monitored sensor velocity vector that is determined based on lidar/radar measurements (or other monitored vehicle sensor measurements) assuming that there exists an error (or a non-negligible error) only in the first component (e.g., azimuth angle). For example, the first test vehicle velocity vector S1T (for testing azimuth) can be compared to S1m and, when it is determined that S1T=S1m (or that the norm of the difference between S1T and S1m is below a threshold amount), then the method 400 can continue to step 440; otherwise, the method 400 continues to step 450.
When the method 400 reaches step 440, it can be determined that the monitored vehicle sensor is erroneous with respect to the first vector component of the vehicle velocity vector (and not any of the other vector components). And, when the method 400 reaches step 450, it can be determined that the monitored vehicle sensor is not erroneous with respect to only the first vector component of the vehicle velocity vector (e.g., error in more than one vector component, no error in the first vector component). Step 440 correspond to a “Yes” for purposes of step 325 (
With reference back to
With reference to
In step 520, a second test vehicle velocity vector is obtained using the second component error value. In one embodiment, the second test vehicle velocity vector is a vehicle velocity vector that is calculated using the second component error value and assuming that the error in the other components (e.g., azimuth angle) is zero (or negligible). For example, the second test vehicle velocity vector can correspond to S2T of Equation 14. The method 500 continues to step 530.
In step 530, the second test vehicle velocity vector is compared to the second component monitored sensor velocity vector. The second component monitored sensor velocity vector is a monitored sensor velocity vector that is determined based on lidar/radar measurements (or other monitored vehicle sensor measurements) assuming that there exists an error (or a non-negligible error) only in the second component (e.g., range). For example, the second test vehicle velocity vector S2T (for testing azimuth) can be compared to S2m and, when it is determined that S2T=S2m (or that the absolute value of the difference between S2T and S2m is below a threshold amount), them the method 500 can continue to step 540; otherwise, the method 500 continues to step 550.
When the method 500 reaches step 540, it can be determined that the monitored vehicle sensor is erroneous with respect to the second vector component of the vehicle velocity vector (and not any of the other vector components). And, when the method 500 reaches step 550, it can be determined that the monitored vehicle sensor is not erroneous with respect to only the second vector component of the vehicle velocity vector (e.g., error in more than one vector component, no error in the second vector component). Step 540 correspond to a “Yes” for purposes of step 335 (
With reference back to
With reference to
Method 600 begins with step 610, wherein the first component multi-test value(s) are determined. In one embodiment, this can include estimating the first component value and the second component value at times k+1 and k−1. For example, the ranger and the azimuth angle ϕ are estimated for times k+1 and k−1. These estimates can correspond to those calculated using Equations 8 and 12 above for times k+1 and k−1. The term {circumflex over (r)}k+1 can represent the range estimate for time k+1 (or second component next time estimate), the term {circumflex over (r)}k−1 can represent the range estimate for time k−1 (or second component prior time estimate), the term a {circumflex over (ϕ)}k+1 can represent the azimuth angle estimate (or first component next time estimate), and the term {circumflex over (ϕ)}k−1 can represent the azimuth angle estimate (or first component prior time estimate). Then, once these estimates {circumflex over (r)}k+1, {circumflex over (r)}k−1, {circumflex over (ϕ)}k+1, and {circumflex over (ϕ)}k−1 are determined, the derivative range estimate {dot over (r)} and the derivate azimuth estimate {dot over (ϕ)} is calculated based on these estimates. The derivative range estimate {dot over (r)} and the derivate azimuth estimate {dot over (ϕ)} can be determined using Equations 3(d) and 3(e) using the estimates {circumflex over (r)}k+1, {circumflex over (r)}k−1, {circumflex over (ϕ)}k+1, and {circumflex over (ϕ)}k−1. Using the derivative range estimate {dot over (r)} and the derivate azimuth estimate {dot over (ϕ)}, a new estimate of ϕ(k1) and {dot over (ϕ)}(k1) is determined. These estimates of ϕ(k1) and/or {dot over (ϕ)}(k1) can be referred to as the first component multi-test values, as discussed below.
In one embodiment, first a new matrix X1m is calculated using the derivative range estimate {dot over (r)} and the derivate azimuth estimate {dot over (ϕ)}, which can be done so using Equation 6(c). Then, for example, Equation 8 above can be used to determine a new estimate of ϕ(k1) and {dot over (ϕ)}(k1) using the new matrix X1m (step 620) derived above. These estimates for ϕ(k1) and ϕ(k1) can each be referred to as first component multi-test values since these estimates represent values of the first component (e.g., azimuth angle) that are to be tested as a part of the multi-component error determination process. The method 600 continues to step 620.
In step 620, the second component multi-test value(s) are determined. In one embodiment, the second component multi-test value(s) include a new estimate of r(k1) and {dot over (r)}(k1). In one embodiment, this can include calculating a second new matrix X2m, which is determined using Equation 10, but where ϕ(k1) is used in place of ϕm and where {dot over (ϕ)}(k1) is used in place of {dot over (ϕ)}m. Then, a new estimate of r(k1) and {dot over (r)}(k1) can be calculated using Equations 11 and 12. These estimates r(k1) and r(k1) can each be referred to as second component values since these estimates represent values of the second component (e.g., range) that are to be tested as a part of the multi-component error determination process. Once these estimates are obtained, then these estimations can be tested to determine whether they represent accurately estimated error values. The method 600 continues to step 630.
In step 630, a multi-component error hypothesis is tested. The multi-component error hypothesis refers to a hypothesis that the present (i.e., current iteration) estimated error values far multiple components (e.g., azimuth angle and range) are accurate (or within a predetermined threshold). In one embodiment, the multi-component tested velocity vector ST(k1) is obtained using the estimated values that were determined above: ϕ(k1), {dot over (ϕ)}(k1), r(k1), and {dot over (r)}(k1). The multi-component tested velocity vector ST(k1) can be considered an estimate of the monitored sensor velocity vector Sm(k1). Then, the difference (or the multi-component velocity vector error) between the monitored sensor velocity vector Sm(k1) and the multi-component tested velocity vector ST(k1) (derived from Equation 7) can be obtained using S(k1)−Sm(k1)=eS. Once the multi-component velocity vector error eS is calculated, then this value can be compared with a vector velocity tolerance limit e1L. The method continues to step 640. In step 640, it is determined whether the multi-component velocity vector error is less than a multi-component velocity vector error threshold (e.g., e1L). If it is determined that the multi-component velocity vector error is less than a multi component velocity vector error threshold e1L the method 600 continues ends (and the method 300 continues to step 345). Otherwise, the method 600 continues to step 610 where the steps 610 through 660 can be repeated continuously until eS<e1L. In other embodiments, the steps above can be repeated continuously until eS≤e1L. Once this stopping condition is met, then the latest calculations, of the estimates for r1 and ϕ1 are now considered to be sufficiently accurate estimates of the correct r1 and ϕ1. These sufficiently accurate estimates can be termed {circumflex over (r)}1L for the sufficiently accurate range estimate and {circumflex over (ϕ)}1L for the sufficiently accurate heading angle estimate. The multi-component velocity vector error eS of the latest (or last) iteration of the steps 610-660 can be used as the vehicle velocity vector error as discussed below in step 345.
In step 345, a remedial vehicle action is carried out in response to the determined vehicle velocity vector error. Step 345 can be reached in three different ways: from step 325 where an error in only the first vector component of the monitored sensor velocity vector is detected; from step 335 where an error in only the second vector component of the monitored sensor velocity vector is detected; or from step 340 where an error in more than one of the vector components of the monitored sensor velocity vector is detected (e.g., the first and second vector component of the monitored sensor velocity vector). The respective error as tested in step 320, step 330, or step 340 can be considered the vehicle velocity vector error. This vehicle velocity vector error can then be communicated to one or more vehicle modules, such as the other system sensors 14, wireless communications module 30, a user interface 28, etc. In one embodiment, the vehicle velocity vector error is stored in a memory device of the vehicle, such as a memory device operatively connected to the DSP and/or the SP & Decision logic controller 140.
A remedial vehicle action can refer to any vehicle action that is performed in order to address the determined vehicle velocity vector error. For example, modifying parameters used in the lidar/radar unit 112 based on the vehicle velocity vector error can be considered a remedial vehicle action. In this example, one or more parameters that are used in obtaining environmental information using a lidar unit and/or a radar unit can be modified based on the vehicle velocity vector error. For example, if the vehicle velocity vector error reflects an azimuth error of 1°, the remedial vehicle action can include storing lidar adjustment information that results in azimuth angle measurements from the lidar unit being adjusted by the azimuth error of 1°. Other remedial vehicle actions include using the wireless communications module 30 to communicate the vehicle velocity vector error to a backend facility, such as a backend vehicle services facility that is located remotely from the vehicle and accessible via a cellular carrier system, for example. The method 300 then ends.
It is to be understood that the foregoing description is of one or more embodiments of the invention. The invention is not limited to the particular embodiment(s) disclosed herein, but rather is defined solely by the claims below. Furthermore, the statements contained in the foregoing description relate to the disclosed embodiments) and are not to be construed as limitations on the scope of the invention or on the definition of terms used in the claims, except where a term or phrase is expressly defined above. Various other embodiments and various changes and modifications to the disclosed embodiment(s) will become apparent to those skilled in the art.
As used in this specification and claims, the terms “e.g.,” “for example,” “for instance,” “such as,” and “like,” and the verbs “comprising,” “having,” “including,” and their other verb forms, when used in conjunction with a listing of one or more elements or other items, are each to be construed as open-ended, meaning that the listing is not to be considered as excluding other, additional elements or items. Other terms are to be construed using their broadest reasonable meaning unless they are used in a context that requires a different interpretation. In addition, the term “and/or” is to be construed as an inclusive OR. Therefore, for example, the phrase “A, B, and/or C” is to be interpreted as covering all of the following: “A”; “B”; “C”; “A and B”; “A and C”; “B and C”; and “A, B, and C.”
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/US2018/046715 | 8/14/2018 | WO | 00 |
Publishing Document | Publishing Date | Country | Kind |
---|---|---|---|
WO2019/036475 | 2/21/2019 | WO | A |
Number | Name | Date | Kind |
---|---|---|---|
5050427 | Cote et al. | Sep 1991 | A |
20070067078 | Salman et al. | Mar 2007 | A1 |
20080186223 | Mayer | Aug 2008 | A1 |
20100295492 | Chakrabarti et al. | Nov 2010 | A1 |
Number | Date | Country |
---|---|---|
2006071473 | Mar 2006 | JP |
20050100990 | Oct 2005 | KR |
Entry |
---|
International Search Report corresponding to International Application No. PCT/US2018/046715, dated Nov. 29, 2018, 3 pages. |
Written Opinion corresponding to International Application No. PCT/US2018/046715, dated Nov. 29, 2018, 3 pages. |
Number | Date | Country | |
---|---|---|---|
20200198650 A1 | Jun 2020 | US |
Number | Date | Country | |
---|---|---|---|
62605429 | Aug 2017 | US |