Azimuth deviation detection device

Abstract

To more accurately detect a deviation in an azimuth.SOLUTION: An azimuth deviation detection device 1 comprises: an acquisition unit 11 that acquires a matching azimuth relating to an azimuth of a target calculated by matching a result of remote sensing performed during self-position estimation with map data, and a movement azimuth relating to an azimuth of the target calculated on the basis of a movement trajectory of the target; and a detection unit 12 that detects a deviation of the matching azimuth on the basis of the matching azimuth and the movement azimuth acquired by the acquisition unit 11. The target may be a vehicle, and the remote sensing may be performed by LiDAR (Light Detection And Ranging) provided on the vehicle. The detection unit 12 may detect the deviation when a difference between the matching azimuth and the movement azimuth satisfies a predetermined criterion.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] One aspect of the present disclosure relates to an orientation deviation detection device that detects an orientation deviation of an object. [Background technology]

[0002] Patent Document 1 listed below discloses a vehicle position estimation device that includes a data acquisition unit that acquires azimuth angle data of the vehicle while the vehicle is traveling at predetermined time intervals, and that acquires the azimuth angle data by matching detection data detected by one detection sensor mounted on the vehicle with map data, and an abnormality degree calculation unit that compares the latest azimuth angle data with multiple azimuth angle data acquired in the past to determine the degree of abnormality of the latest azimuth angle data. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 7392785 Summary of the Invention [Problem to be solved by the invention]

[0004] In the vehicle position estimation device, the latest azimuth angle data acquired by the same method is compared with multiple azimuth angle data acquired in the past. Therefore, if the method itself has some flaw from the past, for example, it is not possible to accurately detect the azimuth deviation due to the flaw. Therefore, it is desired to detect the azimuth deviation more accurately. [Means for solving the problem]

[0005] An orientation deviation detection device according to one aspect of the present disclosure includes an acquisition unit that acquires a matching orientation related to the orientation of an object calculated by matching the results of remote sensing during self-location estimation with map data and a movement orientation related to the orientation of the object calculated based on the movement trajectory of the object, and a detection unit that detects an error in the matching orientation based on the matching orientation and the movement orientation acquired by the acquisition unit. In this aspect, since the detection of an error in the matching orientation is performed based on the matching orientation and the movement orientation, for example, even if there is some kind of defect in the calculation of the matching orientation, the detection of an error in the matching orientation can be performed more accurately. In other words, the detection of an error in the orientation can be performed more accurately.

[0006] In the orientation deviation detection device according to an aspect of the present disclosure, the target may be a vehicle, and the remote sensing may be performed by a LiDAR (Light Detection and Ranging) device provided on the vehicle. In this aspect, for example, a failure of the LiDAR provided on the vehicle can be recognized.

[0007] Furthermore, the detection unit of the orientation deviation detection device according to one aspect of the present disclosure may perform detection when at least one of the traveling direction or speed of the target satisfies a predetermined criterion. In such an aspect, for example, by performing detection when the target is moving straight or traveling at a certain speed, the distance between the trajectories of the movement trajectories becomes longer, and the accuracy of the movement orientation increases, i.e., the detection of orientation deviation can be performed more accurately.

[0008] Furthermore, the detection unit of the orientation deviation detection device according to one aspect of the present disclosure may perform detection when the target is moving in a straight line or when the speed of the target is greater than a predetermined threshold. In such an aspect, for example, by performing detection when the target is moving in a straight line or when the target is moving at a certain speed, the distance between the trajectories of the movement trajectories becomes longer, and the accuracy of the movement orientation increases, i.e., the detection of orientation deviation can be performed more accurately.

[0009] Furthermore, the detection unit of the orientation deviation detection device according to one aspect of the present disclosure may detect the deviation when the difference between the matching orientation and the moving orientation satisfies a predetermined criterion. In such an aspect, the orientation deviation can be detected more accurately based on the difference between the matching orientation and the moving orientation.

[0010] Furthermore, when acquisition is performed continuously by the acquisition unit, the detection unit of the orientation deviation detection device according to one aspect of the present disclosure may calculate the difference between the matching orientation and the moving orientation for each successive acquisition, and perform detection based on the difference calculated for each successive acquisition. In such an aspect, for example, it is possible to more accurately detect orientation deviation without being significantly affected by noise in the difference.

