JP2020004342A - Mobile body controller - Google Patents

Abstract

To provide a mobile body controller capable of easily setting and generating a node and an edge even in a passage that is difficult to pass.SOLUTION: A setting node part is capable of setting setting nodes Ns as a plurality of candidate points through which a moving part 20 on a map M passes. The setting edge part sets a setting edge Es as a line segment linking together the setting nodes Ns. A dynamic node part is capable of generating dynamic nodes Nd as a plurality of points randomly set on the map M. The dynamic edge part is capable of generating a first dynamic edge Ed1, a second dynamic edge Ed2, a third dynamic edge Ed3, and a fourth dynamic edge Ed4. A path planning part is capable of generating a mobile body path Rm based on the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, and the fourth dynamic edge Ed4, and the setting edge Es.SELECTED DRAWING: Figure 8

Description

  The present invention relates to a moving object control device.

  Conventionally, as described in Patent Literature 1, an autonomous mobile robot that sets a plurality of nodes and edges in advance, connects the set edges, and generates a route from a current position to a destination is known. I have.

JP 2006-195966 A

  In the configuration of Patent Literature 1, nodes need to be set in the entire range in which the moving object moves, so that the work time for setting the nodes may increase. In addition, when generating nodes and edges so that the moving body can move on relatively difficult passages such as relatively narrow passages, considering the safety of the moving body so that the robot does not come into contact with obstacles, the May not be generated.

  The present invention has been made in view of such a point, and an object of the present invention is to provide a mobile body control device that can easily set and generate nodes and edges even when there is a difficult passage. It is in.

  The moving object control device of the present invention includes a moving unit (20), a map managing unit (54), a self-position estimating unit (57), and a destination setting unit (58). The mobile control device includes a setting node unit (61), a setting edge unit (62), a dynamic node unit (66), and a dynamic edge unit (67). Furthermore, the moving object control device includes a route planning unit (67) and a movement control unit (71).

The moving unit is capable of moving along a path (14) provided with the first obstacle (11) and the second obstacle (12).
The map management unit can set a map (M) indicating the first obstacle, the second obstacle, and the passage.

The self-position estimating unit can estimate a moving object position (Pm) which is a position of the moving unit on the map.
The destination setting unit can set a mobile destination (Gm) which is a destination of the moving unit on the map.

The setting node unit can set a setting node (Ns) which is a plurality of candidate points through which the moving unit on the map passes.
When the setting node unit sets the setting node, the setting edge unit sets a setting edge (Es) which is a line segment connecting the setting nodes.

The dynamic node unit can generate a dynamic node (Nd), which is a plurality of points set at random on a map, between a moving object position and a moving object destination.
The dynamic edge section can generate a first dynamic edge (Ed1), a second dynamic edge (Ed2), a third dynamic edge (Ed3), and a fourth dynamic edge (Ed4). The first dynamic edge is a line segment connecting the moving object position and the dynamic node. The second dynamic edge is a line segment connecting the dynamic nodes. The third dynamic edge is a line segment connecting the dynamic node and the mobile destination. The fourth dynamic edge is a line segment connecting the setting node and the dynamic node.

The route planning unit is configured to determine whether the moving object is a route from the position of the moving object to the destination of the moving object based on the first dynamic edge, the second dynamic edge, the third dynamic edge, the fourth dynamic edge, and the set edge. A route (Rm) can be generated.
The movement control unit can control the moving unit such that the moving unit moves along the moving body path.

  With the dynamic node unit and the dynamic edge unit, a route can be automatically generated for a passage that is relatively easy to pass. Further, a route can be generated for a passage that is relatively difficult to pass by the setting node unit and the setting edge unit. As a result, nodes and edges only need to be set for paths that are relatively difficult to pass, and the work time for generating nodes and edges is reduced. With the minimum setting and generation, a route that passes through a passage that is relatively difficult to pass is easily generated. Therefore, even if there is a passage that is difficult to pass, the mobile control device can easily generate the route.

FIG. 2 is a perspective view of the moving object control device according to the first embodiment. FIG. 2 is a schematic diagram of a passage along which a moving unit of the moving body control device according to the first embodiment moves. FIG. 2 is a block diagram of the moving object control device according to the first embodiment. FIG. 4 is a schematic diagram for explaining each process of a control unit of the moving object control device according to the first embodiment. FIG. 4 is a schematic diagram for explaining processing of an area display unit of the mobile object control device according to the first embodiment. FIG. 4 is a schematic diagram for explaining correction of a setting node by a setting node correction unit of the mobile control device according to the first embodiment. FIG. 4 is a schematic diagram for explaining correction of a setting node by a setting node correction unit of the mobile control device according to the first embodiment. FIG. 6 is a schematic diagram for explaining processing of a route planning unit of the mobile control device according to the first embodiment. FIG. 6 is a schematic diagram for explaining processing of a route planning unit of the mobile control device according to the first embodiment. FIG. 6 is a schematic diagram for explaining processing of a route planning unit of the mobile control device according to the first embodiment. 5 is a flowchart for explaining control of the moving object control device according to the first embodiment. 5 is a flowchart for explaining control of the moving object control device according to the first embodiment. FIG. 9 is a schematic diagram for explaining processing of a setting node unit of the mobile control device according to the second embodiment. FIG. 9 is a perspective view of a moving unit for describing processing of a setting node unit of the moving object control device according to the second embodiment. 9 is a flowchart for explaining control of a mobile control device according to another embodiment. FIG. 9 is a schematic diagram for explaining processing of a mobile unit control device of a comparative example.

  Hereinafter, a moving object control device according to an embodiment will be described with reference to the drawings. In the description of the plurality of embodiments, substantially the same configuration is denoted by the same reference numeral. This embodiment includes a plurality of embodiments.

