JP6724439B2 - Autonomous mobile device, autonomous mobile system, autonomous mobile method and program - Google Patents

Description

The present invention relates to an autonomous mobile device, an autonomous mobile system, an autonomous mobile method, and a program.

2. Description of the Related Art Autonomous mobile devices that move autonomously according to their use have become popular. For example, an autonomous mobile device that autonomously moves for indoor cleaning is known. Generally, such an autonomous mobile device needs to create a map of the real space and estimate its own position in the real space. A SLAM (Simultaneous Localization And Mapping) method is known as a method for creating a map of a real space. The basic principle of SLAM technology using a monocular camera is described in Non-Patent Document 1 and Non-Patent Document 2, and by tracking the same feature point from multiple frames of a moving image captured by the camera, A process of estimating the three-dimensional position of the own device (camera position) and the three-dimensional position of the feature point (collectively forming the information of the map) alternately or simultaneously is performed.

When the calculation processing such as image recognition and voice recognition of the autonomous mobile device is processed by an external computing device connected to the network, the network communication function is required, but the calculation power and storage capacity of the autonomous mobile device are saved. (Along with them, the omission of the cooling mechanism and the weight reduction of the battery etc.), the large scale of the database, the real time sharing, the ease of updating, and many other advantages can be obtained.

As a prior art of a mobile robot that performs such wireless communication, Patent Document 1 discloses that even when the wireless robot moves to a location where wireless communication is disconnected, it can autonomously move to a location where wireless communication can be restored. Possible mobile robots are disclosed.

JP, 2008-87102, A

Andrew J. Davison, "Real-Time Simultaneous Localization and Mapping with a Single Camera", Proceedings of the 9th IEEE International Conference on Computer Vs. 1403-1410 Richard Hartley, Andrew Zisserman, "Multiple View Geometry in Computer Vision," Second Edition, Cambridge. University Press, March 2004, chapter 9

However, in the technique disclosed in Patent Document 1, when the communication is disconnected, the original destination is ignored and the device moves to a place where communication is possible. Therefore, it is possible to move to the destination without disconnecting the communication. There wasn't.

The present invention has been made to solve the above problem, and an object of the present invention is to provide an autonomous mobile device or the like that can move to a destination so that communication is not disconnected.

In order to achieve the above object, the autonomous mobile device of the present invention,
A map creation unit that creates a map using information of a plurality of images captured by the imaging unit;
A position estimation unit that estimates the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit;
A communication quality measurement unit that measures the communication quality when communicating with an external communication device,
A communication quality map creation unit that creates a communication quality map using the position of the autonomous mobile device estimated by the position estimation unit and the communication quality measured by the communication quality measurement unit,
A route search unit that searches for a route to a destination using the map created by the map creation unit and the communication quality map created by the communication quality map creation unit,
A communication failure time acquisition unit that acquires a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the path searched by the path search unit,
The route search unit searches for a route through the communication failure area when the communication failure time is equal to or less than the allowable latency,
Furthermore, a speed setting unit for setting the speed of the autonomous mobile device is provided,
The communication failure time acquisition unit acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route search unit searches for a route passing through the defective communication area, the speed setting unit sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition unit, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The search unit searches for a route that does not pass through the poor communication area.
And wherein a call.

According to the present invention, it is possible to move to a destination without disconnecting communication.

It is a figure which shows the external appearance of the autonomous mobile device which concerns on embodiment. It is a figure which shows the structure of the autonomous mobile device which concerns on embodiment. (A) It is a figure which shows the structure via a network among the system structures which concern on embodiment. (B) It is a figure which shows the structure in direct communication among the system structures which concern on embodiment. It is a figure which shows the flowchart of the whole autonomous movement control process which concerns on embodiment. It is a figure which shows the flowchart of the process of the own machine position estimation thread in the autonomous movement control process which concerns on embodiment. It is a figure which shows the flowchart of the process of the map creation thread in the autonomous movement control process which concerns on embodiment. It is a figure which shows the specific example of a map and route cost map S (x, y). It is a figure which shows the specific example of communication quality cost map C (x, y). It is a figure which shows the flowchart of the process of the loop closing thread in the autonomous movement control process which concerns on embodiment. It is a figure which shows the flowchart of the route plan to the destination which concerns on 1st Embodiment. (A) It is a figure explaining the specific example of the route plan to the destination which concerns on 1st Embodiment with the map and route cost map S (x, y). (B) It is a figure explaining the specific example of the route plan to the destination which concerns on 1st Embodiment with the route search cost map T(x, y). It is a figure which shows the flowchart of the route planning to the destination which concerns on 2nd Embodiment. (A) It is a figure explaining the specific example of the route plan to the destination which concerns on 2nd Embodiment with the route search cost map T (x, y). (B) It is a figure explaining the specific example of the route plan to the destination which concerns on 2nd Embodiment with the communication quality cost map C (x, y). (A) It is a figure explaining another specific example of the route plan to the destination concerning a 2nd embodiment with route search cost map T (x, y). (B) It is a figure explaining another specific example of the route plan to the destination which concerns on 2nd Embodiment by the updated route search cost map T(x, y).

Hereinafter, an autonomous mobile device according to an embodiment of the present invention will be described with reference to FIG. The autonomous mobile device 100 autonomously moves according to the purpose. Examples of this application include security monitoring, indoor cleaning, pets, toys, and the like.

The autonomous mobile device 100 is externally provided with an imaging unit 41 and a drive unit 42.

The imaging unit 41 includes a monocular imaging device (camera). The imaging unit 41 acquires an image (frame) at 30 fps, for example. The autonomous mobile device 100 performs autonomous movement while recognizing its own position and the surrounding environment in real time based on the images sequentially acquired by the imaging unit 41.

The drive unit 42 is an independent two-wheel drive type, and is a moving unit including wheels and a motor. The autonomous mobile device 100 drives front and rear parallel movements (translational movements) by driving two wheels in the same direction, and performs rotation (direction change) on the spot by driving the two wheels in opposite directions to change speeds of the two wheels. By changing the drive, the turning movement (translation+rotation (direction change) movement) can be performed. In addition, each wheel is equipped with a rotary encoder.By measuring the number of rotations of the wheel with the rotary encoder and using geometrical relationships such as the diameter of the wheels and the distance between the wheels, the amount of translational movement and rotation You can calculate the quantity.

For example, if the diameter of the wheel is D and the number of rotations is R (measured by a rotary encoder), the translational movement amount of the wheel at the ground contact portion is π·D·R. Further, if the diameter of the wheels is D, the distance between the wheels is I, the rotation speed of the right wheel is _RR , and the rotation speed of the left wheel is _RL , the rotation amount of the direction change (when the right rotation is positive) is 360. It becomes (degree)*D*( _{RL- RR} )/(2*I). By sequentially adding the translational movement amount and the rotation amount, the drive unit 42 functions as so-called odometry and measures the position of the own device (the position and the direction based on the position and the direction at the start of the movement). You can

Note that a crawler may be provided instead of the wheels, or a plurality of (eg, two) feet may be provided to move by walking with the feet. In these cases as well, the position and orientation of the vehicle itself can be measured based on the movements of the two crawlers and the movements of the legs, as in the case of the wheels.

As shown in FIG. 2, the autonomous mobile device 100 includes a control unit 10, a storage unit 20, a sensor unit 30, an input unit 43, a communication unit 44, and a power supply 45, in addition to the imaging unit 41 and the driving unit 42.

