JP2020154400A - Movement control method of moving body and movement control system of moving body - Google Patents

Abstract

To provide a movement control method of a moving body for allowing the moving body to accurately move along a previously determined moving scheduled route.SOLUTION: The movement control method of the moving body includes a moving schedule information setting process for setting the moving schedule information for moving the moving body 1 on a moving object surface (floor F), and a moving control process for allowing the moving body 1 to move according to the moving schedule information, and controlling the movement of the moving body by comparing actual moving information of the moving body 1 acquired by moving information acquisition means (TS2) and the moving schedule information.SELECTED DRAWING: Figure 1

Description

The present invention relates to a moving body movement control method for moving the moving body based on the moving schedule information set in the moving body.

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. A method for controlling the movement of a moving body that controls the running of a body (autonomous traveling device) is known (see Patent Document 1).

Japanese Unexamined Patent Publication No. 2009-265801

According to the movement control method of the moving body described above, by mounting a large number of sensors and a control program based on information from the large number of sensors on the moving body, the moving body can be moved while maintaining a distance from an obstacle. Although it can be moved, there is a problem that the moving body cannot be moved along a predetermined movement schedule route.
The present invention provides a movement control method for a moving body that can accurately move the moving body along a predetermined movement scheduled route.

The movement control method of the moving body according to the present invention includes a moving schedule information setting process for setting the moving schedule information of the moving body to be moved on the moving target surface, moving the moving body based on the moving schedule information, and moving information. It is characterized by including a movement control process for controlling the movement of the moving body by comparing the actual moving information of the moving body acquired by the acquisition means with the moving schedule information.
Further, in the movement control process, when the amount of misalignment between the movement schedule information and the movement information exceeds a predetermined value, the movement direction of the moving body is changed by the moving direction changing means mounted on the moving body. It is a feature.
According to the above-mentioned movement control method of the moving body, the moving body can be moved accurately along a predetermined movement schedule route based on the movement schedule information.
The movement schedule information setting process is characterized in that movement schedule information for moving a moving body is set by following a zigzag path in which a straight line path is repeatedly folded back on a movement target surface.
Further, the movement control process is characterized in that when it is determined that the moving body has reached the end point of the linear path, the moving direction of the moving body is reversed by the moving direction changing means mounted on the moving body.
According to the above-mentioned movement control method of the moving body, the moving body can be moved accurately along a predetermined zigzag path based on the movement schedule information.
Further, the movement control process is characterized in that when the moving body approaches the end point of the linear path, the moving speed of the moving body is decelerated one or more times.
According to the above-mentioned movement control method of the moving body, the moving body is decelerated when the moving body approaches the end point of the linear path. Therefore, the accuracy of the folding process of the moving body when the moving body is moved along the zigzag path. Can be improved.
Further, in the movement control process, when the movement information cannot be obtained from the movement information acquisition means, the position of the moving body is estimated based on the movement amount information obtained from the movement amount detecting means mounted on the moving body. As a feature, even if the movement information cannot be obtained from the movement information acquisition means, the moving body can be accurately moved along the predetermined movement schedule route based on the movement schedule information.
Further, in the movement schedule information setting process, in order to set the zigzag path on the movement target surface, the XY coordinate value of the movement start point of the moving body, the XY coordinate value of the end point of the first straight path, and the number of straight paths. And, since the distance between adjacent straight paths is set, it becomes possible to easily set the zigzag path in which the moving body is to be moved.
Further, in the movement schedule information setting process, in order to set the zigzag path on the movement target surface, the XY coordinate value of the movement start point of the moving body, the XY coordinate value of the end point of the first straight line path, and the movement target surface. By setting the XY coordinate value of the diagonal measurement point measured as the diagonal point of the movement start point on the XY coordinates and the initial value of the distance between adjacent straight paths, a plurality of linear paths in the zigzag path can be created. Since the optimum value for the number of straight paths is calculated and set based on the width in the line-up direction and the initial value of the distance between the straight paths, it is easier to set the zigzag path in which the moving body is to be moved. You will be able to set it.
According to the movement control system of the moving body according to the present invention, the moving body configured to be movable on the moving target surface and the moving schedule for setting the moving schedule information on the moving target surface to move the moving body. The information setting means and the movement information acquisition means for acquiring the actual movement information of the moving body and transmitting it to the moving body are provided, and the moving body is provided on the base, the moving means provided on the base, and the base. A lifting device for raising and lowering the front side or the rear side of the moving body and a control means are provided, and the control means moves the moving body by controlling the moving means and the lifting device based on the movement schedule information and the movement information. Since it is characterized by controlling, it is possible to provide a movement control system capable of accurately moving a moving body along a predetermined movement schedule route based on the movement schedule information.
Further, the moving means includes a front wheel, a rear wheel, and a driving means of at least one of the front wheel and the rear wheel, and the elevating device includes a telescopic means that expands and contracts in the vertical direction and a tip of the telescopic means. It is provided with a rolling element provided on the side so as to be able to roll on the moving target surface, and the control means extends the telescopic means of the elevating device to press the rolling body against the moving target surface, and also front wheels and the rear side. With one of the wheels floating from the moving target surface, the other left and right wheels in contact with the moving target surface are rotated in opposite directions to change the horizontal orientation of the moving body. Therefore, it is possible to provide a movement control system capable of accurately performing the movement direction change processing of the moving body.
Further, the movement amount detecting means for detecting the movement amount of the moving body based on the rotation amount of the wheel is provided, and the control means can be obtained from the movement amount detecting means when the movement information cannot be obtained from the movement information acquisition means. Since the position of the moving body is estimated based on the movement amount information, even if the movement information cannot be obtained from the movement information acquisition means, the movement schedule route is set in advance based on the movement schedule information. It is possible to provide a movement control system capable of moving a moving body with high accuracy.

