Route setting method, movement control method, program, route setting device, and mobile object

Abstract

To appropriately set a route in consideration of dynamic characteristics of a moving object.SOLUTION: A route setting method includes the steps of: acquiring moving source information indicating a position of a moving source and a direction of a moving object at the moving source; selecting a motion primitive calculated based on a plurality of equations of motion indicating dynamic characteristics of the moving object based on the moving source; and setting a route of the moving object from a departure position to an arrival position by combining the motion primitives.SELECTED DRAWING: Figure 4

Description

[Technical Field]

[0001] The present disclosure relates to a route setting method, a movement control method, a program, a route setting device, and a moving object. [Background technology]

[0002] There are known techniques for automatically setting routes for moving bodies such as ships, underwater vehicles, aircraft, etc. For example, Patent Document 1 describes a technique for calculating a planned route based on the position and heading of a ship. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent Publication No. 2021-181301 Summary of the Invention [Problem to be solved by the invention]

[0004] In the technology described in Patent Document 1, the dynamic characteristics of a moving object are not taken into consideration when acquiring or calculating a planned route. When automatically setting a route for a moving object, it is required to appropriately set a route that takes into consideration the dynamic characteristics of the moving object.

[0005] The present disclosure is intended to solve the above-mentioned problems, and aims to provide a route setting method, a movement control method, a program, a route setting device, and a moving body that can appropriately set a route that takes into account the dynamic characteristics of the moving body. [Means for solving the problem]

[0006] The route setting method of the present disclosure includes the steps of acquiring source information indicating the source position and the orientation of the moving body at the source position, selecting motion primitives calculated based on a plurality of equations of motion indicating the dynamic characteristics of the moving body based on the source position, and combining the motion primitives to set a route for the moving body from a departure position to an arrival position.

[0007] The movement control method according to the present disclosure includes a step of moving the moving object by a control input based on the route set by the route setting method.

[0008] The program disclosed herein causes a computer to perform the steps of acquiring source information indicating the source position and the orientation of the moving body at the source position, selecting motion primitives calculated based on a plurality of equations of motion indicating the dynamic characteristics of the moving body based on the source position, and combining the motion primitives to set a path for the moving body from the departure position to the arrival position.

[0009] The route setting device according to the present disclosure includes a source information acquisition unit that acquires source information indicating the source position and the orientation of a moving body at the source, a motion primitive selection unit that selects, based on the source, motion primitives calculated based on a plurality of equations of motion that indicate the dynamic characteristics of the moving body, and a first route setting unit that combines the motion primitives to set a route for the moving body from a departure position to an arrival position.

[0010] A mobile body according to the present disclosure includes the route setting device. [Effects of the Invention]

[0011] According to the present disclosure, it is possible to appropriately set a route that takes into account the dynamic characteristics of a moving object. [Brief explanation of the drawings]

[0012] [Figure 1]FIG. 1 is a schematic diagram of a moving body according to this embodiment. [Figure 2] FIG. 2 is a schematic block diagram of the control device according to this embodiment. [Figure 3] FIG. 3 is a schematic diagram showing an example of calculation of a motion primitive. [Figure 4] FIG. 4 is a schematic diagram for explaining the setting of a route. [Figure 5A] FIG. 5A is a schematic diagram illustrating an example of setting a determination criterion based on the direction of travel of a motion primitive. [Figure 5B] FIG. 5B is a schematic diagram showing an example of distance thresholds in each direction. [Figure 6] FIG. 6 is a flowchart illustrating the process flow of the route setting method. DETAILED DESCRIPTION OF THE INVENTION

[0013] Preferred embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. Note that the present disclosure is not limited to these embodiments, and when there are multiple embodiments, the present disclosure also includes configurations in which the respective embodiments are combined.

[0014] (First embodiment) (Mobile) FIG. 1 is a schematic diagram of a moving body according to this embodiment. The moving body 10 according to this embodiment is an autonomously moving device. However, the present invention is not limited to this, and the moving body 10 may also be one that moves by operation of a driver. Furthermore, the moving body 10 may be a moving body used in any environment, but is preferably used in an environment in which the orientation of the moving body 10 from which it moves affects the movement of the moving body 10. In this embodiment, the moving body 10 is a ship that moves on water W (on the water surface), but is not limited to this, and may be, for example, an underwater vehicle that moves within water W (underwater) or an aircraft that flies.

[0015] 1, a moving object 10 has a control target 12 and a control device 14. The control target 12 is a device controlled by the control device 14, such as a drive mechanism that drives the moving object 10.

[0016] Hereinafter, one direction along a horizontal plane is referred to as the X direction, and a direction along the horizontal plane that is perpendicular to the X direction is referred to as the Y direction. In this embodiment, unless otherwise specified, the term "position" refers to a position (coordinate) in a two-dimensional coordinate system of the X direction and the Y direction. Furthermore, unless otherwise specified, the term "orientation" of the moving body 10 refers to the orientation of the moving body 10 in a two-dimensional coordinate system of the X direction and the Y direction, and refers to, for example, the yaw angle (rotation angle) of the moving body 10 when viewed from the vertical direction, with the X direction being 0°. Note that, for example, if the moving body 10 is an aircraft, the "position" may refer to a position (coordinate) in a three-dimensional coordinate system of the X direction, the Y direction, and the vertical direction, and the "orientation" may refer to an orientation in any direction in the X direction, the Y direction, and the vertical direction.