(1st Embodiment)
As shown in FIGS. 1 and 2, the moving object control device 1 includes a moving unit 20, an environment recognition unit 30, a moving object monitor 40, a moving object operation unit 45, and a control unit 50.

  The moving unit 20 can load a conveyed object and can move on a passage 14 provided with a plurality of obstacles. In the first embodiment, two obstacles are provided in the passage 14. One of the obstacles is referred to as a first obstacle 11. The other obstacle is referred to as a second obstacle 12. The obstacle is, for example, a passage wall, a workbench, equipment, a falling object, a noise affecting control, or a person such as a faller, a pedestrian, or a worker. The surface of the first obstacle 11 facing the second obstacle 12 is referred to as a first facing surface 15. The surface of the second obstacle 12 facing the first obstacle 11 is referred to as a second facing surface 16.

  The moving unit 20 has four wheels 21 and an electric motor 22, and is rotatable about an axis extending in the vertical direction. The forward direction of the moving unit 20 is “forward”. The retreat direction of the moving unit 20 is “rear”. The upper side when viewed from the forward direction of the moving unit 20 is “heaven”. The lower side when viewed from the forward direction of the moving unit 20 is referred to as “ground”. The right side when viewed from the forward direction of the moving unit 20 is referred to as “right”. The left side when viewed from the forward direction of the moving unit 20 is referred to as “left”. The width of the moving unit 20 is defined as a moving unit width Wm. The moving unit width Wm is the length of the moving unit 20 in the left-right direction.