The control unit 10 is configured by a CPU (Central Processing Unit) or the like, and executes a program stored in the storage unit 20 so that each unit (a map creating unit 11, a position estimating unit 12, a communication quality measuring unit 13, which will be described later), The functions of the communication quality map creation unit 14 and the route search unit 15) are realized.

The storage unit 20 includes a ROM (Read Only Memory), a RAM (Random Access Memory), and the like, and includes an image storage unit 21, a map storage unit 22, a communication quality map storage unit 23, and a route search map storage unit 24. The ROM stores a program executed by the CPU of the control unit 10 (for example, a program related to communication quality map creation and autonomous movement control processing described later) and data necessary for executing the program in advance. The RAM stores data that is created or changed during execution of the program.

The image storage unit 21 stores the image captured by the image capturing unit 41. However, in order to save the storage capacity, not all captured images may be stored, and the feature amount of the image may be stored instead of the image itself.

The map storage unit 22 stores a map (information on walls and obstacles) created by the map creation unit 11 based on information from the SLAM method and the distance sensor 31 described later.

The communication quality map storage unit 23 stores the communication quality map created by the communication quality map creating unit 14 described later.

The route search map storage unit 24 stores a route search map created from the information in the map storage unit 22 and the communication quality map storage unit 23 and used for searching a route in consideration of communication quality.

A distance sensor 31 is provided as the sensor unit 30. The distance sensor 31 is a sensor that measures the distance to surrounding walls and obstacles, and is, for example, a sensor that uses infrared rays or ultrasonic waves. Note that the distance to a wall or an obstacle may be measured using the imaging unit 41 without mounting the independent distance sensor 31. Further, a bumper sensor (not shown) for detecting a collision with another object may be provided.

As the input unit 43, an operation button for operating the autonomous mobile device 100 is provided. The operation buttons include, for example, a power button, a mode switching button (switching a cleaning mode, a pet mode, etc.), an initialization button (recreating the map creation), and the like. As the input unit 43, a microphone (not shown) that inputs a sound and a voice recognition unit that recognizes a voice of an operation instruction to the autonomous mobile device 100 may be provided.

The communication unit 44 is a wireless module for wirelessly communicating with the external communication device 200. Generally, it is a wireless LAN (Local Area Network) module using radio waves, but infrared or visible light may be used. In communication with the external communication device 200, communication may be performed via a network as shown in FIG. 3A or may be directly performed as shown in FIG. 3B. As shown in FIG. 3, not only the autonomous mobile device 100 but also the communication device 200 includes the communication unit 44.

The power source 45 is a power source for operating the autonomous mobile device 100 and is generally a built-in rechargeable battery, but may be a solar cell or a system in which power is wirelessly supplied from the floor. Is also good. When the power source 45 is a rechargeable battery, the autonomous mobile device 100 is docked at a charging station (home base) to be charged.

Next, the function of the control unit 10 will be described. The control unit 10 includes a map creation unit 11, a position estimation unit 12, a communication quality measurement unit 13, a communication quality map creation unit 14, and a route search unit 15, and creates a communication quality map described later and controls movement of the autonomous mobile device 100. And so on. Further, the control unit 10 is compatible with the multi-thread function and can advance a plurality of threads (different processing flows) in parallel.

The map creation unit 11 determines the characteristic points (Map points) estimated by using the SLAM method based on the information of the image stored in the image storage unit 21 and the information of the position and the direction of the own device at the time of capturing the image. The position, the position of the wall or the obstacle obtained based on the position and orientation information of the own device when the distance sensor 31 detects the wall or the obstacle, and the like are stored in the map storage unit 22 as map information. ..

The position estimation unit 12 estimates the position and orientation of the own device as visual odometry based on the SLAM method described later.

The communication quality measuring unit 13 measures the communication quality when the communication unit 44 communicates with the external communication device 200.

The communication quality map creation unit 14 measures the communication quality when the communication quality measurement unit 13 communicates with the external communication device 200 at each point around the autonomous mobile device 100, and the communication quality map (communication at each point) is performed. Create a map with quality recorded).

The route search unit 15 searches for a route based on the map information stored in the route search map storage unit 24.

The main flow of the autonomous movement control process of the autonomous mobile device 100 will be described with reference to FIG. The control unit 10 first sets an initial value “unset” to a variable indicating a destination (destination) (step S101). Next, the control unit 10 activates the own device position estimation thread (step S102). Then, the map creation thread is activated (step S103). Next, the loop closing thread is activated (step S104). The operation of the own-vehicle position estimation thread and the map creation thread starts generation of map information and visual odometry (information of own-vehicle position estimated using the map and the image) based on the SLAM method. After that, the control unit 10 determines whether the operation is completed (step S105), and if the operation is completed (step S105; Yes), the operation is completed, and if the operation is not completed (step S105; No), the distance sensor 31 and the visual sensor are detected. The map is updated using the odometry (step S106).

Next, the control unit 10 determines whether or not there is an instruction to move to the destination (step S107). If there is a movement instruction (step S107; Yes), the instructed destination is set as a variable indicating the destination (destination) (step S110), and a route to the destination is planned (step S111). Details of route planning to the destination will be described later. Then, the control unit 10 issues an operation instruction to the drive unit 42 to move along the route set by the route plan (step S112), and returns to step S105.

If there is no instruction to move to the destination (step S107; No), the control unit 10 determines whether the destination has not been set (step S108). When the destination is not set (step S108; Yes), free autonomous movement is performed (step S109), and the process returns to step S105. The free autonomous movement may be movement that randomly moves around, or movement that moves around the room to create a map.

