Abnormality diagnosing apparatus, abnormality diagnosing method, and computer program

Abstract

The present application aims to improve the accuracy of abnormality diagnosis of self-position estimation of a vehicle. An abnormality diagnosis device includes: a self-position estimation unit that calculates a change in a self-estimated position of a vehicle based on an output of a GNSS receiver provided on the vehicle; a first movement amount calculation unit that calculates a movement amount of the vehicle based on an output of a LiDAR provided on the vehicle; a second movement amount calculation unit that calculates a movement amount of the vehicle based on an output of a camera provided on the vehicle; and an abnormality detection unit that detects an abnormality in self-position estimation of the vehicle. The first movement amount calculation unit calculates a lateral movement amount of the vehicle, and the second movement amount calculation unit calculates a longitudinal movement amount of the vehicle. The abnormality detection unit detects an abnormality in self-position estimation of the vehicle by comparing the change in the self-estimated position with an estimated movement amount of the vehicle obtained from the lateral movement amount and the longitudinal movement amount.

Description

Technical Field

[0001] This invention relates to an abnormality diagnosis device, an abnormality diagnosis method, and a computer program. Background Technology

[0002] Patent document 1 describes the following: a front LiDAR and a rear LiDAR are installed on a vehicle such as a dump truck, and the outputs of the front LiDAR and the rear LiDAR are used to detect whether there is any abnormality in the vehicle's own position calculated by the self-position estimation device based on GPS positioning data.

[0003] Patent Document 1: Japanese Patent No. 6671152 Summary of the Invention

[0004] In the aforementioned technology, anomaly diagnosis of self-position estimation is based on whether the shoulder feature point detected by the output of the rear LiDAR is located on the trajectory of the shoulder feature point detected by the output of the front LiDAR. In this case, even if anomalies in the lateral self-position estimation of the vehicle can be detected based on the amount of deviation from the trajectory, it is difficult to detect anomalies in the longitudinal (travel direction) self-position estimation of the vehicle.

[0005] Therefore, in view of the above-mentioned issues, the object of the present invention is to improve the accuracy of abnormal diagnosis of vehicle self-position estimation.

[0006] The main points of this invention are as follows.

[0007] (1) An anomaly diagnosis device comprising: a self-position estimation unit that calculates a change in the self-estimated position of the vehicle based on the output of a GNSS receiver installed on the vehicle; a first movement calculation unit that calculates the movement of the vehicle based on the output of a LiDAR installed on the vehicle; a second movement calculation unit that calculates the movement of the vehicle based on the output of a camera installed on the vehicle; and an anomaly detection unit that detects an anomaly in the self-position estimation of the vehicle, wherein the first movement calculation unit calculates a lateral movement of the vehicle, the second movement calculation unit calculates a longitudinal movement of the vehicle, and the anomaly detection unit detects the anomaly in the self-position estimation of the vehicle by comparing the change in the self-estimated position with the estimated movement of the vehicle obtained based on the lateral movement and the longitudinal movement.

[0008] (2) According to the abnormal diagnosis device described in (1) above, the first motion calculation unit calculates the motion of the vehicle based on the output of the LiDAR and the output of the IMU installed on the vehicle, and the second motion calculation unit calculates the motion of the vehicle based on the output of the camera and the output of the IMU.

[0009] (3) According to the abnormality diagnosis device described in (1) or (2) above, wherein the first movement amount calculation unit calculates the lateral movement amount and the longitudinal movement amount, and when the longitudinal movement amount calculated by the first movement amount calculation unit is less than a predetermined value, the abnormality detection unit detects the abnormality by comparing the change in its own estimated position with the estimated movement amount of the vehicle obtained from the lateral movement amount calculated by the first movement amount calculation unit and the longitudinal movement amount calculated by the second movement amount calculation unit, and when the longitudinal movement amount calculated by the first movement amount calculation unit is greater than or equal to the predetermined value, the abnormality detection unit detects the abnormality by comparing the change in its own estimated position with the estimated movement amount of the vehicle obtained from the lateral movement amount and the longitudinal movement amount calculated by the first movement amount calculation unit.

[0010] (4) An anomaly diagnosis method, executed by a computer, the anomaly diagnosis method comprising the following steps: calculating the change in the vehicle's estimated position based on the output of a GNSS receiver installed on the vehicle; calculating the lateral movement of the vehicle based on the output of a LiDAR installed on the vehicle; calculating the longitudinal movement of the vehicle based on the output of a camera installed on the vehicle; and detecting anomalies in the estimated position of the vehicle by comparing the change in its estimated position with the estimated movement of the vehicle obtained based on the lateral movement and the longitudinal movement.

