Device, method and program for control

Abstract

To allow a mobile robot to reach a destination in a shorter time in moving the mobile robot while taking a stop track into consideration.SOLUTION: A control device generates a plurality of first track candidates as candidates for a plan track for a mobile robot. The control device generates, on each of the plurality of first track candidates, a stop track which is a track having as a start point a position of the mobile robot on two or more first track candidates in future time, the stop track being a track upon stopping the mobile robot. The control device sets, on each of the plurality of first track candidates, the first track candidate as a second track candidate if at least one of at least two or more stop tracks in future time represents not to enter a predetermined area. The control device selects a plan track for the mobile robot from a plurality of second track candidates. The control device controls the mobile robot to move along a selected plan track.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present disclosure relates to a control device, a control method, and a control program. [Background technology]

[0002] Conventionally, there is known a technique for controlling a mobile robot (see, for example, Non-Patent Document 1). The technique disclosed in Non-Patent Document 1 is capable of moving a mobile robot while considering a stopping trajectory that prevents the mobile robot from colliding with an obstacle or the like when a failure occurs in the system. [Prior art documents] [Non-patent literature]

[0003] [Non-Patent Document 1] O. Brock and O. Khatib, “High-speed navigation using the global dynamic window approach,” in Proceedings 1999 IEEE international conference on robotics and automation (Cat. No. 99CH36288C), vol. 1. IEEE, 1999, pp. 341-346. Summary of the Invention [Problem to be solved by the invention]

[0004] The technology disclosed in Non-Patent Document 1 searches for a stopping trajectory at time t+1, which is one time unit in the future from the current time t, such that the mobile robot will not collide with obstacles, etc. The technology disclosed in Non-Patent Document 1 then controls the mobile robot so as to realize a trajectory that includes a planned trajectory along which the mobile robot will actually move and a stopping trajectory along which the mobile robot will not collide with obstacles, etc. The stopping trajectory is linked to the planned trajectory, and the actual planned trajectory of the mobile robot is selected from among the candidate planned trajectories that realize a stopping trajectory along which the mobile robot will not collide with obstacles, etc.

[0005] However, the technology disclosed in Non-Patent Document 1 only searches for a stopping trajectory at time t+1. In such a case, the variation of planned trajectory candidates linked to the stopping trajectories is limited, and the number of selectable planned trajectory candidates is reduced. This makes it impossible to select a planned trajectory from a variety of planned trajectory candidates, and as a result, there are cases where only a planned trajectory that slows the mobile robot's arrival at its destination is obtained. Therefore, when the technology disclosed in Non-Patent Document 1 is used, there is a problem that the mobile robot may be delayed in arriving at its destination.

[0006] The present disclosure has been made in consideration of the above points, and aims to allow a mobile robot to reach a destination in a shorter time when moving the mobile robot while taking into account a stopping trajectory. [Means for solving the problem]

[0007] In order to achieve the above-mentioned object, the control device of the present disclosure is a control device comprising: a trajectory generation unit that generates a plurality of first trajectory candidates, which are candidates for a planned trajectory of a mobile robot; a stop trajectory generation unit that generates, for each of the plurality of first trajectory candidates, a stop trajectory that is a trajectory when stopping the mobile robot, the stop trajectory starting from the position of the mobile robot on the first trajectory candidate at at least two or more future times; a selection unit that sets, for each of the plurality of first trajectory candidates, the first trajectory candidate as a second trajectory candidate when at least one of the stop trajectories at at least two or more future times indicates that the stop trajectory will not enter a predetermined area, and selects a planned trajectory of the mobile robot from the plurality of second trajectory candidates; and a control unit that controls the mobile robot to move along the selected planned trajectory.

[0008] In addition, the control method disclosed herein is a control method in which a computer executes a process of generating a plurality of first trajectory candidates, which are candidates for a planned trajectory of a mobile robot, generating, for each of the plurality of first trajectory candidates, a stopping trajectory that is a trajectory when stopping the mobile robot, the stopping trajectory starting from the position of the mobile robot on the first trajectory candidate at at least two or more future times, and is a trajectory when stopping the mobile robot, setting the first trajectory candidate as a second trajectory candidate if, for each of the plurality of first trajectory candidates, at least one stopping trajectory at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, selecting a planned trajectory of the mobile robot from the plurality of second trajectory candidates, and controlling the mobile robot to move along the selected planned trajectory.

[0009] In addition, the control program disclosed herein is a control program for causing a computer to execute a process of generating a plurality of first trajectory candidates, which are candidates for a planned trajectory of a mobile robot; for each of the plurality of first trajectory candidates, generating a stopping trajectory, which is a trajectory when stopping the mobile robot, that starts from the position of the mobile robot on the first trajectory candidate at at least two or more future times; for each of the plurality of first trajectory candidates, if at least one stopping trajectory among the stopping trajectories at at least two or more future times indicates that the mobile robot will not enter a predetermined area, setting the first trajectory candidate as a second trajectory candidate; selecting a planned trajectory of the mobile robot from the plurality of second trajectory candidates; and controlling the mobile robot to move along the selected planned trajectory. [Effects of the Invention]

[0010] According to the control device, control method, and control program of the present disclosure, when moving a mobile robot while taking into consideration the stopping trajectory, it is possible to make the mobile robot reach its destination in a shorter time. [Brief explanation of the drawings]

