Control device and control method, program, and mobile object

1. A control device comprising:

a computer having a central processing unit configured by execution of computer readable instructions to implement

a self-position detector that detects a self-position of the control device on a basis of sensor information applied to the computer;

a position change detector that detects a position change of the control device that is unpredictable in advance on a basis of a detection result by the self-position detector; and

a self-location estimator that is configured to

estimate a self-location of the control device with a first estimation model on a basis of the sensor information; and

estimate a self-location of the control device with a second estimation model different from the first estimation model under a condition that the position change that is unpredictable in advance is detected by the position change detector;

wherein the self-location estimator includes

a time-series information accumulator that accumulates the sensor information therein as time-series information;

a time-series information self-location estimator configured to

use time-series information accumulated in the time-series information accumulator to estimate the self-location of the control device, and

output an estimation result as a time-series information self-location,

wherein in the time-series information accumulator, the time-series information of a past time that has been accumulated is reset in a case where the position change that is unpredictable in advance is detected, and

the time-series information self-location estimator

uses time-series information accumulated in the time-series information accumulator to estimate the self-location, as the first estimation model; and

uses time-series information accumulated in the time-series information accumulator after the time-series information of a past time that has been accumulated in the time-series information accumulator is reset, to estimate the self-location, as the second estimation model.

2. The control device according to claim 1 , wherein

by the reset, the time-series information of a past time that has been accumulated in the time-series information accumulator is deleted, in an order from time-series information accumulated longer than a predetermined time.

3. The control device according to claim 1 , wherein

the time-series information self-location estimator reduces a weight of a self-location estimated with the time-series information as the time-series information has been accumulated for a longer time in the time-series information accumulator after the reset is performed on time-series information of a past time that has been accumulated in the time-series information accumulator, to estimate the time-series information self-location.

4. The control device according to claim 1 , wherein the self-location estimator includes

a time-series information self-location estimator configured to estimate a time-series information self-location by using a Kalman filter that repeats a step of updating an observation value of a prior probability density distribution with time-series information that is latest, wherein

the time-series information self-location estimator

estimates the time-series information self-location by using the Kalman filter that repeats a step of updating an observation value of a prior probability density distribution with time-series information that is latest, as the first estimation model, and

estimates the time-series information self-location by using the Kalman filter after maximizing a weight of the time-series information that is latest, as the second estimation model, in a case where the position change that is unpredictable in advance is detected.

5. The control device according to claim 1 , wherein the self-location estimator includes:

a time-series information accumulator that accumulates as time-series information sensed with the sensor information;

a time-series information self-location estimator configured to use time-series information accumulated in the time-series information accumulator to estimate the self-location, and output an estimation result as a time-series information self-location; and

a current information self-location estimator configured to estimate the self-location on a basis of current information that is current information sensed with the sensor information, and output as a current information self-location,

wherein the self-location estimator

adopts the time-series information self-location as a self-location estimation result in a case where the position change that is unpredictable in advance is not detected, and

adopts the current information self-location as a self-location estimation result in a case where the position change that is unpredictable in advance is detected.

6. The control device according to claim 1 , wherein the self-location estimator

causes transition of an operation mode to at least a first mode, a second mode, and a third mode in accordance with a detection result of the position change detector;

sets the operation mode to the first mode at a start of an operation and estimates the self-location with the first estimation model;

causes, in the first mode, transition of the operation mode to the second mode and notifies that the position change that is unpredictable in advance has been detected in a case where the position change that is unpredictable in advance is detected by the position change detector;

causes, in the second mode, transition of the operation mode to the third mode after a predetermined time has elapsed, and estimates the self-location with the second estimation model; and

causes, in the third mode, transition of the operation mode to the first mode after a predetermined time has elapsed.

7. The control device according to claim 6 , wherein the self-location estimator includes:

a time-series information accumulator that accumulates the sensor information as time-series information;

a time-series information self-location estimator configured to use time-series information accumulated in the time-series information accumulator to estimate the self-location, and output an estimation result as a time-series information self-location; and

a current information self-location estimator configured to

estimate the self-location on a basis of current information that is current information sensed with the sensor information;

output as a current information self-location;

adopts, in the first mode, as a self-location estimation result, the time-series information self-location estimated by the time-series information self-location estimator with use of time-series information accumulated in the time-series information accumulator, as the first estimation model; and

adopts, in the third mode, as a self-location estimation result, the current information self-location estimated by the current information self-location estimator with use of the current information, as the second estimation model.

8. The control device according to claim 7 , wherein

the position change that is unpredictable in advance includes a change to a state in which a change in the self-location and a position loses continuity, and

the first mode includes a normal mode, the second mode includes an uncertain self-location mode, and the third mode includes a current information self-location estimation mode.

9. The control device according to claim 8 , wherein

in a case where the operation mode is the current information self-location estimation mode that is the third mode, when the position change that is unpredictable in advance is detected, the self-location estimator causes transition of the operation mode to the uncertain self-location mode that is the second mode.

10. The control device according to claim 8 , wherein

a state where a change in the self-location and a position loses continuity includes a kidnap state, a slip state of a sole contact surface and a wheel, getting on and off of a non-fixed object, and a sensor abnormal state.

11. The control device according to claim 7 , wherein

the position change that is unpredictable in advance includes a change to an accident occurrence state, and

the first mode includes a normal mode, the second mode includes a right-after-accident mode, and the third mode includes a control recovery mode.

12. The control device according to claim 11 , wherein

