Control device, control method, and control program

Abstract

To provide a control device for controlling a moving body on the basis of an estimated position, the control device being able to prevent control from being continued while the position of the moving body is incorrect.SOLUTION: A control device according to the present disclosure is configured to perform the processes of: performing self-position estimation of a moving body; correcting an estimated position obtained by the self-position estimation so that an amount of displacement relative to a position of the moving body estimated in a previous process falls within a predetermined range; regarding a corrected estimated position obtained by correcting the estimated position, as a position of the moving body in the current process and controlling the moving body, based on the corrected estimated position; detecting accuracy failure of the self-position estimation, based on a magnitude of a difference between the estimated position and the corrected estimated position; and executing emergency control according to the magnitude of the difference between the estimated position and the corrected estimated position in response to detection of the accuracy failure of the self-position estimation.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 vehicle position estimation device that estimates the position of a vehicle on a map based on an observed position calculated from the recognition result of targets around the vehicle and a predicted position calculated from the detection result of an internal sensor of the vehicle. The vehicle position estimation device executes a process of determining whether or not there is fluctuation in the estimation result, and a process of increasing the weighting of the predicted position in the position estimation so that the estimation result approaches the predicted position when it is determined that there is fluctuation in the estimation result.

[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 2020-034441 A [Patent Document 2] JP 2020-001668 A [Patent Document 3] Patent Publication No. 2021-099384 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 the control of a moving object such as autonomous driving control. The estimated position by self-location estimation may suddenly change due to a factor such as an abnormality in a sensor. If a sudden change occurs in the estimated position, the control of the moving object may not be able to follow the estimated position. In addition, there is a risk that the control of the moving object may continue while the position of the moving object is incorrect. Continuing such control is not desirable in terms of ensuring control performance. In addition, in a moving object such as a vehicle traveling with passengers on board, it is also a problem from the viewpoint of safety.

[0006] According to the technology disclosed in Patent Document 1, when an estimated position undergoes a sudden change, the estimated result is deemed to be unstable, and the weighting of the predicted position in the position estimation is increased. However, the predicted position calculated from the detection result of the internal sensor generally has historical errors. For this reason, if the weighting of the predicted position is increased, there is a risk that the control of the moving body will continue while the position of the moving body remains largely inaccurate.

[0007] In view of the above-mentioned problems, one object of the present disclosure is to provide a technology that can prevent control of a moving body based on an estimated position from continuing while the position of the moving body is incorrect. [Means for solving the problem]

[0008] A first aspect of the present disclosure relates to a control device.

[0009] The control device according to the first aspect is characterized in that it is configured to execute the following processes: a process of estimating the self-position of a moving body; a process of correcting the estimated position obtained by the self-position estimation so that the amount of displacement from the position of the moving body estimated in the previous process is within a predetermined range; a process of regarding the corrected estimated position obtained by correcting the estimated position as the position of the moving body in the current process and controlling the moving body based on the corrected estimated position; a process of detecting poor accuracy of the self-position estimation based on the magnitude of the difference between the estimated position and the corrected estimated position; and a process of performing emergency control according to the magnitude of the difference between the estimated position and the corrected estimated position upon detection of poor accuracy of the self-position estimation.

[0010] A second aspect of the present disclosure relates to a control method.

[0011] The control method according to the second aspect is characterized in that it includes the steps of: estimating the self-location of a moving body; correcting the estimated position obtained by the self-location estimation so that the amount of displacement from the position of the moving body estimated in the previous process is within a predetermined range; regarding the corrected estimated position obtained by correcting the estimated position as the position of the moving body in the current process and controlling the moving body based on the corrected estimated position; detecting poor accuracy of the self-location estimation based on the magnitude of the difference between the estimated position and the corrected estimated position; and, upon detection of poor accuracy of the self-location estimation, performing emergency control according to the magnitude of the difference.

[0012] A third aspect of the present disclosure relates to a control program for causing a computer to execute a process for controlling a 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: a process of estimating the self-position of a moving body; a process of correcting the estimated position obtained by the self-position estimation so that the amount of displacement from the position of the moving body estimated in the previous process is within a predetermined range; a process of regarding the corrected estimated position obtained by correcting the estimated position as the position of the moving body in the current process and controlling the moving body based on the corrected estimated position; a process of detecting poor accuracy of the self-position estimation based on the magnitude of the difference between the estimated position and the corrected estimated position; and a process of performing emergency control according to the magnitude of the difference between the estimated position and the corrected estimated position upon detection of poor accuracy of the self-position estimation. Effect of the Invention

[0014] According to the present disclosure, the estimated position obtained by self-location estimation is corrected so that the amount of displacement from the position of the moving body estimated in the previous process falls within a predetermined range. Then, the moving body is controlled based on the corrected estimated position obtained by the correction. This ensures the tracking ability of the control of the moving body even when a sudden change occurs in the estimated position. Furthermore, according to the present disclosure, poor accuracy of the self-location estimation is detected based on the magnitude of the difference between the estimated position and the corrected estimated position. Then, in response to the detection of poor accuracy of the self-location estimation, emergency control is executed according to the magnitude of the difference. This makes it possible to prevent the control from continuing while the position of the moving body is incorrect. [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 showing an example of correction of an estimated position. [Diagram 3] 11 is a conceptual diagram showing an example of emergency control switching determination according to the magnitude of the difference between an estimated position and a corrected 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, 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 self-location estimation, 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 perform self-location estimation, which will be described later, the map information 240 includes feature map information 241 that manages information on features on a map. The feature map information 241 manages at least information on the positions (absolute positions) of features in an absolute coordinate system on a 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 to 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 repeatedly executes a process of estimating the position of the vehicle 1 at a predetermined cycle. The estimated position information 250 manages time-series data of the position of the vehicle 1 estimated by the process executed by the processor 110.

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

[0034] 2-1. Self-location estimation process The processor 110 executes processing related to self-position estimation (self-position estimation processing) for estimating the position (absolute position) of the vehicle 1. Hereinafter, the position of the vehicle 1 will be simply referred to as the “vehicle position.” An initial value of the vehicle position is obtained by, for example, the GNSS sensor 40.

[0035] 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 M 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.

[0036]

number

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

[0038] 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 vehicle position estimated in the previous process. For example, the processor 110 calculates the amount of movement (displacement) of the vehicle 1 based on the steering angle and vehicle speed of the vehicle 1, and calculates the pre-estimated position based on the amount of movement. In addition, for example, the processor 110 calculates the pre-estimated position from a state equation configured with the vehicle position as the state and the vehicle driving information 220 as the explanatory variable. The processor 110 can obtain the vehicle position estimated in the previous process by referring to the estimated position information 250. The vehicle position estimated in the previous process is hereinafter simply referred to as the "previous position".

[0039] 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 with the relative position of the recognized feature. The processor 110 also links the recognized feature with a feature managed in the feature map information 241 based on the object information of the recognized feature. Then, 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. For example, the processor 110 corrects the pre-estimated position by a particle filter, a Kalman filter, or the like, using the vehicle position as a state and the absolute position of the recognized feature as observation data. The processor 110 regards the vehicle position obtained by correcting the pre-estimated position as the processing result of the self-location estimation process.

[0040] As an example, the processor 110 executes the self-location estimation process in this manner. In the control system 10 according to this embodiment, the processor 110 may employ an algorithm different from the self-location estimation process described above. The vehicle position obtained by the processor 110 executing the self-location estimation process is hereinafter simply referred to as the "estimated position." The estimated position may be given by a probability distribution in which each element of the vehicle position is a random variable.

[0041] 2-2. Filter processing In the control system 10 according to this embodiment, the processor 110 further executes a process (filtering process) for correcting the estimated position. In the filtering process, the processor 110 corrects the estimated position so that the amount of displacement from the previous position falls within a predetermined range. Here, the predetermined range may be appropriately set according to the environment in which the control system 10 according to this embodiment is applied. Typically, the predetermined range is a range that can be followed in the control of the vehicle 1. However, unnecessarily narrowing the predetermined range is undesirable because it significantly reduces the nominal performance of the control. The vehicle position obtained by the processor 110 executing the filtering process will hereinafter be referred to simply as the "corrected estimated position".

[0042] Below, some examples of the filtering process performed by the processor 110 will be described.

[0043] In the first example, a certain upper limit value is set for the amount of displacement for each element of the previous position, and a corrected estimated position is calculated so as to limit each element of the estimated position. According to the first example, for one element of the vehicle position, the estimated position is calculated by multiplying the estimated position by x est , previous position x lp , a certain upper limit x max Then, the processor 110 calculates the corrected estimated position by subtracting x from the following equation: out When the vehicle position includes a plurality of elements, the following formula may be applied to each element. In this case, the certain upper limit value may be different for each element.

[0044]

number

[0045] Fig. 2(A) is a conceptual diagram showing an example of correction of an estimated position by a first example of filter processing when the vehicle position is expressed by a two-dimensional coordinate position (x, y). Fig. 2(A) conceptually shows the state of correction by the first example of filter processing for several examples of estimated positions (EPi(i=0,1,...,9)) and the corrected estimated positions (CPi(i=0,1,...,9)). In Fig. 2(A), LP indicates the previous position.

[0046] As shown in FIG. 2A, the first example of the filter process causes the amount of displacement of the corrected estimated position from the previous position LP to fall within a predetermined range PR. Here, in the first example, the predetermined range PR is a range including a certain upper limit value (x max ,y max ) is a rectangular range defined by the above formula. When the estimated position is within the predetermined range PR, the estimated position and the corrected estimated position match. For example, in FIG. 2A, the estimated positions EP1, EP2, and EP3 match the corrected estimated positions CP1, CP2, and CP3.

[0047] In a second example, a predetermined range including the previous position is defined, and the corrected estimated position is calculated so as to be within the defined range. The predetermined range including the previous position may be, for example, an ellipse with the previous position as the center. In this case, according to the second example, the vehicle position is assumed to be a two-dimensional coordinate position, and the estimated position is calculated as (x est ,y est ), the previous position is (x lp ,y lp ), and the parameters that define the length of the major and minor axes of the ellipse are (x max ,y max ), the processor 110 calculates the corrected estimated position by the following formula (x out ,y out ) can be used to calculate the vehicle position. This can be easily expanded to cases where the vehicle position is a three-dimensional coordinate position. In addition, when the vehicle position includes the attitude of the vehicle 1, the following formula can be similarly applied to the attitude of the vehicle 1.

number

[0048] FIG. 2B is a conceptual diagram showing an example of correction of the estimated position by the second example of the filter processing when the vehicle position is expressed by a two-dimensional coordinate position (x, y). Here, the predetermined range PR including the previous position LP is a range including the parameter (x max ,y max 2B, in the second example as well, the amount of displacement of the corrected estimated position with respect to the previous position LP falls within the predetermined range PR.

[0049] In the second example, the predetermined range can be defined so as to give a certain upper limit to the displacement rate with respect to the previous position. For example, in the above formula, the parameters defining the lengths of the major and minor axes of the ellipse can be defined as (x max ,y max )=(Δt×v xmax ,Δt×v ymax ), then, for each element of the vehicle position, the displacement rate from the previous position is limited to a certain upper limit (v xmax ,v ymax ) can be specified. Here, Δt is the time difference between the previous process and the current process.

[0050] The third example is to calculate the corrected estimated position so as to limit each element of the estimated position by setting a certain upper limit value for the displacement rate for each element of the previous position. According to the third example, a certain upper limit value is set for one element of the vehicle position by v max If the time difference between the previous process and the current process is Δt and other expressions are the same as those in the first example, the processor 110 calculates the corrected estimated position by the following formula: out It can be calculated as follows.

[0051]

number

[0052] According to the third example, the corrected estimated position can be set so that the displacement amount from the previous position falls within a predetermined range. In particular, from the viewpoint of the displacement rate, a predetermined range can be given as a range that can be followed in the control of the vehicle 1. The predetermined range in the third example is the same as that shown in FIG. 2A, and is set to a certain upper limit value v with the previous position LP as the center. max The area is the rectangle specified by

[0053] As described above, the processor 110 executes the filter process to calculate a corrected estimated position by correcting the estimated position so that the amount of displacement from the previous position falls within a predetermined range. In the control system 10 according to this embodiment, the corrected estimated position is ultimately estimated to be the position of the vehicle 1. In other words, the previous position is the corrected estimated position calculated in the previous process. The processor 110 stores the corrected estimated position calculated by executing the filter process in the estimated position information 250 of the storage device 120 as the position of the vehicle 1 estimated in the current process.

[0054] 2-3.Inaccuracy detection process The processor 110 executes a process (poor accuracy detection process) for detecting poor accuracy of self-location estimation based on the magnitude of the difference between the estimated position obtained by executing the self-location estimation process and the corrected estimated position obtained by executing the filter process. The corrected estimated position is calculated so that the amount of displacement from the previous position is within a predetermined range. Therefore, a large difference between the estimated position and the corrected estimated position is considered to indicate that the estimated position is rapidly fluctuating from the previous position. Therefore, the magnitude of the difference between the estimated position and the corrected estimated position is an index for evaluating the accuracy of self-location estimation.

[0055] Here, the difference between the estimated position and the corrected estimated position is given, for example, as follows.

[0056] When the vehicle position is represented by a vector, the difference between the estimated position and the corrected estimated position can be the vector of the difference between the vector representing the estimated position and the vector representing the corrected 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 and the corrected 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 and the corrected estimated position is calculated by using the rigid transformation matrix representing the estimated position as M est , the rigid body transformation matrix representing the corrected estimated position is M out Then, M in the following equation dif It can be calculated as follows: dif By extracting the translation vector r of the coordinate system, a vector representation of the difference between the estimated position and the corrected estimated position can be obtained. dif By extracting the rotation matrix R and converting it into Euler angles, a vector representation of the difference between the estimated position and the corrected estimated position regarding the attitude of the vehicle 1 can be obtained.

[0058]

number

[0059] In the accuracy failure detection process, the processor 110 can be configured to detect that the self-location estimation is inaccurate when, for example, the magnitude of the difference between the estimated location and the corrected estimated location exceeds a predetermined threshold (first threshold). The fact that the magnitude of the difference exceeds the first threshold can be determined, for example, when the norm of a vector representing the difference exceeds the first threshold, with the first threshold being a scalar value. Alternatively, it can be determined when the vector representing the difference falls outside the region, with the first threshold being the boundary of the region in the 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 when the magnitude of the difference exceeds the first threshold a certain number of times or more during the certain period of time.

[0060] 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 corrected estimated position obtained by executing the filter 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 automatic driving of the vehicle 1, the processor 110 generates a driving plan including driving judgment and a driving trajectory from the corrected 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.

[0061] In particular, when the processor 110 detects that the accuracy of the self-location estimation is poor in the accuracy detection process, the processor 110 executes a process of performing emergency control according to the magnitude of the difference between the estimated position and the corrected estimated position. Examples of emergency control include a procedure for interrupting the automatic driving, issuing an alarm to the operator of the vehicle 1, control for evacuation by deceleration, control for evacuation by stopping, and the like.

[0062] The processor 110 may be configured to switch between these emergency controls depending on the magnitude of the difference between the estimated position and the corrected estimated position. For example, for the vector of the difference between the estimated position and the corrected estimated position, the processor 110 determines whether to switch between the 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). 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 self-location estimation 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.

[0063] 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.

[0064] 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.

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

[0066] In step S110, the processor 110 executes a self-location estimation process to calculate an estimated location.

[0067] In step S120, the processor 110 performs a filtering process to calculate a corrected estimated position.

[0068] In step S130, processor 110 calculates the difference between the estimated position calculated in step S110 and the corrected estimated position calculated in step S120.

[0069] 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).

[0070] If the magnitude of the difference calculated in step S130 exceeds the first threshold (step S141; Yes), the processor 110 detects that the self-location estimation 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).

