JP7434627B2 - Movement control method of moving object - Google Patents

Description

The present invention relates to a method for controlling the movement of a moving object, which moves the moving object based on movement schedule information set for the moving object.

The main body is equipped with obstacle detection means (sensor), direction detection means (sensor), distance recognition means (sensor), position recognition means, buffer means (sensor), etc., and moves based on the output signals from each of these means. 2. Description of the Related Art A movement control method for a mobile body (autonomous mobile device) is known (see Patent Document 1).

Japanese Patent Application Publication No. 2009-265801

According to the above-mentioned method for controlling the movement of a moving object, the moving object is equipped with a large number of sensors and a control program based on information from the large number of sensors, so that the moving object can be moved while maintaining a distance from obstacles. Although the mobile object can be moved, there is a problem in that the mobile object cannot be moved along a predetermined planned movement route.
The present invention provides a method for controlling the movement of a moving object, which allows the moving object to move accurately along a predetermined planned movement route.

The movement control method of a moving object according to the present invention includes a movement schedule information setting process of setting movement schedule information of a moving object to be moved on a moving target surface, moving the moving object based on the movement plan information, and moving the moving object. a movement control process that controls the movement of the moving object by comparing the actual movement information of the moving object acquired by the information acquisition means with the movement schedule information, and the movement schedule information setting process includes: A moving object movement control method that sets movement schedule information for moving a moving object along a zigzag path in which a straight path is repeatedly turned back and forth. In order to set, the XY coordinate values of the movement start point of the moving object, the XY coordinate values of the end point of the first straight path, and the diagonal measured as the diagonal point of the movement start point on the XY coordinate of the movement target surface. By setting the XY coordinate values of the measurement point and the initial value of the distance between adjacent straight paths, the width of the zigzag path in the direction in which multiple straight paths are lined up and the initial value of the distance between the straight paths It is characterized by calculating and setting the optimal value of the number of straight paths.
According to the above-described movement control method for a moving object, it becomes possible to move the moving object with high precision along a predetermined zigzag route based on the movement schedule information, and the zigzag path along which the moving object is planned to move. can be easily set.

FIG. 1 is a diagram showing a movement control system for a mobile object. FIG. It is a figure which shows the detail of a lifting device, Comprising: (a) is a front view, (b) is a side view. FIG. 3 is an operation procedure diagram showing an operation procedure for changing the moving direction of a moving body using the lifting device. 5 is a flowchart showing movement schedule information setting processing. The figure which shows the movement schedule information setting screen. FIG. 6 is a diagram showing a zigzag route set by movement schedule information setting processing. 5 is a flowchart showing the entire movement control process. 5 is a flowchart showing normal travel processing in movement control processing. FIG. 3 is a diagram illustrating an overview of normal travel processing and movement direction correction processing in the movement control processing of a moving body. FIG. 3 is a diagram illustrating an overview of return processing in movement control processing of a moving body. 5 is a flowchart showing movement direction correction processing in movement control processing. 5 is a flowchart showing processing when irregularity occurs in movement control processing. 12 is a flowchart showing return processing in movement control processing. FIG. 3 is an explanatory diagram showing a rotation center calculation method. An explanatory diagram showing an angle calculation method. An explanatory diagram showing a method of calculating the distance between a vector and a point. An explanatory diagram showing a straight route (lane) updating method. An explanatory diagram showing a self-position estimation method. An explanatory diagram showing positive/negative judgment based on cross product. 5 is a flowchart showing movement schedule information setting processing. The figure which shows the movement schedule information setting screen. FIG. 6 is a diagram showing a zigzag route set by movement schedule information setting processing.