[0011] Furthermore, the detection unit of the orientation deviation detection device according to one aspect of the present disclosure may, when acquisition is continuously performed by the acquisition unit, calculate the difference between the matching orientation and the moving orientation in each successive acquisition, determine whether the difference is greater than a predetermined threshold, and detect a deviation when it is determined that the difference is greater than a predetermined threshold more than a predetermined number of times in the successive acquisitions. In such an aspect, for example, it is possible to more accurately detect orientation deviation without being significantly affected by noise in the difference.

[0012] Furthermore, the detection unit of the orientation deviation detection device according to one aspect of the present disclosure may detect the difference between the matching orientation and the moving orientation based on a value that has been passed through a filter that removes noise. In such an aspect, the orientation deviation can be detected more accurately based on the value from which noise has been removed.

[0013] In the orientation deviation detection device according to the aspect of the present disclosure, the filter may be based on a bilinear transform. In such an aspect, the orientation deviation can be detected more accurately based on the bilinear transform.

[0014] Furthermore, the detection unit of the azimuth deviation detection device according to one aspect of the present disclosure may detect a deviation when the filtered value is greater than a predetermined threshold value. In such an aspect, the detection of the azimuth deviation can be performed more accurately. [Effects of the Invention]

[0015] According to one aspect of the present disclosure, it is possible to more accurately detect deviation in orientation. [Brief explanation of the drawings]

[0016] [Figure 1] FIG. 2 is a diagram illustrating an example of a functional configuration of an orientation deviation detection device according to an embodiment. [Figure 2] 4 is a flowchart illustrating an example of processing executed by an orientation deviation detection device according to an embodiment. [Figure 3] 10 is a flowchart illustrating another example of processing executed by the orientation deviation detection device according to the embodiment. [Figure 4] FIG. 10 is a diagram illustrating an example of a movement trajectory. [Figure 5] 10 is a graph showing an example of raw values ​​of differential orientation. [Figure 6] 6 is a graph showing an example of values ​​after the raw values ​​shown in FIG. 5 have been passed through a filter. [Figure 7] 10 is a graph showing an example of raw values ​​of differential orientations to which an offset has been introduced. [Figure 8] 8 is a graph showing an example of values ​​after the raw values ​​shown in FIG. 7 have been passed through a filter. [Figure 9] This is a graph in which data regarding calculation convergence is added to the graph shown in FIG. 8. DETAILED DESCRIPTION OF THE INVENTION

[0017] Hereinafter, embodiments of the present disclosure will be described in detail with reference to the drawings. In the description of the drawings, the same elements are designated by the same reference numerals, and duplicate explanations will be omitted. Furthermore, the embodiments of the present disclosure in the following description are specific examples of the present invention, and the present invention is not limited to these embodiments unless otherwise specified to limit the present invention.

[0018] 1 is a diagram showing an example of the functional configuration of an orientation deviation detection device 1 (orientation deviation detection device) according to an embodiment. The orientation deviation detection device 1 is a computer device that detects deviation in the orientation of an object.

[0019] The target may be, for example, an object (or may relate to an object). The target may be, for example, a moving object such as a vehicle, an aircraft, a ship, etc. Examples of vehicles include, but are not limited to, bicycles, cars, trucks, buses, and trains.

[0020] The orientation may be the orientation of the target calculated when estimating the target's self-location. Self-location estimation is a technique or method for estimating one's own (current) location. As a method for self-location estimation, scan matching, which matches scan (sensor) data with map data, is assumed, but is not limited to this.

[0021] In this embodiment, it is assumed that the heading deviation detection device 1 is provided in a vehicle (target) and detects a deviation in the target's heading calculated when the vehicle's self-position is estimated, but this is not limiting. In this embodiment, it is assumed that the heading deviation detection device 1 is incorporated as an ECU (Electronic Control Unit) provided in the vehicle, but this is not limiting. The ECU is an electronic control unit having, for example, a CPU (Central Processing Unit), a ROM (Read Only Memory), a RAM (Random Access Memory), and a CAN (Controller Area Network) communication circuit. For example, the ECU loads a program stored in the ROM into the RAM and executes the program loaded in the RAM with the CPU, thereby realizing various functions described below.

