JP7202834B2 - Movement plan calculation device and program - Google Patents

Description

The present invention relates to a movement plan calculation apparatus and program, and more particularly to a movement plan calculation apparatus and program for obtaining movement plans for a plurality of moving bodies.

In recent years, technology has been proposed to determine the flight paths of each drone so that multiple drones can smoothly fly in a coordinated manner without interfering with each other. For example, in Non-Patent Document 1, by determining an objective function that integrates velocity changes (or acceleration changes) in the flight path of each autonomous flying robot, and solving an optimization problem to minimize it, a plurality of autonomous flying robots discloses a technique for smoothly calculating a flight path without interfering with each other.

F. Augugliaro, A. P. Schoellig and R. D'Andrea, "Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach," 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura, 2012, pp. 1917-1922.7

However, as the number of autonomous flying robots increases and the flight space increases, the calculation cost required to calculate the flight route increases. it was difficult to

SUMMARY OF THE INVENTION Accordingly, it is an object of the present invention to provide a movement plan calculation apparatus and a program that can obtain movement plans that do not interfere with each other at a low computational cost.

In order to achieve the above object, a movement plan calculation apparatus according to the present invention is a movement plan calculation apparatus for obtaining movement plans that do not interfere with each other for a plurality of moving bodies moving in a three-dimensional space, the movement plan calculating apparatus comprising: Interference for determining an interfering moving object and an interfering position, which are said moving objects having an interference relationship with each other, based on a movement plan from a current position to a target position for each of the moving objects, based on the spatial proximity between said movement plans determination means; local space setting means for setting a local space including the interference position in the three-dimensional space; a calculation target selection means for selecting an intra-local correction moving body that is a target of calculation of the avoidance plan in which the intra-local correction moving bodies are not in an interference relationship with each other in the local space, for each of the intra-local correction moving bodies calculating and correcting the movement plan, and moving the movement plan of the interfering moving body other than the intra-local corrected moving body at least during the period in which the intra-local corrected moving body passes through the local space, and a movement plan correction means for correcting so as not to pass.

According to the movement plan calculation device according to the present invention, the interference determination means determines from the spatial closeness between the movement plans based on the movement plans from the current position to the target position for each of the plurality of moving bodies. An interfering moving object and an interfering position, which are the moving objects that interfere with each other, are obtained. Local space setting means sets a local space including the interference position in the three-dimensional space.

Then, the calculation target selecting means selects two or more of the interfering moving bodies as intra-local correcting moving bodies to be calculated for the avoidance plan in the local space. A movement plan modifying means calculates, for each of the intra-locally corrected moving bodies, the avoidance plan in which the intra-locally corrected moving bodies do not interfere with each other in the local space to correct the movement plan; The movement plans of the interfering moving bodies other than the corrected moving bodies are corrected so that they do not pass through the local space at least during the period in which the intra-local corrected moving bodies pass through the local space.

Preferably, the moving objects are flying objects, and the calculation target selecting means determines the number based on at least one of acceleration and weight of each of the interfering moving objects in the local space. is.

Further, the calculation target selecting means obtains a three-dimensional volume representing an influence range of each of the interference moving bodies based on at least one of the acceleration and the weight of each of the interference moving bodies in the local space, and It is preferable to determine the number of units so that the ratio of the total volume of the solids of the intra-local correction moving bodies to the local space is equal to or less than a predetermined ratio.

Preferably, the calculation target selection means selects the intra-local correction moving body from the interference moving bodies based on at least one of acceleration and weight of each of the interference moving bodies in the local space. .

Further, the movement plan calculation device further includes a storage unit that stores the priority of each of the moving objects, and the calculation target selecting means selects the interfering moving object based on the priority of each of the interfering moving objects. It is preferred to select the intra-local correction mover from.

A program according to the present invention is a program for obtaining a movement plan that does not interfere with each other for a plurality of moving bodies that move in a three-dimensional space, the program comprising: Interference determining means for determining an interfering moving object and an interfering position based on a moving plan leading to a target position, based on spatial proximity between the moving plans; local space setting means for setting a local space including the interference position; and two or more of the interfering moving bodies are selected as intra-local correction moving bodies to be calculated for an avoidance plan in the local space. and calculating the avoidance plan in which the intra-local correction moving bodies are not in an interfering relationship with each other in the local space for each of the intra-local correction moving bodies, and correcting the movement plan; as movement plan modifying means for modifying the movement plan of the interfering moving body other than the intra-local correcting moving body so as not to pass through the local space at least during the period in which the intra-local correcting moving body passes through the local space It's a program that makes it work.

As described above, according to the movement plan calculation device and program of the present invention, it is possible to obtain movement plans that do not interfere with each other at a low calculation cost.

1 is a schematic diagram showing the configuration of a movement control system according to an embodiment of the present invention; FIG. 1 is a schematic diagram showing the configuration of an autonomous flying robot according to an embodiment of the present invention; FIG. 1 is a schematic diagram showing the configuration of a management device according to an embodiment of the present invention; FIG. It is a figure which shows the structure of route information. FIG. 4 is a diagram showing an example of individual routes; It is a figure for demonstrating an interference position. FIG. 10 is a diagram showing an example of a cylinder representing a wind influence range; It is a figure which shows the example of local space. FIG. 4 is a diagram showing an example of a local space that includes multiple local spaces; (A) A diagram showing an example of an individual route, and (B) a diagram showing an example of a result of correcting the individual route. It is a flowchart which shows operation|movement of the route search process by the management apparatus which concerns on embodiment of this invention. It is a flowchart which shows the operation|movement of the calculation object selection process by the management apparatus which concerns on embodiment of this invention.

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In this embodiment, an example will be described in which the present invention is applied to a movement control system that controls movement of a plurality of drones.