Embodiment 1
As shown in FIG. 1, the movement control method for a moving object according to the first embodiment includes movement schedule information setting in which movement schedule information is set for a moving object 1 to be moved on a floor surface F in a building, for example, as a movement target surface. The mobile object 1 is moved based on the movement schedule information, and the actual movement information (movement This method includes a movement control process of controlling the movement of the mobile body 1 by comparing the sequential position information of the mobile body 1 with movement schedule information.
In this specification, front, back, top, bottom, left, and right will be defined as the directions shown in FIGS. 1 and 2.

In the movement schedule information setting process, for example, a zigzag route Z (see FIG. 7) in which multiple straight routes (lanes) L (L1, L2, L3...) are repeatedly turned back and forth on the floor F is set as the movement schedule information. The moving schedule information for moving the moving body 1 according to the following information is set.

Note that the zigzag route W preset based on the movement schedule information is, for example, a path starting from a movement start point S1 (S1x, S1y) of the moving body 1 on the floor F to an end point along the direction in which the moving body 1 moves forward. A first straight path L1 extending to G1 (G1x, G1y), and a second straight path L2 extending from G1 (G1x, G1y) of the first straight path L1 along a direction perpendicular to the direction in which the moving body 1 is advanced. A straight line route M1 extending to a starting point S2; a straight line route L2 extending from a starting point S2 of the second straight route L2 to an end point G2 of the second straight route L2 along the direction in which the moving body 1 is advanced; A one-cycle return route consists of a linear route M2 that extends from the end point G2 of the second straight route L2 to the starting point S3 of the third straight route L3 along a direction perpendicular to the direction in which the moving body 1 is advanced. A planned travel route that is constructed by repeating multiple cycles.

As shown in FIG. 1, a mobile body control system that realizes the movement control method for a mobile body includes a mobile body 1 configured to be movable on a floor surface F, and a mobile body 1 configured to be movable on a floor surface F. A computer 3, such as a personal computer, serves as a movement schedule information setting means for setting movement schedule information of the mobile body 1, and a TS2 that acquires actual movement information of the mobile body 1 and transmits it to the mobile body 1.

The moving body 1 includes a base body 10, a moving means 20 provided on the lower side of the base body 10, a target T2 such as a prism that serves as a collimation for the TS2 provided on the surface side of the base body 10, and a target T2 provided on the base body 10. The moving body 1 is provided with an elevating device 40 for elevating and lowering the front side of the movable body 1, and a control means 50.

As shown in FIG. 2, the moving means 20 includes, for example, left and right front wheels 21L, 21R provided at the front lower part of the base 10, and left and right rear wheels 22L, 22R provided at the rear lower part of the base 10. , motors 23L and 23R as drive sources for rear wheels 22L and 22R, and a drive control circuit (not shown).

Incidentally, each motor shaft of the motors 23L and 23R is provided with an encoder 25L as a movement amount detection means for detecting the movement distance (movement amount) of the moving body 1 based on the rotation amount of the rear wheels 22L and 22R. , 25R are installed.

The target T2 is composed of a reflecting prism or the like that reflects the light emitted from the TS2. For example, as shown in FIGS. 1 and 2, the target T2 is installed at the center position between the left and right sides on the front side of the upper surface of the base body 10.

For example, as shown in FIG. 3, the lifting device 40 is connected to a linear actuator 42 attached to a mounting portion 41 provided inside the front side of the base body 10, and a lower end (tip) of a rod 43 of the linear actuator 42. The reinforcing body 44 is configured to include a reinforcing body 44, a rolling element 45 provided at the lower end (tip) of the reinforcing body 44, and a guide portion 46 that guides the reinforcing body 44 in the vertical direction.
That is, the elevating device 40, which is provided on the front side of the base body 10 and is configured to be able to move up and down the front side of the moving body 1, is operated by the rod 43 of the linear actuator 42 and the reinforcing body 44 connected to the lower end (tip) of the rod 43. It is provided with an extensible means configured to expand and contract in the vertical direction, and a rolling element 45 provided at the lower end (tip) of the extensible means.
In other words, the elevating device 40 includes an extendable means configured to change the length from the mounting portion 41 to the rolling element 45 by extending and contracting the rod 43 of the linear actuator 42, and a lower end ( This configuration includes a rolling element 45 provided at the tip.
The guide portion 46 includes, for example, a guide wall 47 that serves as a passage for the reinforcing body 44 to move up and down, and a sealing member 48 that is provided on the outer peripheral surface of the reinforcing body 44 and slides on the guide wall 47.
The guide portion 46 functions as a guide for moving the expansion/contraction means in the vertical direction along the rotation center line 10C of the moving body 1, which will be described later, while maintaining parallelism with the rotation center line 10C.

The control means 50 controls the drive control circuits of the motors 23L, 23R of the rear wheels 22L, 22R, based on the movement schedule information, the actual movement information of the moving body 1 from the TS2, and the information from the encoders 25L, 25R. A movement control program that controls the movement of the moving body 1 by controlling a drive control circuit of a telescoping drive source (not shown) that extends and retracts the rod 43 of the linear actuator 42, and a computer etc. that realizes information processing by the movement control program. Consists of hardware resources.

When changing the moving direction (progressing direction) of the movable body 1, the control means 50 extends the rod 43 of the linear actuator 42 of the lifting device 40 from the retracted state to move the rolling body 45 as shown in FIG. 4(a). is pressed against the floor surface F and the front side of the base body 10 is moved upward to lift the front wheels 21L, 21R of the moving body 1 from the floor surface F, as shown in FIG. The motors 23L and 23R of the wheels 22L and 22R are controlled to rotate the left and right rear wheels 22L and 22R that are in contact with the floor surface F in opposite directions.
In this case, by rotating one rear wheel in the direction of rotation that moves the moving body 1 forward, and rotating the other rear wheel in the direction of rotation that moves the moving body 1 backward, the rotation center line 10C of the moving body 1 is Since the left and right rear wheels 22L, 22R and the rolling elements 45 roll on the floor surface F with the center of rotation at Rotates smoothly to the right. Therefore, the horizontal direction of the moving body 1 can be changed smoothly. The rotation center line 10C is a line that extends vertically and is parallel to the central axis of the rod 43, perpendicular to the middle position of the straight line connecting the centers of the left and right rear wheels 22L and 22R.
That is, in this case, as shown in FIG. 4(d), the center point 22C of the contact surface of the left rear wheel 22L with the floor surface F and the contact surface of the right rear wheel 22R with the floor surface F are The center point 22C and the center point 45C of the contact surface of the rolling element 45 with the floor surface F are configured to be located on one arc 10R having the rotation center line 10C of the moving body 1 as the rotation center. , the movable body 1 smoothly rotates to the left or right about the rotation center line 10C.
After the horizontal direction of the movable body 1 is changed, the control means 50 moves the base body 10 by retracting the rod 43 of the linear actuator 42 of the lifting device 40 from the extended state, as shown in FIG. 4(c). The moving direction of the moving body 1 can be changed by moving the front side of the moving body 1 downward and bringing the left and right front wheels 21L, 21R of the moving body 1 into contact with the floor surface F.
As described above, the moving direction of the moving body 1 can be changed, and then the moving body 1 can be moved in the changed moving direction.
That is, the moving direction changing means of the mobile body 1 is constituted by the lifting device 40 and the control means 50 that controls the rotation direction of the lifting device 40 and the left and right rear wheels 22L, 22R.

Hereinafter, a specific example of the movement control method for the mobile body 1 will be described based on FIGS. 5 to 19.
The movement control method for the moving body 1 is as follows: First, a movement schedule information setting process is performed to set movement schedule information on the floor F on which the moving body 1 is to be moved, and then the movement schedule set in the movement schedule information setting process is performed. The information is read into the control means 50 of the mobile body 1, and the control means 50 controls the movement of the mobile body 1 based on the read movement schedule information and the actual movement information of the mobile body 1 sequentially transmitted from the TS2. Perform processing.

The movement schedule information setting process will be explained based on FIG.
・Step S1
Using an input means such as a keyboard on the setting screen 31 (see FIG. 6) of the computer 3 as a movement schedule information setting means, for example, a zigzag route Z as a planned movement route is drawn on the floor F as shown in FIG. In order to set, The distance w between L and L and the number N of straight paths L are set.
Note that the XY coordinate values are the XY coordinate values seen from the TS2 with reference to the fixed position of the TS2 fixed at a predetermined position on the floor surface F, and the control means 50 of the moving body 1 The upper position is managed based on the XY coordinate values seen from the TS2.
That is, on the floor F, a target (not shown) is installed at a predetermined movement start point S1 of the moving body 1 and an end point G1 of the first straight path L1, and the XY coordinate values of the movement start point S1 are set. (S1x, S1y) and the XY coordinate values (G1x, G1y) of the end point G1 of the first straight path L1 are obtained using TS2 fixed at a predetermined position on the floor F, and by this TS2. The setter sets the acquired XY coordinate values via the setting screen 31.
The moving body 1 is installed on the floor F so that the target T2 is positioned directly above the movement start point S1, and the forward direction (movement direction) is toward the end point G1 of the first straight path L1. After that, the movement will be controlled.
・Step S2
Click the start scan button 32 on the settings screen 31.
・Step S3
Computer 3 writes the set movement schedule information into a file in the storage device of computer 3 - Step S4
After the computer 3 transmits movement schedule information to the control means 50 of the mobile body 1 and causes it to be read into the storage device of the control means 50, the computer 3 transmits a movement control program execution command to the control means 50 of the mobile body 1. .

The control means 50 of the mobile body 1 is based on the movement schedule information set by the computer 3 as a movement schedule information setting means, the actual movement information of the mobile body 1 from the TS 2, and the movement amount information from the encoders 25L and 25R. , performs a movement control process as shown in FIG. 8 to control the movement of the mobile body 1.
In the movement control process, normal travel process A, movement direction correction process B, return process C, and irregular occurrence process D are performed.

The normal driving process A (steps S10 to S18) will be explained with reference to FIGS. 9 and 10.
・Step S10
The control means 50 reads the movement schedule information set by the movement schedule information setting process from the file in the storage device, and starts the movement control process for the mobile body 1 based on the movement control program.
・Step S11
The control means 50 acquires the XY coordinate values (Pnx, Pny) of the latest position information Pn of the mobile body 1 from the TS2, and updates the position information of the mobile body 1.
That is, the control means 50 receives the latest position information Pn of the mobile body 1 transmitted from the TS 2 every 100 ms, for example, and performs the process of updating the position information of the mobile body 1.
・Step S12 (determining whether to proceed to processing D when irregularity occurs)
It is determined whether an error has occurred in updating the location information of the mobile object 1. That is, the control means 50 determines whether or not the latest position information Pn of the mobile object 1 has been acquired from the TS2. If it is determined that an error has occurred in updating the position information of the mobile body 1 (Yes in step S12), the process moves to irregular occurrence processing D.
・Step S13
If it is determined that no error has occurred in the location information update (No in step S12), that is, if the control means 50 is able to obtain the latest location information Pn of the mobile object 1 from the TS2, the control means 50 calculates a trajectory vector T, a straight path vector C, and a goal determination vector J of the moving object 1 based on the movement control program.
That is, as shown in FIG. 10, the control means 50 generates a vector that is a directed line segment from the starting point Sn of the current straight line route (lane) L to the end point Gn (Gnx, Gny) of the current straight line route L; That is, each time the linear path vector C is calculated and the latest position (current target (T2) coordinate) Pn of the moving body 1 is obtained, the target (T2) coordinate Pn- of the moving body 1 one phase before is calculated. 1 (Pn-1x, Pn-1y) to the latest position Pn of the moving body 1, that is, the trajectory vector T, is calculated, and further, the current straight line from the latest position Pn of the moving body 1 is calculated. A vector that is a directed line segment of the route L to the end point Gn, that is, a goal determination vector J is calculated.
・Step S14
Based on the movement control program, the control means 50 determines the rotation center coordinates Pr of the moving body 1, the goal determination angle θg, the deviation distance E from the straight path L (the amount of positional deviation between the movement schedule information and the movement information), and the required turning. Calculate the angle θr.
That is, as shown in FIG. 15, the rotation center coordinates Pr (Prx, Pry) of the moving body 1 are the position coordinates Pn (Pnx, Pny), the position coordinates Pn-1 (Pn-1x, Pn-1y), It is determined based on equations (1) and (2) using the trajectory vector T shown in equations (3) and (4).
Further, the goal determination angle θg and the required turning angle θr are determined by a method of determining the angle formed by two vectors a and b, as shown in FIG.
That is, the goal determination angle θg, which is the angle formed by the straight path vector C and the goal determination vector J, is determined based on equation (1) in FIG.
Also, a vector that is a directed line segment from the rotation center coordinate Pr of the moving body 1 to the end point Gn of the current straight line path, that is, a necessary turning angle that is the angle formed by the vector M for determining the necessary turning angle and the trajectory vector T. θr is determined based on equation (1) in FIG.
As shown in FIG. 17, the deviation distance E from the straight path L is determined based on the equation (1) using the straight path vector C shown by the equation (2) and the point Pn (Pnx, Pny). . That is, d=E in equation (1). However, a, b, and c in formula (1) are as shown in formula (3).
-Step S15 (determining whether to proceed to return process C)
It is determined whether the moving object 1 has reached the end point Gn of the current straight path L. That is, it is determined whether the movement control process for the mobile body 1 along the current straight path L has been completed.
That is, based on the movement control program, the control means 50 determines that the moving body 1 has reached the end point Gn of the current straight path L if the goal determination angle θg>90°, and the goal determination angle θg≦90°. If it is, it is determined that the moving object 1 has not reached the end point Gn yet. If it is determined that the moving body 1 has reached the end point Gn (Yes in step S15), the process moves to return processing C.
-Step S16 (determining whether to proceed to movement direction correction processing B)
If it is determined that the moving object 1 has not reached the end point Gn yet (No in step S15), the deviation distance E of the moving object 1 from the straight path L is a predetermined value (for example, the horizontal width dimension of the moving object 1). ) It is determined whether or not the moving direction of the moving object 1 is in a direction away from the current linear path vector C. If the deviation distance E is equal to or greater than a predetermined value and it is determined that the moving direction of the moving object 1 is directed away from the current straight path vector C (if Yes in step S16), the moving object 1 moves. The process moves to direction correction processing B.

The straight-line control process of the moving body 1 in the normal running process A will be explained.
・Step S17
If the deviation distance E is smaller than a predetermined value and it is determined that the moving direction of the moving body 1 is not directed away from the straight path vector C (No in step S16), the goal determination vector Based on the magnitude of J, drive signals to motors 23L and 23R that drive left and right rear wheels 22L and 22R are adjusted. For example, if the size of the goal determination vector J is 1 m or more, the output of the motors 23L and 23R is maximized, and if the size of the goal determination vector J is less than 1 m and 0.5 m or more, the output of the motors 23L and 23R is set to the maximum. , 23R are set to 70%, and if the size of the goal determination vector J is less than 0.5 m, the outputs of the motors 23L and 23R are adjusted to 50%.
・Step S18
The moving body 1 is moved forward by a certain distance while controlling drive signals to the left and right motors 23L and 23R so that the difference between the values of encoders 25L and 25R of the left and right rear wheels 22L and 22R becomes 0. That is, the moving body 1 is moved forward by a certain distance while maintaining straightness.
Note that in step 17 described above, an example is shown in which stepwise deceleration control is performed in which the moving speed of the moving object 1 is gradually decelerated each time the distance between the moving object 1 and the end point Gn of the straight path L reaches a predetermined value. However, continuous deceleration control may be performed to gradually reduce the moving speed of the moving body 1 as the distance between the moving body 1 and the end point Gn of the straight path L becomes shorter. Alternatively, the deceleration control may be performed only once, in which the speed is decelerated to a predetermined speed when the distance between the moving body 1 and the end point Gn of the straight path L becomes less than or equal to a predetermined value. That is, when the moving object 1 approaches the end point Gn of the straight path L, the moving speed of the moving object 1 may be reduced one or more times.

The movement direction correction process B (steps S20 to S22) will be described with reference to FIGS. 12 and 10.
・Step S20
If Yes in step S16, that is, the deviation distance E (positional deviation amount between movement schedule information and movement information) from the current straight path L is greater than or equal to a predetermined value (for example, the horizontal width dimension of the moving body 1), and If it is determined that the direction of the moving body 1 is in a direction away from the current linear path vector C, the rod 43 of the lifting device 40 is extended and the rolling element 45 presses the floor surface F, thereby moving the moving body 1. Raise the left and right front wheels 21L, 21R from the floor F.
・Step S21
The moving direction of the moving body 1 is corrected by rotating the left and right rear wheels 22L, 22R in opposite directions to rotate the direction of the moving body 1 by the required turning angle θr from the direction of the trajectory vector T (Fig. 10 reference).
・Step S22
After retracting the rod 43 of the lifting device 40 to lift the rolling elements 45 from the floor F and bringing the left and right front wheels 21L and 21R of the moving body 1 into contact with the floor F, the moving body 1 is moved. advance.

-Return processing C (steps S30 to S50) will be explained with reference to FIGS. 14 and 11.
・Step S30
If Yes in step S15, that is, if it is determined that the moving object 1 has reached the goal Gn (if it is determined that the movement control process of the moving object 1 along the current straight path L has been completed), the setting It is determined whether or not the movement control processing of the moving body 1 along the N straight-line routes L has been completed.
If No in step S30, that is, if it is determined that all movement control processing has not been completed, turnaround processing C is performed to move to movement control processing for the mobile body 1 along the next straight path L.
・Step S31
When moving the moving body 1 along the zigzag route Z, the moving body 1 is moved along the zigzag route Z in order to align the lines between adjacent turning points (the end point Gn-1 of the previous straight route and the starting point Sn of the next straight route L). From the time when it is determined that the goal is Gn, the moving object 1 is moved forward as it is to the direction change point Pnr (Pnrx, Pnry), which is a distance twice the total length of the moving object 1.
・Step S32
Since the position of the mobile body 1 has changed due to the forward movement in step S31, the position information of the mobile body 1 is updated from TS2 to the latest position information Pnr.
・Step S33
Determine whether an error has occurred when updating location information.
・Step S34
If no error occurs in updating the position information (No in step S33), the trajectory vector T is recalculated.
・Step S35
As shown in FIG. 11, the distance EX to the next straight route L and the turning correction angle θc are calculated.
As shown in FIG. 17, the distance EX to the next straight route L can be calculated using the next straight route vector C shown in formula (2) and point P (=point Pnr) using formula (1). Find based on. That is, d=EX in equation (1).
The turning correction angle θc, that is, the turning correction angle θc, which is the angle formed by the previous straight path vector C and the trajectory vector T, is determined based on equation (1) in FIG.
・Step S36
As shown in FIG. 18, the starting point Sn-1 (Sn-1x, Sn-1y) of the previous straight route (lane), the end point Gn-1 (Gn-1x, Gn-1y) of the previous straight route, the straight line Using the distance w between the routes, the starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are determined.
The starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are determined based on equation (1) or equation (2) in FIG. As shown in FIG. 18, it is assumed that each straight line path is inclined by θ from the X axis of the reference XY coordinates. If the next straight route is on the right side in the direction of travel of the previous straight route, the starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are , is determined based on equation (1) in FIG. Also, if the next straight route is on the left side in the direction of travel of the previous straight route, the starting point Sn (Snx, Sny) of the next straight route and the ending point Gn (Gnx, Gny) of the next straight route are , is determined based on equation (2) in FIG.
・Step S37
By extending the rod 43 of the lifting device 40 and pressing the floor surface F with the rolling elements 45, the left and right front wheels 21L, 21R of the moving body 1 are lifted off the floor surface F.

・Step S38
Conditional branching based on the combination of the patrol pattern and the most recent straight route (lane) number ・Condition 1: If it is a right circuit pattern and the number of the most recent straight route is an odd number, or if it is a left circuit pattern and the most recent straight route If the number is even.
・Condition 2: When the number of the right circular route and the nearest straight route is an even number, or when the left circular pattern and the number of the nearest straight route is an odd number.
Note that the right-hand patrol pattern refers to a pattern in which the mobile object 1 is moved along a zigzag route Z in which subsequent straight routes are set to the right of the first straight route when viewed from the first (initial) straight route. , the left circular pattern refers to a pattern in which the moving body 1 is moved along a zigzag route Z in which subsequent straight routes are set to the left of the first straight route when viewed from the first (initial) straight route.
This left and right patrol pattern can be determined by clicking a patrol pattern determination button (not shown) provided on the setting screen of FIG. The information is transmitted to the control means 50 of.

If the condition 1 is determined in step S38, the moving body 1 is rotated clockwise.
・Step S39
The trajectory of the moving body 1 is corrected by turning the moving body 1 clockwise by a turning correction angle θc+90°.
・Step S40
The rod 43 of the lifting device 40 is retracted to lift the rolling elements 45 off the floor F, and the left and right front wheels 21L, 21R of the moving body 1 are brought into contact with the floor F.
・Step S41
The moving body 1 is moved forward by the distance EX to the next straight path L.
・Step S42
The rod 43 of the lifting device 40 is retracted to lift the rolling element 45 from the floor surface F.
・Step S43
The trajectory of the moving body 1 is corrected by turning the moving body 1 clockwise by 90°.
・Step S44
The rod 43 of the lifting device 40 is retracted to lift the rolling element 45 off the floor F, the left and right front wheels 21L, 21R of the moving body 1 are brought into contact with the floor F, and the moving body 1 is moved to the next straight path L. move along.

In the case of condition 2 in the determination in step S38, processing for turning the moving body 1 counterclockwise is performed.
・Step S45
The trajectory of the moving body 1 is corrected by turning the moving body 1 counterclockwise by a turning correction angle θc+90°.
・Step S46
The rod 43 of the lifting device 40 is retracted to lift the rolling elements 45 off the floor F, and the left and right front wheels 21L, 21R of the moving body 1 are brought into contact with the floor F.
・Step S47
The moving body 1 is moved forward by the distance EX to the next straight path L.
・Step S48
The rod 43 of the lifting device 40 is retracted to lift the rolling element 45 from the floor surface F.
・Step S49
The trajectory of the moving body 1 is corrected by turning the moving body 1 by 90° counterclockwise.
・Step S50
The rod 43 of the lifting device 40 is retracted to lift the rolling element 45 off the floor F, the left and right front wheels 21L, 21R of the moving body 1 are brought into contact with the floor F, and the moving body 1 is moved to the next straight path L. move along.

With reference to FIG. 13, the irregular occurrence processing D (steps S60 to S65) will be described.
・Step S60
If Yes in step S12, that is, if it is determined that an error has occurred in updating the position information, the moving object is 1's self-position is estimated.
That is, as shown in FIG. 19, the position coordinates Pn-1 (Pn-1x, Pn-1y) immediately before losing the self-position, and the position coordinates Pn-2 (Pn-2x, Pn-2x) one phase before the position coordinate Pn-1. , Pn-2y), define the trajectory vector T, and use the values of the encoders 25L and 25R that increased from passing the previous position coordinate Pn-1 until losing track of the self-position to calculate the distance D traveled forward. The estimated self-position Pc (Pcx, Pcy) of the mobile object 1 is calculated based on equations (1) and (2).
・Step S61
It is determined whether the moving object 1 is currently estimated to be near the end point (goal) of the straight path L.
・Step S62
If it is not estimated that the moving object 1 is currently near the end point, it is moved forward a certain distance.
・Step S63
Currently, if the moving object 1 is estimated to be near the end point, it stops and waits.
・Step S64
It is determined whether the number of consecutive errors exceeds a predetermined value (for example, 10 times) or the distance traveled during the continuous errors exceeds a predetermined value (for example, 1 m). If the number of consecutive errors exceeds a predetermined value or the distance traveled during consecutive errors exceeds a predetermined value, the process ends. Further, if the number of consecutive errors does not exceed the predetermined value and the distance traveled during the continuous errors does not exceed the predetermined value, the process returns to step S11.
・Step S65
In step S33, after it is determined that an error has occurred, it is determined whether the number of consecutive errors has exceeded a predetermined value (for example, 10 times). If the number of consecutive errors exceeds a predetermined value, the process ends. If the number of consecutive errors does not exceed the predetermined value, the process returns to step S32.