If the destination is set (step S108; No), the route is set according to the route plan, and therefore the control unit 10 issues an operation instruction to the drive unit 42 in order to move along the route ( (Step S112) and the process returns to step S105. The variable indicating the destination (destination) is stored in the storage unit 20 so that the value can be referred to across threads.

By the processing of this main flow, it is possible to autonomously move while updating the map and move to the destination instructed to move through an efficient moving route. As a typical example, when the autonomous mobile device 100 is first turned on while being placed in a charging station, the autonomous mobile device 100 relies on the distance sensor 31 to move through each room of the house, and the distance sensor 31 causes a wall or the like to move. It is possible to specify the obstacle position of and to create map information including the obstacle position. Once the map has been created to some extent, it is possible to know the area where there is no map information yet, but it is considered to be movable, and it is possible to encourage the creation of a wider map by autonomously moving to that area. Like Then, if the map information of almost the entire movable area is created, the efficient movement operation using the map information becomes possible. For example, from any position in the room, it is possible to return to the charging station by the shortest route or efficiently clean the room.

Next, the self-position estimation thread started in step S102 of the main flow of the autonomous mobile device 100 (FIG. 4) will be described with reference to FIG. This thread is a process in which the position estimation unit 12 first performs an initialization process and then continues the estimation of the position of the own device (the position of the own device is estimated by visual odometry using the image acquired by the imaging unit 41).

The position estimation unit 12 first sets "initializing" in the estimated state variable (step S201). The estimated state variable indicates the state of the own device position estimation processing at that time, and takes three values of “initializing”, “tracking success”, and “tracking failure”. Next, it is determined whether the operation is completed (step S202). If the operation is ended (step S202; Yes), the operation is ended, and if the operation is not ended (step S202; No), it is determined whether or not the estimated state variable is "initializing" (step S203). If the initialization is not in progress (step S203; No), the own position estimation process after step S221 is performed, and if the initialization is in progress (step S203; Yes), the process proceeds to step S204 to perform the initialization process. First, the initialization process will be described.

In the initialization process, the position estimating unit 12 first sets -1 in the frame counter N (step S204), and the image capturing unit 41 acquires an image (step S205). The image can be acquired at, for example, 30 fps (the acquired image is also called a frame). Next, the position estimation unit 12 acquires 2D feature points included in the image acquired by the imaging unit 41 (step S206).

A 2D feature point is a characteristic part in an image, such as an edge part in the image, and may be acquired using an algorithm such as SIFT (Scale-Invariant Future Transform) or SURF (Speed-Up Robust Features). it can.

If the number of acquired 2D feature points is small, calculation by the two-view Structure from motion method described later cannot be performed, and thus the position estimation unit 12 compares the acquired number of 2D feature points with a reference value (for example, 10). If it is less than the reference value (step S207; No), the process returns to step S205 and the image acquisition and the 2D feature point acquisition are repeated until the number of 2D feature points equal to or more than the reference value is obtained. At this point in time, map information has not been created, but in the above-described typical example, since the distance sensor 31 is used as a starting point, all the rooms of the house are starting to move. If image acquisition and 2D feature point acquisition are repeated, image acquisition will be repeated while moving. Therefore, it can be expected that various images can be acquired and eventually an image having a large number of 2D feature points can be acquired.

When the number of acquired 2D feature points is equal to or greater than the reference value (step S207; Yes), the position estimation unit 12 increments the frame counter N (step S208). Then, it is determined whether the frame counter N is 0 (step S209). If the frame counter N is 0 (step S209; Yes), it means that only one image has been acquired yet, so the process returns to step S205 to acquire the second image. Although not shown in the flowchart of FIG. 5, it is preferable that the position of the self-apparatus when the first image is acquired and the position of the self-apparatus when the second image is acquired are apart from each other to some extent. The accuracy of the posture estimated in the subsequent processing is improved. Therefore, when returning from step S209 to step S205, wait until the translation distance by odometry becomes a predetermined distance (for example, 1 m) or more by the free autonomous movement in step S109 of the main flow (FIG. 4), and then step S205. You may return to.

If the frame counter N is not 0 (step S209; No), it means that two images have been acquired. Therefore, the position estimation unit 12 associates the 2D feature points between these two images (the same point in the actual environment is the same). Those that exist in each image and can be associated with each other are acquired (step S210). If the number of corresponding feature points is less than 5, the posture between two images, which will be described later, cannot be estimated. Therefore, the position estimating unit 12 determines whether the number of corresponding feature points is less than 5 (step S211). .. If it is less than 5 (step S211; Yes), the process returns to step S204 to reacquire the initial image. When the number of corresponding feature points is 5 or more (step S211; No), the posture between the two images (the difference between the positions at which the respective images are acquired (by the two-view Structure from Motion method described later) is used. The translation vector t) and the orientation difference (rotation matrix R)) can be estimated (step S212).

The pose estimation by the Two-view Structure from Motion method is specifically performed by obtaining a basic matrix E from the corresponding feature points and decomposing the basic matrix E into a translation vector t and a rotation matrix R ( See Non-Patent Document 2 for details). The value of each element of the translation vector t obtained here (assuming movement in a three-dimensional space has three elements of X, Y, and Z with the position where the first image is acquired as the origin) Since it is different from the value in the real environment (the Two-view Structure from Motion method cannot obtain the value itself in the real environment, but the value in the space similar to the real environment). These are regarded as values in the SLAM space, and will be described below using coordinates in the SLAM space (SLAM coordinates).

When the orientation (translation vector t and rotation matrix R) between two images is obtained, its value is based on the first image (the position where the first image is acquired is the origin of SLAM coordinates, the translation vector is 0 vector, and the rotation matrix is Is the unit matrix I.) is the posture of the second image (the position (translation vector t) and the orientation (rotation matrix R) of the own device when the second image is acquired). .. Here, when the orientations of the two images (the position (translation vector t) and the orientation (rotation matrix R) of the own device at the time of capturing the image (frame), also referred to as the frame orientation) are obtained, the two The map creating unit 11 obtains the 3D position in SLAM coordinates of the 2D feature points (corresponding feature points) that are in correspondence between the two images (step S213).

If the coordinates (frame coordinates: known) of the 2D feature point in the image are (u, v) and the 3D position (unknown) in SLAM coordinates of the 2D feature point is (X, Y, Z), these are the same. These relationships when expressed by the secondary coordinates are expressed by the following equation (1) using the perspective projection matrix P. Here, the symbol "~" means "equal except constant multiples of non-zero" (that is, equal or multiplied by a constant (non-zero) times), and the symbol "'" represents "transposition". ..
(Uv1)'-P(XYZ1)'... (1)

In the above equation (1), P is a 3×4 matrix, and the following equation is derived from the 3×3 matrix A indicating the internal parameters of the camera and the external parameters R and t indicating the orientation (frame orientation) of the image. It is represented by (2). Here, (R|t) represents a matrix in which the translation column vector t is arranged to the right of the rotation matrix R.
P=A(R|t)...(2)

In the above equation (2), R and t are obtained as the frame postures thereof as described above. Further, since the internal parameter A of the camera is determined by the focal length and the size of the image pickup element, it becomes a constant if the image pickup unit 41 is decided.

One of the 2D feature points that are associated between the two images is one of the frame coordinates (u ₁ , v ₁ ) of the first image and the frame coordinates (u ₂ , v _{2 of the second} image. ), the following equations (3) and (4) can be obtained. Here, I is a unit matrix, 0 is a zero vector, and (L|r) represents a matrix in which a column vector r is arranged on the right side of the matrix L.
(U ₁ v ₁ 1)′ to A(I|0)(XYZ 1)′... (3)
_{(U 2 v 2 1) '} ~A (R | t) (X Y Z 1)' ... (4)

In the above equations (3) and (4), since there are equations for u ₁ , v ₁ , u ₂ , and v _2, respectively, there are four equations, but there are three unknowns, X, Y, and Z. , X, Y, Z can be obtained, and this is the 3D position of the 2D feature point in SLAM coordinates. Since the number of equations is larger than the number of unknowns, for example, X, Y, Z obtained by u ₁ , v ₁ , u ₂ and X, Y, Z obtained by u ₁ , v ₁ , v _2. Can be different from. In such a case, the simultaneous linear equations with excess conditions are present, and there is generally no solution, but the map creation unit 11 uses the least squares method to find the most probable X, Y, and Z.

When the 3D position (X, Y, Z) in the SLAM coordinates of the 2D feature point is obtained, the map creation unit 11 stores it in the map storage unit 22 as a Map point, which is also called a Map point database (Map point DB (Database)). Registered) (step S214). As elements to be registered in the Map point database, at least "X, Y, Z which are 3D positions in SLAM coordinates of 2D feature points" and "feature amount of that 2D feature point" (for example, feature amount obtained by SIFT or the like). )is necessary.

Then, the map creation unit 11 determines whether or not all of the 2D feature points (corresponding feature points) that are associated with each other between the two images are registered in the Map point database (step S215), and all can be registered. If not registered (step S215; No), the process returns to step S213, and if all are registered (step S215; Yes), the process proceeds to step S216.

Then, the position estimation unit 12 initializes the counter NKF of the key frame (which indicates an image to be processed by the subsequent thread) to 0 (step S216), and sets the second image as the key frame in the frame database (frame). It is also referred to as a DB and is stored in the image storage unit 21) (step S217).