<System configuration>
A configuration of an embodiment of the present invention will be described below with reference to FIG. 1 showing a schematic configuration of a movement control system 1000 to which the present invention is applied.

(Movement control system 1000)
A movement control system 1000 has a management device 100 and a plurality of autonomous flying robots 200 . Communication is performed between the management device 100 and the autonomous flying robot 200 via a predetermined closed network (wireless LAN, etc.). Note that the management device 100 is an example of a movement plan calculation device.

(Autonomous flying robot 200)
The autonomous flying robot 200 is, for example, a quadrotor-type small unmanned helicopter. The present invention is not limited to quad-rotor type small unmanned helicopters, but can also be applied to single-rotor type small unmanned helicopters.

The autonomous flying robot 200 receives information (route information described later) on a flight route, which is an example of a movement plan, from the management device 100, and controls the robot to fly along the flight route. The position/orientation sensor 210, communication unit 220, storage unit 230, control unit 240, motor 250, and rotor 260 that constitute the autonomous flying robot 200 will be described in detail below (see FIG. 2).

(Position/orientation sensor 210)
The position/orientation sensor 210 is a sensor for acquiring the current position and orientation of the autonomous flying robot 200 . The position/orientation sensor 210 includes, for example, a receiver that receives radio waves (navigation signals) transmitted from a navigation satellite (artificial satellite) such as a GNSS (Global Navigation Satellite System), an acceleration sensor that measures acceleration, and an azimuth measurement. It has an electronic compass and a gyro sensor that measures angular velocity. Specifically, in the position/orientation sensor 210, the receiver receives navigation signals transmitted from a plurality of navigation satellites, outputs them to the control unit 240, and outputs measurement signals from the electronic compass and the gyro sensor to the control unit 240. do.

Other known sensors may be used to obtain information for obtaining current position and attitude using known prior art techniques, such as using laser scanners and barometric pressure sensors instead of receivers to obtain current position. .

(Communication unit 220)
The communication unit 220 is a communication module for communicating with the management device 100 .

(storage unit 230)
The storage unit 230 is an information storage device such as a ROM (Read Only Memory), a RAM (Random Access Memory), and an HDD (Hard Disk Drive).

The storage unit 230 stores various programs and various data, and inputs/outputs such information to/from the control unit 240 .

The various data include information used for each process of the control unit 240, such as the position/orientation information 232 and the route information 234. FIG.

The position/orientation information 232 is a position and orientation history obtained by cyclically storing the current position and orientation of the autonomous flying robot 200 acquired by the position/orientation sensor 210 a predetermined number of times. Here, the most recent position and orientation in the history are particularly referred to as the current position and current orientation, and the other positions and orientations are referred to as past positions and past orientations.

The route information 234 is information about the flight route along which the autonomous flying robot 200 is scheduled to travel. Specifically, it is information that associates a coordinate sequence (x, y, z) on the flight path with time, velocity, and acceleration at each position (coordinate). The route information 234 is received from the management device 100 .

The control unit 240 is a computer having a CPU, a ROM, a RAM, etc., reads programs and data from the storage unit 230, and functions as a position/attitude calculation means 242, a flight control means 244, etc. according to the programs.

The position/orientation calculation means 242 calculates the current position and orientation of the autonomous flying robot 200 in the flight space from the output of the position/orientation sensor 210 and stores them in the storage unit 230 as the position/orientation information 232 .

The position/attitude calculation means 242 obtains the latitude/longitude/altitude from the navigation signal output by the position/attitude sensor 210, for example, and converts them into positions in the flight space coordinate system using a conversion rule stored in advance.

Also, the position/attitude calculation means 242 obtains the current attitude in the coordinate system of the flight space from the measurement signals of the acceleration sensor and the gyro sensor output by the position/attitude sensor 210 .

Note that the azimuth in the coordinate system of the flight space may be obtained from the electronic compass measurement signal output by the position/orientation sensor 210, and the current attitude may be calculated using measurement signals from other sensors.

As described above, the position/orientation sensor 210, the communication section 220, and the position/orientation calculation means 242 (control section 240) cooperate to detect the current position and orientation of the autonomous flying robot 200. FIG.

Note that the position/orientation calculation unit 242 transmits the current position to the management device 100 via the communication unit 220 every time it calculates the current position.

The flight control means 244 refers to the route information 234 and the position/orientation information 232 and controls the rotation speed of the motor 250 so that the autonomous flying robot 200 follows the flight route described in the route information 234 .

Specifically, the flight control means 244 rotates the motor 250 so that the error between the position (coordinates) at the current time written in the route information 234 and the current position written in the position/attitude information 232 is small. Control speed.

Further, the flight control means 244 calculates the current velocity and acceleration of the autonomous flying robot 200 based on the position/orientation information 232, and calculates the error between these values and the velocity and acceleration at the current time described in the route information 234. is controlled so that the rotation speed of the motor 250 becomes small. At this time, the information related to the orientation in the position/orientation information 232 is also considered.

(motor 250, rotor 260)
The autonomous flying robot 200 is equipped with four units consisting of four rotors 260 and four motors 250 whose rotation shafts are respectively connected to the corresponding rotors 260 .