According to the first embodiment, the zigzag route Z is the planned movement route set based on the movement plan information determined by the XY coordinate values on the floor F measured by TS2, that is, the movement start point S1 of the moving body 1. Based on the XY coordinate values (S1x, S1y), the XY coordinate values (G1x, G1y) of the end point G1 of the first straight path L, the distance W between adjacent straight paths L, and the number N of straight paths L. The moving object 1 can be moved so as to follow the set zigzag route Z, and the XY coordinate values on the floor surface F of the moving object 1 while moving on the floor surface F acquired by TS2 and the zigzag route Z By controlling the movement of the moving body 1 by comparing the XY coordinate values of the moving body 1 with the XY coordinate values of .

Furthermore, when it is determined that the moving object 1 has reached the end point of the straight path L, the moving direction changing means mounted on the moving object 1 performs a turning process C in which the moving direction of the moving object 1 is reversed. The moving body 1 can be moved along the zigzag route Z with high precision.

In addition, in the movement control process, when the moving object 1 approaches the end point Gn of the straight path L, the moving speed of the moving object 1 is controlled to be reduced one or more times. It becomes possible to improve the accuracy of the turning process C of the moving body 1 when moving the moving body 1.

In addition, in the movement control process, when the movement information of the mobile body 1 cannot be obtained from the TS2, the movement amount information of the mobile body 1 is determined based on the movement amount information obtained from the encoder as the movement amount detection means mounted on the mobile body 1. Since the position is estimated, even if movement information of the moving object 1 cannot be obtained from the TS 2, the moving object 1 can be accurately moved along the predetermined zigzag route Z based on the movement schedule information. I can do it.