[0011] (5) A computer program that causes a computer to perform the following steps: calculating a change in the estimated position of the vehicle based on the output of a GNSS receiver mounted on the vehicle; calculating a lateral movement of the vehicle based on the output of a LiDAR mounted on the vehicle; calculating a longitudinal movement of the vehicle based on the output of a camera mounted on the vehicle; and detecting an anomaly in the estimated position of the vehicle by comparing the change in the estimated position with the estimated movement of the vehicle obtained based on the lateral movement and the longitudinal movement.

[0012] Invention Effects

[0013] According to the present invention, the accuracy of abnormal diagnosis of vehicle self-position estimation can be improved. Attached Figure Description

[0014] Figure 1 This is a schematic structural diagram of an abnormality diagnosis system including the abnormality diagnosis device according to the first embodiment of the present invention.

[0015] Figure 2 This is a functional block diagram of the ECU's processor.

[0016] Figure 3 It is a diagram that roughly represents the state of deviation in the estimated position of the vehicle.

[0017] Figure 4 This is a flowchart representing the control routines related to the abnormality diagnosis and handling in the first embodiment.

[0018] Figure 5 This is a flowchart representing the control routines related to the abnormality diagnosis and handling in the second embodiment.

[0019] Figure 6 This is a flowchart representing the control routines related to the abnormality diagnosis and handling in the third embodiment. Detailed Implementation

[0020] Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings. Furthermore, in the following description, the same reference numerals are used to denote the same constituent elements.

[0021] <First Implementation Method>

[0022] The following is for reference. Figures 1-4 The first embodiment of the present invention will be described. Figure 1 This is a schematic structural diagram of an anomaly diagnosis system 100, which includes the anomaly diagnosis device according to the first embodiment of the present invention. The anomaly diagnosis system 100 is mounted on a vehicle 1 and performs anomaly diagnosis to estimate the position of the vehicle 1. In this embodiment, the vehicle 1 is a four-wheeled vehicle.

[0023] like Figure 1 As shown, the anomaly diagnostic system 100 includes a GNSS receiver 2, a Laser Imaging Detection and Ranging (LiDAR) device 3, a camera 4, an Inertial Measurement Unit (IMU) 5, a vehicle speed sensor 6, an actuator 7, a map database 8, an output device 9, and an Electronic Control Unit (ECU) 10. Specifically, the GNSS receiver 2, LiDAR 3, camera 4, IMU 5, vehicle speed sensor 6, actuator 7, map database 8, output device 9, and ECU 10 are installed in the vehicle 1. The GNSS receiver 2, LiDAR 3, camera 4, IMU 5, vehicle speed sensor 6, actuator 7, map database 8, and output device 9 are electrically connected to the ECU 10 via an in-vehicle network conforming to standards such as Controller Area Network (CAN) or Ethernet.

[0024] GNSS receiver 2 measures the position of vehicle 1 and generates its own position data. Specifically, GNSS receiver 2 detects the current position of vehicle 1 (e.g., latitude, longitude, and altitude of vehicle 1) based on positioning information obtained from multiple (e.g., more than three) positioning satellites. A specific example of a GNSS receiver is a Global Positioning System (GPS) receiver. The output of GNSS receiver 2, i.e., the current position of vehicle 1 detected by GNSS receiver 2, is sent to ECU 10.

[0025] The LiDAR3 illuminates the perimeter of vehicle 1 with a laser beam and receives the reflected light. Based on the reflected light, the LiDAR3 generates point cloud data representing the surrounding terrain features of vehicle 1. The point cloud data includes information such as the distance to each reflecting point, reflection intensity, and illumination angle. For example, the LiDAR3 is positioned at the front of vehicle 1 (e.g., the front bumper) to detect terrain features in front of and to the sides of vehicle 1. The output of the LiDAR3, i.e., the point cloud data generated by the LiDAR3, is sent to ECU 10.

[0026] Camera 4 captures images of the surroundings of vehicle 1 to generate images of the vehicle's perimeter. For example, camera 4 may be positioned at the front of vehicle 1 (e.g., on the front bumper, behind a rearview mirror inside the vehicle interior, etc.) to generate images of the front and front sides of vehicle 1. Camera 4 can be a monocular camera or a stereo camera. The output of camera 4, i.e., the peripheral images generated by camera 4, is sent to ECU 10.

