JP2013237125A - Method for controlling movement of movable body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To make a movable body reliably grip an object.SOLUTION: A method for controlling the movement of a movable body includes: a step of setting an object grip region where a robotic arm is capable of gripping an object to be gripped, on the basis of an object detectable region where a gripping object detecting unit installed in a movable body is capable of detecting the object to be gripped, and an object reachable region where the robotic arm mounted to the movable body is capable of reaching the object to be gripped; a step of determining a possibility of reaching the object grip region on the basis of self position variations and following gaps of the movable body; and a step of generating a route plan for allowing the movable body to reach the object grip region.

Description

  The present invention relates to a movement control method for a moving object.

  2. Description of the Related Art In recent years, development of a moving body that travels a target travel route by automatic control and performs operations such as gripping of a target object after reaching a target position has been advanced.

  In Patent Document 1, a dividing line passing through the target position is set in advance, and the mobile body has reached the target position when the mobile body transitions from the first divided area to the second divided area by self-position estimation. A control method for a moving body is disclosed that determines and stops the movement.

  Patent Document 2 discloses a control method for a moving body that controls to stop an automatic guided vehicle when a target stop line determined by a current position and a steering angle of the automatic guided vehicle is on the end point or exceeds the end point. Is disclosed.

  In Patent Document 3, a region in the working work space of the robot arm is used as a work determination region, and a step of arranging grid points in each work determination region, and whether a work by a mobile robot can be performed at the lattice points. A method for controlling a moving body is disclosed which includes a step of determining whether or not to determine a workable area corresponding to a plurality of work.

JP 2007-257195 A JP2011-141665A Japanese Patent Laid-Open No. 2005-088164

  However, when a dividing line that passes through the destination is set in advance and the movement is stopped when the moving body reaches the dividing line, a route in which the dividing line and the route are substantially parallel is generated. There is a possibility that the moving body does not reach the dividing line and the moving body does not stop. Also, when it is assumed that the mobile body that runs on its own uses a robot arm, the self-position estimation results may vary, and when combined with the path tracking error, the deviation from the destination may become large. is there. That is, there is a case where it is not possible to reach an area where the moving body can perform work.

The movement control method for a moving body according to the present invention includes an object detectable area in which a gripping object detection unit installed on the moving body can detect a gripping target object, and a robot installed on the moving body. Setting an object grippable area that is an area in which the robot arm can grip a gripping target object based on an object reachable area that is an area where the arm can reach the gripping target object; Determining the reachability to the object grippable area based on the self-position variation of the mobile body and the tracking error, and generating a path plan for causing the mobile body to reach the object grippable area Steps.
Thereby, even when there is a self-position variation or a follow-up deviation, it is guaranteed that the moving body enters the object grippable region.

  The moving body can reliably hold the object.

1 is a diagram illustrating a configuration of a moving object according to a first embodiment. It is the figure which showed the external appearance of the mobile body concerning Embodiment 1. FIG. 3 is a flowchart of the operation of the moving body according to the first exemplary embodiment. 4 is a detailed flowchart of calculation of a grippable area according to the first embodiment. FIG. 3 is a diagram illustrating a detection range according to the first embodiment. FIG. 6 is a diagram illustrating calculation of a detectable area according to the first embodiment. FIG. 3 is a diagram illustrating a set of hand points of the robot arm according to the first embodiment. It is a figure which shows calculation of the reachable area | region concerning Embodiment 1. FIG. It is a figure which shows the reachable area | region and detectable area | region concerning Embodiment 1. FIG. FIG. 6 is a diagram illustrating calculation of a grippable area according to the first embodiment. FIG. 6 is a diagram illustrating calculation of a confidence ellipse according to the first embodiment. 3 is a flowchart of determination processing according to the first exemplary embodiment; It is the figure which showed the relationship between the mobile body concerning Embodiment 1, and a reliability ellipse. FIG. 5 is a diagram showing a relationship between a straight line and a straight line connecting a destination from a gripping target object according to the first exemplary embodiment. 6 is a flowchart for generating a route that easily falls within the grippable range according to the first embodiment; It is the figure which showed the state which installed the some route point concerning Embodiment 1. FIG. It is the figure which showed the state which installed the some path | route point concerning Embodiment 1 and determined the path | route. 3 is a flowchart for generating a route that surely enters a detection range according to the first embodiment; It is the figure which showed the state which set the end point of the detectable area | region concerning Embodiment 1 to the destination. It is the figure which showed the state which has arrange | positioned several route points from the destination concerning Embodiment 1. FIG. It is the figure which showed the state which produced | generated the route after arrange | positioning several route points from the destination concerning Embodiment 1. FIG. It is the figure which showed the state immediately before the mobile body concerning Embodiment 1 enters into a detectable region. It is the figure which showed the state advanced until the mobile body concerning Embodiment 1 entered into a detectable region. FIG. 3 is a diagram illustrating a state in which a moving body is moved from a known relative positional relationship according to the first embodiment.