[0022] As shown in FIG. 1, the orientation deviation detection device 1 includes a storage unit 10, an acquisition unit 11 (acquisition unit), and a detection unit 12 (detection unit). Each functional block of the orientation deviation detection device 1 is assumed to function within the orientation deviation detection device 1, but this is not limited to this. For example, some of the functional blocks of the orientation deviation detection device 1 may function within a computer device different from the orientation deviation detection device 1 and connected to the orientation deviation detection device 1 via a network, while appropriately transmitting and receiving information to and from the orientation deviation detection device 1. Furthermore, some functional blocks of the orientation deviation detection device 1 may be omitted, multiple functional blocks may be integrated into one functional block, or one functional block may be separated into multiple functional blocks.

[0023] The storage unit 10 stores any information that is used or output during processing of the orientation deviation detection device 1. The storage unit 10 may store information calculated by each function of the orientation deviation detection device 1. The information stored by the storage unit 10 may be referred to by each function of the orientation deviation detection device 1 as appropriate.

[0024] The acquisition unit 11 acquires a matching direction related to the target's direction calculated by matching the results of remote sensing during self-position estimation with map data, and a movement direction related to the target's direction calculated based on the target's movement trajectory.

[0025] Remote sensing is a technique or method for measuring (sensing) the shape and properties of an object from a remote location without touching the object. Remote sensing may be performed by LiDAR (Light Detection and Ranging), but is not limited to this. Remote sensing may also be performed by a LiDAR mounted on a vehicle (object). In this embodiment, it is assumed that remote sensing is performed by a LiDAR mounted on a vehicle. The results of remote sensing may be data (point cloud) measured by the LiDAR.

[0026] The map data may be data of a three-dimensional map made up of a three-dimensional point cloud, or may be data stored in advance by the storage unit 10.

[0027] The matching may be the above-mentioned scan matching. The matching may be performed by NDT (Normal Distributions Transform) to match two point clouds (NDT matching). In this embodiment, the matching is assumed to be NDT matching between a point cloud measured by LiDAR and a point cloud of map data, but is not limited to this.

[0028] The matching orientation may be the orientation of the target calculated by matching, or any information related to the orientation of the target calculated by matching. The matching orientation may be obtained after some filtering process is performed on the orientation of the target calculated by matching.

[0029] The calculation of the orientation of an object based on the movement trajectory of the object is performed using existing technology.

[0030] The movement direction may be the direction of the object calculated based on the movement trajectory of the object, or any information related to the direction of the object calculated based on the movement trajectory of the object. The movement direction may be obtained after some kind of filtering is performed on the direction of the object calculated based on the movement trajectory of the object.

[0031] The acquisition unit 11 may acquire the matching orientation from another device via a network or the like, or may acquire the matching orientation calculated by a calculation unit (not shown) provided in the orientation deviation detection device 1 by matching the results of remote sensing during self-position estimation with map data, or may acquire the matching orientation stored in advance by the storage unit 10.

[0032] The acquisition unit 11 may acquire the movement direction from another device via a network or the like, or may acquire the movement direction calculated by a calculation unit (not shown) provided in the direction deviation detection device 1 based on the movement trajectory of the target, or may acquire the movement direction stored in advance by the storage unit 10.

[0033] It is assumed that the matching direction and the moving direction acquired by the acquisition unit 11 are directions at the same timing or time, but this is not limited to this.

[0034] The acquisition unit 11 may output the acquired matching direction and movement direction to the detection unit 12, or may cause the storage unit 10 to store them.

[0035] The detection unit 12 detects a deviation of the matching orientation based on the matching orientation and the movement orientation acquired (input) by the acquisition unit 11.

[0036] The detection unit 12 may perform detection (of a deviation in the matching orientation) when at least one of the traveling direction or speed of the target satisfies a predetermined criterion. That is, the detection unit 12 may perform detection when the traveling direction of the target satisfies a predetermined criterion, may perform detection when the speed of the target satisfies a predetermined criterion, or may perform detection when both the traveling direction of the target and the speed of the target satisfy the predetermined criterion.

[0037] The detection unit 12 may perform detection (of a deviation in the matching orientation) when the target is moving straight or when the speed of the target is greater than a predetermined threshold. That is, the detection unit 12 may perform detection when the target is moving straight, may perform detection when the speed of the target is greater than a predetermined threshold, or may perform detection when the target is moving straight and the speed of the target is greater than a predetermined threshold.

[0038] The detection unit 12 may detect a deviation (of the matching orientation) when the difference between the matching orientation and the movement orientation satisfies a predetermined criterion.

