JP2007257195A - Mobile body and control method for it - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a mobile body and a control method for it capable of easily controlling a stop position. <P>SOLUTION: In this mobile body, a control part 15 performing movement control for following a target route to a target position is provided with a position estimation part 24 estimating the position of the mobile body moving along the target route and an area setting part 23 setting a set area C including the target position and divided into a division area A and a division area B by a division line D passing the target position. When the estimated position estimated by the position estimation part 24 makes a transition from a first division area to a second division area, it is determined that the mobile body reaches the target position and its movement is stopped. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a moving body and a control method thereof, and more particularly to a moving body that follows a target route and a control method thereof.

  2. Description of the Related Art Conventionally, techniques for automatically moving a moving body such as a robot or a vehicle to a target route have been proposed. For example, there is a technique of automatically following a feedback control of a lateral deviation amount or an azimuth angle deviation amount of a moving body with respect to a target route. In such an autonomous moving body, a target route from the movement start position to the target position is planned. The moving body moves following the target route while being feedback controlled. There is also a robot that performs a predetermined operation and work on such a moving body at a stop position.

  In such an autonomous moving body, it is difficult to move following the target route accurately. In order to solve this problem, in addition to the operation amount by feedback control according to the amount of relative lateral deviation and azimuth angle deviation from the target route, the operation by feedforward control according to the curvature and lateral deviation amount of the target route A technique for determining the steering angle based on the amount has been proposed (see Patent Document 1). In this technique, an arbitrary target path whose curvature changes and a coordinate system having coordinate axes along the target path are converted to calculate a lateral deviation amount and the like.

JP 2002-215239 A

  However, it is difficult to accurately control the moving body even using the above technique. Therefore, there arises a problem that the stop position of the moving body deviates from the target position. In the above-mentioned document, the moving body stops when it moves to a predetermined coordinate on the coordinate axis along the target route. Therefore, when the lateral deviation amount increases in the vicinity of the target position, the stop position deviates greatly from the target position. When a predetermined operation or work is performed at the stop position, if the shift of the stop position occurs, the predetermined operation or work cannot be performed. For example, in the case of a moving body having a hand or the like that grips an object, there arises a problem that the object cannot be gripped if the stop position deviates from the target position. Thus, the conventional mobile body has a problem that it is difficult to accurately control the stop position.

  The present invention has been made to solve such a problem, and an object of the present invention is to provide a moving body capable of easily controlling a stop position and a control method therefor.

  A moving body according to a first aspect of the present invention is a moving body having a control unit that performs movement control so as to follow a target path to a target position, and the control unit follows the target path. A position estimation unit that estimates the position of a moving body that moves and a target area that includes the target position, and is divided into a first divided area and a second divided area by a dividing line that passes through the target position. An area setting unit for setting a target area, and when the estimated position estimated by the position estimation unit transitions from the first divided area to the second divided area, the moving body has reached the target position. Judgment is made to stop the movement of the moving body. Thereby, control of a stop position can be performed easily.

  The moving body according to a second aspect of the present invention is the moving body according to the first aspect, wherein the dividing line is a line segment in a direction orthogonal to the direction of the target path at the target position. The two divided areas are divided. As a result, the stop position can be controlled more accurately.

  The moving body according to the third aspect of the present invention is such that the target area is set as a circle having a radius k in the above moving body.

  According to a fourth aspect of the present invention, there is provided a movable body, wherein the movable body has a route planning unit that plans a target route to the target position, and an estimated position estimated by the position estimating unit. And a determination unit that determines that a predetermined distance from the position on the target route planned by the route planning unit is provided, wherein the route planning unit is configured such that the moving body moves from the target route to the target area. When it is determined that a distance greater than the radius k is left, the target route to the target position is re-planned. Thereby, it is possible to follow the target route.

  In the mobile body according to the fifth aspect of the present invention, in the mobile body described above, the region setting unit sets a movable region based on the target route and the value of the radius k, and the determination unit includes: When it is determined that the estimated position is out of the movable area, the target route is re-planned. This makes it possible to easily determine without performing complicated processing.

  A moving body according to a sixth aspect of the present invention further includes a task processing mechanism that executes a predetermined task in the above moving body, so that the task processing mechanism executes a task from a stop position of the moving body. The value of the radius k is set based on the task possible range set by the required distance. Thereby, the task at the stop position can be reliably executed.

  A control method for a moving body according to a seventh aspect of the present invention is a control method for controlling movement of a moving body that follows a target path, the target area including the target position, and the target position. Setting a target region that is divided into a first divided region and a second divided region by a dividing line passing through the step, estimating a position of the moving body, and the estimated position of the divided regions And a step of determining that the target position has been reached at the time of transition from the first divided region to the second divided region, and stopping the movement of the moving body. Thereby, control of a stop position can be performed easily.

  A control method for a moving body according to an eighth aspect of the present invention is the control method described above, wherein the dividing line is defined as a line segment in a direction orthogonal to the direction of the target path at the target position. The divided area and the second divided area are divided. As a result, the stop position can be controlled more accurately.

  A control method for a moving body according to a ninth aspect of the present invention is the control method described above, wherein the target region is set as a circle having a radius k.

  A control method for a mobile unit according to a tenth aspect of the present invention is the control method described above, wherein the mobile unit plans a target route to the target position, and is estimated by the position estimation unit. Determining that the estimated position is a predetermined distance away from the position on the target route planned by the route planning unit, wherein the moving body is not less than a radius k of the target region from the target route. When it is determined that the distance has been increased, the target route to the target position is re-planned. Thereby, it is possible to follow the target route.

  A control method for a moving body according to an eleventh aspect of the present invention is the control method described above, further comprising a step of setting a movable area based on the target route and the threshold value, and performing the determination. In the step, the target route is re-planned when it is determined that the estimated position is out of the movable region. This makes it possible to easily determine without performing complicated processing.

  A control method for a mobile body according to a twelfth aspect of the present invention is the control method described above, further comprising a step of turning the mobile body on the spot to a target posture after stopping the position movement. . Thereby, the stop position and posture can be easily controlled.

  ADVANTAGE OF THE INVENTION According to this invention, the moving body which can control a stop position easily, and its control method can be provided.