Embodiment 1
Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a diagram showing a configuration of a moving body according to the present invention. FIG. 2 is a view showing the appearance of the moving body according to the present invention. A broken arrow in FIG. 2 indicates a moving path of the moving body 1.

  The moving body 1 includes a robot arm 11, a distance sensor 12, an environment map data storage unit 13, a self-position estimation unit 14, a gripping object detection unit 15, a route plan unit 16, a drive control unit 17, and a calculation unit. 18.

  The robot arm 11 is an arm provided on the moving body 1. Typically, the robot arm 11 includes an arm portion having a joint and a grip portion for gripping a target object. Here, it is assumed that the robot arm 11 has a narrow operating range and high directivity. That is, the robot arm 11 has a low degree of freedom and can only be moved in a specific direction (front / rear / left / right / oblique). Information regarding the operating range of the robot arm 11 is output to the route planning unit 16.

  The distance sensor 12 is a sensor that measures the distance from the moving body 1 to surrounding objects. For example, the distance sensor 12 is an ultrasonic rangefinder that generates ultrasonic waves and measures distances based on reflected waves from surrounding objects. The distance sensor 12 outputs the acquired data to the self-position estimation unit 14.

  The environmental map data storage unit 13 stores environmental map data. Here, the environmental map data stores position information of various objects installed in the environment where the mobile body 1 travels. The environmental map data storage unit 13 outputs the stored environmental data to the self-position estimation unit 14.

  The self-position estimation unit 14 is based on the distance data from the moving body 1 acquired by the distance sensor 12 to surrounding objects and the environmental map data stored in the environmental map data storage unit 13. Estimate the position. The self-position estimating unit 14 outputs the estimated self-position to the route planning unit 16.

  The gripping object detection unit 15 is an imaging unit such as a camera. The gripping object detection unit 15 outputs information related to the detection range to the route planning unit 16.

  The route planning unit 16 plans a moving route of the moving body 1. More specifically, the path planning unit 16 uses the robot arm 11 by the mobile body 1 based on information such as the self-position of the mobile body 1 estimated by the self-position estimation unit 14 and the operating range of the robot arm 11. Planning a path for moving the object to be gripped to a position where it can be gripped. At this time, the path planning unit 16 assumes a task in which the moving body 1 is positioned in front of the object to be grasped and grasps the object. The route planning unit 16 outputs the planned route to the drive control unit 17.

  The drive control unit 17 controls the operation of the drive unit so that the moving body 1 travels on the route planned by the route planning unit 16. A drive part is a motor which operates the wheel provided in the lower part of the mobile body 1, for example.

  The calculation unit 18 performs various calculations. For example, the calculation unit 18 obtains a detectable region that is a destination of the moving body 1 and is necessary for the gripping object detection unit 15 to detect the gripping target object. In addition, based on information such as the operating range of the robot arm 11, the calculation unit 18 calculates reachable points that are necessary for the robot arm 11 to reach the object to be grasped and become the destination of the moving body 1. Further, the calculation unit 18 calculates a grippable region that is the destination of the moving body 1 in order to grip the gripping target object based on the information of the detectable region and the reachable point.

  Next, an operation in which the moving body 1 performs autonomous movement and performs a gripping target object will be described. FIG. 3 is a flowchart of the operation of the moving body 1.

  A gripping target object is set in the gripping object detection unit 15 (step S11). For example, what the gripping target object is is registered in the gripping object detection unit 15.

  The moving body 1 calculates the object grippable area (step S12). FIG. 4 is a detailed flowchart of the calculation of the object grippable area.

  The calculation unit 18 calculates a detectable area of the gripping target object (step S121). FIG. 5A and FIG. 5B are diagrams obtained by geometrically calculating back a region where the gripping target object can be detected based on the detection range of the gripping object detection unit 15. A broken line portion in FIG. 5A indicates a detection range of the gripping object detection unit 15. The broken line portion in FIG. 5B indicates a detectable region that is the region that the moving object 1 should reach in order for the gripping object detection unit 15 to detect the gripping target object, which is obtained by the calculation of the calculation unit 18.