[0027] To detect the dynamic behavior of vehicle 1, IMU5 includes a 3-axis gyroscope sensor for detecting angular velocities in three axes and a 3-axis accelerometer sensor for detecting accelerations in three axes. This structure allows IMU5 to sample the attitude changes, rotational movements, and acceleration / deceleration of vehicle 1 at a high frequency. For example, IMU5 is positioned near the center of gravity of vehicle 1. The output of IMU5, namely the angular velocities and accelerations detected by IMU5 in the three axes, is sent to ECU 10.

[0028] Vehicle speed sensor 6 detects the speed of vehicle 1. For example, vehicle speed sensor 6 detects the speed of vehicle 1 by detecting the rotational speed of the wheels of vehicle 1. The output of vehicle speed sensor 6, that is, the speed of vehicle 1 detected by vehicle speed sensor 6, is sent to ECU 10.

[0029] Actuator 7 causes vehicle 1 to move according to instructions from ECU 10. The actuator includes a drive actuator, a brake actuator, and a steering actuator. The drive actuator controls the acceleration of vehicle 1 via a drive unit of vehicle 1 (e.g., an internal combustion engine, an electric motor, or a hybrid configuration of both). The brake actuator controls the braking of vehicle 1 via the braking system of vehicle 1. The steering actuator controls the steering of vehicle 1 via the steering system of vehicle 1. In this embodiment, ECU 10 uses actuator 7 to control the behavior of vehicle 1 (e.g., acceleration, braking, and steering of vehicle 1). That is, vehicle 1 is an automated driving vehicle in which the acceleration, braking, and steering of vehicle 1 are automatically controlled.

[0030] Map information is stored in map database 8. This map information includes, for example, road location information, road shape information (e.g., types of curves and straight sections, curve curvature, road slope, etc.), road categories, speed limits, and other road information. Furthermore, map database 8 can be located externally to vehicle 1 (e.g., as a server), and ECU 10 can obtain map information from the outside of vehicle 1.

[0031] Output device 9 notifies the occupants of vehicle 1 (e.g., the driver). Output device 9 includes at least one of a display, warning light, speaker, buzzer, and vibration unit. Output device 9 notifies the occupants of vehicle 1 of an output corresponding to a signal sent from ECU 10.

[0032] ECU10 performs various controls on vehicle 1. For example... Figure 1 As shown, the ECU 10 includes a communication interface 11, a memory 12, and a processor 13. The communication interface 11 and the memory 12 are connected to the processor 13 via signal lines. In this embodiment, only one ECU 10 is provided, but multiple ECUs may be provided for each function. Furthermore, the communication interface 11, the memory 12, and the processor 13 can be configured as a single integrated circuit, or they can be configured as separate circuits.

[0033] The communication interface 11 has interface circuitry for connecting the ECU 10 to the in-vehicle network. The ECU 10 connects to other in-vehicle devices via the communication interface 11. The communication interface 11 transmits signals received from the GNSS receiver 2, LiDAR 3, camera 4, IMU 5, vehicle speed sensor 6, and map database 8 to the processor 13. Furthermore, the communication interface 11 transmits signals output from the processor 13 to the actuator 7 and output device 9.

[0034] The memory 12 may include, for example, volatile semiconductor memory (e.g., Dynamic Random Access Memory (DRAM), Static Random Access Memory (SRAM), etc.) and non-volatile semiconductor memory (e.g., Read Only Memory (ROM), Electrically Erasable Programmable Read Only Memory (EEPROM), Flash memory, etc.). The memory 12 stores temporary data, computer programs (control programs for ECU 10) for various processes of the processor 13, ECU 10 setting data, log data, vehicle information, etc.

[0035] Processor 13 has one or more central processing units (CPUs) and their peripheral circuitry. Processor 13 executes computer programs stored in memory 12. Additionally, processor 13 may also have other arithmetic circuitry such as logic units, numerical processing units, or graphics processing units.

[0036] In this embodiment, ECU10 functions as an anomaly diagnostic device for diagnosing anomalies in estimating the self-position of vehicle 1. Furthermore, ECU10 is an example of an anomaly diagnostic device.