Embodiment 1 of the Invention
A moving body according to the present embodiment will be described with reference to FIG. FIG. 1A is a top view schematically showing the configuration of the moving body according to the present embodiment, and FIG. 1B is a side view schematically showing the configuration of the moving body according to the present embodiment. It is. Reference numeral 10 denotes a moving body, 11 denotes a vehicle body, 12 denotes driving wheels, 13 denotes driven wheels, 14 denotes a driving mechanism, and 15 denotes a control unit. In the present embodiment, the moving body 10 is described as an opposed two-wheeled wheel moving robot.

  The moving body 10 is an autonomous mobile robot. Drive wheels 12 are provided below the side surface of the vehicle body 11. The drive wheels 12 are respectively provided on opposite side surfaces of the vehicle body 11. A driven wheel 13 such as a caster is provided near the corner of the vehicle body 11. The two drive wheels 12 rotate independently by a drive mechanism 14 having a motor or the like. That is, the moving body 10 moves when the drive mechanism 14 rotates the left and right drive wheels 12. The two drive wheels 12 are connected to different drive mechanisms 14, respectively. Therefore, the moving direction and moving speed of the moving body 10 can be controlled by rotating the drive wheels 12 at different rotating directions and rotating speeds. Specifically, by rotating the drive wheels 12 at the same direction and at different speeds, the moving body 10 moves while changing its moving direction. That is, the moving body 10 moves while curving in accordance with the distance between the two drive wheels 12 and the difference in rotational speed. Further, by rotating the driving wheel 12 in the opposite direction, the moving body 10 turns on the spot. Further, the moving body 10 moves straight by rotating the drive wheels 12 in the same direction and at the same speed.

  The control unit 15 controls the drive mechanism 14 so that the moving body 10 follows the target path. For example, the control unit 15 plans a target route based on the current position of the moving body 10 and the target position. Then, the control unit 15 performs feedback control or feedforward control on the drive mechanism 14 so that the moving body 10 moves following the target path. The control unit 15 estimates the current position based on measured values from, for example, a rotary encoder attached to the motor of the drive mechanism 14 or the drive wheel 12, a gyro sensor attached to the vehicle body 11, or the like. Then, the control unit 15 performs feedback control based on the estimated position. As a result, the motor of the drive mechanism 14 rotates at an appropriate rotation angle. And the driving wheel 12 rotates and the mobile body 10 moves following a target path | route.

  The control unit 15 of the moving body according to the embodiment of the present invention is realized by, for example, an arithmetic processing device mounted on the vehicle body 11. The arithmetic processing unit includes, for example, a central processing unit (CPU), a storage device such as a ROM, a RAM, and a hard disk, an input / output interface, and the like. In a storage device such as a ROM, an application program can be recorded by giving a command to the CPU in cooperation with the operating system, and the program is executed by being loaded into the RAM. This application program includes a unique program for realizing the control method according to the present invention. For example, there are a program for planning a target route, a program for estimating a position based on a measurement value from a sensor, and the like. The follow-up control by the control unit 15 is executed when the central processing unit develops the application program on the RAM, reads the data stored in the storage device, and performs processing according to the application program, and stores it. .

  The movement control by the control unit 15 will be described. FIG. 2 is a diagram schematically showing a target route to the target position and orientation. Here, it is assumed that the position and orientation of the moving body 10 is R (rx, ry, rt). That is, the position of the moving body 10 in the two-dimensional orthogonal coordinate system is indicated by rx, ry, and the posture (direction) of the moving body 10 is indicated by rt. Here, the center of the drive wheel 12 is the position of the moving body 10. Further, the target position and orientation T (tx, ty, tt) is set in the control unit 15. The control unit 15 plans a target route P to the target position. The control unit 15 plans the target route P from the target position / posture T (tx, ty, tt) and the initial position / posture R (rx, ry, rt) of the moving body. The control unit 15 performs feedback control and the like based on the estimated position and the target route P that are estimated. Thereby, the moving body 10 is controlled to move along the target route P. Further, a local coordinate system with the target position / posture T as the origin and the direction of t as the x-axis is assumed to be L (lx, ly). At this time, the local coordinates of the moving body 10 are indicated by Rl (lrx, lry).

  Here, the control unit 15 sets the movement allowable region based on the planned target route P. When the moving body 10 is outside the movement allowable area, the control unit 15 re-plans the target route. That is, when the estimated position is outside the movement allowable region, the control unit 15 starts replanning of the target route. At this time, the control unit 15 re-plans the target route based on the estimated current position and orientation and the target position and orientation T. And the control part 15 controls the drive mechanism 14 so that the mobile body 10 may follow the re-planned target path | route.

  Here, the movement allowable region is set so that the moving body 10 does not move away from the target route P by k or more. When the moving body 10 deviates from the movement allowable area, the target route P is replanned. When the distance of the moving body 10 to the target route P exceeds the threshold value k, the control unit 15 executes re-planning. Accordingly, a region sandwiched between the permissible lines Q1 and Q2 at a distance of k from the target route P is a movement permissible region. In other words, the locus of the circle when the center of the circle with the radius k is moved along the target route P is the movement allowable region. When the moving body 10 is separated by k or more in the direction (left-right direction) perpendicular to the target route P, the target route is re-planned. That is, when the distance to the nearest point of the target route P becomes k or more, the target route is re-planned with the current position and orientation R as the starting point. The threshold value k is about 10 cm, for example.

  Furthermore, the control unit 15 sets a circle with a radius k centered on the target position (tx, ty). A circle having the center (tx, ty) and the radius k is set as a setting region C. The setting area C includes the target position but becomes the target area. And the control part 15 divides | segments the setting area | region C into two with the dividing line D which passes along target position (tx, ty). As a result, a semicircular divided area A and a divided area B are generated. The dividing line D is a line segment orthogonal to the target posture tt and having a length based on the threshold value k. That is, the divided area A and the divided area B are adjacent to each other via a line segment in a direction orthogonal to the target posture tt. A dividing line D that divides the setting area C into the dividing area A and the dividing area B is a line segment that passes through the target position and has a length of 2k.