The elements to be registered in the frame database are the "key frame number" (the value of the key frame counter NKF at the time of registration), the "attitude" (the position in the SLAM coordinate of the device at the time of capturing the image (translation vector t), and Direction (rotation matrix R), “all extracted 2D feature points”, “point of which 3D position is known as Map point among all 2D feature points”, “feature of key frame itself”, In addition to these, the “posture in the actual environment measured by odometry” (the position and orientation of the own device, which is obtained based on the moving distance by the drive unit 42 in the actual environment) may be registered.

In the above description, the “feature of the key frame itself” is data for improving the efficiency of the process of obtaining the image similarity between the key frames, and normally a histogram of 2D feature points in the image or the like should be used. The image itself may be the “feature of the key frame itself”. Further, the “posture in the real environment measured by odometry” can be represented by the translation vector t and the rotation matrix R, but since the autonomous mobile device 100 normally moves on a two-dimensional plane, two-dimensional data is obtained. The two-dimensional coordinates (X, Y) based on the position (origin) and the direction at the start of movement and the direction φ may be simplified.

Next, the position estimation unit 12 notifies the map creation thread of the fact that a key frame has been created, in the key frame queue of the map creation thread (the queue (Queue) has a first-in first-out data structure), The key frame counter NKF is set (step S218).

Since the initialization process of the own device position estimation thread is completed as described above, the position estimation unit 12 sets “successful tracking” in the estimated state variable (step S219) and obtains the scale correspondence between SLAM coordinates and actual environment coordinates. Therefore, the scale S is obtained by dividing the translational distance by odometry (obtained by the coordinates in the actual environment) by the translational distance d by the SLAM coordinates estimated by the above processing (step S220).

Then, through steps S202 and S203, the process proceeds to step S221, which is the process in the case of initialization.

The processing when the initialization has been completed will be described. This process is a normal process of the own device position estimation thread, and the position estimation unit 12 sequentially estimates the current position and direction of the own device (translation vector t and rotation matrix R in SLAM coordinates). Processing.

First, the position estimation unit 12 captures an image with the imaging unit 41 (step S221) and increments the frame counter N (step S222). Then, the feature point acquisition unit 11 acquires the 2D feature points included in the image captured by the imaging unit 41 (step S223).

Next, the position estimation unit 12 determines whether or not the estimated state variable is “successful tracking” (step S224). If the tracking is successful (step S224; Yes), the position estimating unit 12 changes the information of the previous key frame (image whose key frame number is NKF) registered in the frame database from the information of the image. Of the included 2D feature points, the 2D feature point whose 3D position is known (being the Map point registered in the Map point database) is acquired, and the feature point acquired in step S223 among them is acquired. The number of 2D feature points (corresponding feature points) (corresponding number of feature points) that can be associated with is acquired (step S225).

If the value of the estimated state variable is not “successful tracking” (step S224; No), the position estimation unit 12 determines that the key frame is two or more previous key frames registered in the frame database (images whose key frame number is less than NKF). From among these, the key frame having the maximum number of correspondences between the Map points and the feature points acquired in step S223 and the corresponding number of feature points are acquired (step S226).

Then, the position estimating unit 12 determines whether or not the number of corresponding feature points is larger than a threshold value (for example, 30. Hereafter, referred to as “reference corresponding feature point number”) (step S227), and if the number is less than or equal to the reference corresponding feature point (step S227). In S227; No), the accuracy of the posture estimated by the SLAM method becomes poor, so "tracking failure" is set in the estimated state variable (step S230), and the position is not estimated and the process returns to step S202.

If the number of corresponding feature points is larger than the number of corresponding corresponding feature points (step S227; Yes), a key frame in the vicinity of the key frame for which the corresponding number of corresponding feature points is acquired in step S225 or step S226 (key frame for which the corresponding number of corresponding feature points is acquired) And the number of feature points (corresponding feature points) acquired in step S223 (corresponding feature point correspondence number) that can be associated with Map points included in a key frame in which feature points overlap at a predetermined ratio or more (for example, 30% or more) ) Is acquired by the position estimation unit 12 (step S228).

Then, the position estimation unit 12 determines whether or not the number of corresponding feature points is larger than a threshold value (for example, 50. Hereinafter, referred to as “second reference corresponding feature point number”) (step S229), and the second reference corresponding feature. If it is less than or equal to the score (step S229; No), "tracking failure" is set in the estimated state variable (step S230), and the position is not estimated and the process returns to step S202. If the number of corresponding feature points is larger than the number of second corresponding feature points (step S229; Yes), the process proceeds to step S231.

In step S231, the position estimation unit 12 sets the tracking success to the estimated state variable (step S231). The position estimation unit 12 acquires the 3D positions (X _i , Y _i , Z _i ) of the corresponding feature points acquired in step S228 from the Map point database, and the corresponding feature points included in the image acquired in step S221. The current attitude (the position and orientation of the own machine represented by the translation vector t and the rotation matrix R) is estimated using the correspondence relationship with (step S232).

A supplementary description will be given of the method of estimating the attitude of the own device. Let (u _i , v _i ) be the frame coordinates of the corresponding feature points included in the image acquired in step S221, and let the 3D position of the corresponding feature points be (X _i , Y _i , Z _i ) (where i is 1) to the number of corresponding feature points) and the 3D position (X _i , Y _i , Z _i ) of each corresponding feature point projected onto the frame coordinate system by the following equation (5) (ux _i , Ideally, vx _i ) and the frame coordinates (u _i , v _i ) should match.
(Ux _i vx _i 1)' to A(R|t)(X _i Y _i Z _i 1)'... (5)

However, in reality, since (X _i , Y _i , Z _i ) and (u _i , v _i ) include an error, (ux _i , vx _i ) and (u _i , v _i ) become Seldom match. Then, the unknowns are only R and t (three-dimensional in three-dimensional space, respectively, and 3+3=6 is the number of unknowns), but the mathematical expression exists twice as many as the number of corresponding feature points (for one corresponding feature point. Since there is an equation for each of u and v of the frame coordinates), a simultaneous linear equation with excessive conditions is obtained, and it is obtained by the least square method as described above. Specifically, the position estimation unit 12 obtains the posture (the translation vector t and the rotation matrix R) that minimizes the cost function E1 of the following Expression (6). This is the attitude of the ship itself in the SLAM coordinates obtained by the SLAM method (the position and orientation of the ship represented by the translation vector t and the rotation matrix R). In this way, the position estimation unit 12 estimates the attitude of the own device (step S232).

Since the current attitude (translation vector t and rotation matrix R) in SLAM coordinates has been obtained, the position estimation unit 12 multiplies this by the scale S to obtain VO (visual odometry) (step S233). The VO can be used as the position and orientation of the own device in the real environment.

Next, the position estimation unit 12 has a predetermined distance (for example, 1 m.) from the position of its own when the immediately preceding key frame (image whose key frame number is NKF) registered in the frame DB is photographed. It is determined whether or not it has moved (step S234), and if it has moved (step S234; Yes), the key frame counter NKF is incremented (step S235), and the current frame is set as a key frame. It is registered in the frame DB (step S236). If it has moved less than the reference translation distance (step S234; No), the process returns to step S202.

Here, the movement distance of the own machine compared with the reference translation distance is the translation distance from the immediately preceding key frame to the current frame (the absolute value of the vector of the difference between the translation vectors of both frames (the square root of the sum of squares of elements)). It may be obtained from odometry or may be obtained from VO described above. As described above, the contents to be registered in the frame DB are “key frame number”, “posture”, “all extracted 2D feature points”, and “point of which 3D position is known as Map point among all 2D feature points”. ", "Characteristics of the key frame itself".