The wheel 21 is a Mecanum wheel and includes four or more cylinders 24.
The cylinder 24 has a barrel shape and is provided on the circumference of the wheel 21. The cylinder 24 is provided such that the axis of the cylinder 24 is inclined at 45 degrees with respect to the axis of the wheel 21. Further, the cylinder 24 is freely rotatable with respect to the wheel 21.

  The electric motor 22 is a motor, and is rotatable when energized. The electric motor 22 is connected to each wheel 21. Each wheel 21 is rotatable by the electric motor 22. The moving unit 20 can move in all directions by combining the rotation directions of the wheels 21. The moving unit 20 is rotatable around an axis extending in the vertical direction by the wheels 21 and the electric motor 22 and is movable in all directions. The electric motor 22 is provided with an encoder, and detects the motor rotation speed Nm, which is the rotation speed of the electric motor 22. The detected motor rotation speed Nm is output to the speed detection unit 51 of the control unit 50.

  A plurality of environment recognition units 30 are provided on the front side and the rear side of the moving unit 20. The environment recognition unit 30 has a radar, a sonar, a camera, or a motion capture, and can detect an obstacle on the front, rear, left, right, or top and bottom sides. Obstacles and the like detected by the environment recognition unit 30 are obtained as environment information Ie. The acquired environment information Ie is output to the movement control unit 71.

  The environment recognition unit 30 can acquire position information Ip including a relative position from an obstacle or an object on the passage 14 to the moving unit 20. The environment recognition unit 30 transmits and receives, for example, radio waves, sound waves, or light, and acquires the position information Ip. Alternatively, the environment recognition unit 30 acquires the position information Ip using the image of the camera. The acquired position information Ip is output to self-position estimating section 57.

  The moving body monitor 40 has a screen 41 and is provided in front of the moving unit 20. The operation state of the moving unit 20, the state of the conveyed goods loaded on the stacking unit, the state of the moving body control device 1, and the like can be displayed on the screen 41. On the screen 41, for example, the image of the environment recognition unit 30, the name of the conveyed object, the map M, the starting point of the moving body, the destination Gm of the moving body, the moving body route Rm, the corrected moving body route Rc, and the moving body re-route are described later. An abnormality or warning related to Rm_R or the mobile unit control device 1 is displayed.

  The moving body operation unit 45 is integrated with the moving body monitor 40, and the moving body monitor 40 is a touch panel. The moving body monitor 40 and the moving body operation unit 45 may be separate bodies. The moving body operation unit 45 is a teaching pendant or the like, can change the control of the control unit 50, and can manually operate the movement of the moving unit 20. The moving body monitor 40 and the moving body operation unit 45 are not limited to being provided on the front side of the moving unit 20, and may be provided on the rear side, right side, or left side of the moving unit 20. The moving body monitor 40 and the moving body operation unit 45 may be rotatable by an actuator or the like. Further, the moving body operation unit 45 may be separate from the moving unit 20.

  The control unit 50 is mainly configured by a microcomputer, and includes a CPU, a readable non-transitory tangible recording medium, a ROM, an I / O, a bus line connecting these components, and the like. Each process of the control unit 50 may be a software process by executing a program stored in a substantial memory device such as a ROM by a CPU or a hardware process by a dedicated electronic circuit. Good.

  As shown in FIG. 3, the control unit 50 includes a speed detection unit 51, an acceleration sensor 52 as an acceleration detection unit, a gyro sensor 53 as an angular velocity detection unit, a map management unit 54, an area display unit 55, and a self-posture calculation unit 56. , A self-position estimating unit 57 and a destination setting unit 58. Further, the control unit 50 includes a margin distance setting unit 59, an inter-obstacle distance calculating unit 60, a setting node unit 61, a setting edge unit 62, a setting node correcting unit 63, a setting edge correcting unit 64, a dynamic node unit 65, and a dynamic node. It has a target edge 66. Further, the control unit 50 includes a route planning unit 67, a route distance calculation unit 68, a required time calculation unit 69, an energy consumption calculation unit 70, and a movement control unit 71.

  The speed detecting unit 51 can calculate and detect a moving body speed Vm, which is the speed of the moving unit 20, based on the motor rotation speed Nm. Note that the speed detection unit 51 may directly measure the moving object speed Vm by extracting the frequency shift generated in the radio wave or light reflected by the Doppler effect using the radio wave or light. The detected moving body speed Vm is output to the self-position estimating unit 57 and the set node correcting unit 63.

  The acceleration sensor 52 forms an IMU with the gyro sensor 53 and is configured integrally with the gyro sensor 53. The IMU is an abbreviation for the Intelligent Measurement Unit. The acceleration sensor 52 can detect a moving body acceleration Am, which is the acceleration of the moving unit 20, and can detect a moving body acceleration Am in three XYZ axes. The detected moving object acceleration Am is output to the self-position estimating unit 57.

  The gyro sensor 53 can detect the moving body angular velocity ω, which is the angular velocity of the moving unit 20, and the gyro sensor 53 can detect the moving body angular velocity ω of three axes around the XYZ axes. In the present embodiment, the moving object angular velocity ω is an angular velocity around the Z axis, that is, an axis extending in the vertical direction. The detected moving object angular velocity ω is output to the self-posture calculation unit 56 and the self-position estimation unit 57.

  As illustrated in FIG. 4, the map management unit 54 can set a map M including a range in which the first obstacle 11, the second obstacle 12, other obstacles, the passage 14, and the moving unit 20 move. In the figure, a part of the map M is schematically illustrated. In the map M, Cartesian coordinates of X and Y are set. In the figure, the X axis extends from the left to the right on the paper, and a direction from the left to the right on the paper is defined as a positive direction of the X axis. The Y-axis extends upward from the bottom of the paper, and a direction from the bottom to the top of the paper is defined as a positive direction of the Y-axis. In the map M, reference coordinates O are set. The X coordinate of the reference coordinate O is set to zero, and the Y coordinate of the reference coordinate O is set to zero. In this specification, zero includes a common sense error range.

  As shown in FIG. 5, the area display unit 55 can display a movable area Em that is a range in which the moving unit 20 can move on the map M. In the figure, the movable area Em is indicated by a two-dot chain line.

  Returning to FIG. 4, the self-posture calculation unit 56 can calculate the moving body angle θm, which is the angle of the moving unit 20 with respect to the reference on the map M, based on the moving body angular velocity ω. The reference of the moving object angle θm is, for example, a half line extending northward on the map M from the reference coordinates O. The calculated moving body angle θm is output to the self-position estimating unit 57.

  The self-position estimating unit 57 estimates the moving object position Pm, which is the position of the moving unit 20 on the map M, based on the position information Ip, the moving object speed Vm, the moving object acceleration Am, the moving object angular velocity ω, and the moving object angle θm. It is possible. The self-position estimating unit 57 estimates the center of the moving unit 20 as the moving body position Pm. In the figure, the moving body position Pm is indicated by a black circle. Note that the self-position estimating unit 57 may estimate the moving body position Pm by offsetting from the center of the moving unit 20. The self-position estimating unit 57 estimates the moving object position Pm using, for example, a Kalman filter. The estimated moving object position Pm is output to the route planning unit 67.

  The destination setting unit 58 can set a mobile destination Gm which is a destination of the moving unit 20 on the map M. The moving body destination Gm is set arbitrarily and is set according to the purpose of the moving body control device 1. The set mobile destination Gm is output to the dynamic edge unit 66 and the route planning unit 67.

  The allowance distance setting unit 59 can set a first allowance distance Lo1 that is a predetermined distance from the first obstacle 11. A range from the first obstacle 11 to the first margin distance Lo1 is defined as a first accessible area B1. In addition, the margin distance setting unit 59 can set a second margin distance Lo2 that is a predetermined distance from the second obstacle 12. The range from the second obstacle 12 to the second margin distance Lo2 is defined as a second accessible area B2. In the figure, the first accessible area B1 and the second accessible area B2 are described with dot patterns. The first allowance Lo1 and the second allowance Lo2 are set based on the road surface condition of the first obstacle 11, the second obstacle 12, the passage 14, the moving portion width Wm, and the like. The set first margin distance Lo1 and second margin distance Lo2 are output to the inter-obstacle distance calculation unit 60 and the set node correction unit 63.

  The inter-obstacle distance calculation unit 60 can calculate an inter-obstacle distance Lf which is a distance from the first obstacle 11 to the second obstacle 12 on the map M. The inter-obstacle distance calculation unit 60 is an area that is a distance from the first accessible area B1 to the second accessible area B2 based on the first extra distance Lo1, the second extra distance Lo2, and the inter-obstacle distance Lf. The distance Lb can be calculated. The calculated distance Lf between obstacles and distance Lb between regions are output to the route planning unit 67.

  The setting node unit 61 can set a plurality of setting nodes Ns, which are candidate points through which the moving unit 20 passes on the map M. In the figure, the setting node Ns is described by a white circle. The setting node Ns is arbitrarily set by the user of the mobile control device 1. The setting node Ns includes a moving object speed Vm, a moving object acceleration Am, a moving object angular velocity ω, a moving object angle θm, on / off of an avoidance function of the moving unit 20, on / off of various sensors, on / off of alarm generation, a first allowance Lo1 and It includes information on the second margin distance Lo2.

  Note that the setting node Ns may be automatically set without depending on the user. A line segment connecting the setting nodes Ns is set as a setting edge Es. The set setting node Ns is output to the setting edge unit 62, the setting node correction unit 63, and the dynamic edge unit 66.

  The setting edge unit 62 sets the setting edge Es when the setting node unit 61 sets the setting node Ns. The setting edge unit 62 sets a line segment connecting the setting node Ns to another setting node Ns as the setting edge Es.

  The set edge Es is, similarly to the set node Ns, the moving object speed Vm, the moving object acceleration Am, the moving object angular velocity ω, the moving object angle θm, the on / off of the avoidance function of the moving unit 20, the on / off of various sensors, and the on / off of alarm generation. , The first margin distance Lo1 and the second margin distance Lo2. The set edge Es is output to the set edge correction unit 64 and the path planning unit 67.

  The setting node correction unit 63 corrects the setting node Ns based on the first margin distance Lo1, the second margin distance Lo2, the moving body speed Vm, the moving body angle θm, and the moving part width Wm. The setting node correction unit 63 may automatically correct each setting node Ns. The setting node correction unit 63 may manually correct each setting node Ns. The corrected setting node Ns is referred to as a correction node Nc. The correction node Nc includes the same information as the setting node Ns.

  As illustrated in FIG. 6, the setting node correction unit 63 adjusts the first accessible area B1 such that, for example, the first margin distance Lo1 decreases. Further, the setting node correction unit 63 adjusts the second accessible area B2 such that the second margin distance Lo2 decreases. At this time, the setting node correction unit 63 moves the moving unit 20 to the first obstacle 11 or the second obstacle 12 based on the adjusted first accessible region B1, second accessible region B2, and moving unit width Wm. The setting node Ns is corrected so that does not touch. In FIG. 6, the first accessible area B1 and the second accessible area B2 before adjustment are indicated by a two-dot chain line. The setting node Ns is described by a broken white circle. The set edge Es is described by a broken line. The correction node Nc is indicated by a solid white circle.

  As illustrated in FIG. 7, the setting node correction unit 63 is configured such that the setting node Ns is located on a center line Ob provided at the center between the adjusted first accessible area B1 and the adjusted second accessible area B2. Alternatively, the setting node Ns may be corrected. In FIG. 7, similarly, the setting node Ns is indicated by a broken white circle. The set edge Es is described by a broken line. The correction node Nc is indicated by a solid white circle.

  Further, the setting node correction unit 63 may correct the setting node Ns based on the moving object speed Vm, the moving object angle θm, the distance between obstacles Lf, and the length of the setting edge Es. Alternatively, the setting node correction unit 63 may correct the setting node Ns based on the distance Lf between obstacles such that the setting edge Es attenuates linearly. Further, the setting node correction unit 63 may correct the setting node Ns based on the distance Lf between obstacles so that the setting edge Es attenuates exponentially. The correction node Nc is output to the set edge correction unit 64.

  The set edge correction unit 64 corrects the set edge Es based on the correction node Nc. The corrected set edge Es is defined as a corrected edge Ec. The correction edge Ec includes the same information as the set edge Es. The corrected edge Ec is output to the path planning unit 67.

  The dynamic node unit 65 generates a dynamic node Nd, which is a plurality of points set at random on the map M. In the figure, the dynamic node Nd is described by a black circle. When the dynamic node Nd is generated on an obstacle or the like on the map M, it is rejected. The generated dynamic node Nd is output to the dynamic edge unit 66.

  A line segment connecting the moving body position Pm and the dynamic node Nd is defined as a first dynamic edge Ed1. A line segment connecting the dynamic nodes Nd is defined as a second dynamic edge Ed2. A line segment connecting the mobile destination Gm and the dynamic node Nd is defined as a third dynamic edge Ed3. A line segment connecting the dynamic node Nd and the setting node Ns is defined as a fourth dynamic edge Ed4. Alternatively, a line segment connecting the dynamic node Nd and the correction node Nc is defined as a fourth dynamic edge Ed4.

  When the dynamic node unit 65 generates the dynamic node Nd, the dynamic edge unit 66 generates the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, and the fourth dynamic edge Ed4. Generate. The dynamic edge unit 66 includes a first dynamic edge Ed1, a second dynamic edge Ed2, a third dynamic edge Ed2 based on the moving object position Pm, the setting node Ns, the correction node Nc, the dynamic node Nd, and the moving object destination Gm. A dynamic edge Ed3 and a fourth dynamic edge Ed4 are generated.

  Further, the dynamic edge unit 66 generates a first dynamic edge Ed1, a second dynamic edge Ed2, a third dynamic edge Ed3, and a fourth dynamic edge Ed4 using, for example, RRT or PRM. RRT is an abbreviation for Rapidly-exploring Random Tree. PRM is an abbreviation for Probablistic Roadmap Method. The length of the first dynamic edge Ed1 is defined as a first dynamic distance Ld1. The length of the second dynamic edge Ed2 is defined as a second dynamic distance Ld2. The length of the third dynamic edge Ed3 is defined as a third dynamic distance Ld3. The length of the fourth dynamic edge Ed4 is defined as a fourth dynamic distance Ld4.

  Furthermore, the dynamic edge unit 66 performs the first dynamic distance Ld1, the second dynamic distance Ld2, the third dynamic distance Ld3, and the fourth dynamic distance Ld4 such that the first dynamic distance Ld1 is equal to or less than the distance threshold Ld_th. An edge Ed1, a second dynamic edge Ed2, and a third dynamic edge Ed3 are respectively generated. The distance threshold Ld_th is arbitrarily set, and is set by an experiment or a simulation. Further, the distance threshold Ld_th may be a different value for each of the first dynamic distance Ld1, the second dynamic distance Ld2, the third dynamic distance Ld3, and the fourth dynamic distance Ld4.

  In the initial state, when the dynamic node unit 65 generates the dynamic node Nd, the dynamic edge unit 66 generates the first dynamic edge Ed1 such that the first dynamic distance Ld1 is equal to or less than the distance threshold Ld_th. I do. The dynamic node unit 65 further generates the dynamic node Nd, and the dynamic edge unit 66 generates the second dynamic edge Ed2 such that the second dynamic distance Ld2 is equal to or less than the distance threshold Ld_th. The dynamic edge unit 66 repeats generation of the second dynamic edge Ed2.

  When the dynamic node Nd is relatively close to the setting node Ns or the correction node Nc, the dynamic edge unit 66 sets the fourth dynamic edge Ed4 such that the fourth dynamic distance Ld4 becomes equal to or less than the distance threshold Ld_th. Generate. Further, when the dynamic node Nd is relatively close to the mobile destination Gm, the dynamic edge unit 66 sets the third dynamic edge Ed3 such that the third dynamic distance Ld3 is equal to or less than the distance threshold Ld_th. Generate.

  The dynamic edge unit 66 connects each node after verifying that no obstacle is located on the edge. The dynamic edge unit 66 does not connect each node when an obstacle is located on the edge. The generated first dynamic edge Ed1, second dynamic edge Ed2, third dynamic edge Ed3, and fourth dynamic edge Ed4 are output to the path planning unit 67.

  As illustrated in FIG. 8, the route planning unit 67 determines whether the moving object is based on the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, the fourth dynamic edge Ed4, and the set edge Ed. At least one route Rm can be generated. The moving body route Rm is a route from the moving body position Pm to the moving body destination Gm. In the figure, the moving object route Rm is indicated by a thick line. Note that when the setting node Ns is not set by the setting node unit 61, the route planning unit 67 sets the moving object based on the first dynamic edge Ed1, the second dynamic edge Ed2, and the third dynamic edge Ed3. A route Rm is generated.

  As shown in FIG. 9, when the setting node Ns is corrected by the setting node correction unit 63, the path planning unit 67 sets the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, At least one corrected moving object route Rc can be generated based on the fourth dynamic edge Ed4 and the corrected edge Ec. The corrected moving body route Rc is a route from the moving body position Pm to the moving body destination Gm, similarly to the moving body route Rm. In the figure, the corrected moving body route Rc is indicated by a thick line.

  As shown in FIG. 10, when the moving unit 20 is located on the fourth dynamic edge Ed4 and the environment recognition unit 30 detects the third obstacle 13 on the set edge Es, the set edge correction unit 64 sets Cut or erase the edge Es. After the set edge correction unit 64 cuts the set edge Es, the route planning unit 67 generates at least one moving object re-route Rm_R.

  Similarly, when the moving unit 20 is located on the fourth dynamic edge Ed4 and the environment recognition unit 30 detects the third obstacle 13 on the correction edge Ec, the setting edge correction unit 64 cuts the correction edge Ec. I do. After the set edge correction unit 64 cuts the correction edge Ec, the route planning unit 67 generates at least one moving object re-route Rm_R.

  The moving object re-route Rm_R is generated based on the second dynamic edge Ed2. Also, the moving body re-route Rm_R does not pass between the first obstacle 11 and the second obstacle 12, but passes through the opposite side of the first opposing surface 15 or the opposite side of the second opposing surface 16. Generated. In FIG. 10, the description of the dynamic node Nd and the second dynamic edge Ed2 of the moving object re-route Rm_R is omitted. The setting node Ns and the setting edge Es are indicated by broken lines.

  The route distance calculation unit 68 can calculate a route distance Lr that is the distance of the moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R. The calculated route distance Lr is output to the route planning unit 67 and the required time calculation unit 69.

  The required time calculating unit 69 calculates the required time Tm required for the moving unit 20 to move along the moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R to reach the moving body destination Gm. It is possible. The required time calculation unit 69 calculates the required time Tm based on, for example, the moving object speed Vm, the moving object acceleration Am, the moving object angular velocity ω, and the path distance Lr. The calculated required time Tm is output to the route planning unit 67 and the energy consumption calculating unit 70.

  The energy consumption calculation unit 70 consumes energy that is necessary until the moving unit 20 moves along the moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R and reaches the moving body destination Gm. The energy Cw can be calculated. The energy consumption calculation unit 70 calculates the consumed energy Cw based on, for example, the power supplied to the electric motor 22 and the required time Tm. The consumed energy Cw may be energy per unit time or energy for the required time Tm. The calculated energy consumption Cw is output to the route planning unit 67.

  Further, the route planning unit 67 generates the moving object route Rm or the corrected moving object route Rc based on the distance between obstacles Lf, the route distance Lr, the required time Tm, or the consumed energy Cw. For example, when the distance Lf between obstacles is smaller than the threshold Lf_th between obstacles, the route planning unit 67 moves the moving body route so as to pass through the opposite side to the first facing surface 15 or the opposite side to the second facing surface 16. Rm or the corrected moving object route Rc is generated. The threshold Lf_th between obstacles is a predetermined threshold, is arbitrarily set, and is set based on the moving part width Wm, the moving body speed Vm, the moving body acceleration Am, and the moving body angular velocity ω.

  Further, the route planning unit 67 generates the moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R such that the route distance Lr and the energy consumption Cw are reduced, for example. In addition, the route planning unit 67 generates the moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R such that the required time Tm is shortened. The generated moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R is output to the movement control unit 71.

  The movement control unit 71 can control the moving unit 20 so that the moving unit 20 moves along the moving body route Rm, the corrected moving body route Rc, or the moving body re-route Rm_R. Further, the movement control unit 71 can control the movement unit 20 based on information included in the set node Ns, the set edge Es, or the correction edge Ec.

  The control of the mobile control device 1 will be described with reference to the flowcharts of FIGS. In the flowchart, “S” means a step.

