Control device, control method, and control program

Abstract

This control device generates a plurality of first trajectory candidates that are candidates for the planned trajectory of a mobile robot. The control device generates, for each of the plurality of first trajectory candidates, a stop trajectory, which is a trajectory having, as a starting point, the position of the mobile robot on the first trajectory candidate at two or more future times, and which is a trajectory when the mobile robot is to be stopped. For each of the plurality of first trajectory candidates, the control device sets the first trajectory candidate as a second trajectory candidate if at least one stop trajectory among the stop trajectories at the two or more future times indicates that same does not enter a prescribed region. The control device selects a planned trajectory of the mobile robot from a plurality of second trajectory candidates. The control device performs control so as to move the mobile robot along the selected planned trajectory.

Description

Control device, control method, and control program

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

[0002] Conventionally, techniques for controlling mobile robots have been known (see, for example, Reference 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.)). The technique disclosed in Reference 1 can move a mobile robot while considering a stopping trajectory that prevents the mobile robot from colliding with obstacles, etc., when a system failure occurs.

[0003] The technology disclosed in Literature 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 Literature 1 then controls the mobile robot 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 candidate planned trajectories that realize a stopping trajectory along which the mobile robot will not collide with obstacles, etc.

[0004] However, the technology disclosed in Literature 1 only searches for a stopping trajectory at time t+1. In such a case, the variations of planned trajectory candidates linked to the stopping trajectories are 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, only a planned trajectory that slows the mobile robot's arrival at the destination may be obtained. Therefore, when the technology disclosed in Literature 1 is used, there is a problem that the mobile robot may be delayed in arriving at the destination.

[0005] 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.

[0006] 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 the mobile robot is stopped, which trajectory starts 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 specified 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.

[0007] In addition, the control method disclosed herein is a control method in which a computer executes processing to generate 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, generate 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 of the stopping trajectories at at least two or more future times indicates that the mobile robot will not enter a predetermined area, set the first trajectory candidate as a second trajectory candidate; select a planned trajectory of the mobile robot from the plurality of second trajectory candidates; and control the mobile robot to move along the selected planned trajectory.

[0008] 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, 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, the stopping trajectory being 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 will 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] 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.

[0010] FIG. 1 is a diagram for explaining an outline of this embodiment. FIG. 2 is a diagram for explaining the prior art. FIG. 3 is a diagram for explaining an outline of this embodiment. FIG. 4 is a diagram for explaining the difference between GDWA and the control method of this embodiment. FIG. 5 is a diagram for explaining each time in this embodiment. FIG. 6 is a diagram for explaining the process of randomly generating the speed of the mobile robot at each time. FIG. 7 is a diagram for explaining a speed map. FIG. 8 is a block diagram showing a schematic configuration of the control system of this embodiment. FIG. 9 is a block diagram showing the hardware configuration of a control device according to this embodiment. FIG. 10 is a flowchart showing the flow of control processing in this embodiment. FIG. 11 is a diagram showing the results of this embodiment.

[0011] 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.

[0012] 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, the mobile robot 1 starts moving at time t=0.0 [s]. 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, such as that shown in the time t=2.0 [s] scene in FIG. 1 , is calculated at each time, and the mobile robot 1 is controlled to realize the planned trajectory.

[0013] FIG. 2 is a diagram for explaining the above-mentioned document 1. Note that, hereinafter, the technology disclosed in the above-mentioned 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 safely stop without colliding with an obstacle or the like.

[0014] 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 it is difficult to select an appropriate trajectory from the various planned trajectory candidates. For this reason, when controlling the mobile robot 1 using the GDWA, it may take a long time to reach the destination.

[0015] 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 times are sometimes expressed as "t, t+Δt, t+2Δt", in this embodiment, times are expressed as "t, t+1, t+2, ...".

[0016] FIG. 3 is a diagram for explaining an overview of this embodiment. As shown in FIG. 3, in this embodiment, a search is also made for a stopping trajectory from time t+1 to time t+T (shown by a solid black line in FIG. 3). Note that the candidate stopping trajectories include stopping trajectories that may result in collision with an obstacle or the like (e.g., NG in FIG. 3). Of the stopping trajectories shown in FIG. 3, 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, 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.

[0017] 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.

[0018] FIG. 4 is a diagram illustrating the difference between the GDWA and the control method of this embodiment. The diagram on the left side of FIG. 4 illustrates the GDWA control method, and the diagram on the right side of FIG. 4 illustrates the control method of this embodiment. As shown in FIG. 4, the GDWA control method 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 after 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.

[0019] 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. A specific description will be given below.

[0020] <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.

[0021]

[0022] Here, q(t) is a state variable at time t, and u(t) is a 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.

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

[0024]

[0025] U is a set of control inputs determined according to the upper and lower limits of acceleration and deceleration of the motor that drives the mobile robot 1. Ｆ 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. Ｆ (q ０ , t, T), the following two sets of control inputs U Ｇ (q ０ , t, T) and U Ｓ (q ０ , t, T) are defined.