[0071] 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.

[0072] 3.Effects As described above, according to this embodiment, the estimated position obtained by self-position estimation is corrected so that the amount of displacement relative to the position of the vehicle 1 estimated in the previous process falls within a predetermined range. Then, the vehicle 1 is controlled based on the corrected estimated position obtained by the correction. As a result, even if a sudden change occurs in the estimated position, the tracking ability of the control of the vehicle 1 can be ensured. In addition, smooth behavior of the vehicle 1 can be realized. Furthermore, according to this embodiment, the accuracy of the self-position estimation is detected based on the magnitude of the difference between the estimated position and the corrected estimated position. Then, in response to the detection of the accuracy of the self-position estimation, emergency control is executed according to the magnitude of the difference. As a result, when the magnitude of the difference between the estimated position and the corrected estimated position is excessive and there is a risk that control will continue with the position of the vehicle 1 being incorrect, automatic driving can be interrupted, etc. In addition, it is possible to prevent control from continuing with the position of the vehicle 1 being incorrect.

[0073] Fig. 5 is a conceptual diagram showing an example of control of the vehicle 1 by the control system 10 according to this embodiment. Fig. 5 shows a scene in which the vehicle 1 is about to change lanes by autonomous driving. Fig. 5 shows an estimated position EP and a corrected estimated position CP calculated at each time (t1, , t6).

