CROSS REFERENCE TO RELATED APPLICATIONS

This application is a U.S. National Phase of International Patent Application No. PCT/JP2018/041095 filed on Nov. 6, 2018, which claims priority benefit of Japanese Patent Application No. JP 2017-222457 filed in the Japan Patent Office on Nov. 20, 2017. Each of the above-referenced applications is hereby incorporated herein by reference in its entirety.

TECHNICAL FIELD

The present disclosure relates to a control device, and a control method, a program, and a mobile body, and more particularly relates to a control device, and a control method, a program, and a mobile body capable of efficiently searching surroundings.

BACKGROUND ART

In order to achieve autonomous traveling of a mobile body such as a robot, it is necessary to recognize and estimate an own position thereof, which is a starting point for planning an action. Accordingly, a technology has been proposed to recognize a surrounding situation of itself by a device such as a sensor, estimate the own position, and plan autonomous traveling.

For example, in a case of sequentially estimating the own position using information of own positions continuously detected in the past, if traveling to an own position completely different from a previous own position occurs due to some external force, continuity with information of the own position in the past is lost, making the own position unrecognizable.

In such a case, in order to re-estimate the own position, the mobile body needs to search the surroundings while traveling around itself, sense the surroundings to obtain information, and estimate the own position from the obtained surrounding information.

At this time, if the mobile body travels around itself at random, it may repeatedly sense an area that has already been sensed, and it is possible that the surroundings cannot be searched efficiently.

Accordingly, it is conceivable to apply a technique to travel while covering the entire area by using a mapping module that indicates “unsearched”, “traversed”, “edge”, and “occupied” (see Patent Document 1).

Furthermore, it is conceivable to apply a technique to store indoor structure data indicating a sensed travelable area in an already searched place, and to sequentially travel in a travelable direction (see Patent Document 2).

Moreover, it is conceivable to apply a technique to store an operation of searching for an obstacle or store a sensed area in order to obtain an area where the own apparatus can travel (see Patent Documents 3, 4).

CITATION LIST

Patent Document

Patent Document 1: Japanese Patent Application Laid-Open No. 2016-095858Patent Document 2: Japanese Patent Application Laid-Open No. 2010-217995Patent Document 3: Japanese Patent Application No. 2003-092347Patent Document 4: Japanese Patent Application No. 2002-073770

SUMMARY OF THE INVENTION

Problems to be Solved by the Invention

However, by the technique of Patent Document 1, since it is a technique to travel while covering the entire area, the surroundings cannot be searched efficiently.

Furthermore, by the technique of Patent Document 2, a travelable area obtained from information at the time of search is stored, but cannot be used as information for efficiently searching the surroundings.

Moreover, by the techniques of Patent Documents 3, 4, an operation of searching for an obstacle or a sensed area is stored in order to obtain an area where the own apparatus can travel but the surface of the obstacle is neither sensed nor stored, and the surroundings cannot be efficiently searched according to the surface condition of the obstacle.

The present disclosure has been made in view of such situations, and has been made in particular to enable efficient search of surroundings.

Solutions to Problems

A control device according to an aspect of the present disclosure includes an own position estimation unit that estimates an own position, an obstacle position information generation unit that generates obstacle position information including a position of an obstacle in surroundings, a surface sensing unit that senses a surface of the obstacle, a surface-sensed area recording unit that records a surface-sensed area of the obstacle on the basis of the own position, the obstacle position information, and a surface sensing possible range of the surface sensing unit, and a route planning unit that plans a route on the basis of the surface-sensed area of the obstacle.

The surface-sensed area recording unit can record, as the surface-sensed area of the obstacle, a surface of the obstacle within the surface sensing possible range of the surface sensing unit as the surface-sensed area of the obstacle.

In the route planning unit, the route planning unit can include a priority setting unit that sets an incomplete area where surface sensing of the obstacle has not been completed, on the basis of the surface-sensed area of the obstacle and the obstacle position information, sets a divided area obtained by dividing the incomplete area as waypoints, and sets priorities to the waypoints. The route via the waypoints can be planned on the basis of the priorities set by the priority setting unit.

The priority setting unit can set the priorities of the waypoints according to an order of passing through the waypoints in a route on which a priority cost, which is a cost set on the basis of the priority condition, is the lowest among routes passing through all the waypoints.

The priority condition can be a total traveling distance or a total power consumption, and the priority cost can be a cost that is set according to a traveling distance or a cost set according to a power consumption.

The priority setting unit can set the priorities of the waypoints according to an order of passing through the waypoints in a route on which a priority cost, which is a cost set on the basis of the priority condition and body movement information, is the lowest among routes passing through all the waypoints.