The calculation unit 18 calculates the object reachable region (step S122). Specifically, the calculation unit 18 represents the operating range of each joint of the robot arm 11 as a set of discrete points, and calculates hand coordinates r = f (θ) in all combinations of joint angles. FIG. 6A is an example of calculating the hand coordinates of the robot arm 11 by the calculation unit 18, and the calculated hand coordinates are indicated by black circles.
Further, the calculation unit 18 calculates the reachable area of the moving body 1 geometrically by performing a reverse calculation of the hand coordinates based on the coordinates of the object to be grasped. That is, the calculation unit 18 calculates the position of the moving body 1 for the hand of the robot arm 11 to reach the object to be grasped. Black circles in FIG. 6B indicate reachable points that are position coordinates that the moving body 1 should reach in order for the hand of the robot arm 11 to reach the object to be grasped. Note that an area where reachable points are gathered is defined as a reachable area.

  The calculation unit 18 calculates an object grippable area (step S123). Specifically, the detectable region calculated by the calculation unit 18 in step S121 and the reachable point calculated by the calculation unit 18 in step S122 are combined, leaving only the reachable point in the detectable region, and the grippable region And FIG. 7A shows a state where the detectable area and the reachable point are combined. FIG. 7B shows a grippable area that is a common part of the detectable area and the reachable point.

  The computing unit 18 calculates an n% confidence ellipse of the set of points representing the grippable area (step S124). When n = 100, the confidence ellipse becomes infinite, so n is a value other than 100, for example, n = 99. FIG. 8 is a diagram in which a 99% confidence ellipse is generated for the grippable region. The inside of this trust ellipse is defined as a grippable area.

  The moving body 1 sets the destination as the center point of the confidence ellipse (step S13).

  The route planning unit 16 generates a route toward the destination (step S14). In the route generation by the route planning unit 16, the self-position estimation unit 14 performs self-position estimation based on the distance data measured using the distance sensor 12 and the environment data stored in the environment map data storage unit 13. Do.

  The calculation unit 18 performs arrival determination in consideration of self-position variation and follow-up deviation (step S15). FIG. 9 is a flow of detailed determination processing. FIG. 10 is a diagram illustrating the relationship between the moving object 1 and the confidence ellipse.

  The calculation unit 18 sets the straightness m that is perpendicular to the direction in which the generated path is incident on the destination and passes through the destination (step S151).

ここで、信頼楕円の長径を２ａ、短径を２ｂとする。 The computing unit 18 obtains an intersection between the straight line m and the reliable ellipse of the grippable region, and calculates a distance L between the intersections (step S152). Here, considering a coordinate system in which the long axis direction of the confidence ellipse is the x axis and the single axis direction is the y axis, and m is converted to the coordinate system of the x axis and the y axis, L is expressed by the following equation (1). Can be sought.

Here, the major axis of the confidence ellipse is 2a and the minor axis is 2b.