[0039] When acquisition is continuously performed by the acquisition unit 11, the detection unit 12 may calculate a differential orientation, which is the difference between the matching orientation and the moving orientation, for each consecutive acquisition, and detect (a deviation in the matching orientation) based on the differential orientation calculated for each consecutive acquisition. When acquisition is continuously performed by the acquisition unit 11, the detection unit 12 may calculate a differential orientation, which is the difference between the matching orientation and the moving orientation, for each consecutive acquisition, and determine whether the differential orientation is greater than a predetermined threshold, and detect a deviation (in the matching orientation) if it is determined that the differential orientation is greater than a predetermined number of times in each consecutive acquisition.

[0040] The detection unit 12 may detect (the deviation of the matching orientation) based on a value obtained by passing a differential orientation, which is the difference between the matching orientation and the moving orientation, through a filter that removes noise. The filter may be based on bilinear transformation. That is, the detection unit 12 may detect (the deviation of the matching orientation) based on a value obtained by passing the differential orientation through a filter that removes noise (or vibration noise specific to large vehicles) and is based on bilinear transformation. The detection unit 12 may detect a deviation (of the matching orientation) when the value passed through the filter is greater than a predetermined threshold.

[0041] The details of the processing by the detection unit 12 and the like will be described later with reference to FIG.

[0042] The detection unit 12 may output a detection result including whether or not there is a deviation in the matching orientation (abnormal) or not (normal). More specifically, the detection unit 12 may transmit the detection result to another device via a network or the like, or may store the detection result in the storage unit 10.

[0043] 2 is a flowchart showing an example of processing (orientation deviation detection method) executed by the orientation deviation detection device 1. First, the acquisition unit 11 acquires a matching orientation related to the orientation of the target calculated by matching the results of remote sensing during self-location estimation with map data, and a movement orientation related to the orientation of the target calculated based on the movement trajectory of the target (step S1, acquisition step). Next, the detection unit 12 detects a deviation in the matching orientation based on the matching orientation and movement orientation acquired in step S1 (step S2, detection step).

[0044] Fig. 3 is a flowchart showing another example of the process (orientation deviation detection method) executed by the orientation deviation detection device 1. Details of the process of the acquisition unit 11 and the detection unit 12 will be described with reference to Fig. 3. The process shown in Fig. 3 is performed in a vehicle (target).

[0045] First, a matching process is performed (step S10). The matching process is executed by software on any device (including the orientation deviation detection device 1) provided on the vehicle. The matching process is a process based on the output of the LiDAR provided on the vehicle. For example, the output of the LiDAR is 20 Hz (20 rounds of output per second), and the output of the matching process is also 20 Hz accordingly. In the matching process, the x, y, z, roll, pitch, and yaw values ​​indicating the position and attitude that best matches the point cloud used as reference on the map and the LiDAR point cloud measured at that time are output as matching results.

[0046] Next, the matching result is filtered (step S11). More specifically, at least a portion of the matching result is passed through a UKF (Unscented Kalman Filter) / Kalman filter. For example, filtering is performed on x, y, yaw, output of an IMU (Inertial Measurement Unit), and vehicle speed (vehicle speed). Purposes of filtering include noise reduction and outputting 20 Hz at 100 Hz (high frequency) (by interpolating gaps). The matching orientation may be yaw obtained by LiDAR matching passed through a Kalman filter. The acquisition unit 11 acquires the matching result (including the matching orientation) that has been filtered in step S11.