[0011] [Figure 1] FIG. 1 is a diagram for explaining an overview of the present embodiment. [Figure 2] FIG. 1 is a diagram for explaining a conventional technique. [Figure 3] FIG. 1 is a diagram for explaining an overview of the present embodiment. [Figure 4] 10A and 10B are diagrams for explaining the difference between the control method of GDWA and the control method of this embodiment. [Figure 5] FIG. 2 is a diagram for explaining each time in this embodiment. [Figure 6] FIG. 10 is a diagram for explaining a process for randomly generating the speed of a mobile robot at each time. [Figure 7] FIG. 10 is a diagram for explaining a velocity map. [Figure 8] 1 is a block diagram showing a schematic configuration of a control system according to an embodiment of the present invention; [Figure 9]FIG. 2 is a block diagram showing the hardware configuration of the control device according to the present embodiment. [Figure 10] 4 is a flowchart showing the flow of a control process in the present embodiment. [Figure 11] FIG. 10 is a diagram showing the results of this example. DETAILED DESCRIPTION OF THE INVENTION

[0012] An example of an embodiment of the present disclosure will be described below with reference to the drawings. In this embodiment, a control system equipped with a control device according to the present disclosure will be described as an example. Note that the same reference numerals are used in the drawings to designate identical or equivalent components and parts. Furthermore, the dimensions and proportions of the drawings are exaggerated for the sake of explanation and may differ from the actual proportions.

[0013] FIG. 1 is a diagram illustrating this embodiment. FIG. 1 is a diagram showing the movement of a mobile robot 1 as seen from above. As shown in FIG. 1, at time t=0.0 [s], the mobile robot 1 starts moving. Then, from time t=1.0 [s] to time t=5.0 [s], the mobile robot 1 moves so as not to collide with obstacles (white squares in FIG. 1), and at time t=6.0 [s], the mobile robot 1 reaches the destination g. In this embodiment, a planned trajectory P as shown in the time t=2.0 [s] scene in FIG. 1 is calculated at each time, and the mobile robot 1 is controlled so as to realize the planned trajectory.

[0014] FIG. 2 is a diagram for explaining Non-Patent Document 1. Note that hereinafter, the technology disclosed in Non-Patent Document 1 will also be simply referred to as GDWA (Global dynamic window approach). As shown in FIG. 2, in GDWA, for example, a planned trajectory P of the mobile robot 1 is calculated for each time t to t+T (FIG. 2 shows up to time t+6). Furthermore, in GDWA, a stopping trajectory S is searched for at time t+1, which is one time ahead of the current time t. This stopping trajectory S is a trajectory that allows the mobile robot 1 to be stopped safely without colliding with an obstacle or the like.

[0015] However, as shown in Figure 2, when only the stopping trajectory at time t+1 is considered, the variations of planned trajectory candidates associated with that stopping trajectory are limited, and the selection of an appropriate trajectory from the various planned trajectory candidates is restricted. For this reason, when controlling the mobile robot 1 using GDWA, it may take a long time to reach the destination.

[0016] Therefore, in this embodiment, not only the stopping orbit at time t+1 but also the stopping orbit from time t+1 to time t+T is searched for. Note that although time may be expressed as "t, t+Δt, t+2Δt", in this embodiment time is expressed as "t, t+1, t+2, . . . "

[0017] FIG. 3 is a diagram for explaining an overview of this embodiment. As shown in FIG. 3, in this embodiment, a stopping trajectory from time t+1 to time t+T is also searched for (drawn by a solid black line in FIG. 3). Note that, at this time, the candidate stopping trajectories also include a stopping trajectory that may result in collision with an obstacle or the like (for example, NG in FIG. 3). Of the stopping trajectories shown in FIG. 3, the stopping trajectories marked with NG are trajectories that may result in the mobile robot 1 colliding with an obstacle or the like. On the other hand, the stopping trajectories marked with OK are trajectories that allow the mobile robot 1 to be stopped safely without colliding with an obstacle or the like.

[0018] In this embodiment, if at least one of the stopping trajectories from time t+1 to time t+T indicates that the vehicle will not collide with an obstacle, etc., then that stopping trajectory can also be adopted. This increases the number of stopping trajectory candidates, making it possible to generate a wider variety of planned trajectories.

[0019] FIG. 4 is a diagram illustrating the difference between GDWA and the control method of this embodiment. The diagram on the left side of FIG. 4 illustrates the control method of GDWA, and the diagram on the right side of FIG. 4 illustrates the control method of this embodiment. As shown in FIG. 4, the control method of GDWA searches only for a stopping trajectory in the planned trajectory of the mobile robot 1, starting from the position of the mobile robot 1 at time t+1, which is one time point in the future from the current time t. On the other hand, as shown in FIG. 4, the control method of this embodiment searches for a stopping trajectory in the planned trajectory of the mobile robot 1, starting from the position of the mobile robot 1 at each time from time t+1 to time t+T. In the example of FIG. 4, the control method of this embodiment considers a stopping trajectory starting from the position of the mobile robot 1 at time t+7.

[0020] In this way, candidates for planned trajectories that enable the mobile robot 1 to reach the destination in a shorter time are also generated, and as a result, it becomes possible for the mobile robot 1 to reach the destination in a shorter time. This will be explained in detail below.