[0017] (Control device) 2 is a schematic block diagram of a control device according to this embodiment. The control device 14 is, for example, a computer, and as shown in FIG. 2, has an input unit 20, a display unit 22, a storage unit 24, and a control unit 26. The control device 14 may be configured as a standalone device, may be configured integrally with other devices, or may be configured as a system combining various devices such as a calculation device and a data server, and is not particularly limited. The moving body according to this embodiment acquires information from the motion primitive creation device 11.

[0018] First, the motion primitive creation device 11 will be described. The motion primitive creation device 11 is a computing device that performs various calculations using a CPU or a GPU. The motion primitive creation device 11 calculates a motion primitive MP based on an equation of motion of the moving body 10. The equation of motion of the moving body 10 is a formula that indicates the dynamic characteristics of the moving body 10. In this embodiment, the equation of motion of the moving body 10 is a mathematical formula that takes a control input (e.g., an operation amount such as a steering angle or an applied torque) to the moving body 10 at the source of movement as an input value and a control output (e.g., the position and orientation of the moving body 10) when that control input is made as an output value. The equation of motion of the moving body 10 is set in advance based on, for example, the performance, conditions, etc. of the moving body 10. When the moving body 10 moves in a constant motion (e.g., the moving body 10 moves forward at a constant speed), one equation of motion corresponding to that motion is set. Furthermore, when the motion of the moving body 10 changes (the moving body 10 moves forward and then stops), the dynamic characteristics of the moving body 10 also change. In this case, multiple equations of motion for the moving body 10 are set according to the magnitude of the operation amount. For example, multiple equations of motion for the moving body 10 are set for each of the cases where the moving body 10 moves forward and where the moving body 10 decelerates and stops. Multiple equations of motion for the moving body 10 may be set according to the state quantities.

[0019] Fig. 3 is a schematic diagram showing an example of calculation of a motion primitive. In the example of this embodiment, multiple sets of destination information (destination and orientation of the moving body 10) are set for one origin, so the motion primitive creating device 11 calculates a motion primitive MP for each piece of destination information. In other words, the motion primitive creating device 11 calculates multiple motion primitives MP from one origin. The diagram on the left side of Fig. 3 shows an example of a motion primitive MP when the origin is lattice point N1 and the orientation of the moving body 10 at the origin is the Y direction. In the example on the left side of Figure 3, the following motion primitives are calculated: a motion primitive MP1 whose destination is lattice point N2B and whose orientation at the destination is the Y direction; a motion primitive MP2 whose destination is lattice point N2C and whose orientation at the destination is the Y direction; a motion primitive MP3 whose destination is lattice point N2C and whose orientation at the destination is shifted 45 degrees from the Y direction; a motion primitive MP4 whose destination is lattice point N2C and whose orientation at the destination is the X direction; a motion primitive MP5 whose destination is lattice point N2A and whose orientation at the destination is the Y direction; a motion primitive MP6 whose destination is lattice point N2A and whose orientation at the destination is shifted 45 degrees from the Y direction; and a motion primitive MP7 whose destination is lattice point N2A and whose orientation at the destination is the X direction. 3 shows an example of a motion primitive MP when the origin is lattice point N1 and the orientation of the moving object 10 at the origin is shifted 45 degrees from the Y direction. In the example of the right diagram of FIG. 3, the following motion primitives are calculated: a motion primitive MP8 whose destination is lattice point N2C and whose orientation at the destination is shifted 45 degrees from the Y direction; a motion primitive MP9 whose destination is lattice point N2C and whose orientation at the destination is the X direction; and a motion primitive MP10 whose destination is lattice point N2C and whose orientation at the destination is the Y direction. Note that the motion primitives shown in FIG. 3 are merely examples.

[0020] More specifically, the motion primitive creation device 11 calculates a motion primitive MP suitable for the motion of the moving object 10. The motion primitive creation device 11 calculates the motion primitive MP using an optimization calculation that minimizes the evaluation function. In other words, the motion primitive MP is a solution to the optimization calculation, and can be said to be a trajectory T of the state quantities of the moving object 10 that reflects the equation of motion. The motion primitive creation device 11 calculates the motion primitive MP using an optimization calculation that minimizes the evaluation function for each condition of the moving object 10. Examples of the conditions here include the position and speed of the moving object 10, the direction of rotation (forward / reverse) and number of rotations of a drive unit (e.g., a propeller), and the motion of the moving object 10 (forward movement, turning, stopping of the moving object 10, and combinations thereof). The conditions under which the motion primitive creation device 11 calculates the motion primitive MP are not limited to the above examples and may be any conditions.

[0021] The motion primitive MP is calculated in accordance with the motion of the moving body 10. That is, in one motion primitive MP, the moving body 10 may perform a constant motion, or the motion of the moving body 10 may change. When the moving body 10 performs a constant motion in one motion primitive, the motion primitive creation device 11 calculates the motion primitive MP using one equation of motion. When the motion of the moving body 10 changes in one motion primitive (when the dynamic characteristics change), the motion primitive creation device 11 calculates the motion primitive MP using multiple equations of motion. The motion primitive creation device 11 calculates the motion primitive MP by switching the equation of motion used in the optimization calculation depending on the magnitude of the operation amount.

[0022] For example, the propeller fluid moment Np is an example of a dynamic characteristic that changes in one motion primitive MP of the moving body 10. The propeller fluid moment Np is expressed by the following formula (1) using multiple equations of motion depending on the value of the propeller rotation speed np.

number