In step 101, the self-position estimating unit 57 estimates the moving body position Pm.
In step 102, the destination setting unit 58 sets the mobile destination Gm.

  In step 103, the setting node correction unit 63 determines whether to correct the setting node Ns. When the setting node correction unit 63 corrects the setting node Ns, the process proceeds to Step 106. When the setting node correction unit 63 does not correct the setting node Ns, the process proceeds to Step 104.

In step 104, the setting node unit 61 sets the setting node Ns via step 103.
In step 105, the setting edge unit 62 sets the setting edge Es.

In step 106, via step 103, the setting node unit 61 sets the setting node Ns, and the setting edge unit 62 sets the setting edge Es. The setting node correction unit 63 corrects the setting node Ns and generates a correction node Nc.
In step 107, the set edge correction unit 64 corrects the set edge Es based on the correction node Nc, and generates a corrected edge Ec.

In step 108, the dynamic node unit 65 generates a dynamic node Nd via step 105 or step 107.
In step 109, the dynamic edge unit 66 generates a first dynamic edge Ed1, a second dynamic edge Ed2, a third dynamic edge Ed3, and a fourth dynamic edge Ed4. Thereafter, the process proceeds to step 201.

  In step 201, when the moving unit 20 is moving on the fourth dynamic edge Ed4, the environment recognizing unit 30 determines whether the third obstacle 13 on the set edge Es has been detected. When the third obstacle 13 on the set edge Es is detected, the process proceeds to Step 203. If the third obstacle 13 on the set edge Es has not been detected, the process proceeds to step 202.

  Alternatively, similarly, in step 201, when the moving unit 20 is moving on the fourth dynamic edge Ed4, the environment recognizing unit 30 determines whether the third obstacle 13 on the correction edge Ec has been detected. judge. When the third obstacle 13 on the correction edge Ec is detected, the processing shifts to Step 203. If the third obstacle 13 on the correction edge Ec has not been detected, the process proceeds to step 202.