[0021] <Framework of this embodiment> (A. Formulation) The state equation of the mobile robot 1 is expressed by the following equation (1), where f is a predetermined function.

[0022]

number

[0023] Here, q(t) is the state variable at time t, and u(t) is the control input for the mobile robot 1. q(t) includes the position coordinates (x(t), y(t)) of the mobile robot 1 in the global coordinate system, the orientation θ(t) of the mobile robot 1 in the global coordinate system, the velocity v(t) of the mobile robot 1, and the angular velocity ω(t) of the mobile robot 1.

[0024] Here, the set of feasible control inputs U is expressed by the following equation (2): F Think about it.

[0025]

number

[0026] U is a set of control inputs determined by the upper and lower limits of acceleration and deceleration of the motor that drives the mobile robot 1. F is a set of state variables that are determined according to the upper and lower limits of acceleration and deceleration of the motor that drives the mobile robot 1, and that do not collide with obstacles, etc. The set of control inputs U F By using (q0,t,T), the following two sets of control inputs U G (q0,t,T) and U S (q0,t,T) is defined.

[0027]

number

[0028] Here, Q G is a set of state variables that allows the mobile robot 1 to reach the destination g without colliding with obstacles. S is a set of states in which the mobile robot 1 is at rest without colliding with an obstacle or the like.

[0029] U G (q0,t,T) is a set of control inputs that allows the mobile robot 1 to reach the destination g at time t+T. On the other hand, U S (q0,t,T) is a set of control inputs that can safely stop the vehicle at time t+T without colliding with an obstacle or the like.

[0030] Considering the above equations (1) to (4), the control problem of the mobile robot 1 in this embodiment can be said to be finding a control input u(t) as shown in the following equation (5).

[0031]

number

[0032] where T E represents the time limit for switching from the planned trajectory to the stopping trajectory when a fault occurs in the control system of the mobile robot 1. S (>T B ) represents the time required for the mobile robot 1 to stop after a failure occurs in the control system of the mobile robot 1.

[0033] 5 is a diagram for explaining the above-mentioned time periods. After the mobile robot 1 starts moving at the current time t, if a problem such as a failure occurs in the control system or communication system of the mobile robot 1, the time t+T B At time t+T, the planned orbit is switched to the stopping orbit. B is from time t to time t+T E The time is any time up to t+T. B If the planned orbit is switched to the stopping orbit at time t+T S If the mobile robot 1 moves along the planned trajectory without switching to the stopping trajectory, the mobile robot 1 will stop safely at time t+T G reaches destination g.

[0034] The above formula (5) expresses the time T required to travel to the destination g. G The control input u(t) that minimizes is the candidate set of control inputs U G (q0,t,T). As mentioned above, U G (q0,t,T) is a set of control inputs that allows the mobile robot 1 to reach the destination g at time t+T. As shown in the above formula (5), B Control input u(t+T B ) is the candidate set of control inputs U S (q(t+T B ),t+T B ,T S -T B ) must be selectable from the above. S(q(t+T B ),t+T B ,T S -T B ) is at time t+T S It is a set of control inputs that can safely stop the vehicle without colliding with obstacles.

[0035] By selecting the control input u(t) as shown in the above equation (5), the time T required to reach the destination g is G is minimized and a planned trajectory is realized that allows the robot to stop safely when an obstacle occurs.

[0036] (B. Generation of a planned trajectory taking into account the speed and angular velocity of the mobile robot 1) In this embodiment, when determining the control input u(t) according to the above equation (5), the speed and angular velocity of the mobile robot 1 are taken into consideration.

[0037] Specifically, in this embodiment, when determining the speed and angular velocity of the mobile robot 1 at each time on a candidate planned trajectory, multiple accelerations and angular accelerations of the mobile robot 1 at each time are randomly generated based on the upper and lower limits of acceleration and deceleration of the motor, which is an example of a drive unit that moves the mobile robot 1, and for each of the multiple accelerations and angular accelerations generated, a candidate planned trajectory determined from the acceleration and angular acceleration is generated.

[0038] In the above-described GDWA, the planned trajectory is set on the assumption that the speed and angular velocity of the mobile robot 1 are constant.

[0039] However, it is preferable to take into consideration that the speed and angular velocity of the mobile robot 1 can be changed. In particular, when moving the mobile robot 1 to avoid obstacles, it is preferable to appropriately change the speed and angular velocity of the mobile robot 1 rather than keeping the speed and acceleration constant.

[0040] Therefore, in this embodiment, the speed and angular velocity of the mobile robot 1 are randomly changed, and a set of candidates for planned trajectories that are realized by the randomly changed speed and angular velocity is generated. Then, from the set of candidates for planned trajectories thus obtained, a planned trajectory that minimizes the time required to reach the destination g is selected.

[0041] 6 is a diagram for explaining the process of randomly generating the speed of the mobile robot 1 at each time. In the diagram shown in FIG. 6, the horizontal axis represents time, and the vertical axis represents the speed of the mobile robot 1.

[0042] As shown in Fig. 6, at time t = 0.0, the speed of the mobile robot 1 is assumed to be 1.50 m / s. In this case, at time t = 0.1, three speeds, v1, v2, and v3, are set according to the upper and lower limits of acceleration and deceleration of the motor of the mobile robot 1. Furthermore, as shown in Fig. 6, at time t = 0.2, three more speeds are set so as to branch off from each of the three speeds, v1, v2, and v3, according to the upper and lower limits of acceleration and deceleration of the motor. A speed is randomly selected from the multiple speeds generated at each time in this manner.