Then, the position estimation unit 12 sets the key frame counter NKF in the key frame queue of the map creation thread in order to notify the map creation thread that a new key frame has occurred (step S237). Then, the process returns to step S202. The key frame counter NKF, scale S, Map point DB, frame DB, estimated state variable, and VO (visual odometry) are stored in the storage unit 20 so that the values can be referred to across threads.

Next, the map creation thread started in step S103 of the main flow of the autonomous mobile device 100 (FIG. 4) will be described with reference to FIG. In this thread, the map creation unit 11 calculates the 3D position of the corresponding feature point in the key frame and acquires the information about the Map point, and the distance sensor 31 acquires the information about the distance to the wall or the obstacle. Map information is created from the map information and stored in the map storage unit 22. Then, the communication quality map creation unit 14 creates a communication quality map using the communication quality information acquired from the communication quality measurement unit 13, and stores it in the communication quality map storage unit 23.

First, the map creation unit 11 determines whether the operation is completed (step S301). If the operation is completed (step S301; Yes), the operation is ended, and if the operation is not ended (step S301; No), it is determined whether or not the key frame queue is empty (step S302). If the key frame queue is empty (step S302; Yes), the process returns to step S301. If not empty (step S302; No), data is extracted from the key frame queue and MKF (key frame of key frame processed by map creation thread) (A variable representing a number) is set (step S303). The map creation unit 11 determines whether MKF is larger than 0 (step S304), and when MKF is 0 (step S304; No), returns to step S301 and waits for data to enter the key frame queue. When MKF is 1 or more (step S304; Yes), the process proceeds to the following process.

The map creation unit 11 refers to the frame DB and identifies 2D feature points of the previous key frame (key frame whose key frame number is MKF-1) and 2D feature points of the current key frame (key frame whose key frame number is MKF). A 2D feature point (corresponding feature point) that can be corresponded with is extracted (step S305). Since the attitudes (translation vector t and rotation matrix R) of each key frame are also registered in the frame DB, the 3D position of the corresponding feature point is processed in the same manner as in the processing at the time of initialization of the own position estimation thread. Can be calculated. The map creation unit 11 registers the corresponding feature points for which the 3D position has been calculated as Map points in the Map point DB (step S306). The map creation unit 11 also registers the 3D position for the 2D feature point for which the 3D position could be calculated this time for other key frames in the frame DB (step S307).

If the corresponding feature point extracted by the map creation unit 11 has already been registered in the Map point DB, the 3D position calculation is skipped and the next corresponding feature point (not registered in the Map point DB) is processed. The process may proceed, or the 3D position calculation may be performed again to update the 3D position registered in the Map point DB or the 3D position for the corresponding feature point in the frame DB.

Next, the map creation unit 11 determines whether or not the estimated state variable of the own device position estimation thread is “successful tracking” (step S308). If it is not “successful tracking” (step S308; No), it means that the position of the own device has not been estimated, and therefore the measurement of obstacles and communication quality is skipped and the process proceeds to step S311.

If the estimated state variable is “successful tracking” (step S308; Yes), the map creation unit 11 measures the distance to the wall or obstacle with the distance sensor 31, and determines the position and orientation of the own device indicated by VO. Based on the measured distance, the position of the wall or the obstacle and the distance from the own position to the wall or the obstacle are registered in the map storage unit 22 (step S309).

Then, the communication quality map creating unit 14 measures the communication quality by the communication quality measuring unit 13, and uses the value of the communication quality obtained by the measurement as the value of the communication quality at the own position indicated by VO to communicate. It registers in the quality map storage unit 23 (step S310).

The communication quality can be acquired by using the electric field strength and the received power that can be observed by the communication unit 44, or by using the RTT (Round-Trip Time) of the actual communication, the effective speed, the error rate, and the delay. You may use the evaluation value which compounded these various evaluation values. Further, in the case of using an infrared or visible light communication link, binary information indicating whether or not a signal has arrived (whether or not there is a shield between them) may be used. If the communication quality has already been measured at that position, the average value or median of the measurement results up to that point may be adopted, or the worst value may be adopted. Further, the average value, the median, the best value, the worst value, and the like may be stored separately and used properly according to the moving purpose of the autonomous mobile device 100.

The value registered in the communication quality map storage unit 23 is converted into a smaller positive number (cost value) as the communication quality is better, and then registered. For example, since the electric field strength and the effective speed increase as the communication quality becomes better, the reciprocal is registered.

Next, the map creation unit 11 determines whether the key frame queue is empty (step S311). If it is empty (step S311; Yes), the bundle adjustment process is performed on the postures of all the key frames and the 3D positions of all the map points to improve the accuracy (step S312). When a Map point having a large error is found as a result of the bundle adjustment process, the Map point is deleted from the Map point DB (step S313), and the process proceeds to step S314. If the key frame queue is not empty (step S311; No), the process proceeds to step S314 without doing anything.

In step S314, the map creation unit 11 stores the map/route cost map in the array S(x, y) based on the information stored in the map storage unit 22 (step S314). The map/route cost map is “S(x,y)=∞” if the coordinates (x,y) are immovable areas (walls or obstacles), and “S(x,y)=” if they are not immovable areas. It is an array in which values are stored such that the larger the shortest distance from the coordinates (x, y) to the immovable area, the smaller the positive number. For example, when the coordinate (x, y) is not the immovable area, the reciprocal of the shortest distance from the coordinate (x, y) to the immovable area can be stored in S(x, y). The array S is stored in the map storage unit 22.

FIG. 7 shows a specific example of the array S(x, y) of the map/route cost map. In FIG. 7, a black area indicates an immovable area (a wall or an obstacle), and S(x, y)=∞ in this area. The white area indicates an area where the shortest distance to the immovable area is large, and the value of S(x, y) in this area is small (for example, less than 1). The gray area indicates an area where the shortest distance to the immovable area is small, and the value of S(x, y) in this area is large (for example, 1 or more).

Next, the communication quality map creation unit 14 stores the communication quality cost map in the array C(x, y) based on the information stored in the communication quality map storage unit 23 (step S315). The communication quality cost map is an array in which a smaller positive number is stored in C(x, y) as the communication quality at the coordinates (x, y) is better. For example, the electric field strength at the coordinates (x, y) and the reciprocal of the effective velocity can be stored in C(x, y). The array C is stored in the communication quality map storage unit 23.

A concrete example of the communication quality cost map of the array C(x, y) is shown in FIG. An external communication device 200 (not shown) is present at the left end of FIG. In FIG. 8, a white area indicates an area with good communication quality, and C(x, y) in this area is small (for example, less than 1). A light gray area indicates an area with poor communication quality, and the value of C(x, y) in this area is relatively large (for example, 1 or more and less than 3). The dark gray area indicates an area in which the communication quality is so bad that communication cannot be performed, and the value of C(x, y) in this area is very large (for example, 3 or more).

Since the coordinates (x, y) that the autonomous mobile device 100 passes through are generally sparse, the values of S(x, y) and C(x, y) are often sparsely set. Therefore, interpolation/extrapolation is performed to densely set a value for each coordinate (x, y).

Next, the map creation unit 11 sets MKF in the key frame queue of the loop closing thread (step S316), and returns to step S301.

The map/route cost map array S(x, y) and the communication quality cost map array C(x, y) created by the above processing are both stored so that the values can be referred to across threads. It is stored in the unit 20.

Note that the bundle adjustment process is a non-linear optimization method that simultaneously estimates the camera posture (key frame posture) and the 3D position of the Map point, and the error that occurs when the Map point is projected on the key frame. The optimization is performed so as to minimize it.