Each motor 250 is connected to the control unit 240 and instructed by the control unit 240 about the rotation speed. The independent rotation of the four rotors 260 causes the autonomous flying robot 200 to generate acceleration in any direction.

(Management device 100)
The management device 100 is installed at a predetermined position in the flight space, determines a target position to which the autonomous flying robot 200 flies, and reaches the target position from the current position based on the current position received from the autonomous flying robot 200. A flight path is obtained and transmitted to the autonomous flying robot 200 .

The management device 100 communicates with a center device (not shown) installed in a remote monitoring center via a wide area network (the Internet, a mobile phone network, etc.), receives instructions from the center device, and based on the instructions, A target position may be set, and various information related to the autonomous flying robot 200 may be transmitted to the center device.

The communication unit 110, the storage unit 120, and the control unit 130, which constitute the management device 100, will be described in detail below (see FIG. 3).

(Communication unit 110)
The communication unit 110 is a communication module for communicating with the autonomous flying robot 200 .

(storage unit 120)
The storage unit 120 is an information storage device such as a ROM (Read Only Memory), a RAM (Random Access Memory), and an HDD (Hard Disk Drive).

The storage unit 120 stores various programs and various data, and inputs/outputs such information to/from the control unit 130 .

The various data include information used for each process of the control unit 130, such as position information 122, space information 124, aircraft information 126, route information 128, and the like.

(Position information 122)
The position information 122 is information indicating the current position and target position of the autonomous flying robot 200 .

When the current position is received from the autonomous flying robot 200 , it is stored in association with the identifier (body ID) of the autonomous flying robot 200 .

Also, when the target position of a certain autonomous flying robot 200 is set by the target position setting means 132, the target position is stored in association with the body ID of the autonomous flying robot 200 concerned.

(Spatial information 124)
The space information 124 is information representing the three-dimensional structure of the flight space. The space information 124 in this embodiment is information representing the structure of obstacles in the flight space by dividing the flight space into a plurality of voxels (unit spaces) as a voxel space. Spatial information 124 is preset and stored by a system administrator.

In this embodiment, the flight space is equally divided into voxels of a predetermined unit size (for example, 15 cm × 15 cm × 15 cm), and the voxel ID, which is an identifier for each voxel, corresponds to the position (coordinates) of the voxel in the flight space. It is stored as the spatial information 124 with the data.

The spatial information 124 also includes information indicating attributes of each voxel. For example, as an attribute, at least an obstacle attribute indicating whether or not the movement of the autonomous flying robot 200 is hindered is added.

That is, a voxel located in an obstacle such as a building is set as an obstacle attribute as a space in which the autonomous flying robot 200 cannot move.

In this way, by using the positions and attributes of voxels as space information 124, it is possible to express the positions and sizes (shapes) of objects such as obstacles that exist in the flight space.

(Aircraft information 126)
The aircraft information 126 is information indicating the characteristics (performance) and movement priority of each autonomous flying robot 200 . The device information 126 is preset and stored by the system administrator.

Specifically, the body information 126 associates the body ID of each autonomous flying robot 200, characteristic values (body weight, size, maximum speed, maximum acceleration, and maximum jerk) with task priority. Information.

The task priority is the importance of a task that the autonomous flying robot 200 is performing (e.g., rushing to an intrusion point, patrol security, AED transport, luggage transport, returning for charging, chasing a thief, etc.). is a numerical value set by the administrator of the mobile control system 1000 according to , and a larger value is set for the autonomous flying robot 200 to which priority is given.

(Route information 128)
The route information 128 is information regarding the flight route of each autonomous flying robot 200 . Specifically, the route information 128 includes the body ID of the autonomous flying robot 200, coordinate values (x, y, z) on the route, passage time at the coordinate values, speed, acceleration, jerk (Jerk), acceleration It is stored in association with the value of the amount of change in acceleration (Snap).

For example, in the route information 128 shown in FIG. 4, the passage time of the coordinate values (x1, y1, z1) on the route for the autonomous flying robot 200 whose body ID is "A" is t1, and the acceleration is a1. , the jerk is j1 and the amount of jerk change is s1, the passage time of the coordinate values (x2, y2, z2) on the route is t2, the acceleration is a2, It is stored that the jerk is j2 and the amount of change in the jerk is s2. Here, the accelerations a1 and a2 are vectors whose elements are the x-axis component, the y-axis component and the z-axis component of the acceleration, and the jerk j1 and j2 are the x-axis component, the y-axis component and the z-axis component of the jerk. The jerk change amounts s1 and s2 are vectors whose elements are the x-axis component, the y-axis component, and the z-axis component of the jerk change amount.

The route information 128 is updated by route calculation means 134 and route correction means 142 .

(control unit 130)
The control unit 130 is a computer having a CPU, a ROM, a RAM, etc., reads programs and data from the storage unit 120, and according to the program, controls a target position setting means 132, a route calculation means 134, an interference determination means 136, and a local space setting. It functions as means 138, calculation target selection means 140, route correction means 142, and the like. Note that the route correction means 142 is an example of the movement plan correction means.

(Target position setting means 132)
The target position setting means 132 sets the target position of each autonomous flying robot 200 and stores it in the storage unit 230 as the position information 122 in association with the machine body ID.

For example, in the case of the autonomous flying robot 200 whose task is patrol security, the target position is set so that a predetermined patrol position becomes the target position at a predetermined time. In the case of the autonomous flying robot 200 whose task is to deal with intruders, the vicinity of the detection position of a sensor (not shown) may be set as the target position based on the intrusion detection output from the sensor.