In the movement schedule information setting process, in order to set the zigzag route Z on the floor F, the XY coordinate values of the movement start point S1 of the moving body 1, the XY coordinate values of the end point G1 of the first straight route L1, and By simply setting the number of straight routes L and the distance w between adjacent straight routes L, it becomes possible to easily set the zigzag route Z along which the mobile object 1 is scheduled to move.

The moving body 1 also operates on the floor F with the control means 50 extending the telescopic means of the elevating device 40 to press the rolling elements 45 against the floor F and lifting the front wheels 21L and 21R from the floor F. The horizontal direction is changed by rotating the rear wheels 22L and 22R that are in contact with each other in opposite directions, so when changing the direction of movement, the horizontal direction can be changed smoothly. Can be changed.
That is, with the base body 10 floating above the floor F and the rolling elements 45 in contact with the floor F, the left and right rear wheels 22L, 22R are rotated in opposite directions to rotate around the rotation center line 10C. Since the left and right rear wheels 22L, 22R and the rolling element 45 roll on one circular arc 10R on the floor F, the moving body 1 rotates clockwise or counterclockwise around the rotation center line 10C. The horizontal direction of the moving body 1 can be changed smoothly.
According to the movable body 1, even when the movable body 1 is heavy, the horizontal orientation of the movable body 1 can be changed smoothly, so this configuration is suitable for a heavy movable body.
Further, since the elevating device 40 is provided with a guide portion 46 for moving the extensible means of the elevating device 40 in the vertical direction along the rotational center line 10C while maintaining parallel to the rotational center line 10C of the movable body 1, the elevating device 40 Since the expansion/contraction means can move vertically accurately to accurately form the rotation center line 10C, the horizontal direction of the movable body 1 can be changed smoothly.