[0043] Specifically, for example, assume that at time t=0.1, speed v1 is randomly selected from three speeds v1, v2, and v3. In this case, at time t=0.2, three speeds v1-1, v1-2, and v1-3 are set, and one of these speeds is randomly selected. In this embodiment, the trajectory realized by the series of speeds selected in this manner is set as a candidate for the planned trajectory.

[0044] Note that in FIG. 6, a series of velocities is used for the sake of simplicity, but in reality, the acceleration of the mobile robot 1 is generated randomly, and multiple series of accelerations are generated based on the principle shown in FIG. 6. When calculating the acceleration, the difference between velocities can also be calculated and converted into acceleration information. Furthermore, similar processing is performed not only on the velocity of the mobile robot 1 but also on the angular velocity. Furthermore, similar processing is performed not only on the acceleration of the mobile robot 1 but also on the angular acceleration.

[0045] For each of the multiple accelerations and angular accelerations generated in this way, a candidate planned trajectory determined from the acceleration and angular acceleration is generated. Then, by processing described below, a planned trajectory that minimizes the time required to reach the destination g is selected from the candidate planned trajectory set.

[0046] (C. Estimation of the time required for the mobile robot 1 to move to the destination g) In this embodiment, the planned trajectory candidate that minimizes the time required for the mobile robot 1 to move from the end of the planned trajectory candidate to the destination g is selected as the planned trajectory of the mobile robot 1.

[0047] When estimating the time required for the mobile robot 1 to move from the end of the candidate planned trajectory to the destination g, the estimated time is calculated so that the closer the mobile robot 1 is to an obstacle, etc., the slower the speed of the mobile robot 1, and the farther the mobile robot 1 is from an obstacle, etc., the faster the speed of the mobile robot 1.

[0048] In the above-described GDWA, a planned trajectory is set assuming a constant speed of the mobile robot 1. In contrast, in this embodiment, a planned trajectory is adopted that realizes a speed according to the distance between the mobile robot 1 and an obstacle, etc.

[0049] Specifically, it is assumed that at the position x of the mobile robot 1, the mobile robot 1 can calculate the distance d(x) between the mobile robot 1 and an obstacle, etc., based on data acquired from the sensors mounted on the mobile robot 1 itself. In this case, the maximum allowable speed v(x) of the mobile robot 1 is expressed by the following equation. max is the upper limit of deceleration.

[0050]

number

[0051] The upper limit of the speed of mobile robot 1 is v max Taking this into consideration, the mobile robot 1 can move at a position x with a velocity v(x) given by the following equation:

[0052]

number

[0053] Therefore, in this embodiment, a velocity map of the mobile robot 1 is generated using v(x) expressed by the above equation (7). FIG. 7 is a diagram for explaining the velocity map. FIG. 7 is a map of the velocities at each point x that the mobile robot 1 can travel. In the example of FIG. 7, black areas indicate areas where obstacles or the like are present, and the velocity in those areas is zero. On the other hand, areas far from obstacles or the like are white, and in those areas the mobile robot 1 can move at a high velocity. For example, a high velocity at which the mobile robot 1 moves is the maximum speed of a motor, which is an example of a drive unit of the mobile robot 1, or a velocity obtained by subtracting a predetermined margin from the maximum motor speed. In this embodiment, the time required for the mobile robot 1 to move from the end of the candidate planned trajectory to the destination g is estimated by referring to the velocity map shown in FIG. 7. Then, a planned trajectory that is estimated to take the shortest time for the mobile robot 1 to move from the end of the candidate planned trajectory to the destination g is selected.

[0054] In this case, the time T G is the time φ(x;g) for the mobile robot 1 to reach the destination g at position x and the time T E It is expressed as the sum of and.

[0055]

number

[0056] When calculating the velocity map shown in FIG. 7, for example, the technique disclosed in Reference 1 below can be used.

[0057] Reference 1: JA Sethian, “A fast marching level set method for monotonically advancing fronts.” proceedings of the National Academy of Sciences, vol. 93, no. 4, pp. 1591-1595, 1996.

[0058] <Control System 10> 8 is a block diagram showing the schematic configuration of the control system 10 of this embodiment. As shown in FIG. 8, the control system 10 mounted on the mobile robot 1 includes a sensor group 12, a control device 14, and a motor 16.

[0059] The sensor group 12 is attached to the mobile robot 1 and sequentially acquires various sensor data for calculating the above-mentioned state variable q(t). Then, the sensor group 12 outputs the acquired various data to the control device 14.

[0060] Fig. 9 is a block diagram showing the hardware configuration of the control device 14 according to this embodiment. As shown in Fig. 9, the control device 14 has a CPU (Central Processing Unit) 42, a memory 44, a storage device 46, an input / output I / F (Interface) 48, a storage medium reader 50, and a communication I / F 52. Each component is connected to each other via a bus 54 so as to be able to communicate with each other.