The figure which shows the movement control system of a moving body. The perspective view which shows the moving body. It is a figure which shows the detail of a lifting device, (a) is a front view, (b) is a side view. The operation procedure diagram which shows the movement direction change operation procedure of the moving body by an elevating device. A flowchart showing the movement schedule information setting process. The figure which shows the movement schedule information setting screen. The figure which shows the zigzag route set by the movement schedule information setting process. A flowchart showing the entire movement control process. The flowchart which shows the normal running process in the movement control process. The figure which shows the outline of the normal traveling process and the moving direction correction process in the movement control process of a moving body. The figure which shows the outline of the folding process in the movement control process of a moving body. The flowchart which shows the moving direction correction processing in the movement control processing. The flowchart which shows the irregular occurrence processing in the movement control processing. The flowchart which shows the return process in the movement control process. Explanatory drawing which shows the rotation center calculation method. Explanatory drawing which shows the angle calculation method. Explanatory drawing which shows the method of calculating the distance between a vector and a point. Explanatory drawing which shows the straight line route (lane) update method. Explanatory drawing which shows the self-position estimation method. Explanatory diagram showing positive / negative judgment by cross product. A flowchart showing the movement schedule information setting process. The figure which shows the movement schedule information setting screen. The figure which shows the zigzag route set by the movement schedule information setting process.

Embodiment 1
As shown in FIG. 1, the movement control method of the moving body according to the first embodiment is to set the moving schedule information on the moving body 1 that moves on the floor surface F in the building, for example, as the moving target surface. The actual movement information (movement) of the moving body 1 acquired by the automatic tracking type total station (hereinafter referred to as TS) 2 as the moving information acquisition means while moving the moving body 1 based on the processing and the moving schedule information. This is a method including a movement control process for controlling the movement of the moving body 1 by comparing the moving schedule information with the sequential position information of the body).
In this specification, the front, rear, top, bottom, left, and right are defined as the directions shown in FIGS. 1 and 2.

In the movement schedule information setting process, as the movement schedule information, for example, a zigzag path Z (see FIG. 7) in which a plurality of linear paths (lanes) L (L1, L2, L3 ...) are repeatedly folded back on the floor surface F is used. Set the movement schedule information for moving the moving body 1 by tracing.

The zigzag path W preset based on the movement schedule information is, for example, an end point along the direction in which the moving body 1 is advanced from the moving start point S1 (S1x, S1y) of the moving body 1 on the floor surface F. The first linear path L1 extending to G1 (G1x, G1y) and the second linear path L2 along the direction orthogonal to the direction in which the moving body 1 is advanced from G1 (G1x, G1y) of the first linear path L1. An inter-straight line path M1 extending to the start point S2, a straight line path L2 extending from the start point S2 of the second straight line path L2 to the end point G2 of the second straight line path L2 along the direction in which the moving body 1 is advanced. A one-cycle turnaround path consisting of an inter-straight line path M2 extending from the end point G2 of the second straight line path L2 to the start point S3 of the third straight line path L3 along a direction orthogonal to the direction in which the moving body 1 is advanced. It is a planned movement route that is constructed by repeating multiple cycles.

As shown in FIG. 1, the moving body control system that realizes the moving control method of the moving body is a moving body 1 configured to be movable on the floor surface F and a moving body F on which the moving body 1 is moved. It is provided with a computer 3 such as a personal computer as a movement schedule information setting means for setting the movement schedule information, and a TS2 that acquires the actual movement information of the moving body 1 and transmits it to the moving body 1.

The moving body 1 is provided on the base 10, a moving means 20 provided on the lower side of the base 10, a target T2 such as a prism for collimating the TS2 provided on the surface side of the base 10, and the base 10. A lifting device 40 for raising and lowering the front side of the moving body 1 and a control means 50 are provided.

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

The motor shafts of the motors 23L and 23R have an encoder 25L as a movement amount detecting 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, respectively. , 25R is installed.

The target T2 is composed of a reflection prism or the like that reflects the light emitted from the TS2. As shown in FIGS. 1 and 2, for example, 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 substrate 10.

As shown in FIG. 3, for example, the elevating device 40 is connected to a linear actuator 42 attached to an attachment portion 41 provided inside the front side of the base 10 and a lower end (tip) of a rod 43 of the linear actuator 42. The reinforcing body 44 is provided with a rolling body 45 provided at the lower end (tip) of the reinforcing body 44, and a guide portion 46 for guiding the reinforcing body 44 in the vertical direction.
That is, the elevating device 40 provided on the front side of the substrate 10 and configured to be able to elevate and elevate the front side of the moving body 1 is formed 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 includes an expansion / contraction means that is configured to expand and contract in the vertical direction, and a rolling element 45 provided at the lower end (tip) of the expansion / contraction means.
In other words, the elevating device 40 has a telescopic means configured to change the length from the mounting portion 41 to the rolling element 45 by expanding and contracting the rod 43 of the linear actuator 42, and the lower end of the telescopic means ( It is configured to include a rolling element 45 provided at the tip).
The guide portion 46 is composed of, for example, a guide wall 47 that serves as a passage for the reinforcing body 44 to move up and down, and a seal 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 telescopic means in the vertical direction along the rotation center line 10C while maintaining parallelism with the rotation center line 10C of the moving body 1, which will be described later.