の場合には（ステップＳ１５３でＹｅｓ）、経路計画部１６は、移動体１が把持可能領域に到達可能であるものとして、直線ｍを用いて把持可能範囲に収まりやすい経路を生成する（ステップＳ１６）。ステップＳ１６の手順については、後に詳述する。

の場合には（ステップＳ１５３でＮｏ）、ステップＳ１５５に進む。 The calculation unit 18 determines that the radius of the variation range of the self position of the moving body 1 is known and the tracking deviation range Δl is already known (step S153).

In this case (Yes in step S153), the route planning unit 16 assumes that the mobile body 1 can reach the grippable region, and generates a route that easily fits in the grippable range using the straight line m (step S16). ). The procedure of step S16 will be described in detail later.

In the case of (No in step S153), the process proceeds to step S155.

の場合には（ステップＳ１５４でＹｅｓ）、ステップＳ１５５に進む。

の場合には（ステップＳ１５４でＮｏ）、経路計画部１６は、移動体１が検出範囲に確実に入る経路生成を行う（ステップＳ１７）。ステップＳ１７の手順については後に詳述する。 The computing unit 18 compares the radius of the variation range of the self-position of the moving body 1 with the total value of Δp and the tracking deviation range Δl with the length of the long side of the confidence ellipse (step S154).

In the case of (Yes in step S154), the process proceeds to step S155.

In the case of (No in step S154), the route planning unit 16 generates a route in which the moving body 1 surely enters the detection range (step S17). The procedure of step S17 will be described in detail later.

となるｃを計算し、目的地と把持対象物体とを結んだ直線と、直線ｍがなす角θの値を算出する（ステップＳ１５５）。図１１は、把持対象物体から目的地までを結んだ直線と、直線ｍとの関係を示した図である。 The calculation unit 18

C is calculated, and the value of the angle θ formed by the straight line m connecting the destination and the object to be grasped and the straight line m is calculated (step S155). FIG. 11 is a diagram illustrating a relationship between a straight line connecting the target object to the destination and the straight line m.

である場合には（ステップＳ１５６でＮｏ）、直線ｍを用いて把持可能範囲に収まりやすい経路生成（ステップＳ１６）に進む。

である場合には（ステップＳ１５６でＹｅｓ）、移動体１の把持対象物体に対するアプローチが見た目上不自然となってしまうため、検出範囲確実に入る経路生成（ステップＳ１７）を行う。 The computing unit 18 determines whether or not the angle θ is smaller than a predetermined threshold θ _TH (step S156).

If it is (No in step S156), the process proceeds to a path generation (step S16) that easily falls within the grippable range using the straight line m.

If this is the case (Yes in step S156), the approach of the moving object 1 to the object to be grasped will be visually unnatural, and therefore a path that enters the detection range is generated (step S17).

  If it is determined in step S15 that the moving body 1 can reach the grippable region, the route planning unit 16 generates a route that easily falls within the grippable range (step S16). The route planning unit 16 generates a route according to the following procedure, and the moving body 1 moves along the route. FIG. 12 is a detailed flowchart for generating a route that the route planning unit 16 easily fits in the grippable range.

  The route planning unit 16 creates a route according to the arrival determination result (step S161). Specifically, the route planning unit 16 arranges several route points in a direction perpendicular to the straight line m, and creates a route connecting these points and the moving body 1. FIG. 13A is an example in which a plurality of route points are arranged in a direction perpendicular to the straight line m from the destination, and FIG. 13B shows a route plan connecting the mobile body 1 and the destination through the plurality of route points. This is an example.

  The drive control unit 17 performs path following control until the moving body 1 arrives at the destination (step S162).

  The drive control unit 17 turns at the destination so that the moving body 1 faces the front with respect to the object to be grasped (step S163). Thereafter, the process proceeds to step S18.

  The moving body 1 ends autonomous movement (step S18).

  If it is determined in step S15 that the moving body 1 cannot reach the grippable area, the route planning unit 16 generates a route that reliably enters the detection range (step S17). The route planning unit 16 generates a route according to the following procedure, and the moving body 1 moves along the route. FIG. 14 is a detailed flowchart in which the route planning unit 16 generates a route that reliably enters the detection range.

  The route planning unit 16 sets the destination at the end of the detectable area (step S171). Specifically, the calculation unit 18 calculates the barycentric point of the grippable area. Next, the path planning unit 16 sets the point farthest from the gripping target object as the destination among the intersections of the straight line connecting the gripping target object and the barycentric point and the end line of the detectable region. FIG. 15 is an example of setting a destination.

  The route planning unit 16 generates a route from the current location of the moving body 1 to the destination (step S172). Specifically, the route planning unit 16 generates a route from the moving body 1 after arranging several route points from the destination in a direction parallel to a straight line connecting the gripping target object and the destination. FIG. 16A shows a state in which several route points are arranged from the destination, and FIG. 16B shows a state in which a route from the moving body 1 is generated after several route points are arranged from the destination.

  The drive control unit 17 controls the movement of the moving body 1 so as to move to the destination (step S173).