In step 202, the set edge Es or the correction edge Ec is maintained.
In step 203, when the third obstacle 13 on the set edge Es is detected, the set edge correction unit 64 cuts off the set edge Es. Alternatively, in step 203, when the third obstacle 13 on the correction edge Ec is detected, the set edge correction unit 64 cuts off the correction edge Ec.

  In step 204, when the set edge Es is maintained via step 202, the route planning unit 67 determines that the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, and the fourth dynamic edge Ed3. It is determined whether or not there is a route from the moving body position Pm to the moving body destination Gm based on the target edge Ed4 and the set edge Es. When there is no route from the moving body position Pm to the moving body destination Gm, the processing returns to step 108. When there is a route from the moving body position Pm to the moving body destination Gm, the process proceeds to Step 205.

  In step 204, when the correction edge Ec is maintained via step 202, the path planning unit 67 determines that the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, Based on the four dynamic edges Ed4 and the correction edge Ec, it is determined whether or not there is a route from the moving body position Pm to the moving body destination Gm. When there is no route from the moving body position Pm to the moving body destination Gm, the processing returns to step 108. When there is a route from the moving body position Pm to the moving body destination Gm, the process proceeds to Step 205.

  Further, in step 204, when the set edge Es or the corrected edge Ec is cut off via step 203, the path planning unit 67 sets the first dynamic edge Ed1, the second dynamic edge Ed2, and the third dynamic edge Ed2. Based on Ed3, it is determined whether there is a route from the moving body position Pm to the moving body destination Gm. When there is no route from the moving body position Pm to the moving body destination Gm, the processing returns to step 108. When there is a route from the moving body position Pm to the moving body destination Gm, the process proceeds to Step 205.

  In step 205, when the set edge Es is maintained via step 202, the route planning unit 67 generates the moving object route Rm. In step 205, when the correction edge Ec is maintained via step 202, the route planning unit 67 generates the corrected moving object route Rc. Further, in Step 205, when the set edge Es or the correction edge Ec has been cut through Step 203, the route planning unit 67 generates the moving object re-route Rm_R.

  When the moving object route Rm is generated in Step 206, the movement control unit 71 controls the moving unit 20 so that the moving unit 20 moves along the moving object route Rm. In addition, when the corrected moving body route Rc is generated in Step 206, the movement control unit 71 controls the moving unit 20 so that the moving unit 20 moves along the corrected moving body route Rc. Further, when the moving body re-route Rm_R is generated in Step 206, the movement control unit 71 controls the moving unit 20 so that the moving unit 20 moves along the moving body re-route Rm_R. Thereafter, the process ends.

  As illustrated in FIG. 16, a route may be generated using a dynamically generated graph such as RRT or PRM, as in the mobile unit control device 90 of the comparative example. In this case, it is difficult to generate a route that passes through a relatively difficult passage such as a relatively narrow passage, such as between the first obstacle 11 and the second obstacle 12.

  Further, the passage may be closed due to the sensor noise, and a path that can automatically pass through may not be generated even though the passage is actually passable. Therefore, the moving object control device 1 of the present embodiment can easily generate a route even if there is a passage that is difficult to pass.