Note that the rolling element 45 is, for example, a wheel configured to be able to roll on one circular arc 10R with the rotation center line 10C of the moving body 1 as the rotation center, as shown in FIG. 4(d); Alternatively, a rolling element such as a ball may be used.

Embodiment 2
The movement schedule information setting process in the movement control method for a moving body according to the second embodiment will be described based on FIGS. 21 to 23 and FIG. 20.
As shown in FIG. 23, in the movement schedule information setting process according to the second embodiment, in order to set a zigzag route Z as a planned movement route on the floor F, the XY coordinate values of the movement start point S1 of the moving body 1 are (S1x, S1y), the XY coordinate values (G1x, G1y) of the end point G1 of the first straight path L1, and the diagonal measurement point measured as the diagonal point of the movement start point S1 on the XY coordinates of the floor F. The XY coordinate values (Rdx, Rdy) of Rd are acquired using TS2 fixed at a predetermined position on the floor surface F, and these acquired XY coordinate values are combined with the adjacent linear paths (lanes) L, L. By setting the initial value w' of the distance w between the two lines by the setting person via the setting screen 31, for example, the computer 3 as the movement schedule information setting means can select the optimum straight line based on the movement schedule information setting program. The number N of routes L is calculated, and a process of determining a circulation pattern is performed.