[0037] Figure 2 This is a functional block diagram of processor 13 in ECU10. (Example) Figure 2 As shown, the processor 13 includes a self-position estimation unit 14, a first movement calculation unit 15, a second movement calculation unit 16, and an anomaly detection unit 17. The self-position estimation unit 14, the first movement calculation unit 15, the second movement calculation unit 16, and the anomaly detection unit 17 are functional modules implemented by the processor 13 of the ECU 10 executing a computer program stored in the memory 12 of the ECU 10. Furthermore, these functional modules can each be implemented by dedicated arithmetic circuits provided in the processor 13.

[0038] The self-position estimation unit 14 estimates the self-position of vehicle 1. In this embodiment, the self-position estimation unit 14 obtains the self-estimated position of vehicle 1 based on the output of GNSS receiver 2. In this embodiment, the self-estimated position of vehicle 1 is used for the control of autonomous driving of vehicle 1. For example, ECU 10 generates the driving path of vehicle 1 based on the self-estimated position of vehicle 1.

[0039] Figure 3 This is a diagram that roughly represents the state of deviation in the estimated position of vehicle 1. Figure 3 In the diagram, the actual positions of vehicle 1 at time t and time t+1 are represented by solid lines, while the estimated position of vehicle 1 at time t+1 is represented by dashed lines. Figure 3 In the example, at time t+1, there is a deviation between the actual position of vehicle 1 and its estimated position. This deviation is caused by the effects of multipath, clock error, radio interference, etc.

[0040] If a deviation occurs between the actual position and the estimated position, it becomes difficult to continue autonomous driving of vehicle 1. Therefore, it is preferable to be able to diagnose anomalies in the estimated position of vehicle 1. For example, anomaly diagnosis of the estimated position can be performed by comparing the change in the estimated position of vehicle 1 calculated based on GNSS receiver 2 with the amount of movement of vehicle 1 calculated based on LiDAR 3.

[0041] However, in the point cloud data generated by LiDAR3, the longitudinal resolution of vehicle 1 is typically lower than its lateral resolution. Furthermore, unlike camera 4, LiDAR3 cannot acquire detailed texture information. Therefore, in the above method, even if the lateral deviation of vehicle 1's self-estimated position can be detected, it is difficult to detect the longitudinal deviation of vehicle 1's self-estimated position. Only terrain features with minimal shape changes along the longitudinal direction of vehicle 1 exist around vehicle 1 (e.g., road shoulders). Figure 3 This is especially evident in cases involving riverbanks, etc. Furthermore, in this specification, the longitudinal direction of vehicle 1 refers to the direction of travel of vehicle 1. Figure 3 (The vertical direction in the text) The lateral direction of vehicle 1 refers to the direction perpendicular to the direction of travel of vehicle 1. Figure 3 (Left and right directions in the middle).

[0042] In view of the above, in this embodiment, not only LiDAR 3 but also camera 4 is used to calculate the movement of vehicle 1, thereby performing anomaly diagnosis for vehicle 1's self-position estimation. This improves the accuracy of anomaly diagnosis for vehicle 1's self-position estimation. The specific control method will be described below.

[0043] The self-position estimation unit 14 calculates the change in the self-estimated position of vehicle 1 (hereinafter referred to as "change in self-estimated position") based on the output of GNSS receiver 2. The self-position estimation unit 14 calculates the change in its self-estimated position within a specified time interval. For example, the self-position estimation unit 14 obtains the self-estimated position at time t=1 based on the output of GNSS receiver 2, and calculates the difference between these self-estimated positions as the change in its self-estimated position.

[0044] On the other hand, the first motion calculation unit 15 calculates the motion of vehicle 1 based on the output of LiDAR 3, and the second motion calculation unit 16 calculates the motion of vehicle 1 based on the output of camera 4. In this case, the first motion calculation unit 15 is also referred to as the LiDAR odometer calculation unit, and the second motion calculation unit 16 is also referred to as the camera odometer calculation unit.

[0045] In particular, in this embodiment, the first movement calculation unit 15 calculates the lateral movement of the vehicle 1 based on the output of the LiDAR 3, and the second movement calculation unit 16 calculates the longitudinal movement of the vehicle 1 based on the output of the camera 4. By using both the LiDAR 3 and the camera 4, the shortcomings of each can be compensated, thereby enabling high-precision calculation of the longitudinal and lateral movement of the vehicle 1.

[0046] The first movement calculation unit 15 calculates the lateral movement of vehicle 1 within a predetermined time interval. For example, the first movement calculation unit 15 calculates the lateral movement of vehicle 1 between time t and time t+1. The second movement calculation unit 16 calculates the longitudinal movement of vehicle 1 within a predetermined time interval. For example, the second movement calculation unit 16 calculates the longitudinal movement of vehicle 1 between time t and time t+1.