Here, the control unit 15 stops the position movement of the moving body 10 when any one of the following three stop conditions is satisfied.
(1) When passing through the divided area A and entering the divided area B (2) When passing through the divided area B and entering the divided area A (3) When lx = 0, ly = 0

Thereafter, the vehicle turns on the spot and stops when rx = tt. Thereby, it is ensured that the position and orientation R of the moving body 10 at the time of stop satisfies the following only by appropriately determining one variable k.
(A) rt = tt, that is, the posture matches the target posture.
(B) lrx = 0, −k <lry <k, that is, the moving body 10 stops on a line in a direction orthogonal to the direction of tt. And lry falls within the range of ± k. Thus, in the moving body 10 according to the present embodiment, the stop position can be easily controlled.

  The control unit 15 determines whether or not the stop conditions (1) and (2) are satisfied based on the estimated position. As described above, by setting the stop conditions (1) and (2), the stop position of the moving body 10 satisfies (b). Furthermore, by setting the stop condition (3), even when the moving body 10 moves along the dividing line D, the moving body 10 stops at the target position. In this way, by stopping the position movement of the moving body 10 based on the timings (1) to (3), it is possible to prevent the stop position from shifting.

  By controlling in this way, the stop position can be easily controlled. Conventionally, when an attempt is made to move to a target position with high stop position accuracy, the moving body may rotate on the spot. However, the stop position error can be easily reduced by utilizing the present invention. Thus, the movement is stopped when the estimated position changes from the divided area A to the divided area B, or when the estimated position changes from the divided area B to the divided area A. That is, when the moving body 10 crosses the dividing line, it is determined that the moving body 10 has reached the target area, and the movement is stopped.

  Next, the configuration of the control unit 15 will be described with reference to FIG. FIG. 3 is a block diagram showing a configuration of the control unit 15 used in the moving body 10 according to the present embodiment. The control unit 15 includes a drive control unit 21, a route planning unit 22, a region setting unit 23, a position estimation unit 24, a determination unit 25, and a stop position control unit 26.

  The drive control unit 21 controls the drive mechanism 14 for driving the drive wheels 12. Specifically, the number of rotations and the direction of rotation of a motor provided in the drive mechanism 14 are controlled. That is, the drive control unit 21 outputs drive signals to the left and right drive mechanisms 14, respectively. Further, a measurement signal from a sensor provided in the drive mechanism 14 is input to the drive control unit 21. A measurement signal from the sensor is input to the position estimation unit 24 via the drive control unit 21. The position estimation unit 24 estimates the current position of the moving body 10 based on the measurement signal from the sensor. Further, the drive control unit 21 performs feedback control based on the target route planned by the route planning unit 22 and the current position estimated by the position estimation unit 24. The target route planning by the route planning unit 22 and the position estimation by the position estimating unit 24 will be described later. Furthermore, the drive control part 21 performs feedforward control in addition to feedforward control.

  When the target position / posture T is set, the path planning unit 22 sets the target path P based on the initial position / posture of the moving body 10 and the target position / posture T. For example, the target route that is the target trajectory of the mobile object 10 is often given as a set of coordinates. The coordinates are expressed by, for example, grid coordinates discretized in a grid shape. The advantage of using the grid coordinates is that a shortest route search can be easily performed and a route that can reliably reach the destination can be obtained. However, since the target route expressed in the grid coordinates is a discontinuous route in which a straight line is connected, it is difficult for the moving body 10 to follow such a route. Therefore, a smooth target path that can be used for follow-up control of the moving body 10 can be created from given discrete path coordinates by, for example, obtaining an interpolation curve using a weighted moving average. Thus, the target route P can be easily planned by forming a grid for a given map. Of course, you may plan the target path | route P by methods other than the above.

  The position estimation unit 24 estimates the current position and posture of the moving body 10. For example, the position estimation unit 26 estimates the current position based on an output from a sensor such as a rotary encoder provided in the drive mechanism 14 or a gyro sensor. For example, the current position can be estimated using a rotary encoder that is an internal sensor. The rotational speed of the left and right motors or drive wheels 12 is measured by a rotary encoder. And the control part 15 calculates the moving amount | distance of the drive wheel 12 based on the measured rotation speed. The current estimated position can be calculated by obtaining the movement locus from the initial position and orientation. Further, a gyro sensor or the like can be used for posture estimation. The position estimation is not limited to the above method. For example, a sensor using a sensor such as an acceleration sensor, a laser range finder, or a camera is used. Of course, the position may be estimated using a plurality of types of sensors.

  The area setting unit 23 sets the movement allowable area and the setting area C based on the target route P planned by the route planning unit 22. Further, the area setting unit 23 divides the setting area C and sets a divided area A and a divided area B. That is, the setting area C having the radius k is divided by the dividing line D to generate the dividing area A and the dividing area B. Therefore, the divided area A and the divided area B are partitioned by the dividing line D. That is, the dividing line D is a boundary line between the dividing area A and the dividing area B. The area setting unit 23 sets a movement allowable area, a setting area C, and divided areas A and B for a given map. The area setting unit 23 sets a circular area having a radius k including the target position as the setting area C. Here, the center of the setting area C is the target position. Here, the setting area C is included in the movement allowable area, and the divided areas A and B are included in the setting area C. The area setting unit 23 stores these areas in a storage device such as a RAM. Thus, each area is set with reference to the target route and the target position and orientation.

  The determination unit 25 determines whether the moving body 10 is out of the movement allowable region. Thereby, it is determined whether or not re-planning is performed. Specifically, it is determined whether or not the position estimated by the position estimation unit 24 is included in the movement allowable region set by the region setting unit 23. Then, when it is determined that the estimated position is not included in the movement allowable area, the determination unit 25 causes the route planning unit 22 to perform re-planning. On the other hand, when it is determined that the estimated position is included in the movement allowable region, the movement of the moving body 10 is continued as it is. Note that the determination unit 25 is not limited to the unit that performs the re-planning determination based on the movement allowable area. For example, the determination unit 25 may calculate the distance between the current estimated position and the nearest point of the target route, and compare the distance with the threshold value k. The movement allowable area is set based on allowable lines Q1 and Q2 that are separated from the target route by a threshold value k. By making a determination based on this movement allowable area, it is not necessary to calculate the distance to the nearest point of the target route, and the arithmetic processing can be simplified.

  As described above, when the determination unit 25 determines that it is outside the movement allowable region, the route planning unit 22 re-plans the target route. Further, the route planning unit 22 causes the region setting unit 23 to reset the region for the replanned target route. The area setting unit 23 sets a movement allowable area for the re-planned target route. That is, since the target route P has changed, the area setting unit 23 resets the movement allowable route. Here, since there is no change in the target position and orientation T, the divided area A, the divided area B, and the setting area C do not change. That is, the divided area A, the divided area B, and the setting area C do not change before and after the re-planning. Then, the determination unit 25 determines whether to perform re-planning based on the reset allowable movement area. And when the mobile body 10 protrudes from the reset movement allowable region, the same processing is performed again.

  The stop position control unit 26 controls the stop position of the moving body 10. Specifically, the stop position control unit 26 determines whether or not the above conditions (1) to (3) are satisfied. If the stop position control unit 26 determines that any one of the conditions (1) to (3) is satisfied, the position movement is stopped. Thereby, the stop position of the mobile body 10 is determined. That is, it is determined whether or not the position estimated by the position estimation unit 24 is included in the divided area A or the divided area B. Then, it is determined whether or not the next estimated position is included in the other divided region. Further, it is determined whether or not the estimated position matches the stop position. When any of the above conditions (1) to (3) is satisfied, the position movement is stopped. As described above, the stop position control unit 26 determines that the target position has been reached at the time of transition from one divided region to the other divided region. Then, the movement is stopped.

  For example, when the estimated position is in the divided area A, the stop position control unit 26 determines whether or not the next estimated position is included in the divided area B. If the next estimated position is not in the divided region B, the drive control unit 21 continues to move the moving body 10. On the other hand, when the next estimated position enters the divided area B, it is determined that the moving body 10 has entered the divided area B from the divided area A. That is, it is determined that the moving body 10 has crossed the dividing line D. Then, the stop position control unit 26 stops the position movement. Specifically, the stop position control unit 26 instructs the drive control unit 21 to stop the position movement. As a result, the drive control unit 21 controls the drive mechanism 14 to stop the rotation of the drive wheels 12. Thereafter, the drive control unit 21 rotates the left and right drive wheels 12 in opposite directions and turns on the spot. Thereby, the mobile body 10 becomes the target posture tt. Of course, when the moving body 10 enters the divided area A from the divided area B, the position movement is similarly stopped.

  As described above, based on the timing when one of the estimated positions estimated continuously in time is included in the divided area A and the other is included in the divided area B, the position movement of the moving body 10 is stopped. . Thereby, the position movement stops based on the timing of crossing the dividing line D and entering the second divided area (for example, divided area B) from the first divided area (for example, divided area A). And if a position movement stops, the mobile body 10 will be turned to the target attitude | position on the spot. In this way, the movement of the moving body 10 to the target position / posture T can be controlled. Thereby, the error of a stop position can be reduced. Specifically, the error of the stop position in the direction orthogonal to the target posture can be suppressed to ± k.

  The control unit 15 stores, for example, a position estimation program, a route planning program, an area setting program, a replanning determination program, a stop position control program, and the like. The above processing is performed by executing these programs.

  Next, the control method according to the present embodiment will be described with reference to FIG. FIG. 4 is a flowchart showing a method for controlling the moving body 10 according to the present embodiment.

  First, the route planning unit 22 plans a route based on the set target position and orientation T and the initial position and orientation of the moving body 10 (step S1). Thereby, a target route is set. Then, the area setting unit 23 sets an area based on the target route P (step S2). Here, a movement allowable area, divided areas A and B, and a setting area C are set. Next, the drive control part 21 drives the drive mechanism 14, and starts the movement of the mobile body 10 (step S3). Thereby, the mobile body 10 moves following the target route.

  When the movement starts, the position estimation unit 24 estimates the current position of the moving body 10 (step S4). The determination unit 25 determines whether to perform re-planning based on the latest estimated position (step S5). That is, the determination unit 25 confirms whether or not the estimated position is out of the movement allowable region set in step S2. If it is outside the movement allowable area, the route is re-planned (step S6). As a result, the target route P is re-planned based on the current estimated position / posture and target position / posture. Here, for example, the movement of the moving body 10 is stopped, and the route is re-planned. When the replanning is completed, the process returns to the movement start step S3 of the moving body 10 and the process is repeated. Of course, the route may be re-planned while the moving body 10 is moved.

  If the estimated position does not deviate from the movement allowable region, the stop position control unit 26 performs position stop determination (step S7). Specifically, as described above, it is determined whether or not the conditions (1) to (3) are satisfied. If the conditions (1) to (3) are not satisfied, the process returns to the step (S4) for estimating the current position, and the process is repeated. That is, the current position is estimated again. Therefore, the position estimation unit 24 updates the current position of the moving body 10 at predetermined time intervals. Here, the position estimated by the position estimation unit 24 may be stored in a memory or the like.

  If any one of the conditions (1) to (3) is satisfied, the position movement is stopped (step S8). That is, the rotation of the left and right drive wheels 12 in the same direction stops, and the stop position is determined. Thereby, the mobile body 10 stops at or near the target position. Then, the left and right drive wheels 12 are rotated in opposite directions to perform a rotation operation (step S9). Thereby, the mobile body 10 becomes a target posture. And the drive of the drive mechanism 14 is stopped and the movement to a target position and attitude | position is complete | finished. Thereby, the error of a stop position can be made small. The stop posture can be matched with the target posture.

  In the above example, the dividing line D is described as a line segment that bisects the circular setting region C, but the present invention is not limited to this. The dividing line D may be a line passing through the target position. Therefore, the dividing line D is not limited to a straight line but may be a curved line. Further, the length of the dividing line D is not limited to 2k, and may be any length according to k. Further, the setting area C is not limited to a circle having a radius k. That is, the setting area C may be an area that includes the target position and has a size corresponding to the threshold value k. Further, the number of divided areas is not limited to two.