[0023] When the moving body 10 is moving forward, the motion primitive creation device 11 determines through optimization calculation that the propeller rotation speed np is greater than 0, and calculates the propeller fluid moment Np using the upper equation of equation (1) to calculate the motion primitive MP. On the other hand, when the moving body 10 is decelerating or stopped, the motion primitive creation device 11 determines through optimization calculation that the propeller rotation speed np is 0 or less, and calculates the propeller fluid moment Np using the lower equation of equation (1) to calculate the motion primitive MP. The changing dynamic characteristics of the moving body 10 are not limited to the propeller fluid moment Np, and may be other dynamic characteristics.

[0024] The motion primitive generating device 11 inverts the calculated motion primitive MP. The motion primitive generating device 11 inverts the motion primitive MP so that it corresponds to the four quadrants, from the first quadrant to the fourth quadrant. This makes it possible to generate a larger number of motion primitive MPs.

[0025] Next, each part of the control device 14 will be described. The input unit 20 is a mechanism that accepts operations of the moving object 10 by, for example, the driver. The display unit 22 is a display device such as a monitor, and displays information about the route R set by the control device 14. The storage unit 24 is a memory that stores the calculation contents and program information of the control unit 26, and includes at least one of, for example, a random access memory (RAM), a main storage device such as a read only memory (ROM), and an external storage device such as an HDD (hard disk drive). The storage unit 24 stores a plurality of motion primitives MP created by the motion primitive creating device 11.

[0026] The control unit 26 is a computing device and includes a computing circuit such as a CPU (Central Processing Unit). The control unit 26 includes a source information acquisition unit 32, a destination setting unit 34, a motion primitive selection unit 36, a first path setting unit 38, and a movement control unit 40. The control unit 26 implements the source information acquisition unit 32, the destination setting unit 34, the motion primitive selection unit 36, the first path setting unit 38, and the movement control unit 40 by reading and executing a program (software) from the storage unit 24. The control unit 26 may implement the source information acquisition unit 32, the destination setting unit 34, the motion primitive selection unit 36, the first path setting unit 38, and the movement control unit 40 and executes their processing. The control unit 26 may implement the processing using a single CPU or may include multiple CPUs and execute the processing using the multiple CPUs. At least some of the source information acquisition unit 32, the destination setting unit 34, the motion primitive selection unit 36, the first path setting unit 38, and the movement control unit 40 may be implemented using hardware circuits. The program for the control unit 26 stored in the storage unit 24 may be stored in a recording medium readable by the control device 14.

[0027] In this embodiment, the source information acquisition unit 32, the destination setting unit 34, the motion primitive selection unit 36, and the first path setting unit 38 set a path R for the moving body 10, which will be described later. Therefore, the source information acquisition unit 32, the destination setting unit 34, the motion primitive selection unit 36, and the first path setting unit 38 constitute a path setting unit 30 that sets the path R for the moving body 10. In this embodiment, the control device 16 functions as a path setting device that includes the path setting unit 30 and sets the path R, and also functions as a control device that includes the movement control unit 40 and controls the movement of the moving body 10. However, this is not limited thereto, and the path setting device that includes the path setting unit 30 and sets the path R and the control device that includes the movement control unit 40 and controls the movement of the moving body 10 may be separate devices. When the path setting device and the control device are separate devices, the path setting device may or may not be provided in the moving body 10.

[0028] (route) FIG. 4 is a schematic diagram for explaining route setting. As shown in FIG. 4, the control device 16 sets a route R from a departure position S to an arrival position G. In the example of FIG. 4, the departure position S is a position on the open sea side of the port facility, and the arrival position G is a docking position provided within the port facility. In other words, in the example of FIG. 4, the route R can be said to be a route for docking the mobile body 10. However, this is not limited to this, and the departure position S and the arrival position G may be any positions. The departure position S and the arrival position G may be set by the driver, for example.

[0029] The control device 16 acquires multiple motion primitives MP, selects motion primitives MP to be used from the acquired multiple motion primitives MP, and connects the selected multiple motion primitives MP to set a route R from a starting position S to a destination position G. In this embodiment, the motion primitive MP is a pre-calculated path of movement of the moving object 10 in an area of a predetermined size. The motion primitive MP is path information calculated based on the state quantities of the moving object 10 at the source and destination using an optimization calculation that minimizes an evaluation function. Specifically, the motion primitive MP includes an operation amount for bringing the moving object 10 to a desired state quantity and a state quantity at the destination. The motion primitive MP is a solution that minimizes a predetermined evaluation function under the conditions (boundary conditions) of the desired state quantities of the moving object 10 at the source and destination in an area of a predetermined size. The motion primitive MP is data indicating a trajectory T of the moving object 10 from the source to the destination in an area of a predetermined size. A specific method for calculating the motion primitive MP will be described later. Furthermore, the state quantity refers to the position, angle, speed, etc. of the moving body 10 at the origin and destination, and the operation quantity refers to the steering angle of the moving body 10 and the applied torque, etc., in the state quantity. Furthermore, in this embodiment, the evaluation function may be any evaluation function, but is preferably a function that takes into consideration the deviation between the state quantity and the desired state quantity, the operation quantity, the gradient of the operation quantity, and the time required for control.

[0030] (Movement source information acquisition unit) The source information acquisition unit 32 acquires source information to be used for setting the route R. The source information is information indicating the position (coordinates) of the source and the orientation of the moving object 10 at the source. The source indicates the current position of the moving object 10.