[0047] The anomaly detection unit 17 detects anomalies in the estimated self-position of vehicle 1 by comparing the change in its estimated self-position calculated by the self-position estimation unit 14 with the estimated movement of vehicle 1 obtained from the lateral movement of vehicle 1 calculated by the first movement amount calculation unit 15 and the longitudinal movement of vehicle 1 calculated by the second movement amount calculation unit 16. For example, the anomaly detection unit 17 determines that the estimated self-position of vehicle 1 is abnormal when the difference between the change in its estimated self-position and the estimated movement of vehicle 1 is above a predetermined threshold, and determines that the estimated self-position of vehicle 1 is normal when the difference is below the threshold.

[0048] The following is for reference. Figure 4 The flowchart illustrates the processing flow used to perform the above controls. Figure 4 This is a flowchart illustrating the control routine related to the abnormality diagnosis processing in the first embodiment. This control routine is executed repeatedly by the processor 13 of the ECU 10 at predetermined execution intervals, for example, according to a computer program stored in the memory 12 of the ECU 10.

[0049] First, in step S101, the self-position estimation unit 14 of the processor 13 uses the GNSS receiver 2 to estimate the self-position of the vehicle 1. Specifically, the self-position estimation unit 14 obtains the estimated self-position of the vehicle 1 based on the output of the GNSS receiver 2.

[0050] Next, in step S102, the self-position estimation unit 14 calculates the change in the self-estimated position of vehicle 1 within a predetermined time interval. For example, the predetermined time interval is the execution interval of this control routine, and the self-position estimation unit 14 calculates the difference between the self-estimated position obtained in step S101 of the current control routine and the self-estimated position obtained in step S101 of the previous control routine as the change in the self-estimated position.

[0051] Next, in step S103, the first motion calculation unit 15 of the processor 13 calculates the lateral movement of the vehicle 1 within the aforementioned predetermined time interval based on the output of the LiDAR3. When the predetermined time interval is the time interval between time t and time t+1, the first motion calculation unit 15 calculates the lateral movement of the vehicle 1 by comparing the point cloud data generated by the LiDAR3 at time t+1 with the point cloud data generated by the LiDAR3 at time t+1. The point cloud data at time t+1 is the point cloud data from the previous frame, and the point cloud data at time t+1 is the point cloud data from the current frame.

[0052] For example, the first motion calculation unit 15 uses LiDAR odometry to calculate the lateral movement of vehicle 1. Specifically, the first motion calculation unit 15 calculates the lateral movement of vehicle 1 by matching feature points of point cloud data between consecutive frames to calculate the coordinate transformation parameters (e.g., coordinate transformation matrix) of the two point cloud data. When calculating the coordinate transformation parameters, the first motion calculation unit 15 can use known algorithms such as Iterative Closest Point (ICP) or Normal Distributions Transform (NDT). Furthermore, to remove outliers or noise from the point cloud data, the first motion calculation unit 15 can use known methods such as Random Sample Consensus (RANSAC) or Statistical Outlier Removal (SOR).

[0053] Next, in step S104, the second motion calculation unit 16 of the processor 13 calculates the longitudinal movement of the vehicle 1 within the aforementioned predetermined time interval based on the output of the camera 4. When the predetermined time interval is the time interval between time t and time t+1, the second motion calculation unit 16 calculates the longitudinal movement of the vehicle 1 by comparing the image generated by the camera 4 at time t+1 with the image generated by the camera 4 at time t+1. The image at time t is the image from the previous frame, and the image at time t+1 is the image from the current frame.

[0054] For example, the second motion calculation unit 16 uses visual odometry to calculate the longitudinal motion of vehicle 1. Specifically, the second motion calculation unit 16 calculates the longitudinal motion of vehicle 1 by estimating the self-motion of camera 4 by matching feature points of images between consecutive frames. When matching feature points of two images, the second motion calculation unit 16 can use known methods such as nearest neighbor search or k-nearest neighbor search. Furthermore, when estimating the self-motion of camera 4, the second motion calculation unit 16 can use known methods such as estimation of the essential matrix or estimation of the homography matrix.