Alternatively, the target position may be set based on an instruction from a center device (not shown).

(Route calculation means 134)
The route calculation means 134 calculates a flight route from the current position of each autonomous flying robot 200 to a target position, and performs individual route calculation processing to store the calculated route in the storage unit 230 as the route information 128 . At this time, the calculated flight path is a flight path calculated only in consideration of not interfering with obstacles without considering interference with other autonomous flying robots 200. Hereinafter, the path calculation means 134 The calculated flight route is called an individual route.

As will be described later, the route calculation means 134 calculates a basic route by a route search method such as the A* route search method, sets waypoints based on the basic route, and determines an optimum route using the waypoints as a constraint condition. Individual paths are calculated by solving the transformation problem. Details of the individual route calculation process will be described later.

(Interference determination means 136)
The interference determination means 136 can interfere with the interference position, which is a position where the autonomous flying robots 200 can interfere with each other due to the spatial proximity between individual routes (the flight of other autonomous flying robots 200 can be adversely affected). Interference determination processing for determining the autonomous flying robot 200 (hereinafter referred to as an interfering mobile object) is performed. Details of the collision determination process will be described later. Also, interference means that they do not come closer than a predetermined standard.

(Local space setting means 138)
The local space setting means 138 performs local space setting processing for setting a local space including the interference position obtained by the interference determination means 136 . Details of the local space setting process will be described later.

(Calculation target selection means 140)
The calculation target selection means 140 selects two or more autonomous flying robots 200 according to the size of the local space from among the interfering moving bodies as targets for calculating an avoidance route in the local space (hereinafter referred to as intra-local correction movement Calculation object selection processing is performed to select a field. Details of the calculation target selection process will be described later.

(Route correction means 142)
The route correction means 142 calculates an avoidance route in which the intra-local correction moving bodies do not interfere with each other in the local space by performing an optimization calculation, corrects the route information based on the calculated avoidance route, Individual route correction processing is performed to correct the route information of interfering mobile bodies other than the inner corrected mobile body so that they do not pass through the local space. Details of the individual route correction processing will be described later. Note that the avoidance route is an example of an avoidance plan.

(Regarding Individual Route Calculation Processing)
Here, the details of the individual route calculation process will be described.

First, based on the spatial information, the central position of the voxel to which no obstacle attribute is assigned is obtained as a node. Then, a graph structure is obtained in which line segments connecting each obtained node and adjacent nodes are used as edges. Note that the graph structure may be generated and stored in advance before the route search processing.

Using the generated graph structure and position/orientation information 232, a path from the voxel node corresponding to the current position (hereinafter referred to as the "start node") to the voxel node corresponding to the target position (hereinafter referred to as the "target node") The A* route search method is used to search for a route that minimizes the total cost value obtained from the length of . A route obtained by this search is called a “basic route”. The basic route search method is not limited to the A* route search method, and other route search methods such as the Dijkstra method may be applied. Since the route search method by the A* route search method is the same as the method described in reference 1, for example, detailed description thereof will be omitted.

[Reference 1]: JP 2017-016359 A

Then, a waypoint is set based on the basic route. For example, among the nodes forming the basic path, the remaining nodes after excluding the nodes arranged on a straight line are selected as waypoints.

Positional information, positions of waypoints, aircraft performance (maximum velocity, maximum acceleration, maximum jerk, etc.) are set as constraints, and a path (polynomial path) that minimizes the objective function is calculated as an individual path.

A polynomial path is a path represented by a polynomial consisting of time variables and coefficients. There is a polynomial path for each XYZ axis. Below is an equation representing the polynomial path for the X axis.
x ₍ ^t ) _{= c5t5} + ^c4t4 ₊ ^c3t3 + _c2t2 + ^c1t + _c0

Also, a polynomial path is set for each waypoint of the basic path. For example, as shown in FIG. A polynomial path is set as an individual path R.

In order to calculate an individual route that the autonomous flying robot 200 can easily follow, the individual route that minimizes the change in jerk is calculated after satisfying the following constraints. Therefore, the objective function is the integral of the jerk change of the path, and the polynomial path that minimizes this is obtained. Alternatively, an individual route that minimizes changes in acceleration may be obtained.

<Constraints>
a. The starting point of an individual path is the current position. Also, the velocity, acceleration, jerk, and change in jerk at the starting point are zero.
b. The end point of the individual path is the target position. Also, the velocity, acceleration, jerk, and change in jerk at the end point are zero.
c. The position of the waypoint is the position set above. In order to smoothly connect the paths before and after the waypoints, the velocity, acceleration, and jerk of the waypoints take the same values in the polynomial paths before and after the waypoints.
d. In order to create a route that satisfies aircraft performance, the maximum values of route speed, acceleration, and jerk shall be within specified values.

By finding the route that minimizes the objective function after satisfying the above constraints a. to d. calculate. A method of calculating an individual route that satisfies the constraint conditions is the same as the method described in Reference 2 below, so detailed description thereof will be omitted.

[Reference 2]: D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors," 2011 IEEE International Conference on Robotics and Automation, Shanghai, 2011, pp. 2520-2525.

(Regarding interference determination processing)
Next, details of the interference determination process will be described.