[0074] At time t1, the estimated position EP and the corrected estimated position CP are the same. Next, at time t2, the estimated position EP and the corrected estimated position CP are almost the same, although there is a slight difference. Therefore, from time t1 to t2, the control device 100 controls the vehicle 1 by normal control.

[0075] Next, at time t3, a sudden change occurs in the estimated position EP. On the other hand, since the control device 100 performs control based on the corrected estimated position CP, the influence of the sudden change on the control is suppressed. Furthermore, since the difference between the estimated position EP and the corrected estimated position CP is relatively large at time t3, the control device 100 detects poor accuracy of the self-location estimation and decelerates the vehicle 1 by emergency control. As a result, it is expected that the difference between the estimated position EP and the corrected estimated position CP will become smaller after time t3. At this time, the control device 100 may be configured to issue an alarm to the operator of the vehicle 1 to notify the operator that poor accuracy of the self-location estimation has been detected and control to decelerate the vehicle 1 is being performed.

[0076] Next, at time t5, the difference between the estimated position EP and the corrected estimated position CP becomes smaller to a certain extent. At this time, the control device 100 may be configured to return to normal control and control the vehicle 1. Alternatively, the control device 100 may be configured to notify the operator that the accuracy of the self-location estimation has been resolved. In this case, it is assumed that the operator will decide to return to normal control in consideration of safety. Then, upon receiving the operator's decision to return to normal control, the control device 100 returns to normal control and controls the vehicle 1. Note that, if the difference between the estimated position EP and the corrected estimated position CP becomes even larger after time t3, the control device 100 may execute emergency control to ensure safety, such as stopping the vehicle 1.