[0055] Next, in step S105, the anomaly detection unit 17 of the processor 13 determines whether the difference between the change in its own estimated position and the estimated movement of vehicle 1 obtained from the lateral and longitudinal movements of vehicle 1 is above a predetermined threshold. The threshold is predetermined considering factors such as the desired accuracy of the vehicle 1's own position estimation. For example, if at least one of the differences between the lateral movement of its own estimated position and the lateral movement of vehicle 1, and the differences between the longitudinal movement of its own estimated position and the longitudinal movement of vehicle 1, is above the threshold, the anomaly detection unit 17 determines that the difference between the change in its own estimated position and the estimated movement of vehicle 1 is above the threshold. Furthermore, if both differences are above the threshold, the anomaly detection unit 17 can determine that the difference between the change in its own estimated position and the estimated movement of vehicle 1 is above the threshold. Moreover, the anomaly detection unit 17 can calculate the change in its own estimated position and the estimated movement of vehicle 1 as vectors, respectively, and compare these vectors to determine whether the difference between the change in its own estimated position and the estimated movement of vehicle 1 is above the threshold.

[0056] If, in step S105, the difference between the estimated change in the vehicle's own position and the estimated movement of vehicle 1 is determined to be above a threshold, this control routine proceeds to step S106. In step S106, the anomaly detection unit 17 determines that the estimated position of vehicle 1 is abnormal. That is, the anomaly detection unit 17 detects an anomaly in the estimated position of vehicle 1. In this case, for example, the anomaly detection unit 17 notifies the occupants of vehicle 1 of the anomaly via output device 9 and uses actuator 7 to stop vehicle 1 at a safe location such as a road shoulder. Additionally, the anomaly detection unit 17 may request the occupants of vehicle 1 to switch from automatic driving to manual driving (so-called handover) via output device 9. After step S106, this control routine ends.

[0057] On the other hand, if in step S105 it is determined that the difference between the change in the estimated position of the vehicle and the estimated movement of vehicle 1 is less than a threshold, this control routine proceeds to step S107. In step S107, the anomaly detection unit 17 determines that the estimated position of vehicle 1 is normal. After step S107, this control routine ends.

[0058] In addition, in step S101, the self-position estimation unit 14 can use at least one of the IMU5 and the vehicle speed sensor 6, in addition to the GNSS receiver 2, to estimate the self-position of the vehicle 1.

[0059] <Second Implementation Method>

[0060] The structure and control of the anomaly diagnostic device according to the second embodiment are basically the same as those of the anomaly diagnostic device according to the first embodiment, except for the points described below. Therefore, the second embodiment of the present invention will be described below focusing on the parts that differ from the first embodiment.

[0061] In the second embodiment, the first motion calculation unit 15 calculates the motion of vehicle 1 based on the output of LiDAR 3 and IMU 5, and the second motion calculation unit 16 calculates the motion of vehicle 1 based on the output of camera 4 and IMU 5. This allows for more accurate calculation of the motion of vehicle 1, thereby further improving the accuracy of anomaly diagnosis for vehicle 1's self-position estimation. In the second embodiment, the first motion calculation unit 15 is also referred to as a LiDAR inertial odometry calculation unit, and the second motion calculation unit 16 is also referred to as a camera inertial odometry calculation unit.

[0062] Figure 5 This is a flowchart illustrating the control routine related to the abnormality diagnosis processing in the second embodiment. This control routine is executed repeatedly by the processor 13 of the ECU 10 at predetermined execution intervals, for example, according to a computer program stored in the memory 12 of the ECU 10.

[0063] Steps S201 and S202 and Figure 4 Steps S101 and S102 are performed in the same way.

[0064] Following step S202, in step S203, the first motion calculation unit 15 of the processor 13 calculates the lateral movement of vehicle 1 within a predetermined time interval (e.g., the time interval between time t and time t+1) based on the outputs of LiDAR 3 and IMU 5. For example, the first motion calculation unit 15 uses LiDAR odometry to calculate the lateral movement of vehicle 1. That is, the first motion calculation unit 15 integrates the outputs of LiDAR 3 and IMU 5 to calculate the lateral movement of vehicle 1.

[0065] For example, the first mobility calculation unit 15 integrates the outputs of LiDAR3 and IMU5 using a loosely coupled method. In this case, the first mobility calculation unit 15 independently calculates the estimated values ​​based on the outputs of LiDAR3 and IMU5, respectively, and integrates them in a post-filter (e.g., a Kalman filter) to calculate the final estimated value. Alternatively, the first mobility calculation unit 15 can integrate the outputs of LiDAR3 and IMU5 using a tightly coupled method. In this case, the first mobility calculation unit 15 incorporates the outputs of LiDAR3 and IMU5 into the same optimization problem or state estimation framework (e.g., nonlinear optimization, factor graph, extended Kalman filter, particle filter, etc.) to calculate the estimated value.