The calculation unit 18 determines whether or not the gripping object detection unit 15 can detect the gripping target object when the moving body 1 arrives near the destination (step S174). When the gripping object detection unit 15 cannot detect the gripping target object (No in step S174), the drive control unit 17 advances the moving body 1 until the gripping object detection unit 15 detects the gripping target object ( Step S175). The case where the object cannot be detected when the moving body 1 arrives at the destination is a case where the autonomous moving body does not enter the detectable region due to the influence of the self-position variation and the tracking error. FIG. 17A is an example of a case where the moving body 1 does not enter the detectable area, and FIG. 17B shows a state where the moving body 1 is advanced until it completely enters the detectable area.
When the gripping object detection unit 15 detects a gripping target object (Yes in step S174), the process proceeds to step S176.

  The drive control unit 17 aligns the moving body 1 based on the relative positional relationship between the gripping target object and the moving body 1 (step S176). At this time, the position of the moving body 1 is farther from the center of gravity of the grippable region with respect to the gripping target object. Therefore, even when the drive control unit 17 moves the movable body 1 to the center of gravity of the grippable area, the drive control unit 17 does not direct the movable body 1 in a direction substantially opposite to the gripping target object. FIG. 18 is a diagram illustrating the positional relationship between the movable body 1, the grippable region center of gravity, and the gripping target object, and the moving direction of the movable body 1. Thereafter, the moving body 1 stops moving (step S18).

Therefore, when it is determined that the mobile body 1 is likely to be within the grippable range, the straight line m serving as the reaching line of the mobile body 1 is provided, so that the self-position estimation accuracy is poor and the mobile body 1 has a self-position variation or drive control. Even if the path tracking accuracy by the unit 17 is poor and the path may be off, it can be ensured to some extent that the moving body 1 enters the grippable range.
Also, when path generation is performed by determining that it is difficult to fit in the grippable area, from the intersection of the straight line connecting the gripping target object and the grippable area centroid and the end line of the detectable area, from the gripping target object By disposing several path points in the direction parallel to the straight line connecting the object to be grasped and the destination, the moving body 1 can be surely placed in the detectable region.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, although an ultrasonic distance meter is used as the distance sensor 12, other distance measurement methods such as a distance sensor using a laser may be used.

DESCRIPTION OF SYMBOLS 1 Mobile body 11 Robot arm 12 Distance sensor 13 Environmental map data storage part 14 Self-position estimation part 15 Grasping object detection part 16 Path planning part 17 Drive control part 18 Calculation part

Claims (1)

An object detectable area, which is an area in which a gripping object detection unit installed on a moving body can detect a gripping target object, and a robot arm installed on the moving body can reach the gripping target object Setting an object grippable area, which is an area where the robot arm can grip a gripping target object, based on the object reachable area that is
Determining reachability to the object grippable area based on self-position variation of the moving body and tracking deviation;
Generating a route plan for causing the moving body to reach the object grippable region, and a moving control method for the moving body.

Images