By performing the process of the bundle adjustment, it is possible to improve the accuracy of the key frame posture and the 3D position of the Map point. However, this process does not cause a special problem because the amount of calculation is large and the accuracy cannot be improved without this process. Therefore, here, the bundle adjustment process is performed only when the key frame is empty (when other processes are relatively light). If the communication quality is good, the autonomous mobile device 100 itself does not perform the bundle adjustment process, but the information in the image storage unit 21 or the map storage unit 22 is transmitted to the external communication device 200 by the communication unit 44, and the external communication device 200 is transmitted. The result of the bundle adjustment process performed by the communication device 200 may be returned to the autonomous mobile device 100.

Further, when the bundle adjustment process is performed, a Map point having an error when projected on the key frame is larger than a predetermined value may be found. Map points having such a large error have an adverse effect on SLAM estimation, and are therefore deleted from the map point DB in step S313. It should be noted that instead of deleting from the Map point DB, a flag may be set in the Map point DB to identify a Map point requiring a great deal of error.

Next, the loop closing thread started in step S104 of the main flow of the autonomous mobile device 100 (FIG. 4) will be described with reference to FIG. In this thread, the control unit 10 continues to check whether the loop closing process can be performed, and when the loop closing process can be performed, the control unit 10 performs the loop closing process. Note that the loop closing process uses the deviation between the posture value when the user was at this same place before and the value of the current posture when he/she recognizes that he has returned to the same place where he has come before. , It means to correct the 3D position of the key frame and the related Map point in the locus from the time when it came before until now.

First, the control unit 10 determines whether the operation is completed (step S401). If the operation ends (step S401; Yes), the operation ends. If the operation is not completed (step S401; No), it is determined whether the key frame queue is empty (step S402). If the key frame queue is empty (step S402; Yes), the procedure returns to step S401. If the key frame queue is not empty (step S402; No), data is extracted from the key frame queue and the LKF (loop closing thread processes the key frame It is set to a variable representing the key frame number) (step S403). Next, the control unit 10 determines whether LKF is larger than 1 (step S404). If LKF is 0 or 1 (step S404; No), the process returns to step S401 and waits for data to enter the key frame queue. When LKF is 2 or more (step S404; Yes), the following processing is performed.

The control unit 10 refers to the frame DB, and the similarity between the current key frame (key frame whose key frame number is LKF) and the “feature of the key frame itself” is a predetermined similarity (for example, 0.9. Hereinafter, “reference image”). A key frame having the above degree is searched from the frame DB (step S405). Here, when the feature of an image (key frame) is represented by a feature vector, this similarity is obtained by normalizing the absolute values of the feature vectors of two images (square root of sum of squares of elements) to 1. The inner product of can be the similarity of the two images. Alternatively, the reciprocal of the distance (square root of the sum of squares of the differences between the elements) between the feature vectors (absolute values normalized to 1) of the two images may be used as the similarity.

The control unit 10 determines whether or not a key frame in which the similarity of the “feature of the key frame itself” is equal to or higher than the reference image similarity is found (step S406), and if not found (step S406; No) step S401. Returning to step S406, if found (step S406; Yes), the posture of the key frame in the locus from the found key frame to the current key frame and the 3D position of the Map point included in the key frame in the locus are corrected ( Step S407). For example, the control unit 10 corrects the attitude of the current key frame to be the same as the attitude of the discovered key frame. Then, using the difference between the discovered attitude of the key frame and the attitude of the current key frame, the attitude of each key frame in the locus from the discovered key frame to the current key frame is linearly corrected. Furthermore, the 3D position of the Map point included in each of these key frames is also corrected according to the correction amount of the posture of each key frame. Then, the process returns to step S401.

Since the above loop closing process requires a large amount of calculation, when the communication quality is good, the autonomous mobile device 100 itself does not perform the loop closing process, but the information in the image storage unit 21 or the map storage unit 22 is transferred to the communication unit 44. May be transmitted to the external communication device 200 and the result of the loop closing process performed by the external communication device 200 may be sent back to the autonomous mobile device 100.

Next, the route planning to the destination, which is the process of step S111 of the main flow of the autonomous mobile device 100 (FIG. 4), will be described. In the conventional route planning, when an array S(x, y) which is a map and route cost map, a current position and a destination position are given, the minimum accumulated cost route from the current position to the destination position is selected. Specifically, if each graph is constructed by assigning each coordinate as a node, an adjacency relation as an edge, and weighting nodes or edges according to the array S(x, y), the shortest path search of the graph theory such as Dijkstra algorithm can be performed. An algorithm can be used to efficiently select the minimum cumulative cost path. In the present invention, not only the array S(x, y) but also the array C(x, y) which is a communication quality map is used, so that a route that maintains the communication quality can be selected.

(First embodiment)
Now, a first embodiment of the route planning to the destination will be described with reference to FIG. First, the route search unit 15 sets the threshold value to a value when the communication quality is considered to be good (step S501). Next, x and y are initialized to 0 in order to update the values for all the coordinates of each array (step S502).

Next, the route search unit 15 substitutes the value of (x, y) of the array S which is the map/route cost map into the array T(x, y) which is the route search cost map (step S503). The array T is an array used in place of the conventional map/route cost map S when searching for a route, and is stored in the route search map storage unit 24.

Next, the route search unit 15 determines whether the value of (x, y) of the array C, which is the communication quality map, is larger than the threshold value (step S504). If it is larger than the threshold value (step S504; Yes), it means that the communication quality at the coordinates (x, y) is not good, so that T(x, y) is set to ∞ in order to prevent passing through the place. Is set (step S505), and the process proceeds to step S506. If it is less than or equal to the threshold value (step S504; No), it means that the communication quality at that location (x, y) is good, so the value of T(x, y) remains unchanged at S(x, y). It proceeds to step S506.

In step S506, the route search unit 15 determines whether or not the processes of steps S503 to S505 have been completed for all x and y (step S506). If not completed (step S506; No), the process returns to step S503.

When the processes of steps S503 to S505 have been completed for all x and y (step S506; Yes), it means that the setting of the array T(x, y) which is the route search cost map has been completed. The unit 15 performs the route search by applying the conventional route planning algorithm to T (step S507).

Then, the route search unit 15 determines whether or not the route could be found in step S507 (step S508). When the route is found (step S508; Yes), the process ends. When the route does not exist (step S508; No), the route search unit 15 raises the threshold value (step S509), returns to step S502, and repeats the retry.

In addition, when the value of the threshold value is so large as to indicate that the communication quality is “incommunicable” in step S509, the route search is abandoned and terminated. Although not shown in the figure because the flow chart becomes complicated, in this case, it is necessary to start over from the setting of the destination in step S110 of the main flow (FIG. 4).

It should be noted that, before performing this processing, it is possible to expand, contract, etc. the area of poor communication quality (area of light gray or dark gray in FIG. 8) indicated by the array C(x, y) which is the communication quality map. Good. If you want to communicate more reliably, expand the area with poor communication quality. In addition, when it is desired to allow a wide range of motion by allowing momentary interruption of communication during movement, a region with poor communication quality is contracted.