[0047] Next, the detection unit 12 determines whether the speed of the vehicle is greater than a first threshold value and whether the vehicle is traveling straight (whether the absolute value of the vehicle's heading is smaller than a predetermined threshold value) (step S12). This determination is expressed, for example, in the following programming language. If mSpeed ​​> HEADING_ERROR_SPEED_LOWER_LIMIT AND |mYawRate| < HEADING_ERROR_YAW_RATE_LIMIT

[0048] The determination in step S12 determines that the deviation of the matching heading is detected only when the vehicle is moving at a speed exceeding the set minimum threshold and traveling straight. Note that the determination in step S12 may be omitted and the process may proceed to step S13 (described later) following step S11.

[0049] If it is determined in step S12 that the vehicle speed is greater than the first threshold and the vehicle is traveling straight (S12: YES), a calculation unit (not shown) of the orientation deviation detection device 1 calculates the traveling orientation (step S13). The traveling orientation is, for example, an azimuth angle calculated from the traveling trajectory x and y. FIG. 4 is a diagram showing an example of a traveling trajectory. The traveling trajectory shown in FIG. 4 indicates that the vehicle was located at coordinates (x1, y1) at one time, at coordinates (x2, y2) at the next time, at coordinates (x3, y3) at the next time, and at coordinates (x4, y4) at the next time. The calculation unit may calculate the traveling orientation, which is the orientation of the vehicle while it is traveling, using a trigonometric function (ArcTangent, ATAN) of the difference (Δx, Δy) between the coordinate values ​​while the vehicle is traveling. The acquisition unit 11 acquires the traveling orientation calculated in step S13. The traveling trajectory may be based on a GPS (Global Positioning System).

[0050] 3, next, the detection unit 12 calculates a differential heading, which is the difference between the matching heading (acquired by the acquisition unit 11 in steps S11 and S13) and the moving heading (step S14). In this embodiment, the differential heading is appropriately referred to as mHeadingDiff.

[0051] Next, the detection unit 12 determines whether (the absolute value of) the differential azimuth calculated in step S14 is greater than a second threshold (step S15). This determination is expressed, for example, in the following programming language. If ABS(mHeadingDiff) > HEADING_DIFF_ANGLE_LIMIT

[0052] If it is determined in step S15 that the differential azimuth is not greater than the second threshold (S15: NO), the counter is set to "0" (step S16). In this way, if mHeadingDiff is equal to or less than the condition, the counter is set to "0". On the other hand, if it is determined in step S15 that the differential azimuth is greater than the second threshold (S15: YES), the counter is incremented ("1" is added to the counter) (step S17). In this way, if the calculated differential azimuth is too large, the counter is incremented, and when the counter reaches a fourth threshold, as described below, an abnormality is detected and, for example, an error signal is transmitted. In this embodiment, the counter is appropriately referred to as mHeadingErrorCounter.

[0053] Following S16 or S17, the detection unit 12 performs a filter process on the differential heading calculated in step S14 (step S18). The detection unit 12 passes the calculated differential heading through a filter to reduce noise and smooth the calculation. The filter may be based on a bilinear transform. For example, abnormality determination using a bilinear transform is a one-shot determination, but as described below, it takes, for example, 13 to 23 seconds for the calculation to converge. In this embodiment, the differential heading after filtering is appropriately referred to as mFilteredHeadingDiff.

[0054] Next, the detection unit 12 determines whether the differential orientation filtered in step S18 is greater than a third threshold value or whether the counter is greater than a fourth threshold value (step S19). This determination is expressed, for example, in the following programming language. If |mFilteredHeadingDiff| > HEADING_DIFF_AVERAGE_LIMIT OR mHeadingErrorCounter > HEADING_ERROR_COUNTER_LIMIT

[0055] For example, the detection unit 12 determines whether the filtered differential azimuth is greater than "0.003" radians or whether the counter is greater than "5" (whether the differential azimuth has been determined to be greater than the second threshold five times in a row).

[0056] In step S19, if it is determined that the filtered differential orientation is greater than the third threshold value or the counter is greater than the fourth threshold value (S19: YES), the detection unit 12 detects an abnormality (deviation of the matching orientation) (step S20). When detecting an abnormality, the detection unit 12 may output an error signal (to another device, etc.).

[0057] On the other hand, if it is determined in step S19 that the filtered differential orientation is not greater than the third threshold and the counter is not greater than the fourth threshold (S19: NO), and if it is determined in step S12 that the vehicle speed is greater than the first threshold or that the vehicle is not traveling straight (S12: NO), the process returns to step S10 (repeats the process from step S10).

[0058] FIG. 5 is a graph showing an example of the raw value of the differential heading. More specifically, the graph shown in FIG. 5 shows an example of mHeadingDiff before the filtering process in step S18 of FIG. 3 is performed. Note that the vertical axis of the graphs shown in FIGS. 5 to 9 is radians, and the horizontal axis is time step (time). FIG. 6 is a graph showing an example of the value after the raw value shown in FIG. 5 has been filtered. More specifically, the graph shown in FIG. 6 shows an example of mFilteredHeadingDiff after the filtering process in step S18 of FIG. 3 is performed. As shown in FIGS. 5 and 6, the heading deviation detection device 1 passes the filter to, for example, reduce noise and mitigate the effect of sudden spikes in the calculated differential heading. With the correct TF, the difference is expected to be approximately 0 radians. Before filtering (FIG. 5), the differential heading changes significantly from time step to time step, tending to produce large differences that affect the detection accuracy of the detection unit 12. After filtering (FIG. 6), the graph becomes smoother and remains near 0 radians.

[0059] FIG. 7 is a graph showing an example of raw values ​​of differential heading with an offset introduced. In the graph shown in FIG. 7, the offsets have been manually applied to the yaw of vehicle TF, introducing offsets of ±0.2 degrees and ±0.1 degrees (see "mHeadingDiff +0.2," "mHeadingDiff -0.2," "mHeadingDiff +0.1," and "mHeadingDiff +0.1" in FIG. 7). FIG. 8 is a graph showing an example of values ​​after filtering the raw values ​​shown in FIG. 7. As can be seen from the graph in FIG. 8, the filtered graph remains within a range of approximately ±0.0017 radians and ±0.0035 radians.

[0060] FIG. 9 is a graph in which data relating to calculation convergence has been added to the graph shown in FIG. 8. For example, consider the case where a deviation of 0.2 degrees is to be detected. 0.2 degrees is approximately 0.0035 rad (radians). If we focus on 0.0035 on the vertical axis in the graph of FIG. 9, we can see that the calculation converges in 1300 to 2300 time steps. Since 100 Hz is assumed in this embodiment, we can see that the calculation converges in 13 to 23 seconds.

[0061] Next, the effects of the azimuth deviation detection device 1 will be described.

[0062] The orientation deviation detection device 1 includes an acquisition unit that acquires a matching orientation related to the orientation of the target calculated by matching the results of remote sensing during self-location estimation with map data and a movement orientation related to the orientation of the target calculated based on the movement trajectory of the target, and a detection unit that detects an error in the matching orientation based on the matching orientation and movement orientation acquired by the acquisition unit. In this aspect, since the detection of an error in the matching orientation is performed based on the matching orientation and the movement orientation, for example, even if there is some kind of defect in the calculation of the matching orientation, the detection of an error in the matching orientation can be performed more accurately. In other words, the detection of an error in the orientation can be performed more accurately.

[0063] Furthermore, in the orientation deviation detection device 1, the target may be a vehicle, and the remote sensing may be performed by a LiDAR provided on the vehicle. In this aspect, for example, a failure of the LiDAR provided on the vehicle can be recognized.

[0064] Furthermore, the detection unit 12 of the azimuth deviation detection device 1 may perform detection when at least one of the traveling direction or speed of the target satisfies a predetermined standard. In this aspect, for example, by performing detection when the target is moving straight or traveling at a certain speed, the distance between the trajectories of the movement trajectories becomes longer, and the accuracy of the movement direction increases, that is, the detection of the azimuth deviation can be performed more accurately.

[0065] Furthermore, the detection unit 12 of the azimuth deviation detection device 1 may perform detection when the target is moving in a straight line or when the speed of the target is greater than a predetermined threshold. In this aspect, for example, by performing detection when the target is moving in a straight line or when the target is moving at a certain speed, the distance between the trajectories of the movement trajectories becomes longer, and the accuracy of the movement direction increases, i.e., the detection of the azimuth deviation can be performed more accurately.

[0066] Furthermore, the detection unit 12 of the orientation deviation detection device 1 may detect the deviation when the difference between the matching orientation and the moving orientation satisfies a predetermined criterion. In this aspect, the orientation deviation can be detected more accurately based on the difference between the matching orientation and the moving orientation.

[0067] Furthermore, when acquisition is performed continuously by the acquisition unit 11, the detection unit 12 of the orientation deviation detection device 1 may calculate the difference between the matching orientation and the moving orientation for each successive acquisition, and perform detection based on the difference calculated for each successive acquisition. In this aspect, for example, it is possible to more accurately detect orientation deviation without being significantly affected by noise in the difference.

[0068] Furthermore, when acquisition is continuously performed by the acquisition unit 11, the detection unit 12 of the orientation deviation detection device 1 may calculate the difference between the matching orientation and the moving orientation in each successive acquisition, determine whether the difference is greater than a predetermined threshold, and detect a deviation when it is determined that the difference is greater than a predetermined threshold for more than a predetermined number of consecutive acquisitions. In this aspect, for example, it is possible to more accurately detect an orientation deviation without being significantly affected by noise in the difference.

[0069] Furthermore, the detection unit 12 of the orientation deviation detection device 1 may detect the difference between the matching orientation and the moving orientation based on a value that has been passed through a filter that removes noise. In this aspect, the orientation deviation can be detected more accurately based on the value from which noise has been removed.

[0070] Furthermore, the filter may be based on a bilinear transformation in the orientation deviation detection device 1. In this aspect, the orientation deviation can be detected more accurately based on the bilinear transformation.

[0071] Furthermore, the detection unit 12 of the azimuth deviation detection device 1 may detect a deviation when the filtered value is greater than a predetermined threshold value. In this aspect, the detection of azimuth deviation can be performed more accurately.

[0072] Previously, there was no technology to detect deviations in the direction of a vehicle's self-location estimation, and when deviations occurred, the direction was estimated to be different from the direction of travel, causing the estimated trajectory to meander and affecting vehicle control.

[0073] The azimuth deviation detection device 1 monitors the azimuth deviation of the self-location estimation and judges it to be abnormal when it exceeds a threshold. The azimuth deviation of the self-location is calculated by LiDAR matching, but to determine whether it is valid, the azimuth deviation during movement is calculated using the trigonometric function (ArcTangent) of the difference in coordinate values ​​during movement and compared with this value. To improve accuracy, the vehicle speed and IMU yaw_rete are checked, and a comparison is made when the vehicle is traveling at a certain speed and in a straight line. The azimuth deviation detection device 1 can be said to have an azimuth angle monitoring function.

[0074] The azimuth deviation detection device 1 detects deviations in the azimuth of the vehicle's own position estimation based on a comparison between the azimuth of the vehicle's own position calculated by matching the LiDAR mounted on the vehicle and the vehicle's moving azimuth (the azimuth of the vehicle while moving calculated based on the vehicle's movement trajectory on a map). This makes it possible to identify, for example, LiDAR malfunctions or installation problems. Since LiDARs installed on large vehicles are mounted on the cab, they are subject to a lot of shaking and vibration relative to the chassis, and are significantly affected by noise. The azimuth deviation detection device 1 also incorporates a filter process to remove vibration noise specific to large vehicles, making it possible to detect azimuth deviations with a resolution as small as 0.1 degrees.