[0066] Next, in step S204, the second motion calculation unit 16 of the processor 13 calculates the longitudinal motion of vehicle 1 within a predetermined time interval (e.g., the time interval between time t+1) based on the output of camera 4 and IMU 5. For example, the second motion calculation unit 16 uses visual inertial odometry to calculate the longitudinal motion of vehicle 1. That is, the second motion calculation unit 16 integrates the output of camera 4 and the output of IMU 5 to calculate the longitudinal motion of vehicle 1. For example, the second motion calculation unit 16 integrates the output of camera 4 and the output of IMU 5 through extended Kalman filter or nonlinear optimization.

[0067] After step S204, steps S205 to S207 and Figure 4 Steps S105 to S107 are performed in the same manner.

[0068] <Third Implementation Method>

[0069] The structure and control of the anomaly diagnostic device according to the third embodiment are basically the same as those of the anomaly diagnostic device according to the first embodiment, except for the points described below. Therefore, the following description of the third embodiment of the present invention will focus on the parts that differ from the first embodiment.

[0070] In the third embodiment, the first movement calculation unit 15 calculates the lateral and longitudinal movement of vehicle 1 based on the output of LiDAR 3. Furthermore, when the longitudinal movement calculated by the first movement calculation unit 15 is less than a predetermined value, the anomaly detection unit 17 detects an anomaly in the vehicle 1's self-estimated position by comparing the change in its self-estimated position calculated by the self-position estimation unit 14 with the estimated movement of vehicle 1 obtained from the lateral movement calculated by the first movement calculation unit 15 and the longitudinal movement calculated by the second movement calculation unit 16. Conversely, when the longitudinal movement calculated by the first movement calculation unit 15 is greater than or equal to a predetermined value, the anomaly detection unit 17 detects an anomaly in the vehicle 1's self-estimated position by comparing the change in its self-estimated position calculated by the self-position estimation unit 14 with the estimated movement of vehicle 1 obtained from the lateral and longitudinal movement calculated by the first movement calculation unit 15. Thus, anomaly diagnosis of the vehicle 1's self-position estimation can be performed effectively and with high precision using LiDAR 3 and camera 4.

[0071] Figure 6 This is a flowchart illustrating the control routine related to the abnormality diagnosis processing in the third embodiment. This control routine is executed repeatedly by the processor 13 of the ECU 10 at predetermined execution intervals, for example, according to a computer program stored in the memory 12 of the ECU 10.

[0072] Steps S301 and S302 and Figure 4 Steps S101 and S102 are performed in the same way.

[0073] Following step S302, in step S303, the first movement calculation unit 15 of processor 13 calculates the lateral and longitudinal movement of vehicle 1 within a predetermined time interval (e.g., the time interval between time t+1) based on the output of LiDAR3. The method for calculating the longitudinal movement is related to... Figure 4 The calculation method for the lateral movement amount in step S103 is the same as described above.

[0074] Next, in step S304, the anomaly detection unit 17 of the processor 13 determines whether the longitudinal movement of the vehicle 1 calculated by the first movement calculation unit 15 is above a predetermined value. If it is determined that the longitudinal movement of the vehicle 1 is less than the predetermined value, this control routine proceeds to step S305. Steps S305 to S308 are related to... Figure 4 Steps S104 to S107 are performed in the same manner.

[0075] On the other hand, if it is determined in step S304 that the longitudinal movement of vehicle 1 is above a predetermined value, this control routine proceeds to step S306. In step S306, the anomaly detection unit 17 determines whether the difference between the change in its own estimated position calculated by the self-position estimation unit 14 and the estimated movement of vehicle 1 obtained by calculating the lateral and longitudinal movement amounts calculated by the first movement amount calculation unit 15 is above a predetermined threshold. The determination method is related to... Figure 4 Step S105 is the same as the method described above.

[0076] If, in step S306, the difference between the change in its estimated position and the estimated movement of vehicle 1 is above a threshold, this control routine proceeds to step S307. In step S307, with... Figure 4 Similarly, in step S106, the anomaly detection unit 17 determines that the vehicle 1's self-position is presumed to be abnormal. After step S307, this control routine ends.

[0077] On the other hand, if in step S306 it is determined that the difference between the change in the estimated position of the vehicle and the estimated movement of vehicle 1 is less than a threshold, this control routine proceeds to step S308. In step S308, the anomaly detection unit 17 determines that the estimated position of vehicle 1 is normal. After step S308, this control routine ends.