The control means 50 is 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 and 25R, and the drive control circuits of the motors 23L and 23R of the rear wheels 22L and 22R, and A movement control program that controls the movement of the moving body 1 by controlling a drive control circuit of an expansion / contraction drive source (not shown) that expands / contracts the rod 43 of the linear actuator 42, and a computer or the like that realizes information processing by the movement control program. Consists of hardware resources.

When changing the moving direction (traveling direction) of the moving body 1, the control means 50 extends the rod 43 of the linear actuator 42 of the elevating device 40 from the retracted state as shown in FIG. 4A, and the rolling body 45 Is pressed against the floor surface F and the front side of the base 10 is moved upward to lift the front wheels 21L and 21R of the moving body 1 from the floor surface F, as shown in FIG. 4B, on the left and right rear sides. The motors 23L and 23R of the wheels 22L and 22R are controlled to rotate the left and right rear wheels 22L and 22R in contact with the floor surface F in opposite directions.
In this case, by rotating one rear wheel in the direction of rotation for moving the moving body 1 forward and rotating the other rear wheel in the direction of rotation for moving the moving body 1 backward, the rotation center line 10C of the moving body 1 Since the left and right rear wheels 22L and 22R and the rolling element 45 roll on the floor surface F with the rotation center, the moving body 1 moves to the left or left with the rotation center line 10C as the rotation center on the floor surface F. It rotates smoothly to the right. Therefore, the horizontal orientation of the moving body 1 can be smoothly changed. The rotation center line 10C is a line extending in the vertical direction orthogonal to the intermediate position of the straight line connecting the centers of the left and right rear wheels 22L and 22R and parallel to the central axis of the rod 43.
That is, in this case, as shown in FIG. 4D, 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 Since the center point 22C and the center point 45C of the contact surface of the rolling element 45 with the floor surface F are located on one arc 10R centered on the rotation center line 10C of the moving body 1. , The moving body 1 smoothly rotates to the left or right with the rotation center line 10C as the center of rotation.
After the horizontal orientation of the moving body 1 is changed, the control means 50 retracts the rod 43 of the linear actuator 42 of the elevating device 40 from the extended state as shown in FIG. 4C, thereby causing the base 10 to be retracted. 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 and 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 elevating device 40, the elevating device 40, and the control means 50 for controlling the rotation directions of the left and right rear wheels 22L and 22R constitute a moving direction changing means for the moving body 1.

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

The movement schedule information setting process will be described with reference to FIG.
・ Step S1
For example, using an input means such as a keyboard on the setting screen 31 (see FIG. 6) of the computer 3 as the movement schedule information setting means, the zigzag path Z as the movement schedule route Z as the movement schedule information is set on the floor surface F as shown in FIG. In order to set, the XY coordinate values (S1x, S1y) of the movement start point S1 of the moving body 1 and the XY coordinate values (G1x, G1y) of the end point G1 of the first linear path (lane) L1 are adjacent to each other. The distance w between L and L and the number N of the linear paths L are set.
The XY coordinate values are XY coordinate values viewed 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 is the floor surface F. The upper position is managed based on the XY coordinate values viewed from the TS2.
That is, on the floor surface 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 line path L1, and the XY coordinate value of the movement start point S1 is set. (S1x, S1y) and the XY coordinate values (G1x, G1y) of the end point G1 of the first linear path L1 are acquired by using TS2 fixed at a predetermined position on the floor surface F, and the TS2 is used. The setter sets the acquired XY coordinate values via the setting screen 31.
Then, the moving body 1 is installed on the floor surface F so that the target T2 is positioned directly above the movement start point S1 and the forward direction (movement direction) is the direction toward the end point G1 of the first straight path L1. After that, the movement will be controlled.
・ Step S2
Click the scan start button 32 on the setting screen 31.
・ Step S3
The movement schedule information set by the computer 3 is converted into a file and written to the storage device of the computer 3. ・ Step S4
After the computer 3 transmits the movement schedule information to the control means 50 of the mobile body 1 and reads it 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 moving body 1 is based on the moving schedule information set by the computer 3 as the moving schedule information setting means, the actual moving information of the moving body 1 from the TS2, and the moving amount information from the encoders 25L and 25R. , The movement control process as shown in FIG. 8 for controlling the movement of the moving body 1 is performed.
In the movement control process, the normal traveling process A, the moving direction correction process B, the turn-back process C, and the irregular occurrence process D are performed.