[0031] The origin information acquisition unit 32 acquires the position of the departure position S and the orientation of the moving body 10 at the departure position S as origin information. The orientation of the moving body 10 at the departure position S may be set as appropriate, and for example, if the current position of the moving body 10 is on the departure position S, the orientation of the moving body 10 at the current position may be set as the orientation of the moving body 10 at the departure position S. Also, for example, if the current position of the moving body 10 is not on the departure position S, a predicted value of the orientation of the moving body 10 when it arrives at the departure position S may be set as the orientation of the moving body 10 at the departure position S.

[0032] (Destination setting section) The destination setting unit 34 sets destination information to be used for setting the route R based on the source information. The destination information is information indicating the position (coordinates) of the destination and the orientation of the moving body 10 at the destination. The destination refers to an intermediate position of the moving body 10. The destination setting unit 34 sets, as the destination, a position that is closer to the arrival position G than the source and has a predetermined relationship with the source. In this embodiment, the destination setting unit 34 sets, as the destination, a position that satisfies at least one of being away from the source by a predetermined unit distance in the X direction or being away from the source by a predetermined unit distance in the Y direction and is closer to the arrival position G than the source. In addition, the destination setting unit 34 sets, as the orientation of the moving body 10 at the destination, an orientation that has a predetermined relationship with the orientation of the moving body 10 at the source. In this embodiment, the destination setting unit 34 sets the orientation of the moving body 10 at the destination to an orientation that is shifted by a predetermined unit angle from the orientation of the moving body 10 at the origin.

[0033] 4, the destination setting unit 34 discretizes the state space to be searched for as the destination in three dimensions, that is, the X direction, the Y direction, and the yaw angle, and in the discretized state space, continuously sets the destination and the orientation of the moving body 10 at the destination up to the arrival position G. The destination setting unit 34 selects, for example, a lattice point N adjacent to the source lattice point N and the orientation of the moving body 10 at that lattice point N for a portion up to the arrival position G, and sets it as the destination and the orientation of the moving body 10 at the destination.

[0034] More specifically, in this embodiment, the destination setting unit 34 sets, as the destination, a location that has a predetermined relationship with the source of movement and that does not interfere with the obstacle M. The criteria for determining whether the obstacle M and the moving body 10 will interfere with each other may be arbitrary. For example, the destination setting unit 34 may determine that a location outside a predetermined distance from the position of the obstacle M will not interfere with the obstacle M, and a location within a predetermined distance from the position of the obstacle M will interfere with the obstacle M. In this case, the destination setting unit 34 obtains, for example, map information indicating the position of the obstacle M, to determine the position of the obstacle M. The predetermined distance here may be set arbitrarily.

[0035] In this embodiment, for one origin, the destination setting unit 34 sets a plurality of destinations with different positions and a direction for each destination of the moving body 10. That is, for one origin, the destination setting unit 34 sets a plurality of sets of destination information, each set indicating the position of the destination and the direction of the moving body 10.

[0036] (Motion Primitive Selection) The motion primitive selection unit 36 acquires multiple motion primitives MP stored in the storage unit 24. Based on the origin of movement, the motion primitive selection unit 36 selects a motion primitive MP calculated based on multiple equations of motion that indicate the dynamic characteristics of the moving object 10. Furthermore, based on the origin information and destination information, the motion primitive selection unit 36 selects a combination of motion primitive MPs along which the moving object 10 moves from the origin of movement to the destination of movement. The motion primitive selection unit 36 selects a combination of motion primitive MPs such that the moving object 10 is oriented as indicated by the origin information at the origin of movement and the moving object 10 is oriented as indicated by the destination information at the destination of movement, and that the combination of motion primitive MPs satisfies a trajectory that starts from the origin of movement and ends at the destination of movement. Here, the motion primitive selection unit 36 repeats the selection of motion primitive MPs until the destination position of the motion primitive MP reaches the arrival position G. The motion primitive selection unit 36 determines whether the trajectory T indicated in the motion primitive MP interferes with an obstacle M, and selects the motion primitive MP that is determined not to interfere with the obstacle M as the motion primitive MP for setting the path.

[0037] An example of a method for selecting a motion primitive is described below. FIG. 5A is a schematic diagram illustrating an example of setting a determination criterion based on the traveling direction of a motion primitive. The motion primitive selection unit 36 may set a determination criterion based on the traveling direction of the motion primitive MP and determine whether the trajectory T indicated by the motion primitive MP interferes with an obstacle based on the determination criterion. Specifically, a distance threshold in a first direction perpendicular to the traveling direction of the motion primitive MP is set as distance threshold D1, and a distance threshold in a second direction opposite to the first direction is set as distance threshold D2. The distance threshold is a threshold for determining whether an obstacle M and a motion primitive MP interfere with each other. If the distance between the obstacle M and the motion primitive MP is equal to or less than the distance threshold, it is determined that interference occurs. In this case, the motion primitive selection unit 36 determines whether the traveling direction of the motion primitive MP intersects with the orientation of the moving object 10 at the start of movement. 5A, when the traveling direction of the motion primitive MPA does not intersect with the orientation of the moving body 10 at the origin (position NA) (when the orientation of the moving body 10 is along the traveling direction of the motion primitive MP), for example, the motion primitive selection unit 36 sets the distance threshold D1 and the distance threshold D2 to the same value. On the other hand, when the traveling direction of the motion primitive MP intersects with the orientation of the moving body 10 at the origin, the motion primitive selection unit 36 sets the distance threshold D1 and the distance threshold D2 to different values. Specifically, as shown in FIG. 5A, when the traveling direction of the motion primitive MPB is oriented toward the first direction side (to the right in the example of FIG. 5A) with respect to the orientation of the moving body 10 at the origin (position NB), the motion primitive selection unit 36 sets the distance threshold D2 to be longer than the distance threshold D1. Furthermore, when the moving direction of the motion primitive MP faces the second direction side with respect to the orientation of the moving object 10 at the movement origin, the motion primitive selection unit 36 selects a motion primitive for which the distance threshold D1 is longer than the distance threshold D2.That is, when the moving body 10 turns, there is a possibility that it will bulge in the opposite direction of the turning direction, so the motion primitive selection unit 36 sets the distance threshold in the opposite direction of the turning direction to a long value and selects a motion primitive that suppresses interference with the obstacle M even if it bulges in the opposite direction of the turning direction.