[0075] The azimuth deviation detection device 1 performs the following processes (1) to (4). (1) Obtain the direction of your own position calculated by matching the LiDAR installed on your vehicle. (2) Calculate the vehicle's heading (based on the vehicle's trajectory (for example, the heading of the vehicle while it is moving, using the trigonometric function (ArcTangent) of the difference in coordinate values ​​while it is moving). When calculating the heading from the trajectory, the distance between the trajectories is longer and the calculation is more accurate when the vehicle is traveling straight ahead. Therefore, this is enabled when the yaw_rate value of the IMU installed in the vehicle is small and the vehicle speed is high. (3) The direction acquired in (1) is compared with the direction calculated in (2), and if the difference is equal to or greater than a threshold, a deviation in the direction of the vehicle's self-location estimation is detected. However, since the difference in the calculated azimuth angle has a large amount of noise, a large threshold is set, and if the difference continues to be greater than the threshold, an abnormality is detected. (4) LiDAR installed on large vehicles is attached to the cab, so it is subject to a lot of shaking and vibration relative to the chassis, and is therefore significantly affected by noise. A filter process is applied to remove vibration noise specific to large vehicles from the calculated azimuth difference, and the azimuth angle difference is calculated more accurately with finer resolution. A bilinear transformation is used to remove noise specific to large vehicles. Because it takes time to calculate the filtered azimuth angle difference value, if a large instantaneous difference occurs, it is detected in (3), and if the difference is continuous, the difference is detected with finer resolution in part (4).