[0077] In this way, according to this embodiment, it is possible to realize smooth behavior of the vehicle 1 while preventing the control from continuing with the vehicle 1 in an incorrect position. [Explanation of symbols]

[0078] 1 vehicle, 10 control system, 100 control device, 110 processor, 120 storage device, 210 control program, EP estimated position, CP corrected estimated position, LP previous position, PR predetermined range

Claims

1. A process of estimating a self-location of a moving object; a process of correcting the estimated position obtained by the self-position estimation so that a displacement amount from the position of the moving body estimated in a previous process falls within a predetermined range; a process of regarding a corrected estimated position obtained by correcting the estimated position as the position of the moving object in a current process, and controlling the moving object based on the corrected estimated position; a process of detecting a lack of accuracy of the self-location estimation based on a magnitude of a difference between the estimated location and the corrected estimated location; A process of performing emergency control in response to detection of poor accuracy of the self-location estimation in accordance with the magnitude of the difference; A control device configured to execute the above.

2. The control device according to claim 1 , Correcting the estimated position includes correcting the estimated position by setting a certain upper limit value to a displacement rate with respect to the position of the moving body estimated in the previous process so as to limit the estimated position. A control device comprising:

3. The control device according to claim 1 or 2, The process of detecting poor accuracy of the self-location estimation includes detecting that the self-location estimation is poor in accuracy when the magnitude of the difference exceeds a first threshold value; The emergency control includes: a first control for issuing an alarm, which is executed 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 device comprising:

4. Estimating a self-location of a moving object; correcting the estimated position obtained by the self-position estimation so that a displacement amount from the position of the moving body estimated in a previous process falls within a predetermined range; a process of regarding a corrected estimated position obtained by correcting the estimated position as the position of the moving object in a current process, and controlling the moving object based on the corrected estimated position; detecting a lack of accuracy of the self-location estimation based on a magnitude of a difference between the estimated position and the corrected estimated position; In response to the detection of poor accuracy of the self-location estimation, performing emergency control according to the magnitude of the difference; A control method comprising:

5. A control program for causing a computer to execute a process for controlling a moving object, A process of estimating a self-location of the moving object; a process of correcting the estimated position obtained by the self-position estimation so that a displacement amount from the position of the moving object estimated in the previous process falls within a predetermined range; a process of regarding a corrected estimated position obtained by correcting the estimated position as the position of the moving object in a current process, and controlling the moving object based on the corrected estimated position; a process of detecting a lack of accuracy of the self-location estimation based on a magnitude of a difference between the estimated location and the corrected estimated location; A process of performing emergency control in response to detection of poor accuracy of the self-location estimation in accordance with the magnitude of the difference; A control program configured to cause the computer to execute the above-mentioned.

Images