[0038] In the example of FIG. 5A , the motion primitive selection unit 36 sets distance thresholds D1 and D2 in a first direction and a second direction (left-right direction) perpendicular to the traveling direction of the motion primitive MP. However, this is not limiting. For example, as shown in FIG. 5B , the distance thresholds in each direction may be set for each direction. FIG. 5B is a schematic diagram showing an example of the distance thresholds in each direction. In this case, for example, as shown in FIG. 5B , the motion primitive selection unit 36 may set a distance threshold in each direction for each direction and set an area enclosed by points that are the distance threshold away from the moving object 10 in each direction as a determination area AR for determining whether or not the moving object 10 will interfere with the obstacle M. That is, in this case, if the obstacle M is located within the determination area AR, it is determined that the obstacle M will interfere with the obstacle M, and if the obstacle M is located outside the determination area AR, it is determined that the obstacle M will not interfere with the obstacle M. Note that any method for setting the distance thresholds in each direction may be used. For example, the motion primitive selection unit 36 may set the distance thresholds D1 and D2 in the left-right direction in the same manner as described for FIG. 5A . For example, the motion primitive selection unit 36 may select a motion primitive in which the distance threshold D3 in the third direction (forward direction) along the traveling direction of the motion primitive MP is longer than the distance threshold D4 in the third direction (backward direction) opposite to the traveling direction of the motion primitive MP. This makes it possible to prevent interference with an obstacle M, for example, even if the moving object 10 moves further in the traveling direction than expected. The motion primitive selection unit 36 may also set the determination area AR based on the shape of the moving object 10. For example, the motion primitive selection unit 36 sets an area that is a set distance threshold away from the outer edge of the moving object 10 in each direction as the determination area AR. Therefore, for example, if the distance threshold for each direction is constant and the moving object 10 is long in the forward / backward direction, the determination area AR will have a shape that is long in the forward / backward direction.

[0039] (First path setting unit) The first path setting unit 38 combines the motion primitives MP selected by the motion primitive selection unit 36 to set a path R from the departure position S to the arrival position G. The first path setting unit 38 connects the trajectories T indicated in the respective motion primitives MP acquired by the motion primitive selection unit 36 to set the path R from the departure position S to the arrival position G.

[0040] In this embodiment, the first path setting unit 38 evaluates the motion primitives MP calculated by the motion primitive selection unit 36 and determines a combination of motion primitives MP to be set on the path R. In other words, the first path setting unit 38 evaluates the motion primitives MP calculated by the motion primitive selection unit 36 and connects the motion primitives MP to be set on the path R from the departure position S to the arrival position G. Any method may be used to connect the motion primitives MP to be set on the path R from among the multiple motion primitives MP, but for example, the first path setting unit 38 connects the motion primitives MP so as to form a path R along which the moving object 10 can travel efficiently.

[0041] For example, the first path setting unit 38 may determine, as the motion primitive MP to be set on the path R, a motion primitive MP that consumes the least amount of energy to move to the destination from among multiple motion primitive MPs set from the same origin. In this embodiment, when multiple motion primitive MPs that do not interfere with an obstacle M are present among the multiple motion primitive MPs set from the same origin, the first path setting unit 38 determines, as the motion primitive MP to be set on the path R, a motion primitive MP that consumes the least amount of energy from among the multiple motion primitive MPs. The first path setting unit 38 may calculate the energy consumption of each motion primitive MP based on each motion primitive MP, and determine the motion primitive MP that consumes the least amount of energy. Note that the energy consumption is an estimated value of the amount of energy consumed when moving from the origin to the destination. For example, the first path setting unit 38 may calculate the energy consumption as the time integral value of the energy consumed by the actuator to move the motion primitive MP. Furthermore, for example, the first path setting unit 38 may determine, among the motion primitives MP that do not interfere with the obstacle M, the motion primitive MP that requires the shortest time to move to the destination as the motion primitive MP to be set on the path R. The required time may be calculated arbitrarily for each motion primitive MP, and may be calculated based on, for example, the length of the motion primitive MP, or may be calculated based on the amount of change in the steering angle operated to move the motion primitive MP.

[0042] The first route setting unit 38 outputs information about the set route R. For example, the first route setting unit 38 may display the route R on the display unit 22, or may transmit information about the set route R to another device.

[0043] As described above, the first path setting unit 38 determines a combination of motion primitives MP to be set on path R from the acquired motion primitives MP. The motion primitive selection unit 36 first sets a motion primitive MP with the start position S as the origin, and repeatedly acquires motion primitive MPs with the end point (the intermediate position of the moving object 10) of the previously selected motion primitive MP as the origin, until the destination reaches the arrival position G. Once the motion primitives MP are acquired, the first path setting unit 38 determines motion primitives MP to be used on path R from those motion primitives MP, and connects each of the determined motion primitives MP to set as path R.