The normal traveling process A (steps S10 to S18) will be described 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 of the storage device, and starts the movement control process of the moving 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 moving body 1 from TS2, and updates the position information of the moving body 1.
That is, the control means 50 receives the latest position information Pn of the moving body 1 transmitted from the TS2, for example, every 100 ms, and performs the position information updating process of the moving body 1.
-Step S12 (determination of whether or not to shift to processing D when irregularity occurs)
It is determined whether or not an error has occurred in updating the position information of the moving body 1. That is, it is determined whether or not the control means 50 has been able to acquire the latest position information Pn of the moving body 1 from the TS2. When it is determined that an error has occurred in updating the position information of the moving body 1 (yes in step S12), the process proceeds to the irregular occurrence processing D.
・ Step S13
When it is determined that no error has occurred in the position information update (No in step S12), that is, when the control means 50 can acquire the latest position information Pn of the moving body 1 from the TS2, the control means 50 calculates the locus vector T, the linear path vector C, and the goal determination vector J of the moving body 1 based on the movement control program.
That is, as shown in FIG. 10, the control means 50 is a vector that is a directed line segment from the start point Sn of the straight line path (lane) L this time to the end point Gn (Gnx, Gny) of the straight line path L this time. That is, every time the linear path vector C is calculated and the latest position (current target (T2) coordinates) Pn of the moving body 1 is acquired, the target (T2) coordinates Pn− one phase before the moving body 1 is acquired. A vector that is a directed line segment from 1 (Pn-1x, Pn-1y) to the latest position Pn of the moving body 1, that is, a locus vector T is calculated, and further, a straight line this time from the latest position Pn of the moving body 1. A vector that is a directed line segment to the end point Gn of the path L, that is, a goal determination vector J is calculated.
-Step S14
Based on the movement control program, the control means 50 has the rotation center coordinates Pr of the moving body 1, the goal determination angle θg, the deviation distance E from the linear path L (the amount of displacement 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), and It is obtained based on the equations (1) and (2) by using the trajectory vector T represented by the equations (3) and (4).
Further, the goal determination angle θg and the required turning angle θr are obtained by a method of obtaining the angle formed by the two vectors a and b, as shown in FIG.
That is, the goal determination angle θg, which is the angle formed by the linear path vector C and the goal determination vector J, is obtained based on the equation (1) of FIG.
Further, 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 linear path this time, that is, a required turning angle that is an angle formed by the required turning angle determination vector M and the locus vector T. θr is obtained based on the equation (1) of FIG.
As shown in FIG. 17, the deviation distance E from the linear path L is obtained based on the equation (1) using the linear path vector C represented by the equation (2) and the points Pn (Pnx, Pny). .. That is, d = E in the equation (1). However, a, b, and c of the formula (1) are as shown in the formula (3).
-Step S15 (determination of whether or not to shift to return processing C)
It is determined whether or not the moving body 1 has reached the end point Gn of the linear path L this time. That is, it is determined whether or not the movement control process of the moving body 1 along the linear path L this time is 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 linear path L this time if the goal determination angle θg> 90 °, and the goal determination angle θg ≦ 90. If °, it is determined that the moving body 1 has not yet reached the goal at the end point Gn. When it is determined that the moving body 1 has reached the end point Gn (Yes in step S15), the process proceeds to the return process C.
-Step S16 (Determination of whether or not to shift to the moving direction correction process B)
When it is determined that the moving body 1 has not yet reached the goal at the end point Gn (No in step S15), the deviation distance E of the moving body 1 from the linear path L is a predetermined value (for example, the left-right width dimension of the moving body 1). ) The above, and it is determined whether or not the direction of the moving body 1 in the moving direction is in the direction away from the linear path vector C this time. Then, when it is determined that the deviation distance E is equal to or greater than a predetermined value and the direction of movement of the moving body 1 is in the direction away from the linear path vector C this time (Yes in step S16), the movement is performed. The process shifts to the direction correction process B.

The straight-ahead control process of the moving body 1 in the normal traveling process A will be described.
・ Step S17
When it is determined that the deviation distance E is smaller than the predetermined value and the direction of movement of the moving body 1 is not in the direction away from the linear path vector C (No in step S16), the goal determination vector. The drive signals to the motors 23L and 23R that drive the left and right rear wheels 22L and 22R are adjusted based on the size of J. 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 motor 23L If the output of the goal determination vector J is less than 0.5 m, the output of the motors 23L and 23R is adjusted to 50%.
・ Step S18
The moving body 1 is advanced by a certain distance while controlling the drive signals to the left and right motors 23L and 23R so that the difference between the values of the encoders 25L and 25R of the left and right rear wheels 22L and 22R becomes 0. That is, the moving body 1 is advanced by a certain distance while maintaining the straightness.
In step 17 described above, an example is shown in which stepwise deceleration control is performed in which the moving speed of the moving body 1 is gradually reduced each time the distance between the moving body 1 and the end point Gn of the linear path L reaches a predetermined value. However, continuous deceleration control may be performed in which the moving speed of the moving body 1 is gradually reduced as the distance between the moving body 1 and the end point Gn of the linear path L becomes shorter. Further, when the distance between the moving body 1 and the end point Gn of the linear path L becomes equal to or less than a predetermined value, the deceleration control may be performed only once to decelerate to a predetermined speed. That is, when the moving body 1 approaches the end point Gn of the linear path L, the moving speed of the moving body 1 may be reduced by one or more times.