・Movement schedule information setting process ・Step S100
Using an input means such as a keyboard on the setting screen 31 of the computer 3 (see FIG. 22), in order to set the zigzag route Z on the floor F as shown in FIG. (S1x, S1y), the end point G1 (G1x, G1y) of the first straight path L1, the diagonal measurement point Rd (Rdx, Rdy), and the initial value w' of the distance w between adjacent straight paths. do.
That is, on the floor F, targets are installed at the movement start point S of the moving body 1, the end point G of the first straight path L1, and the diagonal measurement point Rd, and the movement start point S (S1x, S1y) is set. , the end point G1 (G1x, G1y) of the first straight path L1, and the diagonal measurement point Rd (Rdx, Rdy) are obtained using TS2 fixed at a predetermined position on the floor F. The setter sets each of the XY coordinate values and the initial value w' of the distance w between adjacent linear routes via the setting screen 31.
・Step S101
Click the automatic calculation button 33 (see FIG. 22) on the setting screen 31.
・Step S102
The computer 3 calculates θ for correcting the deviation t of the diagonal measurement point Rd based on the movement schedule information setting program. That is, as shown in FIG. 23, a vector that is a directed line segment from the end point G1 to the diagonal measurement point Rd and a vector from the end point G1 to the diagonal setting point R are calculated, and the angle θ between these vectors is calculated. is determined based on equation (1) in FIG.
・Step S103
It is determined whether θ is large enough to suspect an error in input data or an error in setting the diagonal measurement points.
・Step S104
If it is determined in step S103 that θ is not so large that an input data error or a setting error in the diagonal measurement point Rd is suspected, the area width Lw is calculated based on the calculation formula in step S104. calculate. Note that, as shown in FIG. 23, the area width Lw is the width in the direction in which the plurality of straight paths L, L, . . . in the zigzag path Z are lined up.
・Step S105
In step S103, if it is determined that θ is so large that an error in input data or an error in setting the diagonal measurement point Rd (measurement point error) is suspected, a parameter abnormality is reported and the process ends. do.
・Step S106
Using the area width Lw calculated in step S104 and the initial value w' of the distance w between straight paths, the number of blocks B', which is the initial value of the number of blocks, is calculated based on the calculation formula in step S105. (Also calculate the remainder r). Note that the number of blocks refers to the number of rectangular areas b defined within the area width Lw and surrounded by the adjacent straight-line paths L, L and the straight-line path M, as shown in FIG.
・Step S107
Determine whether the remainder is 0 or not.
・Step S108
In step S107, if the remainder is 0, the initial value w' is determined to be the distance w between the straight routes (lanes) (w=w').
・Step S109
Add 1 to the number of blocks B' to determine the number N of straight routes (lanes) (N=B'+1). In other words, the optimal number N of straight-line routes L is determined.
・Step S110
In step S107, if the remainder is not 0, the number of blocks B' is incremented by 1 to set the number of blocks B (B=B'+1).
・Step S1101
Recalculate the distance w between straight paths (lanes) (w=Lw/B). If there is a remainder in the recalculation, it is rounded down to four significant digits, for example.
・Step S1102
Add 1 to the number of blocks B to determine the number N of straight routes (lanes) (N=B+1). In other words, the optimal number N of straight-line routes L is determined.
Note that, as is clear from FIG. 23, the number N of straight routes (lanes) is always the number of blocks +1.
・Step S111
Next, a vector that is a directed line segment from the movement start point S1 to the end point G1 of the moving body 1 and a vector that is a directed line segment from the movement start point S1 to the diagonal setting point R are calculated, and these vectors are calculated. Find the cross product of vectors.
・Step S112
It is determined whether the Z-axis component (vertical component) of the cross product calculated in step S111 is positive or negative.
・Step S113
If the Z-axis component of the cross product calculated in step S111 is positive, as shown in equation (1) in FIG. 20, the zigzag path Z of the left cyclic pattern is determined.
・Step S114
If the Z-axis component of the cross product calculated in step S111 is negative, as shown in equation (2) in FIG. 20, the zigzag path Z of the right cyclic pattern is determined.
・Step S115
If the Z-axis component of the cross product calculated in step S111 is 0, as shown in equation (3) in FIG. 20, a parameter abnormality is reported and the process ends.
・Step S116
Each determined value is set as movement schedule information.
・Step S117
Click the scan start button 32 (see FIG. 22) on the setting screen 31.
・Step S118
The computer 3 writes the set movement schedule information into a file in the storage device of the computer 3.
・Step S119
After the computer 3 transmits movement schedule information to the control means 50 of the mobile body 1 and causes it to be read into the storage device of the control means 50, the computer 3 transmits a movement control program execution command to the control means 50 of the mobile body 1. .

