Control system, control method, and control program

Abstract

To provide a technique enabling a moving body to evaluate the precision of an estimated position by itself for a control system which controls the moving body based upon the estimated position.SOLUTION: A control system comprises one or more storage devices which store observation data and an estimated position of a moving body and one or more processors. The one or more processors are configured to execute: processing to calculate the estimated position of the moving body at the actual point of time based upon an estimated position of the moving body calculated through at least the last processing and observation data up to the actual point of time; processing to recalculate an estimated position of the moving body at a predetermined past point of time based upon the estimated position of the moving body up to the actual point of time and the observation data up to the actual point of time; and processing to evaluate the precision of the actual estimated position of the moving body at the actual point of time based upon the difference between the estimated position of the moving body up to the predetermined past point of time stored in the one or more storage devices and the estimated position of the moving body at the predetermined past point of time.SELECTED DRAWING: Figure 4

Description

[Technical field]

[0001] The present disclosure relates to a technique for controlling a moving object based on an estimated position. [Background technology]

[0002] Patent Document 1 discloses a driving control device configured to execute the following processes: calculating control information based on map information, positioning information, and lane marking information; controlling the driving of the vehicle based on the control information; calculating multiple estimated positions of the current vehicle by multiple calculation methods using information on the vehicle's most recent past positions; calculating estimated position reliability based on a comparison of position information including the multiple estimated positions; and continuing driving control until the estimated position reliability falls below a threshold value even if lane marking information becomes undetectable.

[0003] Other documents that demonstrate the technical level in the technical field related to the present disclosure include the following Patent Documents 2 and 3. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2018-173304 A [Patent Document 2] JP 2018-173303 A [Patent Document 3] Patent Publication No. 2022-054223 Summary of the Invention [Problem to be solved by the invention]

[0005] Self-location estimation, which estimates the position of a moving object, is one of the important processes in controlling moving objects such as autonomous driving control. In particular, the accuracy of the estimated position by self-location estimation has a significant effect on control performance. For this reason, there is a demand for technology that allows moving objects to evaluate the accuracy of their own estimated position.

[0006] According to the technology disclosed in Patent Document 1, the accuracy (estimation reliability) of the estimated position is calculated by comparing several combinations of multiple estimated positions calculated by multiple calculation methods. Then, when any of the accuracy (estimation reliability) of the estimated positions calculated for several combinations is equal to or lower than a threshold, control is stopped. However, it is generally possible that the estimated positions calculated by different calculation methods differ from each other. Also, when an abnormality occurs in one estimated position, control is stopped even if the other estimated position is normal. Thus, there is a risk that the technology disclosed in Patent Document 1 cannot ensure sufficient usability. Furthermore, since it is necessary to adapt the control to multiple calculation methods, it is difficult to design the control according to the moving body.

[0007] In view of the above problems, one object of the present disclosure is to provide a technique that enables a moving object to evaluate the accuracy of its own estimated position. [Means for solving the problem]

[0008] A first aspect of the present disclosure relates to a control system for controlling a moving object based on an estimated position of the moving object.

[0009] The control system according to a first aspect comprises one or more storage devices that store observation data and an estimated position of the moving body based on information detected by one or more sensors mounted on the moving body, and one or more processors, and the one or more processors are configured to execute at least the following processes: a process of calculating an estimated position of the moving body at a current time based on the estimated position of the moving body calculated in a previous process and observation data up to a current time; a process of recalculating an estimated position of the moving body at a predetermined past time based on the estimated position of the moving body up to the current time and the observation data up to the current time stored in the one or more storage devices; and a process of evaluating the accuracy of the estimated position of the moving body at the current time based on the magnitude of the difference between the estimated position of the moving body at the predetermined past time stored in the one or more storage devices and the recalculated estimated position of the moving body at the predetermined past time.

[0010] A second aspect of the present disclosure relates to a control method for controlling a moving object based on an estimated position of the moving object.

[0011] The control method according to the second aspect is characterized in that it includes storing and managing observation data based on information detected by one or more sensors mounted on the moving body and an estimated position of the moving body in one or more storage devices, calculating an estimated position of the moving body at a current time based on at least the estimated position of the moving body calculated in a previous process and the observation data up to a current time, recalculating an estimated position of the moving body at a predetermined past time based on the estimated position of the moving body up to the current time stored in the one or more storage devices and the observation data up to the current time, and evaluating the accuracy of the estimated position of the moving body at the current time based on the magnitude of the difference between the estimated position of the moving body at the predetermined past time stored in the one or more storage devices and the recalculated estimated position of the moving body at the predetermined past time.