The collision determination process is a process of determining the interference position, which is the position where the autonomous flying robots 200 can interfere with each other, and the autonomous flying robots 200 that can interfere, based on the spatial proximity between the individual paths. For example, as shown in FIG. 6, based on the spatial proximity between the individual routes Ra to Re of the autonomous flying robots 200a to 200e, the interference positions at which the autonomous flying robots 200a to 200e may interfere with each other, and the autonomous flying robot 200a 200e, find an autonomous flying robot 200 that can interfere.

In this embodiment, the “spatial proximity” is determined in consideration of the range affected by the wind caused by the movement of the autonomous flying robot 200 .

First, as described below, the area affected by the wind is obtained.

Based on the aircraft weight, size, and maximum acceleration described in the aircraft information 126, the wind influence range is simply obtained by approximating it to a cylinder.

Specifically, the larger the size of the autonomous flying robot 200, the larger the radius of the bottom surface of the cylinder. . For example, as shown in FIG. 7, cylinders Ca and Cb are set for the autonomous flying robots 200a and 200b to represent the range of influence of the wind.

In addition to the above method (cylindrical approximation), the maximum inclination angle of the airframe, the arrangement and size of the rotor 260, the inclination of the blades, the maximum rotation speed, etc. can be considered for hydrodynamic analysis. You can also find the range of influence of Also, the height of the cylinder may be determined based only on the body weight of the autonomous flying robot 200, or the height of the cylinder may be determined based only on the maximum acceleration of the autonomous flying robot 200.

Then, as will be described below, based on the route information 128, the affected area of the wind is moved to obtain the interference position.

First, with reference to the route information 128, for all the autonomous flying robots 200, the position (coordinates) at a predetermined time on the individual route of each autonomous flying robot 200 is obtained, and the cylinder calculated above is placed at the position. Deploy. Also, the time is changed at predetermined intervals, and the cylinders are arranged similarly at each time. For example, as shown in FIG. 7, cylinders Ca and Cb are arranged at positions Pa and Pb on individual routes Ra and Rb of autonomous flying robots 200a and 200b.

Then, the interference position is determined based on the position where the cylinder of a certain autonomous flying robot 200 and the cylinder of another autonomous flying robot 200 overlap. For example, as shown in FIG. 7, when the positions Pa and Pb of the autonomous flying robots 200a and 200b overlap with the cylinders Ca and Cb, the intermediate point Iab between the positions Pa and Pb of the autonomous flying robots 200a and 200b is the interference position. Ask as Note that the position of the center of gravity of the portion where the cylinders overlap may be obtained as the interference position.

Each autonomous flying robot 200 that interferes with each other is specified as an "interfering mobile object".

(Regarding local space setting processing)
Next, details of the local space setting process will be described.

First, a local space including the interference position is set. In this embodiment, a cube of a predetermined size (eg, 5 m×5 m×5 m) is used, and the center position of the cube is set to be the interference position. For example, as shown in FIG. 8, a cube Lcd centered on the interference position Icd obtained based on the overlapping positions of the cylinders of the autonomous flying robots 200c and 200d is set.

At this time, if the cube overlaps a non-flyable area such as an obstacle when the cube is placed so that the center position is the interference position, the position of the cube is shifted to a position where it does not overlap. Alternatively, a rectangular parallelepiped having a size that does not overlap the non-flying area and including the interference position is set as the local space.

Also, when a plurality of interference positions are detected, a local space is set for each interference position. At this time, if a certain local space partially overlaps with another local space, a rectangular parallelepiped having a size that includes the overlapping local spaces is set as the local space. For example, as shown in FIG. 9, when a cube L1 centered at the interference position I1 partially overlaps with a cube L2 centered at the interference position I2, a rectangular parallelepiped L12 containing the cubes L1 and L2 is Set as local space.

(Regarding avoidance route calculation processing)
Next, details of the avoidance route calculation process will be described.

For the intra-local correction moving body determined in the previous step, avoidance paths that do not interfere with each other in the target local space are calculated as follows.

Constraint conditions are set based on individual paths, local space, and aircraft performance (maximum velocity, maximum acceleration, maximum jerk, etc.), and the path that minimizes the objective function is calculated as the avoidance path.

The avoidance path of each autonomous flying robot 200 is represented by a polynomial path.

The objective function is obtained by integrating the speed change of the avoidance route of each autonomous flying robot 200. After satisfying the following constraints, an optimization problem is calculated to find a polynomial route that minimizes them. A route that minimizes changes in acceleration and jerk may be obtained.

First, for each intra-local correction mover, there are constraints on the start and end points in the local space of interest. This is a constraint condition for smoothly connecting the avoidance route in the target local space and the individual route outside the local space. Specifically, the position (starting point) at which the individual route enters the local space and the change in its velocity, acceleration, jerk, and jerk are obtained, and these values are used as the constraint conditions for the starting point of the avoidance route. Furthermore, the position (end point) at which the individual path exits the local space and its velocity, jerk, and change in jerk are obtained, and these values are used as constraints for the end point of the avoidance path.

In addition, constraints on speed, acceleration, and jerk on the avoidance route are set. In other words, the constraint conditions are to be within the maximum values of the velocity, acceleration, and jerk shown in the airframe performance.

A constraint condition may also be set such that the distance between the intra-local corrective moving bodies is equal to or greater than a certain distance. For example, the restriction condition may be that the influence ranges of the moving bodies set above do not overlap.

As shown in the following equation, an avoidance path that minimizes the objective function is obtained while satisfying the above constraints.

However, X is a vector consisting of coefficients of a polynomial path representing the avoidance path, A _eq X=be _eq represents the constraint on the start point and the end point, and A _in X≦b _in is the constraint on aircraft performance. represents