[0061] The storage device 46 stores a trained model generation program and a control program for executing each process described below. The CPU 42 is a central processing unit that executes various programs and controls each component. That is, the CPU 42 reads the program from the storage device 46 and executes the program using the memory 44 as a work area. The CPU 42 controls each component and performs various arithmetic operations in accordance with the program stored in the storage device 46.

[0062] The memory 44 is made up of RAM (Random Access Memory) and serves as a working area to temporarily store programs and data. The storage device 46 is made up of ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including the operating system and various data.

[0063] The input / output I / F 48 is an interface for inputting data from an external device and outputting data to an external device. Also, input devices for various inputs, such as a keyboard or a mouse, and output devices for various information outputs, such as a display or a printer, may be connected. A touch panel display may be used as the output device, allowing it to function as an input device.

[0064] The storage medium reader 50 reads data stored in various storage media such as CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, and USB (Universal Serial Bus) memory, and writes data to the storage media.

[0065] The communication I / F 52 is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark).

[0066] Next, the functional configuration of the control device 14 will be described. As shown in Fig. 8, the control device 14 functionally includes a planned trajectory generating unit 18, which is an example of a trajectory generating unit, a stop trajectory generating unit 20, a selecting unit 22, and a control unit 24. A data storage unit 17 is also provided in a predetermined storage area of ​​the control device 14. Each functional configuration is realized by the CPU 42 reading out each program stored in the storage device 46, expanding it in the memory 44, and executing it.

[0067] The data storage unit 17 stores various data detected by the sensor group 11.

[0068] The planned trajectory generating unit 18 calculates state variables to be used in each process described later using a known method based on various data detected by the sensor group 11. Then, the planned trajectory generating unit 18 generates a plurality of first planned trajectory candidates, which are candidates for the planned trajectory of the mobile robot 1.

[0069] Specifically, the planned trajectory generating unit 18 generates a plurality of first planned trajectory candidates, which are candidates for the planned trajectory P as indicated by the dashed line in Fig. 3. In the example of Fig. 3, the sequence of positions of the mobile robot 1 at each time from time t to time t+6 corresponds to one first planned trajectory candidate.

[0070] When generating multiple first planned trajectory candidates, the planned trajectory generation unit 18 randomly generates multiple accelerations and angular accelerations of the mobile robot 1 at each time based on the upper and lower limits of acceleration and deceleration of the motor 16 that moves the mobile robot 1, and generates a first planned trajectory candidate determined from the acceleration and angular acceleration for each of the multiple accelerations and angular accelerations generated.

[0071] Specifically, as described above with reference to Figure 6, the planned trajectory generation unit 18 randomly generates multiple accelerations and angular accelerations of the mobile robot 1 at each time, and generates a first planned trajectory candidate determined from those accelerations and angular accelerations.

[0072] The stopping trajectory generating unit 20 generates a stopping trajectory, which is a trajectory that starts from the position of the mobile robot 1 on the first planned trajectory candidate at at least two or more future times, for each of the multiple first planned trajectory candidates generated by the planned trajectory generating unit 18, and is a trajectory that is used to stop the mobile robot 1.

[0073] Specifically, the stopping trajectory generating unit 20 generates a stopping trajectory starting from the position of the mobile robot 1 at each time from time t+1 to time t+6, as shown in Fig. 3. For example, the stopping trajectory is generated based on a trajectory that causes the motor 16 to stop earliest, more specifically, a trajectory that causes the maximum negative acceleration of the motor 16 and the angular acceleration of the motor 16 to become zero (straight ahead).

[0074] The selection unit 22 sets each of the plurality of first planned trajectory candidates as a second planned trajectory candidate when at least one of the stopping trajectories at at least two or more future times indicates that the first planned trajectory candidate does not enter a predetermined area (for example, an area where an obstacle or the like exists). Note that the predetermined area is, for example, an area where the mobile robot 1 is prohibited from entering. For example, an area where an obstacle or the like exists and an area set in advance by the user correspond to the predetermined area as described above.

[0075] For example, in the example shown in Figure 3, the stopping orbits at time t+1 and time t+6 are NG because they enter the specified area. On the other hand, the stopping orbits from time t+2 to time t+5 are OK because they do not enter the specified area.

[0076] This combination of planned orbit candidates and stopping orbits results in four stopping orbits that are OK from time t+1 to time t+6, and at least one stopping orbit does not enter a specified area (for example, an area where an obstacle exists), so that first planned orbit candidate is set as the second planned orbit candidate.

[0077] On the other hand, for example, if all the stopping orbits from time t+1 to time t+6 are NG, that first planned orbit candidate is not adopted and is not set as the second planned orbit candidate.

[0078] Next, the selection unit 22 selects, from among the multiple second planned trajectory candidates, the second planned trajectory candidate that has the shortest estimated time required for the mobile robot 1 to move from the end of the trajectory represented by the second planned trajectory candidate to the destination g of the mobile robot 1 as the planned trajectory of the mobile robot 1.

[0079] In this case, the selection unit 22 calculates the estimated time so that the closer the mobile robot 1 is to the predetermined area, the slower the speed of the mobile robot 1 becomes, and the farther the mobile robot 1 is from the predetermined area, the faster the speed of the mobile robot 1 becomes. Specifically, as described above, the selection unit 22 refers to the speed map calculated from the speed v(x) in equation (7) above, and calculates the estimated time φ(x;g) required for the mobile robot 1 to move from the end of the trajectory represented by the second planned trajectory candidate to the destination g of the mobile robot 1.