According to the movement schedule information setting process according to the second embodiment, the movement start point S (S1x, S1y), the end point G1 (G1x, G1y) of the first straight route L1, and the diagonal measurement point Rd (Rdx, Rdy) By setting the initial value w' of the distance w between adjacent straight routes as the movement schedule information, the movement schedule information setting program calculates the area width Lw, and then calculates the area width Lw and the initial value w'. Based on this, the optimal number of straight-line routes L is calculated, and the process of determining the tour pattern is also performed, making the travel schedule information setting process easier.
Note that the cyclic pattern calculated in steps S111 to S115 can also be determined by clicking the cyclic pattern determination button (not shown) provided on the setting screen of FIG. 6 described in the first embodiment. According to the second embodiment, even if the user forgets to click the tour pattern determination button, or even if the tour pattern determination button is not provided, the travel schedule information setting process of steps S111 to S115 determines the travel pattern. Since this is automatically determined, it becomes possible to reduce the work involved in the movement schedule information setting process.

In addition, in each embodiment, although the moving body of the structure which used the rear side wheel as the driving wheel was illustrated, you may use the moving body of the structure which used the front side wheel as the driving wheel. In this case, the control means sets the moving body so that the rod of the lifting device is extended to press the rolling elements against the floor and the rear wheels of the moving body are lifted off the floor. The configuration may be such that the direction of the moving body is changed by rotating the left and right wheels of the front wheels that are in contact with each other in opposite directions.
That is, in this case, the movable body is configured such that the lifting device is provided on the rear side of the base body so as to be able to raise and lower the rear side of the movable body, and the center point of the contact surface of the left front wheel with the surface to be moved, The center point of the contact surface of the right front wheel with the moving target surface and the center point of the contact surface of the rolling element with the moving target surface are the center of rotation of the moving body when changing the horizontal direction of the moving body It is configured to be located on one circular arc centered on the line.