[0012] A third aspect of the present disclosure relates to a control program that causes a computer to execute a process of controlling a moving object based on an estimated position of the moving object.

[0013] The control program according to the third aspect is characterized in that it is configured to cause a computer to execute the following processes: storing and managing observation data based on information detected by one or more sensors mounted on the moving body and the estimated position of the moving body in one or more storage devices; calculating an estimated position of the moving body at a current time based on at least the estimated position of the moving body calculated in a previous process and the observation data up to the current time; recalculating an estimated position of the moving body at a predetermined past time based on the estimated position of the moving body up to the current time and the observation data up to the current time stored in one or more storage devices; and evaluating the accuracy of the estimated position of the moving body at the current time based on the magnitude of the difference between the estimated position of the moving body at the predetermined past time stored in one or more storage devices and the recalculated estimated position of the moving body at the predetermined past time. Effect of the Invention

[0014] According to the present disclosure, the estimated position of the moving body at the current time is calculated based on at least the estimated position of the moving body calculated in the previous process and the observation data up to the current time. Meanwhile, the estimated position of the moving body at a predetermined past time is recalculated based on the estimated position of the moving body up to the current time and the observation data up to the current time. Then, the accuracy of the estimated position of the moving body at the current time is evaluated based on the magnitude of the difference between the estimated position calculated at the predetermined past time and the recalculated estimated position. In this way, according to the present disclosure, the moving body can evaluate the accuracy of the estimated position of the moving body at the current time used for control by itself. Furthermore, it is not necessary to apply a plurality of calculation methods unnecessarily, and it can be easily applied to conventional control without reducing usability. [Brief description of the drawings]

[0015] [Figure 1] FIG. 2 is a block diagram showing a configuration of a control system according to the present embodiment. [Diagram 2] FIG. 11 is a conceptual diagram for explaining an example of a re-estimation process. [Diagram 3] 11 is a conceptual diagram showing an example of emergency control switching determination according to the magnitude of the difference between a predetermined time point estimated position and a re-estimated position. FIG. [Figure 4] 4 is a flowchart illustrating an example of a process executed by a control device. [Diagram 5] FIG. 1 is a conceptual diagram showing an example of a control system according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0016] The present disclosure relates to control of a moving body. Examples of the moving body include a vehicle, a robot, and the like. The moving body may be capable of autonomous driving. The vehicle may be a self-driving vehicle. As an example, in the following description, a case where the moving body is a vehicle will be considered. When generalizing, "vehicle" in the following description will be read as "moving body".

[0017] 1. Control System 1 is a block diagram showing the configuration of a control system 10 according to this embodiment. The control system 10 controls a vehicle 1. The control system 10 includes an internal sensor 20, an external sensor 30, a Global Navigation Satellite System (GNSS) sensor 40, a traveling device 50, an HMI 60, and a control device 100.

[0018] The internal sensor 20 is mounted on the vehicle 1 and detects the traveling state of the vehicle 1. Examples of the internal sensor 20 include a vehicle speed sensor (wheel speed sensor), a steering angle sensor, an acceleration sensor, and a yaw rate sensor.

[0019] The external sensor 30 is mounted on the vehicle 1 and detects the surrounding conditions of the vehicle 1. Examples of the external sensor 30 include a LiDAR (Laser Imaging Detection and Ranging), a camera, and a radar.

[0020] The GNSS sensor 40 measures the position of the vehicle 1 by receiving signals from satellites.

[0021] The traveling device 50 includes a steering device, a drive device, and a brake device. The steering device steers the wheels. The drive device generates a drive force. The brake device generates a braking force.

[0022] The HMI 60 includes a device that provides an HMI function of the vehicle 1. Examples of the HMI 60 include a display, a speaker, a touch panel, and the like.

[0023] The control device 100 is a computer that controls the vehicle 1. The control device 100 provides at least a function of estimating the position of the vehicle 1 and controlling the traveling of the vehicle 1 based on the estimated position of the vehicle 1. For example, the control device 100 is an ECU (Electronic Control Unit) that provides an automatic driving function of the vehicle 1.

[0024] The control device 100 includes one or more processors 110 (hereinafter simply referred to as processor 110) and one or more storage devices 120 (hereinafter simply referred to as storage device 120). The processor 110 executes various processes described below. For example, the processor 110 can be configured as a CPU (Central Processing Unit) including an arithmetic unit, a register, and the like. The storage device 120 stores various information. For example, the storage device 120 can be configured as a recording medium such as a volatile memory, a non-volatile memory, an HDD (Hard Disk Drive), or an SSD (Solid State Drive). Typically, the control device 100 is mounted on the vehicle 1. Alternatively, a part of the control device 100 may be disposed in an external device outside the vehicle 1 and control the vehicle 1 remotely.