Embodiment 2 of the Invention
The moving body 10 according to the present embodiment will be described with reference to FIG. FIG. 5 is a top view schematically showing the configuration of the moving body 10 according to the present embodiment. The movement control of the moving body 10 according to the present embodiment is basically the same as that of the first embodiment. Therefore, description of the same contents as those in Embodiment 1 is omitted. The moving body 10 according to the present embodiment is a robot that performs a task of gripping an object.

  A hand 33 for holding the object 40 is attached to the vehicle body 11 of the moving body 10. By closing the hand 33, the target object 40 is gripped. An arm 32 is attached to the vehicle body 11 via a rotation mechanism 31. Moreover, the arm 32 has a linear motion actuator and is provided so that expansion and contraction is possible. In the present embodiment, the expansion / contraction range of the arm 32 is 0 to 100 mm. A hand 33 is provided at the tip of the arm 32.

  The rotation mechanism 31 can rotate the arm 32 by a predetermined angle. Here, the rotation range of the rotation mechanism 31 is set to ± 30 °. When the moving body 10 moves to the target position / posture, the moving body 10 holds the object 40 with the hand 33. At this time, the rotation mechanism 31 and the arm 32 are driven. As a result, the hand 33 moves to a position where the object 40 can be gripped. For example, the position of the object 40 can be recognized by a camera such as a CCD camera, and the hand 33 can be moved to a grippable position. As described above, by providing the arm 32 and the rotation mechanism 31, it is possible to absorb the error generated at the stop position of the moving body 10. Therefore, the object 40 can be gripped even when the stop position is deviated. Here, the range that can be gripped by the hand 33 is a grippable range G.

  As described above, the stop position of the moving body 10 has an error of ± k in the direction perpendicular to the target posture. Here, the stop position range of the moving body 10 is indicated by a stop position range E. Further, a hand position range F indicates a hand position error caused by a stop position error when the arm 32 is set to 0 mm and the rotation mechanism 31 is set to 0 °. The stop position range E and the hand position range F have the same size.

  Here, when the rotation mechanism 31 and the linear motion arm 32 are combined, the movable range of the arm 32 is limited. Therefore, the grippable range G of the hand 33 is also limited. If the stop position of the moving body 10 shifts, the object 40 can be gripped. In other words, the object 40 cannot be gripped unless the displacement of the stop position of the moving body 10 is set to a certain allowable range or less. Furthermore, the movable range of the arm 32 has directionality. Therefore, the variation in the stop position allowed for the moving body 10 is also short in the traveling direction and long in the direction orthogonal to the traveling direction. That is, the tolerance of the stop position becomes strict in the same direction as the target posture. On the other hand, in the direction orthogonal to the target posture, some tolerance of the stop position is recognized.

  Here, the threshold value k is set based on the grippable range G. Then, the moving body 10 is moved using the method shown in the first embodiment. Thereby, the position error and the direction error in the traveling direction are eliminated, and the error in the direction orthogonal to the traveling direction can be suppressed to ± k. Thereby, the object 40 to be grasped can be controlled within the graspable range G of the hand 33. Therefore, the object 40 can be reliably gripped. Furthermore, by setting the dividing line D according to the grippable range G, the object 40 can be reliably gripped. For example, the dividing line D can be arcuate.

  In the present embodiment, a robot having a hand capable of gripping the moving body 10 has been described. However, the present invention is not limited to this. The present invention is applicable to the mobile body 10 provided with a task execution mechanism that performs a predetermined task at a stop position. In this case, the threshold value k and the dividing line D may be set according to the task possible range of the task execution mechanism. Here, the task possible range can be set by a distance necessary for executing the task from the stop position of the moving body. For example, in the case where the moving body is a robot-type moving body having a hand unit for gripping an object, an area specified by the maximum distance that the hand can reach from the object to be gripped by the robot is set as a taskable area. Accordingly, if the position of the moving body 10 is included in a range that does not affect the task executed by the moving body 10, the moving body 10 is regarded as having reached the target position and stopped. And a predetermined task can be reliably performed by turning on the spot at this stop position.