[1] The dynamic node unit 65 and the dynamic edge unit 66 can automatically generate a route for a relatively easy-to-pass passage. Further, the setting node unit 61 and the setting edge unit 62 can set a route for a passage that is relatively difficult to pass. As a result, nodes and edges only need to be set for paths that are relatively difficult to pass, and the work time for generating nodes and edges is reduced. With the minimum setting and generation, a route that passes through a passage that is relatively difficult to pass is easily generated. Therefore, even if there is a passage that is difficult to pass, the mobile control device 1 can easily generate the route.

[2] The path planning unit 67 generates the moving object path Rm or the corrected moving object path Rc based on the distance Lf between obstacles, the path distance Lr, the required time Tm, or the consumed energy Cw. This makes it easier to determine whether to generate a route that passes through a relatively hard-to-pass passage or a route that bypasses a relatively hard-to-pass passage. In addition, the time required for the moving unit 20 to reach the moving body destination Gm is easily reduced.

[3] The setting node Ns and the setting edge Es are the first margin Lo1, the second margin Lo2, the moving body speed Vm, the moving body angular velocity ω, the moving body angle θm, the on / off of the avoiding function of the moving unit 20, various sensors. Includes information about ON / OFF of alarm and ON / OFF of alarm occurrence. Accordingly, the moving unit 20 can move safely and in the shortest time on the set edge Es.