[0025] The storage device 120 stores a control program 210, vehicle driving information 220, sensor detection information 230, map information 240, estimated position information 250, observation data information 260, and the like.

[0026] The control program 210 is a computer program for controlling the vehicle 1. The processor 110 executes the control program 210 to realize various processes by the control device 100. The control program 210 may be recorded in a computer-readable recording medium.

[0027] The vehicle driving information 220 is information detected by the internal sensor 20, and indicates the driving state of the vehicle 1. The driving state of the vehicle 1 includes the vehicle speed, the steering angle, the acceleration, the yaw rate, and the like.

[0028] The sensor detection information 230 is information detected by the external sensor 30. For example, the sensor detection information 230 includes point cloud information obtained by a LiDAR. As another example, the sensor detection information 230 may include an image captured by a camera.

[0029] Furthermore, the sensor detection information 230 includes object information on objects around the vehicle 1. Examples of objects around the vehicle 1 include pedestrians, other vehicles, and features. Examples of features include white lines, curbs, poles, telephone poles, signs, road signs, and building corners. The features are used in estimating the position of the vehicle 1, which will be described later. The object information includes at least the relative position of the object with respect to the vehicle 1. For example, based on point cloud information obtained by LiDAR, an object can be recognized and the relative position and relative speed of the object can be obtained.

[0030] The map information 240 includes a general navigation map. Furthermore, in order to estimate the position of the vehicle 1, which will be described later, the map information 240 includes feature map information 241 that manages information on features on the map. The feature map information 241 manages at least information on the positions (absolute positions) of features in an absolute coordinate system on the map. Furthermore, the feature map information 241 may manage information for identifying features (classification, attributes, shape, etc.). The feature map information 241 may be provided in advance depending on the environment in which the control system 10 according to this embodiment is applied.

[0031] Additionally, the map information 240 may include information used for controlling the vehicle 1. For example, the map information 240 may include information such as the shape, height, and brightness of the road surface on the map, and information such as the shape and attributes of structures on the map.

[0032] As described later, the processor 110 executes a process of estimating the position (absolute position) of the vehicle 1 at a predetermined period. The position of the vehicle 1 will be simply referred to as the "vehicle position" hereinafter. The estimated position information 250 manages the vehicle position (estimated position) estimated by the process executed by the processor 110 as time-series data.

[0033] Here, the vehicle position is typically expressed as a coordinate position in an absolute coordinate system on a map. Furthermore, the vehicle position may include the attitude (roll angle, pitch angle, and yaw angle) of the vehicle 1. In this case, the vehicle position is generally expressed as a vector having six elements. Alternatively, the vehicle position can be expressed by a rigid transformation matrix for the reference coordinate position and attitude. For example, if a translation vector of 3×1 size representing a parallel movement with respect to a reference three-dimensional coordinate position is r, and a rotation matrix of 3×3 size representing a rotation with respect to the attitude of the vehicle 1 of the reference three axes is R, the vehicle position can be expressed by a rigid transformation matrix P of 4×4 size of the following formula. In this case, the vector expression of the coordinate position of the vehicle position is obtained by applying the rigid transformation matrix to the vector representing the reference coordinate position. Also, the attitude of the vehicle 1 at the vehicle position is obtained by applying the rotation matrix R to the attitude of the vehicle 1 that is the reference.

[0034]

number

[0035] The observation data information 260 manages observation data based on the vehicle driving information 220 and the sensor detection information 230 as time-series data. Examples of the observation data include the amount of movement (amount of displacement) of the vehicle 1 and the absolute positions of features detected around the vehicle 1. Alternatively, the observation data may be the vehicle driving information 220 or the sensor detection information 230 at each time.

[0036] 2. Processing The process executed by the control device 100, more specifically, the process executed by the processor 110, will be described below.

[0037] 2-1. Self-location estimation process The processor 110 executes a process (self-position estimation process) of calculating an estimated position of the vehicle 1 at the current time based on the estimated position of the vehicle 1 calculated in the previous process and the observation data up to the current time. However, the initial value of the estimated position may be, for example, a value obtained by the GNSS sensor 40. The processor 110 can acquire the estimated position calculated in the previous process by referring to the estimated position information 250. The processor 110 can also acquire the observation data up to the current time by referring to the observation data information 260.

[0038] An example of the self-location estimation process executed by the processor 110 will be described below.