Assuming that the above objective function is f ₀ , the objective function f ₀ is expressed, for example, by the following equation.

However, N is the number of autonomous flying robots 200 serving as local correction moving bodies, K is the number of discrete times, and a _i [k] is the acceleration of the i-th autonomous flying robot 200 at time k. represents

Since the method of finding an avoidance path that satisfies the constraint conditions and minimizes the objective function is the same as the method described in Reference 3 below, detailed description thereof will be omitted.

[Reference 3]: F. Augugliaro, A. P. Schoellig and R. D'Andrea, "Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach," 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura, 2012, pp. 1917-1922.7

(Regarding individual route correction processing)
Next, details of the individual route correction processing will be described.

In the individual route correction process, the individual route is corrected using the calculated avoidance route for the intra-local corrected moving body.

For interfering moving bodies other than the corrected moving bodies within the local area, the individual paths are corrected so that they do not pass through the local space. In this embodiment, the individual path is modified to hover (stop) at a position in front of the local space so as not to pass through the local space. It should be noted that the hovering time is the time required for all the intra-local correction moving objects to leave the local space.

Note that for an interfering moving body other than the intra-local correction moving body, the individual route may be corrected not only by hovering but also by detouring the local space. In this case, the local space is regarded as a large obstacle, the obstacle attribute of the space information is updated, and the individual route is recalculated by applying the above-described individual route calculation processing.

For example, as shown in FIGS. 10A and 10B, the individual routes Ra, Rd, and Re of the autonomous flying robots 200a, 200d, and 200e, which are local corrective moving bodies, are independent routes in the local space. Individual routes Ra', Rd', and Re' after correction are obtained by correcting the avoidance routes ra, rd, and re for the flying robots 200a, 200d, and 200e. Further, by correcting the individual paths Rb and Rc of the autonomous flying robots 200b and 200c, which are interfering moving bodies other than the intra-local correction moving bodies, to hover (stop) at a position in front of the local space, Obtain individual routes Rb' and Rc'.

<Operation of the movement control system>
The operation of route search processing by the mobility control system 1000 to which the present invention is applied will be described below with reference to the flowcharts shown in FIGS. 11 and 12. FIG. A case where the storage unit 120 of the management device 100 stores the position information 122, the space information 124, and the machine information 126 in advance will be described as an example.

When the target position of at least one autonomous flying robot 200 among the plurality of autonomous flying robots 200 is changed, the route search process shown in FIG. 11 is executed.

In the route search processing of the management device 100 shown in FIG. 11, first, one or a plurality of autonomous flying robots 200 whose target positions have been changed are selected as route update targets, and the processing of steps S100 to S102 of loop 1 is performed on each Executed for route update target. Hereinafter, the route update target selected in loop 1 will be referred to as a "targeted robot".

The path calculation means 134 reads the current position, current orientation, target position, and target orientation of the robot of interest from the position information 122 (step S100). Next, an individual route calculation process for calculating a flight route from the current position to the target position, that is, an individual route, is performed for the robot of interest (step S102).

Then, the interference determination means 136 performs interference determination processing to determine the interference position, which is the position at which the autonomous flying robots 200 can interfere, based on the spatial proximity between the individual paths, and the autonomous flying robots 200 that can interfere. A determination process is performed (step S104).

Then, the local space setting means 138 determines whether or not there is an interference position based on the result of the interference determination process in step S104 (step S106). If the interfering position does not exist, the route search processing routine is terminated.

On the other hand, when the interference position exists, the local space setting means 138 performs local space setting processing for setting a local space including the interference position (step S108).

Then, the processing of steps S110 to S112 of loop 2 is executed for each local space. Hereinafter, the local space to be processed in loop 2 will be referred to as a "local space of interest".

The calculation target selection means 140 selects the number of autonomous flying robots 200 corresponding to the size of the local space of interest from among the interfering mobile bodies corresponding to the local space of interest as intra-local correcting mobile bodies for the avoidance route in the local space of interest. Calculation object selection processing to be selected is performed (step S110). Details of the calculation target selection process will be described later.

Then, the route correction means 142 calculates an avoidance route in which the intra-local correction moving bodies do not interfere with each other in the local space of interest by performing optimization calculation (step S112).

The route correction means 142 corrects the route information based on the calculated avoidance route, and also corrects the route information of the interfering moving object other than the intra-local corrected moving object so that it does not pass through the local space of interest. After performing the processing, the route search processing ends.

The process of step S110 is implemented by the calculation target selection process shown in FIG.

In the calculation target selection process of the management device 100 shown in FIG. 12, first, the calculation target selection means 140 calculates, for each interfering moving object, the influence range of the wind caused by the flight of the interfering moving object (step S120). In this embodiment, the cylinder representing the range of influence of the wind is obtained in the interference determination process, so the cylinder is read from the storage unit 120 .

Next, the calculation target selection means 140 determines the task priority of the aircraft information and the cumulative hovering time (cumulative time when hovering without entering the local space. Taking into account the time until exit), the range of influence, etc., the priority for becoming the intra-local corrective moving body is obtained (step S122). In this embodiment, the priority is calculated from the sum of the values obtained by multiplying the task priority and the cumulative hovering time by a predetermined coefficient. Then, if there are interfering moving bodies with the same priority, the autonomous flying robot 200 having a smaller influence range (having a smaller cylinder volume) is added so that the higher priority is given.