[0026]

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

[0028] U Ｇ (q ０ , t, T) is a set of control inputs that allows the mobile robot 1 to reach the destination g at time t+T. Ｓ (q ０, 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.

[0029] Based on 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).

[0030]

[0031] Here, T Ｅ 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. Ｓ (>T Ｂ ) represents the time required for the mobile robot 1 to stop after a failure occurs in the control system of the mobile robot 1.

[0032] 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 Ｂ At time t+T, the planned orbit is switched to the stopping orbit. Ｂ is from time t to time t+T Ｅ The time is any time up to time t+T. Ｂ When the planned orbit is switched to the stopping orbit at time t+T Ｓ 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 Ｇ The vehicle reaches destination g.

[0033] The above formula (5) is the time T required to travel to the destination g. Ｇ The control input u(t) that minimizes is the candidate set U of the control input. Ｇ (q ０ , t, T). As mentioned above, U Ｇ (q ０, 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), Ｂ Control input u(t+T Ｂ ) is the candidate set of control inputs U Ｓ (q(t+T Ｂ ), t + T Ｂ , T Ｓ -T Ｂ ) must be selectable from. Ｓ (q(t+T Ｂ ), t + T Ｂ , T Ｓ -T Ｂ ) is at time t+T Ｓ It is a set of control inputs that can safely stop the vehicle without colliding with obstacles.

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

[0035] (B. Generation of 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 control input u(t) is determined taking into account the speed and angular velocity of the mobile robot 1.

[0036] 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.

[0037] 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.

[0038] 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.

[0039] 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.

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

[0041] 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 in accordance with 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, in accordance with 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.

[0042] 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.

[0043] 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.

[0044] 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.

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

[0046] 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.

[0047] In the GDWA described above, a planned trajectory is set on the assumption that the speed of the mobile robot 1 is constant. 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.

[0048] Specifically, it is assumed that, at a 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 or the like based on data acquired from a group of sensors mounted on the mobile robot 1. In this case, the maximum allowable speed v(x) of the mobile robot 1 is expressed by the following equation. ｍａｘis the upper limit of deceleration.

[0049] (6)

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

[0051] (7)

[0052] 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 travel at a high velocity. For example, a high velocity at which the mobile robot 1 can travel 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 velocity map shown in FIG. 7 is referenced to estimate the time required for the mobile robot 1 to travel from the end of the candidate planned trajectory to the destination g. Then, a planned trajectory that is estimated to require the mobile robot 1 to travel from the end of the candidate planned trajectory to the destination g in the shortest time is selected.

[0053] In this case, the time T Ｇ is expressed as the time φ(x; g) for the mobile robot 1 to reach the destination g from the position x and the time T Ｅ It is expressed as the sum of and.

[0054] (10)

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

[0056] 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.

[0057] <Control System 10> Fig. 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.

[0058] 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). The sensor group 12 then outputs the acquired various data to the control device 14.

[0059] 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 includes 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.

[0060] 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 programs from the storage device 46 and executes the programs using the memory 44 as a work area. The CPU 42 controls each component and performs various arithmetic processes in accordance with the programs stored in the storage device 46.

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

[0062] The input / output I / F 48 is an interface for inputting data from an external device and outputting data to an external device. 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 also be connected. A touch panel display may be used as the output device, allowing it to function as an input device.

[0063] 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.

[0064] 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).

[0065] 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 generation unit 18, which is an example of a trajectory generation unit, a stop trajectory generation unit 20, a selection 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.

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

[0067] The planned trajectory generation 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 generation unit 18 generates a plurality of first planned trajectory candidates, which are candidates for the planned trajectory of the mobile robot 1.

[0068] 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.

[0069] 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.

[0070] Specifically, as explained with reference to Figure 6 above, 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 these accelerations and angular accelerations.

[0071] The stopping trajectory generation 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 generation unit 18, and is a trajectory that is used to stop the mobile robot 1.

[0072] 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 most quickly, more specifically, a trajectory in which the negative acceleration of the motor 16 reaches its maximum value and the angular acceleration of the motor 16 becomes zero (straight ahead).

[0073] The selector 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 will not enter a predetermined area (e.g., 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, as described above, correspond to the predetermined area.

[0074] 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 predetermined area, while the stopping orbits from time t+2 to time t+5 are OK because they do not enter the predetermined area.

[0075] 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, etc. exists), so the first planned orbit candidate is set as the second planned orbit candidate.

[0076] 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.

[0077] 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.

[0078] 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. 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.

[0079] Then, the selector 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.

[0080] 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.

[0081] 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.

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

[0083] 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 in Fig. 10 is repeatedly executed every time a predetermined time (e.g., 0.1 seconds) has elapsed.

[0084] 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.

[0085] 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.

[0086] 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.

[0087] 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.

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