[0039] First, the processor 110 calculates a rough vehicle position (hereinafter referred to as "pre-estimated position") based on the vehicle driving information 220 and the estimated position calculated in the previous process. For example, the processor 110 calculates the amount of movement of the vehicle 1 from the estimated position calculated in the previous process based on the steering angle and vehicle speed of the vehicle 1, and calculates the pre-estimated position based on the amount of movement. Also, for example, the processor 110 calculates the pre-estimated position from a state equation configured with the vehicle position as a state and the vehicle driving information 220 as an explanatory variable.

[0040] Next, the processor 110 recognizes features around the vehicle 1 based on the sensor detection information 230. Furthermore, the processor 110 estimates the absolute position of the recognized feature by combining the calculated pre-estimated position and the relative position of the recognized feature. Furthermore, the processor 110 links the recognized feature with a feature managed in the feature map information 241 based on the object information of the recognized feature. After that, the processor 110 corrects the pre-estimated position of the recognized feature so that the estimated absolute position and the absolute position obtained from the feature map information 241 match as much as possible. Then, the processor 110 sets the vehicle position obtained by correcting the pre-estimated position as the processing result of the self-position estimation process (the estimated position of the vehicle 1 at the current time). For example, the processor 110 calculates the estimated position of the vehicle 1 at the current time by correcting the pre-estimated position using a particle filter, a Kalman filter, or the like, with the vehicle position as the state. In this case, the estimated position is generally a probability distribution with the vehicle position as a random variable. In particular, in this case, it can be considered that the processor 110 estimates a filter distribution of the vehicle position. In this sense, the self-position estimation process can also be called a "filter distribution estimation process."

[0041] As an example, the processor 110 executes the self-location estimation process in this manner. The processor 110 stores the current estimated position of the vehicle 1 obtained by executing the self-location estimation process in the storage device 120 as estimated position information 250. The processor 110 also stores observation data obtained during the execution of the self-location estimation process in the storage device 120 as observation data information 260. For example, the processor 110 stores the amount of movement of the vehicle 1, the relative position of the recognized feature, the absolute position of the recognized feature obtained from the feature map information 241, etc. in the storage device 120 as the observation data information 260.

[0042] 2-2. Re-estimation process The processor 110 executes a process (re-estimation process) of recalculating the estimated position of the vehicle 1 at a predetermined time in the past based on the estimated position of the vehicle 1 up to the present time and the observation data up to the present time. The processor 110 can acquire the estimated position of the vehicle 1 up to the present time by referring to the estimated position information 250.

[0043] The re-estimation process can be performed, for example, so as to minimize a cost function that is configured based on the estimated position of the vehicle 1 up to the present time and the observation data up to the present time. Hereinafter, an example of the re-estimation process executed by the processor 110 will be described with reference to FIG.

[0044] FIG. 2 shows the estimated position (P t-2 ,P t-1 ,P t ), the detection range 2 of the external sensor 30, the recognized feature 3, the feature 4 managed by the feature map information 241, and the relative position (S k t-2 ,S k t-1 ,S k t ), and the displacement of vehicle 1 from the previous point in time (L t-1 ,L t ) is shown. Here, the estimated position (P t-2 ,P t-1 ,P t ) and the displacement of vehicle 1 from the previous point (L t-1 ,L t ) is represented by a rigid transformation matrix. The rigid transformation matrix for the displacement of the vehicle 1 can be constructed by expressing the change in the attitude of the vehicle 1 from the previous point in time detected by the internal sensor 20 as a rotation matrix R, and expressing the movement amount of the vehicle 1 as a translation vector r.

[0045] Now, the estimated position P of vehicle 1 at time t-1 in the past t-1 Consider the case where the cost function is recalculated as follows: 2 can be given.

[0046]

number

[0047] Here, Θ is a vector-valued function that returns a vector including the translation vector r of the argument rigid transformation matrix and the value obtained by converting the argument rigid transformation matrix rotation matrix R into Euler angles. However, Θ may be a vector-valued function that returns either one of the vectors, a vector whose elements are the value obtained by converting the argument rigid transformation matrix translation vector r or the argument rigid transformation matrix rotation matrix R into Euler angles. Ω in is the variance-covariance matrix for Θ. For example, Ω in can be a variance-covariance matrix determined by a probability distribution representing the error of the internal sensor 20. in is given in advance according to the environment in which this embodiment is applied. 2 in For a given estimated position, the difference between the estimated position and the observation data based on the information detected by the internal sensor 20 is given as a cost.