A specific example of the route search will be described with reference to FIGS. 7, 8 and 11. For example, when the route from the current position to the destination position shown in FIG. 11 is searched by the conventional method using the map/route cost map S(x, y) shown in FIG. 7, the arrow in FIG. The route shown is selected. On the other hand, if the communication quality cost map C(x, y) shown in FIG. 8 is applied to the above-described first embodiment, the communication quality is poor except for the white part in FIG. The route search cost map T(x, y) shown is obtained, and if the route from the current position to the destination position is searched in this figure, the route shown by the dotted arrow in FIG. 11B is selected. As described above, by using the route search cost map in consideration of the communication quality, it is possible to follow the route in which the communication is not disconnected and move to the destination.

(Second embodiment)
Next, a second embodiment will be described in which a state in which communication cannot be performed for a fixed time (allowable latency) is allowed. For example, suppose that the autonomous mobile device 100 has a natural language conversation with a human as a task to be executed, and causes the external communication device 200 to perform a voice recognition process required for the natural language conversation. The allowed latency is the time allowed to extend the silence period during the natural language conversation. For example, in this case, the latency can be set to 2 seconds. Further, the amount of data communicated with the external communication device 200 changes depending on the content of the task executed by the autonomous mobile device 100, and the communication quality that satisfies the band required for communication of this data amount is set as the threshold value.

Now, the second embodiment will be described with reference to FIG. First, the route search unit 15 sets the allowable latency in the variable TL based on the content of the task executed by the autonomous mobile device 100 (step S601). In this step, the route search unit 15 corresponds to the allowable latency setting unit. Next, the route search unit 15 sets a communication quality threshold based on the content of the task executed by the autonomous mobile device 100 (step S602). Then, the route search unit 15 substitutes the value of (x, y) of the array S which is the map/route cost map into the array T(x, y) which is the route search cost map (step S603).

Next, the route search unit 15 applies a conventional route planning algorithm to the route search cost map T to perform a route search, and sets the obtained route as a temporary route L (step S604). In this process, the route search unit 15 corresponds to a route candidate search unit. If the route cannot be obtained here, the route search is given up and terminated. Although not shown in the figure because the flow chart becomes complicated, in this case, it is necessary to start over from the setting of the destination in step S110 of the main flow (FIG. 4).

Next, the route search unit 15 traces the temporary route L in order from the current position to the target position, and in a section where C(x, y)>threshold (a section in which communication quality is insufficient when tracing the temporary route L). For the length, the required transit time (hereinafter, represented by TT) required to pass the length is calculated, and it is determined whether TL<TT (step S605). To calculate TT, each time one route of the provisional route L is traced, a value obtained by dividing the length of the one route by the moving speed of the autonomous mobile device 100 may be simply added. In step S605, the route search unit 15 corresponds to the communication failure time acquisition unit.

When the provisional route L is reached from the current position to the target position without a section where TL<TT (step 605; No), the provisional route L is adopted as the main route (step S607), and the process ends. ..

If a section that satisfies TL<TT is found when tracing the tentative route L (step 605; Yes), it means that the tentative route L cannot be adopted. Therefore, the tentative route L is set to TL<TT from the current position. ∞ is substituted for T(x, y) around the point on the section that traces to the point (area in which the section is enlarged by a predetermined margin width) (step S606), and the process returns to step S604. By substituting ∞ for T(x, y) in the peripheral region of the section, the section is not selected as a route in the route search from the next time onward.

A specific example of the route search in the second embodiment will be described with reference to FIGS. 7, 8, 13 and 14. For example, when the route planning process shown in FIG. 12 is performed using the map/route cost map S(x, y) shown in FIG. 7 and the communication quality cost map shown in FIG. A temporary route L indicated by the arrow 13(a) is obtained. Then, in step S605, if the time TT for passing through the light gray portion of FIG. 13B along the arrow is less than or equal to the allowable latency TL, the provisional route L is adopted as the main route.

If the time TT exceeds the allowable latency TL while passing through the light gray portion of FIG. 13B along the arrow, in step S606, as shown in FIG. The area around T(x, y) up to the middle of the arrow (to the position where TT exceeds TL) is set to ∞ (black in the figure). Then, returning to step S604, a new temporary route L is obtained again. This new temporary route L is indicated by a dotted arrow in FIG.

Even if the path of the dotted arrow is followed, the time TT exceeds the allowable latency TL on the way, so in step S606, as shown in FIG. The area around T(x, y) at the position where TT exceeds TL) is set to ∞ (painted black in the figure). Then, returning to step S604, a new temporary route L is obtained again. This new provisional route L is indicated by a dotted arrow in FIG.

The route indicated by the dotted arrow passes only the portion with good communication quality, and thus this route is adopted as the main route.

As described above, by performing the processing shown in FIG. 12, even if there is an area with poor communication quality, it is possible to follow a path that passes therethrough if the time for passing therethrough is less than the allowable latency. In the case where there is no choice but to pass through a region with poor communication quality to move to the destination, the move to the destination was impossible in the first embodiment, but in the second embodiment it took less time than the allowable latency. If the area can be passed, the destination can be moved.

For example, voice data, which is the basis of voice recognition, should be buffered while communication is interrupted and transmitted to an external device for processing after communication quality is restored, in order to protect the allowable latency. Processing can continue. Further, when the task of the autonomous mobile device 100 is a natural language conversation, if a time (for example, 2 seconds) during which the silent period can be extended in the natural language conversation is set as an allowable latency, the area where communication quality is poor can be passed. You can continue natural language conversation even if you have no choice.

If the speed of the autonomous mobile device 100 can be increased from the current speed, even if TT<TL is satisfied in step S605, the process does not immediately proceed to step S606, and the autonomous mobile device 100 moves at the maximum speed in the environment. If the TT is recalculated and the recalculated TT is less than or equal to TL (step S605; No), the route searching unit 15 sets the temporary route L as the main route (step S607), and the control unit 10 drives the driving unit 42. It is also possible to set the maximum speed to and instruct the movement. In this process, the control unit 10 corresponds to a speed setting unit. By performing such a process, even when the normal speed cannot pass through the area with poor communication quality in a time less than the allowable latency, the moving speed can be increased. Therefore, even when the destination cannot be reached at the normal speed, increasing the speed allows the destination to be reached while keeping the allowable latency.

Further, in the above-described embodiment, the communication quality is considered only when the route planning to the destination is performed, but the control unit 10 controls the communication quality during the free autonomous movement in step S109 of the main flow (FIG. 4). By referring to the cost map C(x, y), movement control may be performed so as not to stay for a long time in a place where communication quality is poor. By doing so, it is possible to maintain good communication with an external communication device regardless of the position of the destination or whether or not the destination is set.

Each function of the autonomous mobile device 100 of the present invention can also be implemented by a computer such as a normal PC (Personal Computer). Specifically, in the above embodiment, the program of the autonomous movement control process performed by the autonomous mobile device 100 is described as being stored in the ROM of the storage unit 20 in advance. However, the program is stored and distributed in a computer-readable recording medium such as a flexible disk, a CD-ROM (Compact Disc Read Only Memory), a DVD (Digital Versatile Disc), and an MO (Magneto-Optical Disc), and the program is distributed. You may comprise the computer which can implement|achieve each function mentioned above by reading and installing in a computer.

Although the preferred embodiment of the present invention has been described above, the present invention is not limited to the specific embodiment, and the present invention includes the invention described in the claims and an equivalent range thereof. Be done. The inventions described in the initial claims of the present application will be additionally described below.

(Appendix 1)
A map creation unit that creates a map using information of a plurality of images captured by the imaging unit;
A position estimation unit that estimates the position of the own device using information of a plurality of images captured by the imaging unit,
A communication quality measurement unit that measures the communication quality when communicating with an external communication device,
A communication quality map creation unit that creates a communication quality map using the own position estimated by the position estimation unit and the communication quality measured by the communication quality measurement unit,
Using the communication quality map created by the communication quality map creation unit, a route search unit that searches for a route to a destination,
An autonomous mobile device comprising.