[0080] Then, the selection unit 22 selects, from among the plurality of second planned trajectory candidates, the second planned trajectory candidate that minimizes the estimated time φ(x;g) as the planned trajectory of the mobile robot 1.

[0081] The control unit 24 controls the mobile robot 1 to move along the selected planned trajectory. At this time, the control unit 24 uses the acceleration and angular acceleration randomly generated when generating the selected planned trajectory as control inputs to the motor 16 of the mobile robot 1.

[0082] If a failure such as a crash occurs in the control system 10 or a communication system (not shown) that can communicate with the control system 10, the planned trajectory is switched to a stopping trajectory, and the mobile robot 1 safely stops without entering the predetermined area. At this time, when the planned trajectory is switched to the stopping trajectory, the control unit 24 decelerates the mobile robot 1 to the maximum and stops it.

[0083] Next, the operation of the control system 10 according to this embodiment will be described.

[0084] When the control device 14 mounted on the mobile robot 1 receives a predetermined instruction signal, the CPU 42 of the control device 14 reads out a control program from the storage device 46, loads it into the memory 44, and executes it. As a result, the CPU 42 functions as each functional component of the control device 14, and the control process shown in Fig. 10 is executed. The control process of Fig. 10 is repeatedly executed every time a predetermined time (for example, 0.1 seconds) has elapsed.

[0085] In step S100, the planned trajectory generation unit 18 generates a plurality of first planned trajectory candidates, which are candidates for the planned trajectory of the mobile robot 1, based on the sensor data detected by the sensor group 12. Note that in step S100, the planned trajectory generation unit 18 randomly generates a plurality of accelerations and angular accelerations of the mobile robot 1 at each time based on the upper and lower limits of acceleration and deceleration of the motor 16 that moves the mobile robot 1, as described above, and generates a first planned trajectory candidate determined from these accelerations and angular accelerations.

[0086] In step S102, the stopping trajectory generating unit 20 generates stopping trajectories at least at two or more future times after time t+1 for each of the plurality of first planned trajectory candidates generated in step S100.

[0087] In step S104, for each of the multiple first planned trajectory candidates generated in step S100, if at least one of the stopping trajectories after time t+1 generated in step S102 indicates that the stopping trajectory does not enter a specified area, the selection unit 22 sets the first planned trajectory candidate as a second planned trajectory candidate.

[0088] In step S106, the selection unit 22 selects, from among the multiple second planned trajectory candidates set in step S104, the second planned trajectory candidate that has the shortest estimated time required for the mobile robot 1 to move from the end of the trajectory represented by the second planned trajectory candidate to the destination g of the mobile robot 1, as the planned trajectory of the mobile robot 1.

[0089] In step S108, the control unit 24 controls the mobile robot 1 to move along the selected planned trajectory.

[0090] As described above, the control device according to this embodiment generates a plurality of first planned trajectory candidates, which are candidates for the planned trajectory of the mobile robot. For each of the plurality of first trajectory candidates, the control device generates a stopping trajectory, which is a trajectory that starts from the position of the mobile robot on the first planned trajectory candidate at at least two or more future times and is a trajectory when the mobile robot is stopped. For each of the plurality of first planned trajectory candidates, if at least one stopping trajectory among the stopping trajectories at at least two or more future times indicates that the mobile robot does not enter a predetermined area, the control device sets the first planned trajectory candidate as a second planned trajectory candidate. The control device selects a planned trajectory for the mobile robot from the plurality of second trajectory candidates. The control device then controls the mobile robot to move along the selected planned trajectory. This allows the mobile robot to reach its destination in a shorter time when moving the mobile robot while taking the stopping trajectory into consideration. Specifically, the control device according to this embodiment not only calculates a stopping trajectory at a future time t+1, which is one time from the current time t, but also calculates a stopping trajectory from time t+1 to the end time t+T of the planned trajectory. EThe control device according to the embodiment generates a stopping orbit at each time from time t+1 to the end time t+T. E A planned trajectory candidate is one in which at least one of the stopping trajectories up to a point does not enter the predetermined area. This generates more second planned trajectory candidates, and it becomes possible to select a planned trajectory from these candidates that will enable the vehicle to reach the destination g in a shorter time.

[0091] Furthermore, when generating first planned trajectory candidates, the control device according to this embodiment randomly generates multiple accelerations and angular accelerations for the mobile robot at each time based on the upper and lower limits of acceleration and deceleration of the motor, which is an example of a drive unit that moves the mobile robot, and generates multiple first planned trajectory candidates determined from each of the multiple accelerations and angular accelerations generated. This generates planned trajectory candidates that enable the mobile robot to reach the destination faster while taking into account a stopping trajectory that safely stops the mobile robot. Specifically, while GDWA, a conventional technology, maintains a constant speed for the mobile robot, the control device according to this embodiment randomly generates accelerations and angular velocities within the upper and lower limits of acceleration and deceleration of the motor, and generates a planned trajectory that can be realized using those accelerations and angular velocities. This generates a planned trajectory that enables the mobile robot to reach the destination g more efficiently.