[0044] The path setting unit 30 may correct the path connecting the selected motion primitives MP to set the path R. For example, the path setting unit 30 may relax the path connecting the selected motion primitives MP and set the path R as the path. Relaxation here refers to a process of reducing the change in the direction of the path. The relaxed path R passes through the departure position S and the arrival position G and has a smaller amount of change in the direction of the path than the path connecting the selected motion primitives MP. For example, the path setting unit 30 may perform optimization calculations again on the path connecting the selected motion primitives MP to set the relaxed path R.

[0045] (Processing flow) The processing flow of the method for setting the route R explained above will now be described. FIG. 6 is a flowchart illustrating the processing flow of the method for setting the route. The control device 14 acquires motion primitives MP from the motion primitive generating device 11 and stores them in a storage unit. The control device 14 acquires, via the source information acquiring unit 32, source information indicating the source position and the orientation of the moving object 10 at the source (step S10). The source information acquiring unit 32 acquires the destination information set immediately before as the source information. If the source has reached the arrival position G (step S12; Yes), in other words, if the destination set immediately before has reached the arrival position G, the control device 14 sets the route R based on the motion primitives MP selected up to that point, and ends this processing. On the other hand, if the origin has not reached the arrival position G (step S12; No), in other words, if the previously set destination has not reached the arrival position G, the control device 14 sets destination information indicating the destination position and the orientation of the moving object 10 at the destination based on the acquired origin information using the destination setting unit 34 (step S14), and selects motion primitives MP from the origin to the destination using the motion primitive selection unit 36 (step S16). When the control device 14 has completed acquisition of all motion primitives MP to be used for the path, the first path setting unit 38 determines a combination of motion primitives MP to be set for the path R (step S20), and returns to step S10. The first path setting unit 38 determines a combination of multiple motion primitives MP. For example, the first path setting unit 38 may determine a combination of motion primitives MP that minimizes energy consumption among motion primitives MP that do not interfere with obstacle M as the combination of motion primitives MP to be set for the path R. In addition, the first path setting unit 38 may select, for example, from among the motion primitives MP that do not interfere with the obstacle M, the motion primitives MP that require the shortest time to travel to the destination as the combination of motion primitives MP to be set on the path R.

[0046] As described above, in this embodiment, a motion primitive MP is acquired based on the origin and the orientation of the moving body 10 at the origin, and the destination and the orientation of the moving body 10 at the destination, and a route R is set based on the motion primitive MP. That is, in this embodiment, a motion primitive MP is acquired that takes into account the dynamic characteristics of the moving body 10 based on the origin and the destination, which are set by three-dimensionally discretizing the X direction, Y direction, and yaw angle. Furthermore, the motion primitive MP is calculated taking into account the dynamic characteristics of the moving body 10. Therefore, according to this embodiment, a route R along which the moving body 10 can travel can be automatically and appropriately set. In particular, by taking the dynamic characteristics of the moving body 10 into consideration when calculating the motion primitive MP, a route R that accurately reproduces the actual behavior of the moving body 10 can be set. In other words, an operation amount for the moving body 10 that is close to the actual behavior can also be obtained. In particular, the method of this embodiment can appropriately set a route R for docking from outside a port into a port. Furthermore, in this embodiment, multiple destinations are set for one origin, and a motion primitive MP to each destination is selected from the motion primitives MP to be used to set the route R. Therefore, according to this embodiment, the route R of the moving body 10 can be appropriately set in consideration of the dynamic characteristics of the moving body 10.

[0047] (Movement control unit) The movement control unit 40 controls the control target 12 of the moving body 10 to move the moving body 10. In this embodiment, the movement control unit 40 moves the moving body 10 according to the path R of the moving body 10 set by the first path setting unit 38. In this case, for example, the motion primitive selection unit 36 also calculates control inputs for realizing the acquired motion primitives MP. The movement control unit 40 moves the moving body 10 by controlling the control target 12 based on the control inputs set for each motion primitive MP of the path R.

[0048] In this case, the movement control unit 40 may appropriately measure the amount of deviation between the position of the moving body 10 and the path R, and control the control target 12 based on the control input set for each motion primitive MP and the amount of deviation between the position of the moving body 10 and the path R, i.e., by feedback control. Specifically, the movement control unit 40 calculates a control input for returning the position of the moving body 10 to the path R based on the amount of deviation between the position of the moving body 10 and the path R, and controls the moving body 10 with the calculated control input.

[0049] Furthermore, in this embodiment, for example, if an event that affects the movement of the moving object 10 occurs while the moving object 10 is moving along the set route R, the route R may be updated by resetting the motion primitive MP. An example of such an event is a malfunction of the moving object 10. In this case, the control device 16 acquires information indicating the occurrence of the event and, upon acquiring the information, resets the route R to the arrival position G, for example, using the current position and orientation of the moving object 10 as the initial movement source information. In this case, for example, the control device 16 updates the equation of motion of the moving object 10 set in the motion primitive creation device 11 based on the information indicating the occurrence of the event, and resets the motion primitive MP and route R along which the moving object 10 can move in the route setting unit 30. In this way, even if an event such as a malfunction occurs in the moving object 10 and it becomes difficult to move along the current route R, updating the route R allows an appropriate route to the arrival position G to be set. Note that the update of the route R is not limited to the occurrence of an event that affects the movement of the moving object 10, and the update of the route R may be performed in any case.

[0050] (Second embodiment) In the first embodiment, a source and a destination are set, and motion primitives MP from the source to the destination are repeatedly acquired, and the selected motion primitives MP are connected to set the route R. On the other hand, the second embodiment differs from the first embodiment in that the moving body 10 is moved based on operations from the driver as well. In the second embodiment, explanations of parts of the configuration common to the first embodiment will be omitted.