(Appendix 2)
In the communication quality map created by the communication quality map creation unit, the route search unit searches for a route that does not pass through a poor communication area where the communication quality is lower than a predetermined threshold value.
The autonomous mobile device according to attachment 1.

(Appendix 3)
Furthermore, a communication failure time acquisition unit that acquires a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the route searched by the route search unit,
The route search unit searches for a route that does not pass through the communication failure area when the communication failure time acquired by the communication failure time acquisition unit exceeds the allowable latency, and when the communication failure time is less than or equal to the allowable latency. Searches for a route through the poor communication area,
The autonomous mobile device according to attachment 1.

(Appendix 4)
Furthermore, an allowable latency setting unit that sets the allowable latency based on the task content of the own device is provided.
The autonomous mobile device according to attachment 3.

(Appendix 5)
Furthermore, it is equipped with a speed setting unit that sets the speed of its own machine,
The communication failure time acquisition unit acquires the communication failure time when the autonomous mobile device moves at the maximum speed,
When the communication failure time when the autonomous mobile device moves at the maximum speed is less than or equal to the allowable latency, the route search unit searches for a route passing through the communication failure region, and the speed setting unit Set speed to maximum speed,
The autonomous mobile device according to appendix 3 or 4.

(Appendix 6)
Even if the destination is not set, the route search unit sets a route that avoids a poor communication area where the communication quality is lower than a predetermined threshold value in the communication quality map created by the communication quality map creation unit. To do
The autonomous mobile device according to attachment 1.

(Appendix 7)
The autonomous mobile device according to any one of appendices 1 to 6,
An external communication device that communicates with the autonomous mobile device;
An autonomous mobile system equipped with.

(Appendix 8)
An autonomous moving method for moving autonomously while communicating with an external communication device,
A route is searched according to the communication quality when communicating with the communication device,
Autonomous movement method.

(Appendix 9)
Search for a route that does not pass through a poor communication area, which is an area in which communication quality when communicating with the communication device is lower than a predetermined threshold value,
The autonomous moving method according to attachment 8.

(Appendix 10)
Searching for a route in which the communication quality when communicating with the communication device passes through a communication failure area lower than a predetermined threshold value only for a time equal to or less than the allowable latency,
The autonomous moving method according to attachment 8.

(Appendix 11)
Even if the destination is not set, set a route to avoid a poor communication area, which is an area where the communication quality when communicating with the communication device is lower than a predetermined threshold value,
The autonomous moving method according to attachment 8.

(Appendix 12)
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimating step of estimating the own position using information of a plurality of images captured by the image capturing unit;
A communication quality measuring step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the own position estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
Using the communication quality map created in the communication quality map creating step, a route searching step of searching for a route to a destination,
An autonomous moving method comprising.

(Appendix 13)
On the computer,
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimation step of estimating the own position using information of a plurality of images captured by the imaging unit,
Communication quality measurement step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the own position estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
Using the communication quality map created in the communication quality map creating step, a route searching step for searching for a route to a destination,
A program to execute.

100... Autonomous mobile device, 10... Control part, 11... Map creation part, 12... Position estimation part, 13... Communication quality measurement part, 14... Communication quality map creation part, 15... Route search part, 20... Storage part, 21 ... image storage unit, 22... map storage unit, 23... communication quality map storage unit, 24... route search map storage unit, 30... sensor unit, 31... distance sensor, 41... imaging unit, 42... drive unit, 43... Input unit, 44... Communication unit, 45... Power supply, 200... Communication device

Claims (6)

An autonomous mobile device,
A map creation unit that creates a map using information of a plurality of images captured by the imaging unit;
A position estimation unit that estimates the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit;
A communication quality measurement unit that measures the communication quality when communicating with an external communication device,
A communication quality map creation unit that creates a communication quality map using the position of the autonomous mobile device estimated by the position estimation unit and the communication quality measured by the communication quality measurement unit,
A route search unit that searches for a route to a destination using the map created by the map creation unit and the communication quality map created by the communication quality map creation unit,
A communication failure time acquisition unit that acquires a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the path searched by the path search unit,
The route search unit searches for a route through the communication failure area when the communication failure time is equal to or less than the allowable latency,
Furthermore, a speed setting unit for setting the speed of the autonomous mobile device is provided,
The communication failure time acquisition unit acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route search unit searches for a route passing through the defective communication area, the speed setting unit sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition unit, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The search unit searches for a route that does not pass through the poor communication area.
Autonomous mobile device. Furthermore, an allowable latency setting unit that sets the allowable latency based on the task content of the autonomous mobile device is provided.
The autonomous mobile device according to claim 1 . The route search unit, even if the destination has not been set, the communication quality map the communication quality mapping unit created, it sets the path to avoid the communication failure region,
Autonomous mobile apparatus according to claim 1 or 2. The autonomous mobile device according to any one of claims 1 to 3 ,
An external communication device that communicates with the autonomous mobile device;
An autonomous mobile system equipped with. An autonomous moving method of an autonomous moving device, comprising:
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimation step of estimating the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit,
A communication quality measuring step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the position of the autonomous mobile device estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
A route search step for searching a route to a destination using the map created in the map creating step and the communication quality map created in the communication quality map creating step;
A communication failure time acquisition step of acquiring a communication failure time which is a time required to pass through a communication failure area which is an area in which communication quality is lower than a predetermined threshold in the path searched in the path searching step,
The route search step searches for a route passing through the communication failure area when the communication failure time is equal to or less than an allowable latency,
Furthermore, a speed setting step for setting the speed of the autonomous mobile device is provided,
The communication failure time acquisition step acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route searching step searches for a route passing through the communication defective area, the speed setting step sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition step, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The searching step searches for a route that does not pass through the poor communication area.
Autonomous movement method. In the computer of the autonomous mobile device ,
A map creation step of creating a map using information of a plurality of images captured by the imaging unit,
A position estimation step of estimating the position of the autonomous mobile device using information of a plurality of images captured by the imaging unit,
Communication quality measurement step for measuring communication quality when communicating with an external communication device,
A communication quality map creating step of creating a communication quality map using the position of the autonomous mobile device estimated in the position estimating step and the communication quality measured in the communication quality measuring step,
A route search step of searching for a route to a destination using the map created in the map creation step and the communication quality map created in the communication quality map creation step;
A communication failure time acquisition step of acquiring a communication failure time that is a time required to pass through a communication failure area that is an area in which communication quality is lower than a predetermined threshold value in the path searched in the path search step,
The route searching step searches for a route passing through the communication failure area when the communication failure time is equal to or less than an allowable latency,
Further, causing the computer to execute a speed setting step for setting the speed of the autonomous mobile device,
The communication failure time acquisition step acquires the communication failure time when the autonomous mobile device moves at a maximum speed higher than the current speed,
The acquired communication failure time when the autonomous mobile device moves at the current speed exceeds the allowable latency, and the acquired communication when the autonomous mobile device moves at the maximum speed When the defective time is less than or equal to the allowable latency, the route searching step searches for a route passing through the communication defective area, the speed setting step sets the speed of the autonomous mobile device to the maximum speed,
If the communication failure time acquired by the communication failure time acquisition step, and the acquired communication failure time when the autonomous mobile device moves at the maximum speed, both exceed the allowable latency, the route The searching step searches for a route that does not pass through the poor communication area.
Program.

Images