The moving direction correction process B (steps S20 to S22) will be described with reference to FIGS. 12 and 10.
-Step S20
In the case of Yes in step S16, that is, the deviation distance E (positional deviation amount between the movement schedule information and the movement information) from the linear path L this time is equal to or more than a predetermined value (for example, the lateral width dimension of the moving body 1) and When it is determined that the direction of the moving body 1 is in the direction away from the linear path vector C this time, the moving body 1 is formed by extending the rod 43 of the elevating device 40 and pressing the floor surface F with the rolling body 45. The left and right front wheels 21L and 21R are floated from the floor surface F.
-Step S21
The moving direction of the moving body 1 is corrected by rotating the left and right rear wheels 22L and 22R in opposite directions to rotate the direction of the moving body 1 from the direction of the locus vector T by the required turning angle θr (FIG. 10). reference).
-Step S22
The rod 43 of the elevating device 40 is retracted to lift the rolling element 45 from the floor surface F, and the left and right front wheels 21L and 21R of the moving body 1 are brought into contact with the floor surface F (contact surface), and then the moving body 1 is moved. Move forward.

A folding process C (steps S30 to S50) will be described with reference to FIGS. 14 and 11.
-Step S30
In the case of Yes in step S15, that is, when it is determined that the moving body 1 has reached the end point Gn (when it is determined that the movement control processing of the moving body 1 along the linear path L this time is completed), the setting is made. It is determined whether or not all the movement control processing of the moving body 1 along the linear path L of the number N has been completed.
If No in step S30, that is, if it is determined that all the movement control processes have not been completed, the turnaround process C for shifting to the movement control process of the moving body 1 along the next linear path L is performed.
-Step S31
When moving the moving body 1 along the zigzag path Z, the moving body 1 ends in order to align the lines of the adjacent turning points (the end point Gn-1 of the previous straight line path and the start point Sn of the next straight line path L). From the time when it is determined that the goal is reached to Gn, the moving body 1 is advanced as it is to the direction conversion point Pnr (Pnrx, Pnry) separated by a distance twice the total length of the moving body 1.
-Step S32
Since the position of the moving body 1 has changed due to the advance in step S31, the position information of the moving body 1 is updated from TS2 to the latest position information Pnr.
-Step S33
Determine if an error has occurred when updating the location information.
-Step S34
If no error has occurred in updating the position information (No in step S33), the locus vector T is recalculated.
-Step S35
As shown in FIG. 11, the distance EX to the next straight path L and the turning correction angle θc are calculated.
As shown in FIG. 17, the distance EX to the next linear path L is calculated in the equation (1) by using the next linear path vector C represented by the equation (2) and the point P (= point Pnr). Find based on. That is, d = EX in the equation (1).
The turning correction angle θc, that is, the turning correction angle θc, which is the angle formed by the previous linear path vector C and the locus vector T, is obtained based on the equation (1) of FIG.
-Step S36
As shown in FIG. 18, the start point Sn-1 (Sn-1x, Sn-1y) of the previous straight path (lane), the end point Gn-1 (Gn-1x, Gn-1y) of the previous straight path, and the straight line. Using the distance w between the routes, the start point Sn (Snx, Sny) of the next linear route and the end point Gn (Gnx, Gny) of the next linear route are obtained.
The start point Sn (Snx, Sny) of the next straight path and the end point Gn (Gnx, Gny) of the next straight path are obtained based on the formula (1) or the formula (2) of FIG. As shown in FIG. 18, it is assumed that each linear path is tilted by θ from the X axis of the reference XY coordinates. Then, when the next straight path is on the right side in the traveling direction of the previous straight path, the start point Sn (Snx, Sny) of the next straight path and the end point Gn (Gnx, Gny) of the next straight path are , Based on the equation (1) in FIG. When the next straight path is on the left side in the traveling direction of the previous straight path, the start point Sn (Snx, Sny) of the next straight path and the end point Gn (Gnx, Gny) of the next straight path are , Based on the formula (2) in FIG.
-Step S37
By extending the rod 43 of the elevating device 40 and pressing the floor surface F with the rolling element 45, the left and right front wheels 21L and 21R of the moving body 1 are lifted from the floor surface F.

-Step S38
Conditional branching based on the combination of the patrol pattern and the number of the latest straight route (lane) ・ Condition 1: When the number of the right patrol pattern and the latest straight route is odd, or the left patrol pattern and the latest straight route If the number of is even.
-Condition 2: When the right patrol pattern and the number of the latest straight route are even, or when the left patrol pattern and the number of the latest straight route are odd.
The right patrol pattern refers to a pattern in which the moving body 1 is moved along the zigzag path Z in which the subsequent straight path is set on the right side of the first straight path when viewed from the first (first) straight path. The left patrol pattern refers to a pattern in which the moving body 1 is moved along the zigzag path Z in which the subsequent straight path seen from the first (first) straight path is set on the left side of the first straight path.
The left and right patrol patterns can be determined by clicking the non-figure patrol pattern determination button provided on the setting screen of FIG. 6, and the computer 3 uses the determined patrol pattern as movement schedule information as the moving body 1. 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 elevating device 40 is retracted to float the rolling element 45 from the floor surface F, and the left and right front wheels 21L and 21R of the moving body 1 are brought into contact with the floor surface F.
-Step S41
The moving body 1 is advanced by the distance EX to the next straight path L.
-Step S42
The rod 43 of the elevating 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 elevating device 40 is retracted to float the rolling element 45 from the floor surface F, the left and right front wheels 21L and 21R of the moving body 1 are brought into contact with the floor surface F, and the moving body 1 is brought into the next linear path L. Move along.