[0089] 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 when the mobile robot is stopped, and which starts from the position of the mobile robot on the first planned trajectory candidate at at least two or more future times. 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 will 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 generates not only a stopping trajectory at a future time t+1, which is one time from the current time t, but also a stopping trajectory from time t+1 to the end time t+T of the planned trajectory. Ｅ The control device according to the embodiment generates a stopping orbit at each time from time t+1 to the end time t+T. Ｅ A planned trajectory candidate is one in which at least one of the stopping trajectories from a point a to a point b 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.

[0090] Furthermore, when generating first planned trajectory candidates, the control device according to this embodiment randomly generates multiple accelerations and angular accelerations of 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.

[0091] 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 speed of the mobile robot decreases the closer the mobile robot is to the predetermined area, and increases the farther the mobile robot is from the predetermined area. This selects a planned trajectory that causes the mobile robot to travel farther from the predetermined area, thereby enabling the mobile robot to reach its destination in a shorter time.

[0092] 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.

[0093]

[0094] In Table 1 above, "Radius" represents the distance the 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.

[0095] 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 Ｓ is set to 3.5 [s], and "2.3-bounded (Med.: 6.9 [s])" corresponds to the above-mentioned T Ｓ 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.

[0096] 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.

[0097] In this simulation, the time required for a mobile robot to navigate 100 patterns, each with a different route from the starting point to the destination and different obstacle placements, was measured. The horizontal axis in Figure 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 Figure 11, the results of the method of this embodiment (represented as "3.5-bounded (Med.: 6.6 [s])" and "2.3-bounded (Med.: 6.9 [s])" in Figure 11) are superior to the results of the conventional GDWA (represented as "GDWA (Med.: 10.1 [s])" in Figure 11) and MPPI (represented as "MPPI (Med.: 13.6 [s])" in Figure 11).

[0098] 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.

[0099] 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.

[0100] For example, in the above embodiment, a case has been described in which a stopping trajectory is generated at each time after time t+1 for each of a plurality of first planned trajectory candidates, but this is not limitative. If stopping trajectories are to be generated for 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.

[0101] Furthermore, various processors other than the CPU may execute the processes executed by loading software (programs) in the above embodiments. Examples of processors in this case include programmable logic devices (PLDs) (such as field-programmable gate arrays (FPGAs)) whose circuit configuration can be changed after manufacture, and dedicated electrical circuits, such as application-specific integrated circuits (ASICs), which are processors having circuit configurations designed specifically 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 elements.

[0102] In the above embodiment, the programs are pre-stored (installed) in a storage device, but this is not limiting. The programs may be provided in a form stored in 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.

[0103] (Supplementary Notes) The following supplementary notes are provided regarding aspects of the present disclosure.

[0104] (Supplementary Note 1) 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 which is a trajectory when the mobile robot is stopped, and which has as its starting point 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 mobile robot 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. (Supplementary Note 2) The control device according to Supplementary Note 1, wherein, 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 accelerations and angular accelerations for each of the generated plurality of accelerations and angular accelerations. (Supplementary Note 3) The control device according to Supplementary Note 1 or Supplementary Note 2, wherein the selection unit selects, from 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. (Supplementary Note 4) The control device according to Supplementary Note 3, wherein, when calculating the estimated time, the selection unit calculates the estimated time so that the speed of the mobile robot becomes slower the closer the mobile robot is to the predetermined area and so that the speed of the mobile robot becomes faster the farther the mobile robot is from the predetermined area.(Supplementary Note 5) A control method in which a computer executes the following processes: generating a plurality of first trajectory candidates that are candidates for a planned trajectory of a mobile robot; for each of the plurality of first trajectory candidates, generating 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; 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. (Supplementary Note 6) A control program for causing a computer to execute the following processes: 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, the stopping trajectory starting 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.

[0105] The disclosure of Japanese Patent Application No. 2024-057926, filed on March 29, 2024, is incorporated herein by reference in its entirety. All documents, patent applications, and technical standards mentioned herein are incorporated herein by reference to the same extent as if each individual document, patent application, and technical standard was specifically and individually indicated to be incorporated by reference.

Claims

1. 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 the mobile robot is stopped, and that starts 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 mobile robot will not enter a specified 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.

2. The control device described in claim 1, wherein, when generating the first trajectory candidate, the trajectory generation unit randomly generates multiple 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 multiple accelerations and angular accelerations generated.

3. The control device according to claim 1 or claim 2, wherein 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 the end of the trajectory represented by the second trajectory candidate to the destination of the mobile robot, as the planned trajectory of the mobile robot.

4. The control device described in claim 3, wherein the selection unit calculates the estimated time such that the speed of the mobile robot decreases the closer the mobile robot is to the specified area, and increases the further the mobile robot is from the specified area.

5. A control method in which a computer executes the following processes: generating a plurality of first trajectory candidates that are candidates for a planned trajectory of a mobile robot; for each of the plurality of first trajectory candidates, generating 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; for each of the plurality of first trajectory candidates, if at least one stopping trajectory of the stopping trajectories at at least two or more future times indicates that the mobile robot will not enter a specified 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.

6. A control program for causing a computer to execute the following processes: 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, the stopping trajectory having as its starting point 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.

Images