[0078] <Other Implementation Methods>

[0079] The preferred embodiments of the present invention have been described above, but the present invention is not limited to these embodiments, and various modifications and changes can be implemented within the scope of the claims.

[0080] For example, a server located outside the vehicle 1 and capable of communicating with the vehicle 1 can function as an anomaly diagnostic device. In this case, the information required to perform the aforementioned control is sent from the vehicle 1 to the server.

[0081] Furthermore, the second and third embodiments can be implemented in combination. In this case, in Figure 6 In step S303 of the control routine for abnormal diagnosis processing, the first movement calculation unit 15 calculates the lateral movement and longitudinal movement of vehicle 1 based on the output of LiDAR 3 and IMU 5. Furthermore, in step S305, the second movement calculation unit 16 calculates the longitudinal movement of vehicle 1 based on the output of camera 4 and IMU 5.

[0082] Furthermore, the computer program that enables the computer to perform the functions of the processor 13 of the ECU 10 or the processor of the server can be provided either stored in a computer-readable recording medium or included in a computer program product. The computer-readable recording medium may be, for example, a magnetic recording medium, an optical recording medium, or a semiconductor memory.

[0083] Symbol Explanation

[0084] 1-Vehicle, 2-GNSS receiver, 3-LiDAR, 4-Camera, 10-Electronic control unit (ECU), 13-Processor, 14-Self-position estimation unit, 15-First movement calculation unit, 16-Second movement calculation unit, 17-Anomaly detection unit.

Claims

1. An abnormality diagnostic device, characterized in that, have: The self-position estimation unit calculates the change in the vehicle's self-estimated position based on the output of a GNSS receiver installed on the vehicle. The first movement calculation unit calculates the movement of the vehicle based on the output of the LiDAR installed on the vehicle. The second movement calculation unit calculates the movement of the vehicle based on the output of a camera installed on the vehicle. and The anomaly detection unit detects anomalies presumed by the vehicle's own position. The first movement calculation unit calculates the lateral movement of the vehicle, and the second movement calculation unit calculates the longitudinal movement of the vehicle. The anomaly detection unit detects anomalies in the vehicle's estimated position by comparing the change in its estimated position with the estimated movement of the vehicle obtained based on the lateral movement and the longitudinal movement.

2. The abnormality diagnosis device according to claim 1, characterized in that, The first movement calculation unit calculates the movement of the vehicle based on the output of the LiDAR and the output of the IMU installed on the vehicle. The second movement calculation unit calculates the movement of the vehicle based on the output of the camera and the output of the IMU.

3. The abnormality diagnosis device according to claim 1 or 2, characterized in that, The first movement calculation unit calculates the lateral movement and the longitudinal movement. When the longitudinal movement calculated by the first movement calculation unit is less than a predetermined value, the anomaly detection unit detects the anomaly by comparing the change in its own estimated position with the estimated movement of the vehicle obtained from the lateral movement calculated by the first movement calculation unit and the longitudinal movement calculated by the second movement calculation unit. When the longitudinal movement calculated by the first movement calculation unit is greater than or equal to the predetermined value, the anomaly detection unit detects the anomaly by comparing the change in its own estimated position with the estimated movement of the vehicle obtained from the lateral movement calculated by the first movement calculation unit and the longitudinal movement.

4. An anomaly diagnosis method, executed by a computer, characterized in that it includes the following steps: The change in the vehicle's estimated position is calculated based on the output of the GNSS receiver installed on the vehicle. The lateral movement of the vehicle is calculated based on the output of a LiDAR installed on the vehicle. The longitudinal movement of the vehicle is calculated based on the output of a camera mounted on the vehicle. and Anomalies in the vehicle's self-position estimation are detected by comparing the change in its self-estimated position with the estimated movement of the vehicle obtained based on the lateral movement and the longitudinal movement.

5. A computer program, characterized in that, Have the computer perform the following steps: The change in the vehicle's estimated position is calculated based on the output of the GNSS receiver installed on the vehicle. The lateral movement of the vehicle is calculated based on the output of a LiDAR installed on the vehicle. The longitudinal movement of the vehicle is calculated based on the output of a camera mounted on the vehicle. and Anomalies in the vehicle's self-position estimation are detected by comparing the change in its self-estimated position with the estimated movement of the vehicle obtained based on the lateral movement and the longitudinal movement.

Images