Then, the processing of steps S124 to S130 of loop 3 is executed for each interfering mobile object. Among the interfering mobile objects corresponding to the target local area, the autonomous flying robots 200 having the highest passing priority are selected in descending order and processed in the loop 3 . Hereinafter, the interfering mobile body selected in loop 3 will be referred to as a "interfering mobile body of interest".

The calculation target selection means 140 calculates an evaluation value of the interfering moving body of interest from the influence range (volume of the cylinder) of the wind caused by the interfering moving body of interest (step S124). Here, the evaluation value is calculated as a larger evaluation value as the volume of the cylinder increases.

Then, the calculation target selection unit 140 obtains the integrated value (sum) of the evaluation values obtained so far as the integrated evaluation value in the local space of interest (step S126).

The calculation target selection means 140 determines whether or not the integrated evaluation value is equal to or less than the threshold Th1 (step S128). (step S130). On the other hand, when the integrated evaluation value is greater than the threshold Th1, the calculation target selection process is terminated. Here, the threshold Th1 is, for example, a volume value that is a predetermined proportion of the volume of the local space of interest. Also, the threshold Th1 may be set by an administrator.

That is, when all interfering moving bodies are set as intra-local correction moving bodies, or when the integrated evaluation value becomes larger than the threshold Th1, the calculation target selection processing in the local space of interest ends. As a result, the number of selected intra-local correction moving bodies is determined so that the ratio of the sum of the volumes of the cylinders of the selected intra-local correction moving bodies to the local space of interest is equal to or less than a predetermined ratio. .

Note that the intra-local correction moving body may be set without calculating the evaluation value. For example, the total number of intra-local corrective moving bodies is determined according to the volume of the local space of interest, and the inter-local corrective moving bodies are set in descending order of priority up to the determined total number. In this case, it is assumed that the larger the volume of the target local space is, the larger the total number of units is determined. Alternatively, the total number of intra-local correction moving bodies may be determined in advance regardless of the size of the local space of interest, or the maximum acceleration and body weight of each interfering moving body in the local space may be determined according to at least one of Based on this, the total number of local correction moving bodies may be determined. Alternatively, the intra-local correction moving body may be randomly selected without using the priority.

As described above, in the movement control system 1000 according to the embodiment of the present invention, a local space including a position with a high possibility of interference is obtained from the proximity of the flight paths calculated separately for each autonomous flying robot, Two or more of the interfering moving bodies, which are autonomous flying robots that are in a mutually interfering relationship, are selected as intra-local correcting moving bodies for an avoidance route in the local space, and for each of the intra-local correcting moving bodies Calculates an avoidance route in the local space, corrects the flight route using the calculated avoidance route, and corrects the flight routes of interfering moving bodies other than the intra-local correction moving body so as not to pass through the local space. As a result, flight paths that do not interfere with each other can be obtained at a low computational cost.

<Modification>
Although preferred embodiments of the present invention have been described above, the present invention is not limited to these embodiments. For example, in this embodiment, in the collision determination process, interference is determined based on the coordinate values (x, y, z) at each time on each individual route stored in the route information 128 . However, interference may be determined using coordinate values (x, y, z) without using the time on the individual path. In other words, interference is determined based on the closeness of the spatial distance between each position on the individual route of an autonomous flying robot and each position on the individual route of another autonomous flying robot. Further, in the present embodiment, in the individual route calculation process, a basic route is calculated by a route search method such as the A* route search method, via points are set based on the basic route, and the via points and the like are used as constraint conditions. Individual paths are calculated by solving the optimization problem. However, the individual route may be calculated only by a route search method such as the A* route search method. In this case, the route information of the calculated individual route is information composed of the body ID of the autonomous flying robot and a list of coordinate values (x, y, z) indicating positions on the route. The interference determination process in this case determines interference based on the spatial proximity between each position on the individual route of a given autonomous flying robot and each position on the individual route of another autonomous flying robot.

For example, a cylinder representing the range of influence of the wind is placed at each position on the individual path of a certain autonomous flying robot, and similarly, a cylinder representing the range of influence of the wind is placed at each position on the individual path of another autonomous flying robot. When the cylinders overlap each other, it is determined that they are in an interference relationship. Then, the interference position is obtained by the same method as described above, and the local space is set. Furthermore, in the individual route correction process, the time and position to enter the local space and the time and position to exit the local space are obtained from the individual route and the average moving speed of the autonomous flying robot. Calculate the avoidance path by solving the optimization problem in the same way as

Also, all the functions provided by the management device may be provided in a specific autonomous flying robot. In this case, a particular autonomous flying robot may communicate with other autonomous flying robots to generate flight paths for all autonomous flying robots.

In addition, although the case where the route calculation means of the management device calculates individual routes for each autonomous flying robot has been described as an example, the present invention is not limited to this. individual routes may be used.

Moreover, although the case of using an autonomous flying robot as an example of a mobile object has been described, the present invention is not limited to this, and a mobile object other than an autonomous flying robot may be used. For example, the present invention may be applied when calculating the moving route of an underwater robot.

As described above, those skilled in the art can make various modifications within the scope of the present invention according to the embodiment.

100 Management device 110 Communication unit 120 Storage unit 122 Position information 124 Space information 126 Aircraft information 128 Route information 130 Control unit 132 Target position setting means 134 Route calculation means 136 Interference determination means 138 Local space setting means 140 Calculation target selection means 142 Route correction Means 200 Autonomous flying robot 210 Position/attitude sensor 220 Communication unit 230 Storage unit 232 Position/attitude information 234 Route information 240 Control unit 242 Position/attitude calculation means 244 Flight control means 250 Motor 260 Rotor 1000 Movement control system