If the condition 2 is determined in step S38, the moving body 1 is rotated counterclockwise.
-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 elevating device 40 is retracted to float the rolling element 45 from the floor surface F, and the left and right front wheels 21L and 21R of the moving body 1 are brought into contact with the floor surface F.
-Step S47
The moving body 1 is advanced by the distance EX to the next straight path L.
・ Step S48
The rod 43 of the elevating 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 counterclockwise by 90 °.
-Step S50
The rod 43 of the elevating device 40 is retracted to float the rolling element 45 from the floor surface F, the left and right front wheels 21L and 21R of the moving body 1 are brought into contact with the floor surface F, and the moving body 1 is brought into the next linear path L. Move along.

The irregular occurrence processing D (steps S60 to S65) will be described with reference to FIG.
-Step S60
In the case of Yes in step S12, that is, when it is determined that an error has occurred in updating the position information, the moving body is used by using the locus vector T when the position information is obtained and the detected values from the encoders 25L and 25R. Estimate the self-position of 1.
That is, as shown in FIG. 19, the position coordinate Pn-1 (Pn-1x, Pn-1y) immediately before losing the self-position and the position coordinate Pn-2 (Pn-2x) one phase before the position coordinate Pn-1. , Pn-2y), the locus vector T is defined, and the advanced distance D is calculated by using the values of the encoders 25L and 25R that have increased from passing through the immediately preceding position coordinate Pn-1 until the self-position is lost. Estimate and calculate the estimated self-position Pc (Pcx, Pcy) of the moving body 1 based on the equations (1) and (2).
-Step S61
At present, it is determined whether or not the moving body 1 is presumed to be near the end point (goal) of the straight path L.
-Step S62
Currently, if the moving body 1 is not presumed to be near the end point, it is advanced by a certain distance.
-Step S63
Currently, if it is estimated that the moving body 1 is 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 whether the distance traveled during the continuous error exceeds a predetermined value (for example, 1 m). If the number of consecutive errors exceeds the predetermined value, or the distance traveled during the continuous error exceeds the predetermined value, the process ends. If the number of continuous errors does not exceed the predetermined value and the distance traveled during the continuous error 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 or not the number of consecutive errors exceeds a predetermined value (for example, 10 times). If the number of consecutive errors exceeds the specified 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 path Z as the planned movement route set by the movement schedule information determined by the XY coordinate values on the floor surface F measured by TS2, that is, the movement start point S1 of the moving body 1. Depending on the XY coordinate values (S1x, S1y), the XY coordinate values (G1x, G1y) of the end point G1 of the first linear path L, the distance W between the adjacent linear paths L and L, and the number N of the linear paths L. The moving body 1 can be moved so as to follow the set zigzag path Z, and the XY coordinate values on the floor surface F of the moving body 1 moving on the floor surface F acquired by TS2 and the zigzag path Z. By controlling the movement of the moving body 1 by comparing it with the XY coordinate values of, the moving body 1 can be moved accurately along a predetermined zigzag path Z based on the movement schedule information. ..

Further, when it is determined that the moving body 1 has reached the end point of the linear path L, the moving direction changing means mounted on the moving body 1 performs the turn-back process C for reversing the traveling direction of the moving body 1. The moving body 1 can be moved accurately along the zigzag path Z.

Further, in the movement control process, when the moving body 1 approaches the end point Gn of the linear path L, the moving speed of the moving body 1 is reduced by one or more times, so that the moving body 1 is controlled along the zigzag path Z. It is possible to improve the accuracy of the folding process C of the moving body 1 when moving 1.

Further, in the movement control process, when the movement information of the moving body 1 cannot be obtained from the TS2, the moving body 1 is based on the moving amount information obtained from the encoder as the moving amount detecting means mounted on the moving body 1. Since the position is estimated, even if the movement information of the moving body 1 cannot be obtained from the TS2, the moving body 1 can be accurately moved along the predetermined zigzag path Z based on the moving schedule information. Can be done.

Further, in the movement schedule information setting process, in order to set the zigzag path Z on the floor surface F, the XY coordinate values of the movement start point S1 of the moving body 1 and the XY coordinate values of the end point G1 of the first straight line path L1 are used. By simply setting the number of straight paths L and the distance w between adjacent straight paths L and L, the zigzag path Z to which the moving body 1 is to be moved can be easily set.

Further, in the moving body 1, the control means 50 extends the telescopic means of the elevating device 40 to press the rolling element 45 against the floor surface F, and the front wheels 21L and 21R are floated from the floor surface F. Since the rear wheels 22L and 22R that are in contact with the wheel are rotated in opposite directions to change the horizontal direction, the horizontal direction is smoothly changed when the moving direction is changed. Can be changed.
That is, with the base 10 floating from the floor surface F and the rolling element 45 in contact with the floor surface F, the left and right rear wheels 22L and 22R are rotated in opposite directions to center the rotation center line 10C. Since the left and right rear wheels 22L, 22R and the rolling element 45 roll on one arc 10R on the floor surface F, the moving body 1 rotates clockwise or left with the rotation center line 10C as the rotation center. It rotates smoothly around, and the horizontal orientation of the moving body 1 can be smoothly changed.
According to the moving body 1, even when the moving body 1 is heavy, the horizontal orientation of the moving body 1 can be smoothly changed. Therefore, the configuration is suitable for a heavy moving body.
Further, since the elevating device 40 is provided with a guide portion 46 for moving the telescopic means of the elevating device 40 in the vertical direction along the rotating center line 10C while maintaining parallelism with the rotating center line 10C of the moving body 1. Since the telescopic means of the above can be accurately moved in the vertical direction to accurately form the rotation center line 10C, the horizontal orientation of the moving body 1 can be smoothly changed.