[0051] In the second embodiment, the moving body 10 is moved based on a control input based on the route R and an operation from the driver of the moving body 10. For example, if a disturbance such as wind or waves occurs while the moving body 10 is sailing, the moving body 10 may be switched to a manual control mode. In the manual control mode, a control input is made in accordance with an operation by the driver, and the moving body 10 is moved by the movement control unit 40 based on the detected operation by the driver when a disturbance occurs.

[0052] In the second embodiment, a predicted path of the moving object 10 may be calculated based on the detected control input. Any method for calculating the predicted movement path may be used. For example, in this embodiment, when setting the path R, the control input that realizes the motion primitive MP (the control input corresponding to the motion primitive MP) is also set. Therefore, the control device 16 may calculate a trajectory T corresponding to the detected control input based on the correspondence between the motion primitive MP and the control input corresponding to the motion primitive MP, and use this as the predicted path.

[0053] Once the predicted route is calculated, the movement control unit 40 moves the moving object 10 based on an operation from the driver according to the predicted route. The movement control unit 40 may switch from manual control mode to automatic control mode and automatically control the moving object 10 to move according to the predicted route. The switch to automatic control mode may be performed automatically, or may be performed when the driver inputs an instruction to switch. When the driver inputs an instruction to switch, for example, the control device 16 displays the calculated predicted route on the display unit 22. This allows the driver to confirm whether the predicted route is the intended route.

[0054] In the second embodiment, similarly to the first embodiment, the path R is set so as to connect motion primitives acquired based on the equation of motion of the moving body 10, thereby shortening the braking distance and improving the tracking performance. In particular, in this embodiment, the driver operates the moving body 10 only when the degree of influence on the movement of the moving body 10 due to an external disturbance or the orientation of the moving body 10 at the origin of movement is large, which reduces the effort of the occupants and leads to manpower saving.

[0055] (effect) As described above, the route setting method according to the first aspect of the present disclosure includes the steps of acquiring source information indicating the source position and the orientation of the moving body 10 at the source, selecting, based on the source, motion primitives MP calculated based on a plurality of equations of motion indicating the dynamic characteristics of the moving body 10, and combining the motion primitives MP to set a route R for the moving body 10 from the departure position to the arrival position. Therefore, according to the present disclosure, it is possible to appropriately set a route R that takes into account the dynamic characteristics of the moving body 10.

[0056] A path setting method according to a second aspect of the present disclosure is the path setting method according to the first aspect, in which the motion primitives are calculated based on the state quantities of the source and destination using an optimization calculation that minimizes an evaluation function. According to the present disclosure, a motion primitive can be calculated for each state quantity of the moving object 10, allowing for appropriate path setting.

[0057] A path setting method according to a third aspect of the present disclosure is the path setting method according to the first or second aspect, in which, in the step of selecting a motion primitive MP, selection of a motion primitive is repeated until the destination position of the motion primitive MP reaches the arrival position G, and in the step of setting a path R, the path R is set by connecting the trajectories T indicated by each of the acquired motion primitives MP. According to the present disclosure, since the path R is set by connecting the trajectories T indicated by the motion primitive MP to the arrival position G, it is possible to appropriately set a path R along which the moving object 10 can move.

[0058] A route setting method according to a fourth aspect of the present disclosure is the route setting method according to the first or second aspect, wherein in the step of selecting a motion primitive MP, it is determined whether a trajectory T indicated by the motion primitive MP interferes with an obstacle M, and a motion primitive MP determined not to interfere with the obstacle M is acquired as a motion primitive MP for setting a route. According to the present disclosure, a route R that does not interfere with the obstacle M can be appropriately set.

[0059] A path setting method according to a fifth aspect of the present disclosure is a path setting method according to any one of the first to fourth aspects, wherein in the step of selecting a motion primitive MP, a judgment criterion is set for judging whether the motion primitive MP will interfere with an obstacle M based on the traveling direction of the motion primitive MP, and based on the judgment criterion, it is judged whether the motion primitive MP will interfere with the obstacle M. According to the present disclosure, because the judgment criterion is set based on the traveling direction, it is possible to judge with high accuracy whether there is a risk of interference in accordance with the movement of the moving object.

[0060] A route setting method according to a sixth aspect of the present disclosure is a route setting method according to any one of the first to fifth aspects, in which the route R is updated by resetting the motion primitives MP while the moving object 10 is moving based on the route R. According to the present disclosure, even when the moving object 10 breaks down or otherwise becomes difficult to move along the current route R, the route R can be updated to appropriately set a route to the arrival position G.

[0061] A path setting method according to a seventh aspect of the present disclosure is a path setting method according to any one of the first to sixth aspects, in which the moving body 10 is a ship moving on water or an underwater vehicle moving underwater. The present disclosure is particularly effective for path setting for ships and underwater vehicles. That is, the path setting direction of the present disclosure makes it possible to set a turning trajectory taking into account the complex motion of the moving body 10 in a fluid, such as being influenced by the speed and orientation of the moving body 10 rather than being determined solely by the steering angle, and therefore can be said to be particularly effective for path setting for ships and underwater vehicles. Furthermore, according to the present disclosure, motion elements (motion primitives) are calculated offline in advance and then pieced together. This makes it possible to reduce the computational load to a practical level while taking into account the complex motion of the moving body 10 in a fluid, such as being influenced by the speed and attitude angle rather than being determined solely by the steering angle, making the method of the present disclosure an effective solution.