[0048] {Λ t-i} is a set of indices that give a labeling that distinguishes each of the features 3 recognized at the time ti. That is, {Λ t-i The number of elements of {} is the number of recognized features 3. f is a function that gives the absolute position of the recognized feature 3 from the estimated position of the vehicle 1 and the relative position of the recognized feature 3. f k m is the absolute position of the recognized feature 3 obtained from the feature map information 241. d is the absolute position f calculated from the relative position of the recognized feature 3 and the absolute position f obtained from the feature map information 241. k m f and f k m is typically a vector giving a two- or three-dimensional coordinate position. Thus, d is typically a function of f and fk m is a vector-valued function that gives the distance for each element of Ω. ex is the variance-covariance matrix for d. For example, Ω ex can be a variance-covariance matrix determined by a probability distribution representing the error of the external sensor 30. ex is given in advance according to the environment in which this embodiment is applied. 2 ex For a given estimated position, the difference between the estimated position and observation data based on information detected by the external sensor 30 is given as a cost.

[0049] In this way, the cost function ξ 2 Then, the estimated position P of vehicle 1 at time t-2 in the past is t-2 If we fix t−1, the processor 110 calculates the estimated position P t-1 P in the following formula op t-1 In the above explanation, the cost function ξ 2 is constructed based on the estimated position and observation data of vehicle 1 from time t-2 in the past to the present time, while the cost function ξ 2 may be constructed to extend over a longer period.

[0050]

number

[0051] As an example, the processor 110 executes the re-estimation process in this manner. In addition, the processor 110 can execute the re-estimation process using a particle filter, a Kalman filter, or the like, with the vehicle position as a state. In particular, in this case, the processor 110 can be considered to be estimating a smoothed distribution of the vehicle position. In this sense, the re-estimation process can also be called a "smoothed distribution estimation process."

[0052] 2-3.Accuracy evaluation process Processor 110 executes a process (accuracy evaluation process) for evaluating the accuracy of the estimated position of vehicle 1 at the current time calculated in the current process, based on the magnitude of the difference between the estimated position obtained by execution of self-position estimation process for the past predetermined time that was the subject of recalculation in the re-estimation process (hereinafter simply referred to as the "predetermined time estimated position") and the estimated position recalculated by execution of the re-estimation process (hereinafter simply referred to as the "re-estimated position"). Here, processor 110 can acquire the predetermined time estimated position by referring to estimated position information 250.

[0053] In the re-estimation process, the estimated position at a given time in the past is recalculated based on the estimated position of the vehicle 1 up to the present time and the observation data up to the present time, so the re-estimated position is expected to be more accurate than the estimated position at the given time. For example, it is generally known that a smoothed distribution has a smaller variance and higher accuracy than a filtered distribution. Therefore, a large difference between the estimated position at the given time and the re-estimated position is considered to indicate that the estimated position at the given time is likely to have been erroneously estimated. An error in the estimated position at a past time affects the accuracy of the estimated position thereafter. Therefore, the magnitude of the difference between the estimated position at the given time and the re-estimated position is an index for evaluating the accuracy of the estimated position of the vehicle 1 at the present time calculated in the current process.

[0054] In particular, the closer an error in the estimated position is to the current time, the greater the impact on the accuracy of the estimated position of the vehicle 1 at the current time. For this reason, it is desirable that the specified past time to be recalculated in the re-estimation process is a time in the past that is a certain degree close to the current time (for example, several seconds or several hundred milliseconds before the current time). However, it is not desirable to select a time in the past that is too close to the current time, since there is a risk that the accuracy of the re-estimated position will not be significantly greater than the estimated position at the specified time.

[0055] The difference between the estimated position at a given time and the re-estimated position is given, for example, as follows.

[0056] When the vehicle position is expressed by a vector, the difference between the estimated position at a given time and the corrected estimated position can be the vector of the difference between the vector representing the estimated position at a given time and the vector representing the re-estimated position. Note that when the vehicle position is a vector whose elements are the coordinate position and the attitude of the vehicle 1, the difference between the estimated position at a given time and the re-estimated position may be calculated separately for the coordinate position and the attitude of the vehicle 1.

[0057] When the vehicle position is expressed by a rigid transformation matrix, the difference between the estimated position at a given time and the re-estimated position is calculated by using the rigid transformation matrix P fil , the rigid body transformation matrix representing the re-estimated position is P smt Then, P in the following equation dif It can be calculated as follows: P dif By extracting the translation vector r of P, we can obtain a vector representation of the difference between the estimated position at a given time and the re-estimated position with respect to the coordinate position. dif By extracting the rotation matrix R and converting it into Euler angles, a vector representation of the difference between the estimated position at a given time and the re-estimated position regarding the attitude of the vehicle 1 can be obtained.

[0058]

number

[0059] In the accuracy evaluation process, the processor 110 may be configured to detect that the estimated position of the vehicle 1 at the current time is inaccurate when the magnitude of the difference between the estimated position at a predetermined time and the re-estimated position exceeds a predetermined threshold (first threshold). The fact that the magnitude of the difference exceeds the first threshold can be determined, for example, from the norm of a vector representing the difference exceeding the first threshold, with the first threshold being a scalar value. Alternatively, it can be determined from the vector representing the difference being outside the region, with the first threshold being the boundary of a region in a vector space. The processor 110 may also be configured to detect that the self-location estimation is inaccurate when the state in which the magnitude of the difference exceeds the first threshold continues for a certain period of time, or the magnitude of the difference exceeds the first threshold a certain number of times or more during the certain period of time.

[0060] In addition, when the estimated position is a probability distribution with the vehicle position as a random variable, the difference between the estimated position at a given time and the re-estimated position can be calculated using the Mahalanobis distance. In this case, the processor 110 calculates the Mahalanobis distance to the average of the probability distribution related to the re-estimated position relative to the probability distribution related to the estimated position at a given time (hereinafter simply referred to as the "first Mahalanobis distance") and the Mahalanobis distance to the average of the probability distribution related to the estimated position at a given time relative to the probability distribution related to the re-estimated position (hereinafter simply referred to as the "second Mahalanobis distance") using the following formulas: fl2sm and d sm2fl can be given by

[0061]

number

[0062] Here, μ fil and Σ fil are the mean and variance-covariance matrix of the probability distribution related to the estimated position at a given time, respectively. smt and Σ smt are the mean and variance-covariance matrix of the probability distribution for the re-estimated position, respectively.

[0063] In this case, the magnitude of the difference between the estimated position at a given time point and the re-estimated position can be expressed as either the first Mahalanobis distance or the second Mahalanobis distance, or the sum of both.

[0064] Alternatively, the magnitude of the difference between the estimated position at a given time and the re-estimated position can be expressed by either or both of the significant difference of the first Mahalanobis distance when a probability distribution related to the estimated position at a given time is assumed and the significant difference of the second Mahalanobis distance when a probability distribution related to the re-estimated position is assumed. This utilizes the fact that the square of the Mahalanobis distance when a random variable is assumed to have an n-dimensional probability distribution follows a chi-square distribution with n degrees of freedom. In other words, if the dimension of the vehicle position is n, the magnitude of the difference between the estimated position at a given time and the re-estimated position can be expressed by the smallness of the P value obtained from the chi-square distribution with n degrees of freedom using the square of the first Mahalanobis distance or the square of the second Mahalanobis distance as a statistic. In this case, the magnitude of the difference exceeding a given threshold value can be rephrased as the P value being below a given threshold value. The same applies to the threshold value described below when the magnitude of the difference is expressed as the smallness of the P value.

[0065] In addition, when the magnitude of the difference between the estimated position at a given time and the re-estimated position is expressed by both the significant difference of the first Mahalanobis distance and the significant difference of the second Mahalanobis distance, the chi in the following formula can be expressed from the reproducibility of the chi-square distribution. 2 In other words, if the dimension of the vehicle position is n, the magnitude of the difference between the estimated position at a given time and the re-estimated position is expressed as χ 2 can be expressed as a statistical quantity by the smallness of the P-value obtained from the chi-squared distribution with 2n degrees of freedom.

[0066]

number

[0067] 2-4. Driving control processing The processor 110 executes a process (driving control process) for controlling the driving of the vehicle 1 based on the estimated position obtained by executing the self-position estimation process. The processor 110 generates a control signal for the driving device 50 or the HMI 60 by executing the driving control process. For example, when performing autonomous driving of the vehicle 1, the processor 110 generates a driving plan including driving judgment and a driving trajectory from the estimated position and the map information 240. Then, the processor 110 generates control signals related to acceleration, braking, and steering so that the vehicle 1 drives according to the generated driving plan.

[0068] In particular, the processor 110 may be configured to execute a process of performing emergency control according to the magnitude of the difference between the estimated position at a predetermined time and the re-estimated position when poor accuracy of the estimated position of the vehicle 1 at the current time is detected in the accuracy evaluation process. Examples of emergency control include a procedure for interrupting automatic driving, issuing an alarm to the operator of the vehicle 1, control for evacuation by deceleration, control for evacuation by stopping, etc. By configuring in this way, it is possible to prevent control from being continued with an erroneous estimated position when poor accuracy of the estimated position of the vehicle 1 at the current time is detected.

[0069] Furthermore, the processor 110 may be configured to switch between these emergency controls depending on the magnitude of the difference between the predetermined time estimated position and the re-estimated position. For example, for the vector of the difference between the predetermined time estimated position and the re-estimated position, the processor 110 determines whether to switch between emergency controls as shown in FIG. 3. In FIG. 3, the first threshold, the second threshold, and the third threshold are boundaries of regions in the vector space. In the case shown in FIG. 3, the processor 110 executes the first control when the magnitude of the difference vector (DV1, DV2, DV3) exceeds the first threshold but does not exceed the second threshold (DV1), executes the second control when the magnitude exceeds the second threshold but does not exceed the third threshold (DV2), and executes the third control when the magnitude exceeds the third threshold (DV3). Note that when the magnitude does not exceed the first threshold, the processor 110 executes the normal control. By configuring in this way, it is possible to determine the accuracy of the current estimated position of the vehicle 1 in stages and execute appropriate control according to the degree of accuracy. For example, by having the first control be an alarm, the second control be evacuation by decelerating, and the third control be evacuation by stopping, it is possible to gradually request the operator to take action by issuing an alarm, ensure control performance by decelerating, and ensure safety by stopping.

[0070] The control of the vehicle 1 by the control device 100 is realized by operating the traveling device 50 and the HMI 60 in accordance with control signals generated by the processor 110 executing the traveling control process.

[0071] 2-5. Flowchart Fig. 4 is a flowchart showing an example of processing executed by the processor 110. The flowchart shown in Fig. 4 may be repeatedly executed at a predetermined cycle.

[0072] In step S100, the processor 110 obtains various pieces of information stored in the storage device 120.

[0073] In step S110, the processor 110 executes a self-location estimation process to calculate an estimated location of the vehicle 1 at the current time.

[0074] In step S121, the processor 110 executes a re-estimation process to recalculate the estimated position of the vehicle 1 at a predetermined time in the past (re-estimated position).

[0075] In step S122, the processor 110 acquires the estimated position (predetermined time estimated position) obtained by executing the self-position estimation process for the past predetermined time point when the re-estimation process was executed in step S121.

[0076] In step S130, the processor 110 calculates the difference between the estimated position at the given time point obtained in step S122 and the re-estimated position recalculated in step S121.

[0077] When the magnitude of the difference calculated in step S130 does not exceed the first threshold value (step S141; No), processor 110 executes normal control in the driving control process (step S150).

[0078] If the magnitude of the difference calculated in step S130 exceeds the first threshold (step S141; Yes), the processor 110 detects that the estimated position of the vehicle 1 at the current time calculated in the current process is inaccurate. If the magnitude of the difference exceeds the first threshold (step S141; Yes) but does not exceed the second threshold (step S142; No), the processor 110 executes the first control to issue an alarm (step S151). If the magnitude of the difference exceeds the second threshold (step S142; Yes) but does not exceed the third threshold (step S143; No), the processor 110 executes the second control to perform evacuation by deceleration (step S152). If the magnitude of the difference exceeds the third threshold (step S143; Yes), the processor 110 executes the third control to perform evacuation by stopping (step S153).

[0079] In this manner, the control device 100 executes the processes. Furthermore, by the control device 100 executing the processes in this manner, a control method related to control of the vehicle 1 is realized.

[0080] 3.Effects As described above, according to this embodiment, the estimated position of the vehicle 1 at the current time is calculated based on at least the estimated position of the vehicle 1 calculated in the previous process and the observation data up to the current time, and the vehicle 1 is controlled based on the calculated estimated position of the vehicle 1 at the current time. Meanwhile, the estimated position of the vehicle 1 at a predetermined past time point (re-estimated position) is recalculated based on the estimated position of the vehicle 1 up to the current time point and the observation data up to the current time. Then, the accuracy of the estimated position of the vehicle 1 at the current time point is evaluated based on the magnitude of the difference between the estimated position calculated at the predetermined past time point (estimated position at the predetermined time point) and the re-estimated position. Thus, according to this embodiment, the vehicle 1 can evaluate the accuracy of the estimated position of the vehicle 1 at the current time point used for control by itself.

[0081] In addition, in this embodiment, the calculation of the re-estimated position is characterized by being based on the estimated position of the vehicle 1 up to the present time and the observation data up to the present time, and it is also possible to adopt a calculation method equivalent to that used for the calculation of the estimated position of the vehicle 1 at the present time in terms of the type of data used. Therefore, it can be easily applied to conventional control without deteriorating usability. Furthermore, it is not necessary to apply a plurality of calculation methods unnecessarily.

[0082] Fig. 5 is a conceptual diagram showing an example of this embodiment. Fig. 5 shows examples of the actual position of the vehicle 1 and the recognized feature object 3 for the current time t and past times t-1 and t-2, the estimated position calculated by executing the self-position estimation process for the examples, and the re-estimated position recalculated by executing the re-estimation process. In particular, Fig. 5 shows a case where the feature object 3 was erroneously recognized at the past time t-1.

[0083] As shown in FIG. 5, when the feature 3 that was erroneously recognized in the self-location estimation process at the past time t-1 is linked, the estimated location P t-1 In the example of FIG. 5, even though the feature 3 is correctly recognized at the present time t, the erroneous estimated position P t-1As a result, the accuracy of the estimated position of the vehicle 1 calculated at the current time t is reduced.

[0084] On the other hand, the re-estimated position P op t-1 is recalculated based on the estimated position of vehicle 1 up to the current time t and the observation data up to the current time t, so the estimated position P t-1 In this way, in the case shown in FIG. 5, the estimated position P t-1 and the re-estimated position P op t-1 According to this embodiment, a difference occurs between the estimated position P t-1 and the re-estimated position P op t-1 From the magnitude of the difference between the estimated position of the vehicle 1 at the current time t, the estimated position P t-1 and the re-estimated position P op t-1 When the difference between the estimated position and the actual position is large to a certain extent, emergency control is executed, thereby making it possible to prevent the control from being continued while the estimated position is incorrect. [Explanation of symbols]

[0085] 1 vehicle, 10 control system, 100 control device, 110 processor, 120 storage device, 210 control program, 250 estimated position information, 260 observation data information

Claims

1. A control system for controlling a moving object based on an estimated position of the moving object, comprising: one or more storage devices that store observation data based on information detected by one or more sensors mounted on the moving object and an estimated position of the moving object; one or more processors; Equipped with The one or more processors: A process of calculating an estimated position of the moving object at a current time based on at least the estimated position of the moving object calculated in a previous process and the observation data up to a current time; a process of recalculating an estimated position of the moving object at a predetermined time in the past based on the estimated position of the moving object up to the present time stored in the one or more storage devices and the observation data up to the present time; a process of evaluating accuracy of the estimated position of the moving object at a current time based on a magnitude of a difference between the estimated position of the moving object at the predetermined time stored in the one or more storage devices and a recalculated estimated position of the moving object at the predetermined time; It is configured to run A control system comprising:

2. 2. The control system of claim 1, The process of evaluating the accuracy of the estimated position of the moving object at the current time includes detecting that the estimated position of the moving object at the current time is of poor accuracy when the magnitude of the difference exceeds a first threshold value; The one or more processors further include a process of performing emergency control in accordance with a magnitude of the difference when a poor accuracy of the estimated position of the moving object at the current time is detected. A control system comprising:

3. 3. The control system of claim 2, The emergency control includes: a first control that issues an alarm when the magnitude of the difference exceeds the first threshold but does not exceed a second threshold; a second control for performing evacuation by deceleration, which is executed when the magnitude of the difference exceeds the second threshold value and does not exceed a third threshold value; a third control for performing evacuation by stopping the vehicle when the magnitude of the difference exceeds the third threshold value; Includes A control system comprising:

4. A method for controlling a moving object by a computer based on an estimated position of the moving object, comprising: storing, by the computer, observation data based on information detected by one or more sensors mounted on the moving object and the estimated position of the moving object in one or more storage devices and managing the same; calculating an estimated position of the moving object at a current time based on the estimated position of the moving object calculated by the computer at least in a previous process and the observation data up to a current time; recalculating an estimated position of the moving object at a predetermined time in the past based on the estimated position of the moving object up to the present time stored in the one or more storage devices by the computer and the observation data up to the present time; evaluating accuracy of the estimated position of the moving object at the current time based on a magnitude of a difference between the estimated position of the moving object at the predetermined time stored by the computer in the one or more storage devices and the recalculated estimated position of the moving object at the predetermined time; A control method comprising:

5. A control program for causing a computer to execute a process of controlling a moving object based on an estimated position of the moving object, a process of storing and managing observation data based on information detected by one or more sensors mounted on the moving object and an estimated position of the moving object in one or more storage devices; A process of calculating an estimated position of the moving object at a current time based on at least the estimated position of the moving object calculated in a previous process and the observation data up to a current time; a process of recalculating an estimated position of the moving object at a predetermined time in the past based on the estimated position of the moving object up to the present time stored in the one or more storage devices and the observation data up to the present time; a process of evaluating accuracy of the estimated position of the moving object at a current time based on a magnitude of a difference between the estimated position of the moving object at the predetermined time stored in the one or more storage devices and a recalculated estimated position of the moving object at the predetermined time; A control program configured to cause the computer to execute the above-mentioned.

Images