The body movement information can be information of a maximum speed, a minimum turning radius, and a height of a crossable step on a path in a case where the priority condition is a total traveling distance, or information of a power consumption per unit distance and a charge capacity of a battery in a case where the priority condition is a total power consumption.

The route planning unit can plan the route that passes through all the waypoints on the basis of the priorities that are respectively set to the waypoints by the priority setting unit and of a traveling condition.

The route planning unit can calculate, for all routes that pass through all the waypoints, a route cost that is set on the basis of the priorities that are respectively set to the waypoints by the priority setting unit and of a traveling condition, and select a route on which the route cost is lowest.

The traveling condition can include a condition that the waypoints are passed through in descending order of priorities, a condition that traveling is performed in a shortest time even by ignoring the priorities of the waypoints, or a condition that a waypoint with a higher priority is reached earlier.

In a case where the traveling condition is the condition that a waypoint with a higher priority is reached earlier, the route planning unit can set a product of the priority and a time for reaching the waypoint as the route cost, and select a route on which the route cost is lowest among all the routes.

The own position estimation unit can include a time-series information own position estimation unit that estimates the own position by using time-series information including sensor information supplied in time series and outputs an estimation result as time-series information own position, and the surface-sensed area recording unit can record the surface-sensed area of the obstacle on the basis of the time-series information own position, the obstacle position information, and the surface sensing possible range of the surface sensing unit.

The time-series information can be three-dimensional point cloud information detected by light detection and ranging, laser imaging detection and ranging (LIDAR), and position information, posture, velocity, acceleration, and angular velocity detected by a wheel encoder, and the obstacle position information generation unit can generate, on the basis of the three-dimensional point cloud information, obstacle position information including a relative position and a direction of the obstacle in the surroundings.

The own position estimation unit can further include a current information own position estimation unit that estimates the own position, on the basis of current information including sensor information that outputs a current sensing result, and outputs an estimation result as a current information own position.

The current information can be a parallax image imaged by the surface sensing unit including a stereo camera, the current information own position estimation unit can generate a depth image on the basis of a parallax image that is the current information, extract an image feature amount from the depth image, estimate the own position on the basis of a position associated with the image feature amount, and output an estimation result as the current information own position, and the own position estimation unit can integrate the time-series information own position and the current information own position and output an integrated position as the own position.

A position image feature amount storage unit in which the position and the image feature amount are stored in association with each other can be further included, in which when integrating the time-series information own position and the current information own position and outputting an integrated position as the own position, the own position estimation unit stores the image feature amount corresponding to the current information own position and the own position in association with each other in the position image feature amount storage unit.

In a case where the own position is in an indefinite state, the surface-sensed area recording unit can record the surface-sensed area of the obstacle, on the basis of the own position, the obstacle position information, and the surface sensing possible range of the surface sensing unit.

A control method according to an aspect of the present disclosure includes an own position estimation process that estimates an own position, an obstacle position information generation process that generates obstacle position information including a position of an obstacle in surroundings, a surface sensing process that senses a surface of the obstacle, a surface-sensed area recording process that records a surface-sensed area of the obstacle on the basis of the own position, the obstacle position information, and a surface sensing possible range of the surface sensing process, and a route planning process that plans a route on the basis of the surface-sensed area of the obstacle.

A program according to an aspect of the present disclosure includes an own position estimation unit that estimates an own position, an obstacle position information generation unit that generates obstacle position information including a position of an obstacle in surroundings, a surface sensing unit that senses a surface of the obstacle, a surface-sensed area recording unit that records a surface-sensed area of the obstacle on the basis of the own position, the obstacle position information, and a surface sensing possible range of the surface sensing unit, and a route planning unit that plans a route on the basis of the surface-sensed area of the obstacle.

A mobile body according to an aspect of the present disclosure includes an own position estimation unit that estimates an own position, an obstacle position information generation unit that generates obstacle position information including a position of an obstacle in surroundings, a surface sensing unit that senses a surface of the obstacle, a surface-sensed area recording unit that records a surface-sensed area of the obstacle on the basis of the own position, the obstacle position information, and a surface sensing possible range of the surface sensing unit, a planning unit that plans a route on the basis of the surface-sensed area of the obstacle, an action plan generation unit that generates an action plan on the basis of the route planned by the planning unit, and a control unit that controls an operation of the mobile body on the basis of the action plan determined by the action plan generation unit.

In an aspect of the present disclosure, an own position is estimated, an obstacle position information including a position of an obstacle in surroundings is generated, a surface of the obstacle is sensed, a surface-sensed area of the obstacle is recorded on the basis of the own position, the obstacle position information, and the surface sensing possible range, and a route is planned on the basis of the surface-sensed area of the obstacle.

Effects of the Invention

According to an aspect of the present disclosure, it is particularly possible to efficiently search surroundings.