[0062] A movement control method according to an eighth aspect of the present disclosure includes the steps of moving a moving body 10 using a control input based on a route R set by the route setting method according to any one of the first to seventh aspects, and acquiring an amount of deviation between the position of the moving body 10 and the route R. In the step of moving the moving body 10, a control input for the moving body is set based on the route R and the amount of deviation. According to the present disclosure, the moving body 10 is controlled by feedback control based on the amount of deviation between the position of the moving moving body 10 and the original route R. Therefore, according to the present disclosure, the moving body 10 can be moved appropriately.

[0063] A movement control method according to a ninth aspect of the present disclosure is the movement control method according to the eighth aspect, and in the step of moving the moving body 10, the moving body 10 is moved based on a control input and an operation from a driver of the moving body 10. According to the present disclosure, the driver operates the moving body 10 only when an external disturbance to the moving body 10 or the orientation of the moving body 10 at the origin of the movement has a large effect on the movement, which reduces the effort of the occupants and leads to manpower saving.

[0064] A program according to a tenth aspect of the present disclosure causes a computer to execute the steps of acquiring source information indicating the source position and the orientation of the moving body 10 at the source, selecting, based on the source, motion primitives MP calculated based on multiple equations of motion indicating the dynamic characteristics of the moving body 10, and combining the motion primitives MP to set a route R for the moving body 10 from a departure position S to an arrival position G. According to the present disclosure, the route R can be appropriately set taking into account the dynamic characteristics of the moving body 10.

[0065] A route setting device according to an eleventh aspect of the present disclosure includes a route information acquisition unit 32 that acquires route information indicating the route position and the orientation of the moving body 10 at the route origin, a motion primitive selection unit 36 that selects, based on the route origin, a motion primitive MP that is set based on a plurality of equations of motion that indicate the dynamic characteristics of the moving body 10, and a first route setting unit 38 that combines the motion primitive MP to set a route R for the moving body 10 from a departure position S to an arrival position G. According to the present disclosure, the route R can be appropriately set taking into account the dynamic characteristics of the moving body 10.

[0066] A moving body 10 according to a twelfth aspect of the present disclosure includes the route setting device according to the eleventh aspect. According to the present disclosure, the route R can be appropriately set taking into account the dynamic characteristics of the moving body 10.

[0067] Although the embodiments of the present disclosure have been described above, the embodiments are not limited to the contents of these embodiments. Furthermore, the above-described components include those that can be easily imagined by a person skilled in the art, those that are substantially the same, and those that are within the so-called equivalent range. Furthermore, the above-described components can be combined as appropriate. Furthermore, various omissions, substitutions, or modifications of the components can be made without departing from the spirit of the above-described embodiments. [Explanation of symbols]

[0068] 10 Mobile 11 Motion primitive creation device 12 Control Objects 14 Control device (routing device) 30 Route setting section 32 Source information acquisition unit 34 Destination setting section 36 Motion Primitive Selection 38 First route setting unit 40 Movement control unit MP Motion Primitives R pathway T locus

Claims

1. acquiring origin information indicating an origin position and an orientation of the moving body at the origin; selecting, based on the origin of movement, a motion primitive calculated based on a plurality of equations of motion that indicate dynamic characteristics of the moving object; combining the motion primitives to set a path for the moving object from a starting position to a destination position; Including, Routing methods.

2. 2. The path setting method according to claim 1, wherein the motion primitive is calculated based on state quantities of the source and destination using an optimization calculation that minimizes an evaluation function.

3. In the step of selecting the motion primitive, the selection of the motion primitive is repeated until the destination position of the motion primitive reaches the arrival position; 3. The path setting method according to claim 1, wherein in the step of setting the path, the path is set by connecting trajectories indicated by the acquired motion primitives.

4. 3. A path setting method according to claim 1, wherein in the step of selecting a motion primitive, it is determined whether the trajectory indicated by the motion primitive will interfere with an obstacle, and the motion primitive determined not to interfere with the obstacle is acquired as the motion primitive for setting the path.

5. 5. The path setting method according to claim 4, wherein in the step of selecting a motion primitive, a criterion is set for determining whether the motion primitive will interfere with an obstacle based on a direction of travel of the motion primitive, and whether the motion primitive will interfere with the obstacle is determined based on the criterion.

6. 3. The path setting method according to claim 1, further comprising: updating the path by reacquiring the motion primitives while the moving object is moving based on the path.

7. 3. The route setting method according to claim 1, wherein the moving body is a ship that moves on water or an underwater vehicle that moves underwater.

8. a step of moving the moving object by a control input based on the route set by the route setting method according to claim 1 or 2, Movement control methods.

9. The movement control method according to claim 8 , wherein in the step of moving the moving body, the moving body is moved based on the control input and an operation from a driver of the moving body.

10. acquiring origin information indicating an origin position and an orientation of the moving body at the origin; selecting, based on the origin of movement, a motion primitive calculated based on a plurality of equations of motion that indicate dynamic characteristics of the moving object; combining the motion primitives to set a path for the moving object from a starting position to a destination position; to the computer, program.

11. a source information acquisition unit that acquires source information indicating a source position and a direction of the moving body at the source; a motion primitive selection unit that selects, based on the origin of movement, a motion primitive calculated based on a plurality of equations of motion that indicate dynamic characteristics of the moving object; a first path setting unit that sets a path of the moving object from a departure position to an arrival position by combining the motion primitives; Including, Routing device.

12. A routing device comprising: Mobile object.

Images