Furthermore, in each embodiment, the zigzag route Z is set as the planned movement route, but the planned movement route may be any route set based on the movement schedule information determined by the XY coordinate values on the floor measured by the TS. , any route is acceptable.

Furthermore, in each of the embodiments, an example is shown in which a floor surface inside a building is moved as the surface to be moved, but the surface to be moved may be a surface such as a road or a vacant lot outside the building.

Furthermore, in each of the embodiments, an example has been shown in which a TS (total station) is used as a movement information acquisition means for acquiring actual movement information of a moving object and transmitting it to the moving object. For example, GPS may be used.
That is, in this case, the planned movement route is set based on the movement plan information determined by the XY coordinate values on the movement target plane by GPS, and the movement target plane of the moving object that is moving on the movement target plane acquired by GPS is set. By controlling the movement of the moving object by comparing the XY coordinate values of the object and the XY coordinate values of the planned movement route, the moving object can be moved so as to accurately follow the planned movement route.

1 mobile object, 2 total station (movement information acquisition means),
3 computer (movement schedule information setting means), 10 base, 20 movement means,
21L, 21R Left and right front wheels, 22L, 22R Left and right rear wheels,
23L, 23R motor, 25L, 25R encoder (movement amount detection means),
40 lifting device, 43 rod, 45 rolling element, 50 control means,
F floor surface (movement target surface), L straight path (lane), Z zigzag path (planned movement path).

Claims (1)

a movement schedule information setting process for setting movement schedule information of a moving object to be moved on a movement target surface;
a movement control process of moving the mobile body based on the movement schedule information and controlling the movement of the mobile body by comparing the actual movement information of the mobile body acquired by the movement information acquisition means with the movement schedule information;
Equipped with
The movement schedule information setting process is a movement control method for a movable body that sets movement schedule information for moving the movable body along a zigzag path in which a straight path is repeatedly turned back and forth on a movement target surface, the method comprising:
In the movement schedule information setting process, in order to set a zigzag route on the movement target surface, By setting the XY coordinate values of the diagonal measurement point measured as the diagonal point of the movement start point above and the initial value of the distance between adjacent straight routes, you can determine the direction in which multiple straight routes in the zigzag route are lined up. 1. A method for controlling the movement of a moving object, comprising calculating and setting an optimal value for the number of straight paths based on the width of the straight path and an initial value of the distance between the straight paths.

Images