As the rolling element 45, for example, as shown in FIG. 4D, a wheel configured to be able to roll on one arc 10R centered on the rotation center line 10C of the moving body 1. Alternatively, a rolling element such as a ball ring may be used.

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

-Movement schedule information setting process-Step S100
In order to set the zigzag path Z on the floor surface F as shown in FIG. 23 by using an input means such as a keyboard on the setting screen 31 (see FIG. 22) of the computer 3, the movement start point S1 of the moving body 1 is set. (S1x, S1y), the end point G1 (G1x, G1y) of the first linear path L1, the diagonal measurement point Rd (Rdx, Rdy), and the initial value w'of the distance w between adjacent linear paths are set. To do.
That is, on the floor surface F, targets are set at the movement start point S of the moving body 1, the end point G of the first linear path L1, and the diagonal measurement point Rd, and the movement start point S (S1x, S1y). And the end point G1 (G1x, G1y) of the first linear path L1 and the diagonal measurement point Rd (Rdx, Rdy) are acquired by using TS2 fixed at a predetermined position on the floor surface F, and acquired. The setter sets each XY coordinate value and the initial value w'of the distance w between adjacent straight paths 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 points 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 obtained based on the equation (1) of FIG.
-Step S103
It is determined whether or not θ is large enough to suspect an input data error or a diagonal measurement point installation error.
-Step S104
If it is determined in step S103 that θ is not large enough to suspect an input data error or a setting error of the diagonal measurement point Rd, the area width Lw is set based on the calculation formula in step S104. calculate. As shown in FIG. 23, the area width Lw is the width in the direction in which a plurality of linear paths L, L ... In the zigzag path Z are lined up.
-Step S105
If it is determined in step S103 that θ is large enough to suspect an input data error or an installation error of the diagonal measurement point Rd (measurement point error), it is notified that the parameter is abnormal and the process ends. To do.
-Step S106
Using the area width Lw calculated in step S104 and the initial value w'of the distance w between the 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 the step S105. (Calculate the remainder r). As shown in FIG. 23, the number of blocks refers to the number of rectangular regions b formed within the area width Lw and surrounded by adjacent straight paths L and L and inter-straight paths M.
-Step S107
Determine if the remainder is 0 or not.
-Step S108
If the remainder is 0 in step S107, the initial value w'is determined as the distance w between the straight paths (lanes) (w = w').
-Step S109
Add +1 to the number of blocks B'and determine the number N of straight routes (lanes) (N = B'+1). That is, the number N of the optimum linear paths L is determined.
-Step S110
In step S107, if the remainder is not 0, the number of blocks B'is +1 to obtain the number of blocks B (B = B'+1).
-Step S1101
Recalculation of the distance w between straight paths (lanes) (w = Lw / B). If there is a remainder in the recalculation, it is rounded down to 4 significant figures, for example.
-Step S1102
Add +1 to the number of blocks B and determine the number of straight routes (lanes) N (N = B + 1). That is, the number N of the optimum linear paths L is determined.
As is clear from FIG. 23, the number of linear paths (lanes) N 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 are calculated. Find the outer product of the vectors.
-Step S112
It is determined whether the Z-axis component (vertical component) of the outer product calculated in step S111 is positive or negative.
-Step S113
If the Z-axis component of the outer product calculated in step S111 is positive as shown in the equation (1) of FIG. 20, it is determined to be the zigzag path Z of the left circulation pattern.
-Step S114
If the Z-axis component of the outer product calculated in step S111 is negative, as shown in the equation (2) of FIG. 20, the zigzag path Z of the right circulation pattern is determined.
-Step S115
If the Z-axis component of the outer product calculated in step S111 is 0 as shown in the equation (3) of FIG. 20, it is notified that the parameter is abnormal 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 movement schedule information set by the computer 3 is written as a file in the storage device of the computer 3.
-Step S119
After the computer 3 transmits the movement schedule information to the control means 50 of the mobile body 1 and reads it 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 linear path L1, and the diagonal measurement point Rd (Rdx, Rdy). By setting the initial value w'of the distance w between adjacent straight paths as the movement schedule information, the movement schedule information setting program calculates the area width Lw, and then the area width Lw and the initial value w' Based on the above, the optimum number of linear paths L is calculated, and the process of determining the patrol pattern is also performed, so that the movement schedule information setting process becomes easier.
The patrol pattern calculated and obtained in steps S111 to S115 can also be determined by clicking the non-figure patrol pattern determination button provided on the setting screen of FIG. 6 described in the first embodiment. According to the second embodiment, even if the patrol pattern determination button is forgotten to be clicked or the patrol pattern determination button is not provided, the patrol pattern is set by the movement schedule information setting processing in steps S111 to S115. Is automatically determined, so that the work effort related to the movement schedule information setting process can be reduced.