the accident occurrence state includes a collision with a person, an object, and another machine or vehicle, a puncture, a missing part, and a falling state of a part and a cargo.

13. The control device according to claim 7 ,

wherein the time-series information includes:

three-dimensional point cloud information detected by LIDAR,

location information,

a position,

a velocity,

an acceleration, and

an angular velocity detected by a wheel encoder,

wherein the time-series information self-location estimator

estimates the self-location by using a Kalman filter or a particle filter on a basis of the time-series information, and

outputs an estimation result as a time-series information self-location, the current information includes a parallax image captured by a stereo camera, and

wherein the current information self-location estimator

generates a depth image from the parallax image,

extracts an image feature amount from the depth image,

estimates a self-location on a basis of the image feature amount, and

outputs an estimation result as a current information self-location.

14. The control device according to claim 1 , wherein

the sensor information used when the self-position detector detects the self-position of the control device on a basis of sensor information applied to the computer includes a vertical acceleration, and

the self-position detector includes:

a frequency component extractor configured to extract a frequency component of a change in the vertical acceleration;

a feature amount extractor configured to extract a feature amount from a frequency component extracted by the frequency component extractor; and

a position detector configured to detect the self-position on a basis of the feature amount.

15. The control device according to claim 14 , further comprising:

a database in which the position and a feature amount of the frequency component are registered in association with each other by learning, on a basis of sensor information for a known position,

wherein the position detector detects the position by searching for a corresponding position from the database on a basis of the feature amount.

16. The control device according to claim 15 , wherein

in the database, the position and a feature amount of the frequency component are registered in association with each other by learning using a neural network, on a basis of the sensor information for the known position.

17. A control method comprising:

a self-position detection process of detecting a self-position of a control device on a basis of sensor information applied to a computer;

a position change detection process of detecting a position change of the control device that is unpredictable in advance on a basis of a detection result by the self-position detection process; and

a self-location estimation process of

estimating a self-location of the control device with a first estimation model on a basis of the sensor information, and

estimating a self-location of the control device with a second estimation model different from the first estimation model under a condition that the position change that is unpredictable in advance is detected in the position change detection process;

wherein the self-location estimation process includes

a time-series information accumulation process of accumulating the sensor information therein as time-series information; and

a time-series information self-location process of estimating the time-series information accumulated in the time-series information accumulation process to estimate the self-location of the control device, and output an estimation result as a time-series information self-location,

wherein in the time-series information accumulation process, the time-series information of a past time that has been accumulated is reset in a case where the position change that is unpredictable in advance is detected, and

the time-series information self-location process

uses time-series information accumulated in the time-series information accumulation process to estimate the self-location, as the first estimation model; and

uses time-series information accumulated in the time-series information accumulation process after the time-series information of a past time that has been accumulated in the time-series information accumulation process is reset, to estimate the self-location, as the second estimation model.

18. A non-transitory computer readable medium having a computer program product with computer readable instructions therein that upon execution by a computer cause the computer to function as:

a self-position detector that detects a self-position of a control device on a basis of sensor information applied to the computer;

a position change detector that detects a position change of the control device unpredictable in advance on a basis of a detection result by the self-position detector; and

a self-location estimator that is configured to

estimate a self-location of the control device with a first estimation model on a basis of the sensor information, and

estimate a self-location of the control device with a second estimation model different from the first estimation model under a condition that the position change that is unpredictable in advance is detected by the position change detector

wherein the self-location estimator includes

a time-series information accumulator that accumulates the sensor information therein as time-series information; and

a time-series information self-location estimator configured to use the time-series information accumulated in the time-series information accumulator to estimate the self-location of the control device, and output an estimation result as a time-series information self-location,

wherein in the time-series information accumulator, the time-series information of a past time that has been accumulated is reset in a case where the position change that is unpredictable in advance is detected, and

the time-series information self-location estimator

uses time-series information accumulated in the time-series information accumulator to estimate the self-location, as the first estimation model; and

uses time-series information accumulated in the time-series information accumulator after the time-series information of a past time that has been accumulated in the time-series information accumulator is reset, to estimate the self-location, as the second estimation model.

19. A mobile object comprising:

a computer having a central processing unit configured to execute computer readable instructions to cause the computer to implement:

a self-position detector that detects a self-position of the mobile object on a basis of sensor information applied to the computer;

a position change detector that detects a position change of the mobile object unpredictable in advance on a basis of a detection result by the self-position detector;

a self-location estimator that is configured to

estimate a self-location of the mobile object with a first estimation model on a basis of the sensor information, and

estimate a self-location of the mobile object with a second estimation model different from the first estimation model under a condition that the position change that is unpredictable in advance is detected by the position change detector;

an action plan generator configured to generate an action plan on a basis of a peripheral situation of a self-location of the mobile object estimated by the self-location estimator; and

a controller configured to control an operation of the mobile object on a basis of an action plan determined by the action plan generator;

wherein the self-location estimator includes

a time-series information accumulator that accumulates the sensor information therein as time-series information; and

a time-series information self-location estimator configured to use the time-series information accumulated in the time-series information accumulator to estimate the self-location, and output an estimation result as a time-series information self-location wherein in the time-series information accumulator, the time-series information of a past time that has been accumulated is reset in a case where the position change that is unpredictable in advance is detected, and

the time-series information self-location estimator

uses time-series information accumulated in the time-series information accumulator to estimate the self-location, as the first estimation model; and

uses time-series information accumulated in the time-series information accumulator after the time-series information of a past time that has been accumulated in the time-series information accumulator is reset, to estimate the self-location, as the second estimation model.