Claims (5)

A movement plan calculation device that obtains a movement plan that does not interfere with each other for a plurality of moving bodies that move in a three-dimensional space,
Spatial proximity between the movement plans for each of the plurality of moving bodies , based on the movement plan having the time when each position from the current position to the target position is passed for each of the plurality of moving bodies. and interference determination means for obtaining an interfering moving object and an interfering position, which are the moving objects that are in an interfering relationship with each other based on temporal proximity ;
local space setting means for setting a local space containing the interference position in the three-dimensional space;
Calculation target selection for selecting two or more of the interfering moving bodies interfering with each other in the local space as intra-local correction moving bodies to be calculated for an avoidance plan in the local space. means and
The avoidance plan in which the intra-local correction moving bodies are not in an interfering relationship with each other in the local space is calculated for each of the intra-local correction moving bodies to correct the movement plan; a movement plan modifying means for modifying the movement plan of the interfering moving body so as not to pass through the local space at least during a period in which the intra-local modified moving body passes through the local space;
with
The mobile body is an aircraft,
The calculation target selecting means determines that the number of the interfering moving bodies decreases as the acceleration of each of the interfering moving bodies in the local space increases, or the number of the interfering moving bodies decreases as the weight of each of the interfering moving bodies increases. A movement plan calculation device that determines A movement plan calculation device that obtains a movement plan that does not interfere with each other for a plurality of moving bodies that move in a three-dimensional space,
Spatial proximity between the movement plans for each of the plurality of moving bodies, based on the movement plan having the time when each position from the current position to the target position is passed for each of the plurality of moving bodies. and interference determination means for obtaining an interfering moving object and an interfering position, which are the moving objects that are in an interfering relationship with each other based on temporal proximity;
local space setting means for setting a local space containing the interference position in the three-dimensional space;
Calculation target selection for selecting two or more of the interfering moving bodies interfering with each other in the local space as intra-local correction moving bodies to be calculated for an avoidance plan in the local space. means and
The avoidance plan in which the intra-local correction moving bodies are not in an interfering relationship with each other in the local space is calculated for each of the intra-local correction moving bodies to correct the movement plan; a movement plan modifying means for modifying the movement plan of the interfering moving body so as not to pass through the local space at least during a period in which the intra-local modified moving body passes through the local space;
with
The mobile body is an aircraft,
The calculation target selection means obtains a volume of a solid representing an influence range of each of the interference moving bodies based on at least one of acceleration and weight of each of the interference moving bodies in the local space, and occupies the local space. 7. A movement plan calculation device for obtaining the number of the intra-local correction moving bodies so that the ratio of the sum of the volumes of the solids of each of the intra-local correction moving bodies is equal to or less than a predetermined ratio. A movement plan calculation device that obtains a movement plan that does not interfere with each other for a plurality of moving bodies that move in a three-dimensional space,
Spatial proximity between the movement plans for each of the plurality of moving bodies, based on the movement plan having the time when each position from the current position to the target position is passed for each of the plurality of moving bodies. and interference determination means for obtaining an interfering moving object and an interfering position, which are the moving objects that are in an interfering relationship with each other based on temporal proximity;
local space setting means for setting a local space containing the interference position in the three-dimensional space;
Calculation target selection for selecting two or more of the interfering moving bodies interfering with each other in the local space as intra-local correction moving bodies to be calculated for an avoidance plan in the local space. means and
The avoidance plan in which the intra-local correction moving bodies are not in an interfering relationship with each other in the local space is calculated for each of the intra-local correction moving bodies to correct the movement plan; a movement plan modifying means for modifying the movement plan of the interfering moving body so as not to pass through the local space at least during a period in which the intra-local modified moving body passes through the local space;
with
The mobile body is an aircraft,
The calculation target selection means is a movement plan calculation device that preferentially selects an interfering moving body having a small acceleration or an interfering moving body having a small weight in the local space as the intra-local correction moving body. further comprising a storage unit that stores the priority of each of the moving objects;
3. The movement plan calculation apparatus according to claim 1, wherein the calculation target selection means selects the intra-local correction moving object from the interfering moving objects based on the priority of each of the interfering moving objects. A program for obtaining a movement plan that does not interfere with each other for a plurality of moving bodies moving in a three-dimensional space,
the computer,
Spatial proximity between the movement plans for each of the plurality of moving bodies , based on the movement plan having the time when each position from the current position to the target position is passed for each of the plurality of moving bodies. and interference determination means for obtaining an interfering moving object and an interfering position, which are the moving objects that are in a mutually interfering relationship based on temporal proximity ;
local space setting means for setting a local space containing the interference position in the three-dimensional space;
Calculation target selection means for selecting two or more of the interfering moving bodies interfering with each other in the local space as intra-local correcting moving bodies for an avoidance plan in the local space, and the local calculating the avoidance plan in which the intra-local correction moving bodies are not in a mutually interfering relationship in space for each of the intra-local correction moving bodies to correct the movement plan; A program for functioning as movement plan modification means for modifying the movement plan of the body so that at least the intra-locally modified moving body does not pass through the local space ,
The mobile body is an aircraft,
The calculation target selecting means determines that the number of the interfering moving bodies decreases as the acceleration of each of the interfering moving bodies in the local space increases, or the number of the interfering moving bodies decreases as the weight of each of the interfering moving bodies increases. A program that decides to do so .

Images