[4] The setting node correction unit 63 corrects the setting node Ns based on the first margin distance Lo1, the second margin distance Lo2, the moving body speed Vm, the moving body angular velocity ω, the moving body angle θm, or the moving part width Wm. Then, the correction node Nc is set. The set edge correction unit 64 corrects the set edge Es based on the correction node Nc. This makes it easier to set the corrected moving object route Rc so that the moving unit 20 does not contact the obstacle. Further, the labor when the setting node Ns and the setting edge Es are manually set is reduced.

[5] The area display section 55 can display the movable area Em on the map M. As a result, the setting node Ns is easily set manually, and user convenience is improved.

[6] When the moving unit 20 is located at the fourth dynamic edge Ed4 and the environment recognition unit 30 detects the third obstacle 13 on the set edge Es, the set edge correction unit 64 cuts the set edge Es. . Thereby, the moving unit 20 can determine that the passage 14 is not allowed to pass and can make a detour. Therefore, the safety of the mobile control device 1 is improved.

(2nd Embodiment)
The second embodiment is the same as the first embodiment except that the setting method of the setting node unit is different.

  As shown in FIGS. 13 and 14, in the moving body control device 2 of the second embodiment, the moving unit 20 is manually operated by the moving body operation unit 45. The locus of the movement of the moving unit 20 by the moving body operation unit 45 is defined as a moving body locus Jm. In the second embodiment, the moving body operation unit 45 includes a touch panel monitor capable of displaying the map M, a button, and a joystick. In the drawing, the trajectory Jm of the moving object is indicated by a broken line. The moving body operation unit 45 can delete obstacles on the map M and can edit the map M.

The setting node unit 61 sets the setting node Ns based on the moving object trajectory Jm. The setting node unit 61 sets the setting node Ns on, for example, the moving object trajectory Jm.
The setting edge unit 62 sets the setting edge Es based on, for example, the setting node Ns on the moving object trajectory Jm.

  The second embodiment has the same advantages as the first embodiment. In the second embodiment, a position where the moving unit 20 does not contact an obstacle can be confirmed. Thereby, the setting node Ns and the setting edge Es can be set at positions where the moving unit 20 does not contact the obstacle. Therefore, the safety of the mobile control device 2 is improved.

(Other embodiments)
[I] The wheels of the moving unit are omni wheels, and may include three or more cylinders. The cylinder used for the omni wheel is barrel-shaped and is provided on the circumference of the wheel. This cylinder is freely rotatable with respect to the wheels. The wheels allow the moving section to freely move back and forth and left and right. In addition, a belt made of metal, rubber, resin, or the like may be wound around the wheel.

[Ii] When the moving unit 20 is located on the fourth dynamic edge Ed4 and the environment recognition unit 30 detects the third obstacle 13 on the set edge Es, the moving unit 20 may stop. Alternatively, when the moving unit 20 is located on the fourth dynamic edge Ed4 and the environment recognition unit 30 detects the third obstacle 13 on the correction edge Ec, the moving unit 20 may stop.

  As shown in FIG. 15, in step 301, when the moving unit 20 is moving on the fourth dynamic edge Ed4 via step 109, the environment recognizing unit 30 sets the third obstacle on the set edge Es. 13 is detected. When the third obstacle 13 on the set edge Es is detected, the process proceeds to Step 303. If the third obstacle 13 on the set edge Es has not been detected, the process proceeds to step 302.

  Alternatively, in step 301, when the moving unit 20 moves on the fourth dynamic edge Ed4 via step 109, the environment recognizing unit 30 detects the third obstacle 13 on the correction edge Ec. Determine whether or not. When the third obstacle 13 on the correction edge Ec is detected, the process proceeds to Step 303. If the third obstacle 13 on the correction edge Ec has not been detected, the process proceeds to step 302.

In step 302, the set edge Es or the correction edge Ec is maintained.
In step 303, the movement control unit 71 controls the movement unit 20 so that the movement unit 20 stops. Thereafter, the process returns to step 301.

  In step 304, when the set edge Es is maintained, the path planning unit 67 sets the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, the fourth dynamic edge Ed4, and the set edge Based on Es, it is determined whether there is a route from the moving body position Pm to the moving body destination Gm. When there is no route from the moving body position Pm to the moving body destination Gm, the processing returns to step 108. When there is a route from the moving body position Pm to the moving body destination Gm, the process proceeds to step 305.

  When the correction edge Ec is maintained in Step 304, the path planning unit 67 sets the first dynamic edge Ed1, the second dynamic edge Ed2, the third dynamic edge Ed3, the fourth dynamic edge Ed4, and Based on the corrected edge Ec, it is determined whether there is a route from the moving body position Pm to the moving body destination Gm. When there is no route from the moving body position Pm to the moving body destination Gm, the processing returns to step 108. When there is a route from the moving body position Pm to the moving body destination Gm, the process proceeds to step 305.

  In step 305, when the set edge Es is maintained, the route planning unit 67 generates the moving object route Rm. When the correction edge Ec is maintained in Step 305, the route planning unit 67 generates a corrected moving object route Rc.

  When the moving body route Rm is generated in Step 306, the movement control unit 71 controls the moving unit 20 so that the moving unit 20 moves along the moving body route Rm. In addition, when the corrected moving object route Rc is generated in Step 306, the movement control unit 71 controls the moving unit 20 so that the moving unit 20 moves along the corrected moving object route Rc. Thereafter, the process ends. Even in such control, the same effect as in the first embodiment can be obtained.

  As described above, the present invention is not limited to such an embodiment, and can be implemented in various forms without departing from the spirit of the invention.