[0092] Furthermore, the control device according to this embodiment selects, from among multiple second planned trajectory candidates, the second planned trajectory candidate that has the shortest estimated time required for the mobile robot to move from the end of the trajectory represented by the second planned trajectory candidate to the mobile robot's destination as the planned trajectory for the mobile robot. When calculating the estimated time, the control device calculates the estimated time so that the closer the mobile robot is to the predetermined area, the slower the mobile robot's speed, and so that the farther the mobile robot is from the predetermined area, the faster the mobile robot's speed. This selects a planned trajectory that causes the mobile robot to travel in areas farther from the predetermined area, thereby enabling the mobile robot to reach its destination in a shorter time. [Example]

[0093] Next, an example will be described. In this example, a simulation is performed to verify the effectiveness of the proposed method. In this simulation, the following simulation conditions are set.

[0094] [Table 1]

[0095] In Table 1 above, "Radius" represents the distance a mobile robot can travel in one time step, "Vel." represents the upper and lower limits of the mobile robot's speed, "Rot.Vel." represents the upper and lower limits of the mobile robot's angular velocity, "Accel." represents the upper and lower limits of the mobile robot's acceleration, and "Rot.VAccel." represents the upper and lower limits of the mobile robot's angular acceleration.

[0096] FIG. 11 is a diagram showing the results of a simulation. The simulation results shown in FIG. 11 are the results of the method of this embodiment (indicated as "3.5-bounded (Med.: 6.6 [s])" and "2.3-bounded (Med.: 6.9 [s])" in FIG. 11), and the results of the conventional techniques GDWA (indicated as "GDWA (Med.: 10.1 [s])" and MPPI (indicated as "MPPI (Med.: 13.6 [s])" in FIG. 11). "3.5-bounded (Med.: 6.6 [s])" is the result of the above-mentioned T S is set to 3.5 [s], and "2.3-bounded (Med.: 6.9 [s])" corresponds to the above-mentioned T S This corresponds to the case where the time is set to 2.3 [s]. Also, "MPPI (Med.: 13.6 [s])" corresponds to the technology disclosed in Reference 2 below.

[0097] Reference 2: G. Williams, P. Drews, B. Goldfain, JM Rehg, and EA Theodorou, “Aggressive driving with model predictive path integral control,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016, pp. 1433-1440.

[0098] In this simulation, the time required for a mobile robot to travel 100 patterns, each with a different route from the starting point to the destination and different obstacle placements, was measured. The horizontal axis in FIG. 11 corresponds to each pattern, with the pattern located to the left corresponding to the pattern in which the mobile robot reached the destination faster. As shown in FIG. 11, the results of the method of this embodiment (represented in FIG. 11 as "3.5-bounded (Med.: 6.6 [s])" and "2.3-bounded (Med.: 6.9 [s])") are superior to the results of the conventional GDWA (represented in FIG. 11 as "GDWA (Med.: 10.1 [s])") and MPPI (represented in FIG. 11 as "MPPI (Med.: 13.6 [s])").

[0099] As can be seen from FIG. 11, by using the method of this embodiment, when moving a mobile robot while taking the stopping trajectory into consideration, it is possible to make the mobile robot reach its destination in a shorter time.

[0100] The technology of the present disclosure is not limited to the above-described embodiment, and various modifications and applications are possible within the scope of the gist of this disclosure.

[0101] For example, in the above embodiment, a case where a stopping trajectory is generated at each time after time t+1 for each of a plurality of first planned trajectory candidates has been described as an example, but the present invention is not limited to this. If stopping trajectories are to be generated at at least two or more future times after time t+1, for example, only a stopping trajectory at time t+1 and a stopping trajectory at time t+2 may be generated.

[0102] Furthermore, the processes executed by the CPU after reading the software (program) in the above-described embodiments may be executed by various processors other than the CPU. Examples of such processors include programmable logic devices (PLDs) whose circuit configuration can be changed after fabrication, such as field-programmable gate arrays (FPGAs), and dedicated electrical circuits, such as application-specific integrated circuits (ASICs), which are processors with circuit configurations specifically designed to execute specific processes. Each process may be executed by one of these various processors, or by a combination of two or more processors of the same or different types (e.g., multiple FPGAs, or a combination of a CPU and an FPGA). The hardware structure of these various processors is, more specifically, an electrical circuit that combines circuit elements such as semiconductor devices.

[0103] In the above embodiment, the programs are pre-stored (installed) in a storage device, but the present invention is not limited to this. The programs may be provided in a form stored on a storage medium such as a CD-ROM, DVD-ROM, Blu-ray disc, or USB memory. The programs may also be downloaded from an external device via a network.

[0104] (Addendum) The following additional notes are provided regarding aspects of the present disclosure.

[0105] (Appendix 1) a trajectory generation unit that generates a plurality of first trajectory candidates that are candidates for a planned trajectory of the mobile robot; a stop trajectory generation unit that generates, for each of a plurality of first trajectory candidates, a stop trajectory that starts from a position of the mobile robot on the first trajectory candidate at at least two or more future times and is a trajectory for stopping the mobile robot; a selection unit that, for each of the plurality of first trajectory candidates, when at least one of the stopping trajectories at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, sets the first trajectory candidate as a second trajectory candidate, and selects a planned trajectory of the mobile robot from the plurality of second trajectory candidates; a control unit that controls the mobile robot to move along the selected planned trajectory; A control device comprising: (Appendix 2) When generating the first trajectory candidate, the trajectory generation unit randomly generates a plurality of accelerations and angular accelerations of the mobile robot at each time based on upper and lower limits of acceleration and deceleration of a drive unit that moves the mobile robot, and generates the first trajectory candidate determined from the acceleration and the angular acceleration for each of the generated plurality of accelerations and angular accelerations. 10. The control device of claim 1. (Appendix 3) the selection unit selects, from among the plurality of second trajectory candidates, the second trajectory candidate that has the shortest estimated time required for the mobile robot to move from an end of the trajectory represented by the second trajectory candidate to the destination of the mobile robot, as a planned trajectory of the mobile robot. 10. The control device according to claim 1 or 2. (Appendix 4) the selection unit calculates the estimated time such that the speed of the mobile robot decreases as the mobile robot approaches the predetermined area and increases as the mobile robot moves farther from the predetermined area. 4. The control device according to claim 3. (Appendix 5) generating a plurality of first trajectory candidates which are candidates for a planned trajectory of the mobile robot; generating a stopping trajectory for each of a plurality of first trajectory candidates, the stopping trajectory being a trajectory that starts from a position of the mobile robot on the first trajectory candidate at at least two or more future times and is a trajectory for stopping the mobile robot; for each of the plurality of first trajectory candidates, if at least one of the stopping trajectories at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, the first trajectory candidate is set as a second trajectory candidate, and a planned trajectory of the mobile robot is selected from the plurality of second trajectory candidates; controlling the mobile robot to move along the selected planned trajectory; A control method for computer-implemented processing. (Appendix 6) generating a plurality of first trajectory candidates which are candidates for a planned trajectory of the mobile robot; generating a stopping trajectory for each of a plurality of first trajectory candidates, the stopping trajectory being a trajectory that starts from a position of the mobile robot on the first trajectory candidate at at least two or more future times and is a trajectory for stopping the mobile robot; for each of the plurality of first trajectory candidates, if at least one of the stopping trajectories at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, the first trajectory candidate is set as a second trajectory candidate, and a planned trajectory of the mobile robot is selected from the plurality of second trajectory candidates; controlling the mobile robot to move along the selected planned trajectory; A control program that causes a computer to execute a process. [Explanation of symbols]

[0106] 1. Mobile robot 10. Control System 12 Sensors 14 Control device 16 motors 17 Data storage unit 18 Planned trajectory generation unit 20 Stop trajectory generation section 22 Selection section 24 Control Unit

Claims

1. a trajectory generation unit that generates a plurality of first trajectory candidates that are candidates for a planned trajectory of the mobile robot; a stop trajectory generation unit that generates, for each of a plurality of first trajectory candidates, a stop trajectory that starts from a position of the mobile robot on the first trajectory candidate at at least two or more future times and is a trajectory for stopping the mobile robot; a selection unit that, for each of the plurality of first trajectory candidates, when at least one of the stopping trajectories at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, sets the first trajectory candidate as a second trajectory candidate, and selects a planned trajectory of the mobile robot from the plurality of second trajectory candidates; a control unit that controls the mobile robot to move along the selected planned trajectory; A control device comprising:

2. When generating the first trajectory candidate, the trajectory generation unit randomly generates a plurality of accelerations and angular accelerations of the mobile robot at each time based on upper and lower limits of acceleration and deceleration of a drive unit that moves the mobile robot, and generates the first trajectory candidate determined from the acceleration and angular acceleration for each of the generated plurality of accelerations and angular accelerations. The control device according to claim 1 .

3. the selection unit selects, from among the plurality of second trajectory candidates, the second trajectory candidate that has the shortest estimated time required for the mobile robot to move from an end of the trajectory represented by the second trajectory candidate to the destination of the mobile robot, as a planned trajectory of the mobile robot; The control device according to claim 1 or 2.

4. the selection unit calculates the estimated time such that the speed of the mobile robot decreases as the mobile robot approaches the predetermined area, and increases as the mobile robot moves farther from the predetermined area. The control device according to claim 3 .

5. generating a plurality of first trajectory candidates which are candidates for a planned trajectory of the mobile robot; generating a stopping trajectory for each of a plurality of first trajectory candidates, the stopping trajectory being a trajectory that starts from a position of the mobile robot on the first trajectory candidate at at least two or more future times and is a trajectory for stopping the mobile robot; for each of the plurality of first trajectory candidates, if at least one of the stopping trajectories at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, the first trajectory candidate is set as a second trajectory candidate, and a planned trajectory of the mobile robot is selected from the plurality of second trajectory candidates; controlling the mobile robot to move along the selected planned trajectory; A control method for computer-implemented processing.

6. generating a plurality of first trajectory candidates which are candidates for a planned trajectory of the mobile robot; generating a stopping trajectory for each of a plurality of first trajectory candidates, the stopping trajectory being a trajectory that starts from a position of the mobile robot on the first trajectory candidate at at least two or more future times and is a trajectory for stopping the mobile robot; for each of the plurality of first trajectory candidates, if at least one of the stopping trajectories at at least two or more future times indicates that the stopping trajectory does not enter a predetermined area, the first trajectory candidate is set as a second trajectory candidate, and a planned trajectory of the mobile robot is selected from the plurality of second trajectory candidates; controlling the mobile robot to move along the selected planned trajectory; A control program that causes a computer to execute a process.

Images