In each embodiment, a moving body having a rear wheel as a driving wheel has been illustrated, but a moving body having a front wheel as a driving wheel may be used. In this case, the control means sets the moving body so that the rod of the elevating device is extended to press the rolling element against the floor surface and the rear wheels of the moving body are floated from the floor surface, and the moving body is set on the floor surface. The direction of the moving body may be 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 moving body is configured such that an elevating device is provided on the rear side of the substrate so that the rear side of the moving body can be moved up and down, and the center point of the contact surface of the left front wheel with the moving target surface 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 rotation centers of the moving body when the horizontal direction of the moving body is changed. It is configured to be located on one arc centered on the line.

Further, in each embodiment, the zigzag path Z is set as the planned movement route, but the planned movement route is a route set by the movement schedule information determined by the XY coordinate values on the floor surface measured by TS. , Any route can be used.

Further, in each embodiment, an example of moving the floor surface in the building is shown as the moving target surface, but the moving target surface may be a surface such as a road or a vacant lot outside the building.

Further, in each embodiment, an example in which a TS (total station) is used as a movement information acquisition means for acquiring the actual movement information of the moving body and transmitting it to the moving body is shown, but the movement information acquisition means other than the TS For example, GPS may be used.
That is, in this case, the planned movement route is set by the movement schedule information defined by the XY coordinate values on the movement target surface by GPS, and the movement target surface of the moving body moving on the movement target surface acquired by GPS is used. By controlling the movement of the moving body by comparing the XY coordinate value of the above and the XY coordinate value of the planned movement route, the moving body can be moved so as to follow the planned movement route with high accuracy.

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

Claims (11)

The movement schedule information setting process for setting the movement schedule information of the moving body to be moved on the movement target surface, and
A movement control process that controls the movement of the moving body by comparing the actual moving information of the moving body acquired by the moving information acquisition means with the moving schedule information while moving the moving body based on the moving schedule information.
The feature is that the movement control method of the moving body. The movement control process is characterized in that when the amount of misalignment between the movement schedule information and the movement information exceeds a predetermined value, the movement direction of the moving body is changed by the moving direction changing means mounted on the moving body. The method for controlling the movement of a moving body according to claim 1. The first or second aspect of the movement schedule information setting process is characterized in that the movement schedule information for moving the moving body is set along the zigzag path in which the linear path is repeatedly folded back on the movement target surface. The movement control method of the moving body described. 3. The movement control process is characterized in that, when it is determined that the moving body has reached the end point of the linear path, the moving direction of the moving body is reversed by the moving direction changing means mounted on the moving body. The method for controlling the movement of a moving body according to the above. The movement of the moving body according to claim 3 or 4, wherein in the movement control process, control is performed to reduce the moving speed of the moving body one or more times when the moving body approaches the end point of the linear path. Control method. The movement control process is characterized in that when the movement information cannot be obtained from the movement information acquisition means, the position of the moving body is estimated based on the movement amount information obtained from the movement amount detecting means mounted on the moving body. The method for controlling the movement of a moving body according to any one of claims 1 to 5. In the movement schedule information setting process, in order to set the zigzag path on the movement target surface, the XY coordinate value of the movement start point of the moving body, the XY coordinate value of the end point of the first straight line path, the number of straight line paths, and so on. The movement control method for a moving body according to claim 3, wherein the distance between adjacent straight paths is set. In the movement schedule information setting process, in order to set the zigzag path on the movement target surface, the XY coordinate value of the movement start point of the moving body, the XY coordinate value of the end point of the first straight line path, and the XY coordinate value of the movement target surface. By setting the XY coordinate value 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 paths, the direction in which a plurality of linear paths are lined up in the zigzag path. The movement control method for a moving body according to claim 3, wherein the optimum value for the number of straight paths is calculated and set based on the width of the head and the initial value of the distance between the straight paths. Acquires the moving body configured to be movable on the moving target surface, the moving schedule information setting means for setting the moving schedule information on the moving target surface to move the moving body, and the actual moving information of the moving body. It is equipped with a movement information acquisition means to be transmitted to the moving body.
The moving body includes a base body, a moving means provided on the base body, an elevating device provided on the base body for raising and lowering the front side or the rear side of the moving body, and a control means.
The control means is a movement control system for a moving body, which controls the movement of the moving body by controlling the moving means and the elevating device based on the moving schedule information and the moving information. The means of transportation includes a front wheel, a rear wheel, and a driving means of at least one of the front wheel and the rear wheel.
The elevating device includes a telescopic means that expands and contracts in the vertical direction, and a rolling element that is provided on the tip end side of the telescopic means and is configured to be able to roll on the surface to be moved.
The control means touches the moving target surface in a state where the telescopic means of the elevating device is extended to press the rolling element against the moving target surface and one of the front wheel and the rear wheel is lifted from the moving target surface. The movement control system for a moving body according to claim 9, wherein the other left and right wheels are rotated in opposite directions to change the horizontal direction of the moving body. A moving amount detecting means for detecting the moving amount of the moving body based on the rotation amount of the wheel is provided.
10. The control means according to claim 10, wherein the control means estimates the position of the moving body based on the movement amount information obtained from the movement amount detecting means when the movement information cannot be obtained from the movement information acquisition means. Movement control system for moving objects.

Images