[0076] The azimuth deviation detection device 1 can detect deviations in the azimuth angle of self-location estimation and monitor abnormal conditions. The azimuth deviation detection device 1 can detect deviations in the azimuth angle of the vehicle's self-location estimation with fine resolution without requiring complex processing, thereby making it possible to recognize, for example, LiDAR malfunctions or installation problems.

[0077] The orientation deviation detection device 1 of the present disclosure may have the following configuration.

[0078] [1] an acquisition unit that acquires a matching direction related to the direction of the target calculated by matching the result of remote sensing during self-location estimation with map data, and a movement direction related to the direction of the target calculated based on the movement trajectory of the target; a detection unit that detects a deviation of the matching orientation based on the matching orientation and the movement orientation acquired by the acquisition unit; An orientation deviation detection device comprising:

[0079] [2] The object relates to a vehicle, The remote sensing is performed by a LiDAR (Light Detection And Ranging) device installed in the vehicle. [1] The orientation deviation detection device according to the present invention.

[0080] [3] The detection unit performs the detection when at least one of the traveling direction or the speed of the object satisfies a predetermined criterion. The orientation deviation detection device according to [1] or [2].

[0081] [4] The detection unit performs the detection when the object is moving straight or when the speed of the object is greater than a predetermined threshold. The orientation deviation detection device according to any one of [1] to [3].