11 ... first obstacle, 12 ... second obstacle, 14 ... passage,
20 ... moving part,
54 ・ ・ ・ Map management unit,
57 ... Self-position estimating unit
58 ··· Destination setting unit
61 ... setting node section
62 ... setting edge part,
65 ... dynamic node part,
66 ... dynamic edge part.

Claims (8)

A moving unit (20) capable of moving along a path (14) in which the first obstacle (11) and the second obstacle (12) are provided;
A map management unit (54) capable of setting a map (M) indicating the first obstacle, the second obstacle, and the passage;
A self-position estimating unit (57) capable of estimating a moving object position (Pm) which is a position of the moving unit on the map;
A destination setting unit (58) capable of setting a mobile destination (Gm) which is a destination of the moving unit on the map;
A setting node unit (61) capable of setting a setting node (Ns), which is a plurality of candidate points through which the moving unit passes on the map;
A setting edge unit (62) for setting a setting edge (Es) which is a line segment connecting the setting nodes when the setting node unit sets the setting node;
A dynamic node unit (66) capable of generating a dynamic node (Nd), which is a plurality of points randomly set in the map, between the mobile object position and the mobile object destination;
A first dynamic edge (Ed1) that is a line segment connecting the moving object position and the dynamic node; a second dynamic edge (Ed2) that is a line segment connecting the dynamic nodes; And a third dynamic edge (Ed3) that is a line segment connecting the mobile destination and a fourth dynamic edge (Ed4) that is a line segment connecting the setting node and the dynamic node. A dynamic edge (67);
A path from the moving body position to the moving body destination based on the first dynamic edge, the second dynamic edge, the third dynamic edge, the fourth dynamic edge, and the set edge. A route planning unit (67) capable of generating a moving object route (Rm);
A movement control unit (71) capable of controlling the moving unit so that the moving unit moves along the moving body path;
A mobile control device comprising: An inter-obstacle distance calculator (60) capable of calculating an inter-obstacle distance (Lf) that is a distance from the first obstacle to the second obstacle;
A path distance calculation unit (68) capable of calculating a path distance (Lr) that is a distance of the moving object path;
A required time calculation unit (69) that can calculate a required time (Tm) required for the moving unit to move along the moving body route and reach the destination of the moving body;
An energy consumption calculation unit (70) capable of calculating energy consumption (Cw), which is energy required for the moving unit to move along the moving body route and for the moving unit to reach the moving body destination; ,
Further comprising
The mobile object control device according to claim 1, wherein the route planning unit generates the mobile object route based on the distance between obstacles, the route distance, the required time, or the energy consumption. In the map, a margin distance setting unit (Lo) that can set a first margin distance (Lo1) that is a predetermined distance from the first obstacle and a second margin distance (Lo2) that is a predetermined distance from the second obstacle. 59)
A speed detector (51) capable of detecting a moving body speed (Vm) which is a speed of the moving unit;
An angular velocity detecting section (53) capable of detecting a moving body angular velocity (ω) that is an angular velocity of the moving section;
A self-posture calculation unit (56) capable of calculating a moving body angle (θm) that is an angle of the moving unit with respect to a reference on the map, based on the moving body angular velocity;
Further comprising
3. The moving object control according to claim 1, wherein the setting node and the setting edge include information on the first margin distance, the second margin distance, the moving body speed, the moving body angular velocity, and the moving body angle. 4. apparatus. A setting node correction unit (65) for correcting the setting node based on the first margin distance, the second margin distance, the moving body speed, the moving body angle, or the width (Wm) of the moving unit;
A setting edge corrector (66) for correcting the set edge based on a correction node (Nc) which is the setting node corrected by the setting node corrector;
Further comprising
The path planning unit is configured to determine the first dynamic edge, the second dynamic edge, the third dynamic edge, the fourth dynamic edge, and a corrected edge (Ec) that is the corrected set edge. 4. The moving body control device according to claim 3, wherein a corrected moving body route (Rc), which is a route from the moving body position to the moving body destination, can be generated. An environment recognition unit (30) that can detect an obstacle (11, 12, 13);
The setting edge correction unit cuts the setting edge when the moving unit is located at the fourth dynamic edge and the environment recognition unit detects a third obstacle (13) on the setting edge. Item 5. The moving object control device according to Item 4. An environment recognition unit (30) that can detect an obstacle (11, 12, 13);
The movement control unit is configured to stop the movement unit when the movement unit is positioned at the fourth dynamic edge and the environment recognition unit detects a third obstacle (13) on the set edge. The moving body control device according to any one of claims 1 to 4, which controls the moving unit.   The moving object control device according to any one of claims 1 to 6, further comprising an area display section (55) capable of displaying a movable area (Em), which is a movable area of the moving section, in the map. .   The moving object control device according to claim 1, wherein the setting node unit sets the setting node based on a trajectory (Jm) when the moving unit moves.

Images