Other embodiments.
In the first and second embodiments, the moving body 10 is applied to a two-wheeled moving body. However, the moving body 10 is not limited to this, and can be applied to a four-wheeled robot, for example, more than two wheels. It can also be applied to robots of the above type, bipedal walking and quadruped walking. In the case of a walking robot, it is possible to follow the target route by changing the moving direction by controlling the stride. Furthermore, the present invention is not limited to a robot and can be applied to a two-wheeled or four-wheeled vehicle.

It is a figure which shows the structure of the moving body concerning Embodiment 1 of this invention. It is a figure for demonstrating the control method of the moving body concerning Embodiment 1 of this invention. It is a block diagram which shows the structure of the control part of the moving body concerning Embodiment 1 of this invention. It is a flowchart which shows the control method of the moving body concerning Embodiment 1 of this invention. It is a figure which shows the structure of the moving body concerning Embodiment 2 of this invention.

Explanation of symbols

10 moving bodies, 11 vehicles, 12 driving wheels, 13 driven wheels, 14 driving mechanisms,
15 control unit, 21 drive control unit, 22 route planning unit, 23 area setting unit,
24 position estimation unit, 25 determination unit, 26 stop position control unit,

Claims (12)

A moving body having a control unit that performs movement control so as to follow a target route to a target position,
The control unit is
A position estimator that estimates the position of a moving body that moves following the target path;
An area setting unit that sets a target area that includes the target position and is divided into a first divided area and a second divided area by a dividing line that passes through the target position;
When the estimated position estimated by the position estimation unit transitions from the first divided area to the second divided area, it is determined that the moving body has reached the target position, and the movement of the moving body is stopped. Moving body.   The moving body according to claim 1, wherein the first divided region and the second divided region are divided by using the dividing line as a line segment in a direction orthogonal to the direction of the target route at the target position.   The moving body according to claim 1, wherein the target area is set as a circle having a radius k. A path planning unit that plans a target path to the target position by the moving body;
A determination unit that determines that the estimated position estimated by the position estimation unit is a predetermined distance away from a position on the target route planned by the route planning unit;
The route planning unit re-plans a target route to the target position when it is determined that the moving body has moved away from the target route by a distance equal to or larger than a radius k of the target region. 3. The moving body according to 3. The area setting unit sets a movable area based on the target route and the value of the radius k,
The moving body according to claim 4, wherein when the determination unit determines that the estimated position is out of the movable region, the target path is re-planned. A task processing mechanism for executing a predetermined task;
The value of the radius k is set based on a task possible range set by a distance required for the task processing mechanism to execute a task from the stop position of the moving body. The moving body described in 1. A control method for controlling the movement of a moving body that follows a target route,
Setting a target area that includes the target position and is divided into a first divided area and a second divided area by a dividing line that passes through the target position;
Estimating the position of the moving body;
Determining that the estimated position has reached a target position when the estimated position has changed from the first divided area to the second divided area of the divided areas, and stopping the movement of the moving body. Control method.   The method of controlling a moving body according to claim 7, wherein the first divided region and the second divided region are divided by using the dividing line as a line segment in a direction orthogonal to the direction of the target route at the target position. .   The method for controlling a moving body according to claim 7 or 8, wherein the target area is set to be a circle having a radius k. The mobile planning a target route to the target position;
Determining that the estimated position estimated by the position estimating unit is separated from the position on the target route planned by the route planning unit by a predetermined distance; and
10. The mobile object according to claim 9, wherein when it is determined that the mobile object is separated from the target route by a distance equal to or larger than the radius k of the target region, the target route to the target position is re-planned. . Further comprising setting a movable region based on the target route and the value of the radius k;
The method for controlling a moving body according to claim 10, wherein the target route is re-planned when it is determined in the determining step that the estimated position is out of the movable region. The method for controlling a moving body according to any one of claims 7 to 11, further comprising a step of turning the moving body to a target posture on the spot after stopping the position movement.

Images