[0082] [5] the detection unit detects the deviation when a difference between the matching orientation and the movement orientation satisfies a predetermined criterion. The orientation deviation detection device according to any one of [1] to [4].

[0083] [6] When acquisition is continuously performed by the acquisition unit, the detection unit calculates a difference between the matching orientation and the movement orientation in each of the continuous acquisitions, and performs the detection based on the difference calculated in each of the continuous acquisitions. The orientation deviation detection device according to any one of [1] to [5].

[0084] [7] When acquisition is continuously performed by the acquisition unit, the detection unit calculates a difference between the matching orientation and the movement orientation in each of the continuous acquisitions and determines whether the difference is greater than a predetermined threshold, and detects the deviation when it is determined that the difference is greater than a predetermined threshold for more than a predetermined number of consecutive acquisitions. The orientation deviation detection device according to any one of [1] to [6].

[0085] [8] the detection unit performs the detection based on a value obtained by passing a difference between the matching orientation and the moving orientation through a filter that removes noise. The orientation deviation detection device according to any one of [1] to [7].

[0086] [9] The filter is based on a bilinear transform. [8] The orientation deviation detection device according to [8].

[0087]

[10] the detection unit detects the deviation when the filtered value is greater than a predetermined threshold value. The orientation deviation detection device according to [8] or [9]. [Explanation of symbols]

[0088] 1...orientation deviation detection device, 10...storage unit, 11...acquisition unit, 12...detection unit

Claims

1. an acquisition unit that acquires a matching direction related to the direction of the target calculated by matching the result of remote sensing during self-location estimation with map data, and a movement direction related to the direction of the target calculated based on the movement trajectory of the target; a detection unit that detects a deviation of the matching orientation based on the matching orientation and the movement orientation acquired by the acquisition unit; An orientation deviation detection device comprising:

2. The object relates to a vehicle, The remote sensing is performed by a LiDAR (Light Detection And Ranging) device installed in the vehicle.

2. The azimuth deviation detection device according to claim 1.

3. The detection unit performs the detection when at least one of the traveling direction or the speed of the object satisfies a predetermined criterion.

2. The azimuth deviation detection device according to claim 1.

4. The detection unit performs the detection when the object is moving straight or when the speed of the object is greater than a predetermined threshold.

2. The azimuth deviation detection device according to claim 1.

5. the detection unit detects the deviation when a difference between the matching orientation and the movement orientation satisfies a predetermined criterion.

2. The azimuth deviation detection device according to claim 1.

6. When acquisition is continuously performed by the acquisition unit, the detection unit calculates a difference between the matching orientation and the movement orientation in each of the continuous acquisitions, and performs the detection based on the difference calculated in each of the continuous acquisitions.

2. The azimuth deviation detection device according to claim 1.

7. When acquisition is continuously performed by the acquisition unit, the detection unit calculates a difference between the matching orientation and the movement orientation in each of the continuous acquisitions and determines whether the difference is greater than a predetermined threshold, and detects the deviation when it is determined that the difference is greater than a predetermined threshold for more than a predetermined number of consecutive acquisitions.

2. The azimuth deviation detection device according to claim 1.

8. the detection unit performs the detection based on a value obtained by passing a difference between the matching orientation and the moving orientation through a filter that removes noise. The azimuth deviation detection device according to any one of claims 1 to 7.

9. The filter is based on a bilinear transform.

9. The azimuth deviation detection device according to claim 8.

10. the detection unit detects the deviation when the filtered value is greater than a predetermined threshold value.

9. The azimuth deviation detection device according to claim 8.

Images