JP7106417B2 - Flight plan calculation device and program - Google Patents

Description

The present invention relates to a flight plan calculation device and program, and more particularly to a flight plan calculation device and program for obtaining flight plans for a plurality of aircraft.

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 flight plan calculation apparatus and a program that can obtain flight plans that do not interfere with each other at a low calculation cost.

In order to achieve the above object, a flight plan calculation device according to the present invention is a flight plan calculation device for obtaining flight plans of a plurality of flying objects that fly in a three-dimensional space, the flight plans not interfering with each other. Based on the flight plan from the current position to the target position for each flying object, the interfering flying object and the interfering position are obtained from the spatial proximity between the flight plans. a determination means, and a local space setting for setting a local space including the interference position in the three-dimensional space and having a size determined by at least one of acceleration and weight of each of the interference flying objects. and flight plan correction means for calculating an avoidance plan for each of the interfering flying objects in which the interfering flying objects do not interfere with each other in the local space, and correcting the flight plan. .

According to the flight plan calculation device of the present invention, the collision determination means determines from the spatial closeness between the flight plans based on the flight plans from the current position to the target position of each of the plurality of flying objects. An interfering flying object and an interfering position, which are the flying objects that interfere with each other, are obtained. Also, the local space setting means sets the local space including the interference position in the three-dimensional space and having a size determined by at least one of the acceleration and weight of each of the interference flying objects. do.

Then, the flight plan modifying means calculates, for each of the interfering flying objects, an avoidance plan in which the interfering flying objects do not interfere with each other in the local space, and corrects the flight plan.

Moreover, it is preferable that the local space setting means further uses at least one of the size and the number of the interfering flying objects to determine the size of the local space.

Moreover, it is preferable that the local space setting means sets the local space having a size that does not interfere with the flight plan of the flying object other than the interfering flying object.

Moreover, it is preferable that the local space setting means sets the local space such that the interference position is substantially at the center of the local space.

Moreover, it is preferable that the local space setting means sets the local space to be larger as the wind speed measured by the wind speed measuring means for measuring the wind speed in the three-dimensional space increases.

A program according to the present invention is a program for obtaining flight plans of a plurality of flying objects that fly in a three-dimensional space without interfering with each other, the program comprising: Interference determining means for determining an interfering flying object and an interfering position based on a flight plan leading to a target position, based on the spatial proximity between the flight plans, and an interfering position in the three-dimensional space local space setting means for setting a local space including the interference position, the size of which is determined by at least one of acceleration and weight of each of the interference flying objects; and the interference flight in the local space. A program for functioning as flight plan correction means for calculating an avoidance plan in which the bodies are not in an interfering relationship with each other for each of the interfering flying objects and correcting the flight plan.

As described above, according to the flight plan calculation device and program of the present invention, it is possible to obtain flight 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. 10 is a diagram showing an example of setting the size of a local space according to the number of interfering flying objects; FIG. 4 is a diagram showing an example of a local space that includes multiple local spaces; FIG. 10 is a diagram showing another 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. 4 is a flowchart showing operations of local space setting processing by the management device according to the embodiment of the present 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 flight 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 flight 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 and outputs them to the control unit 240, and also 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 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) of each autonomous flying robot 200 . The device information 126 is preset and stored by the system administrator.

Specifically, the body information 126 is information in which the body ID of each autonomous flying robot 200 and characteristic values (body weight, size, maximum speed, maximum acceleration, and maximum jerk) are associated with each other.

(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, route correction means 142, and the like. In addition, the route correction means 142 is an example of a flight 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 flight route calculated is a flight route calculated only considering that there is no interference with obstacles without considering interference with other autonomous flying robots 200. Hereinafter, the route calculation means The flight path calculated at 134 is called an individual path.

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 finding an autonomous flying robot 200 (hereinafter referred to as an interfering flying 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.

(Route correction means 142)
The route correction means 142 calculates an avoidance route in which the interfering flying objects do not interfere with each other in the local space by performing an optimization calculation, and performs individual route correction processing for correcting the route information based on the calculated avoidance route. conduct. 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 jerk change (snap) 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 flying 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, based on the body weight, maximum acceleration, etc. of each autonomous flying robot 200, which is an interfering flying object, a local space with a size that takes into consideration the range of influence of the wind caused by the movement of the autonomous flying robot is set.

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, the heavier the body weight of the autonomous flying robots 200c and 200d, the larger the local space is set, and the larger the maximum acceleration of the autonomous flying robots 200c and 200d, the larger the local space is set. This is because the greater the maximum acceleration and body weight of the autonomous flying robot 200 , the greater the wind generated by the rotor 260 , and the more likely it is to interfere with the autonomous flying robot 200 . On the other hand, when the maximum acceleration and body weight of the autonomous flying robot are small, interference with other autonomous flying robots 200 is less likely to occur. Become.

Also, the size of the local space may be set further considering the number of autonomous flying robots 200 that are interfering flying objects. For example, as shown in FIG. 9A, there are three autonomous flying robots 200a to 200c interfering flying objects in local space Lac, and in local space Lae as shown in FIG. In the case where the interfering flying objects are five autonomous flying robots 200a to 200e, the local space Lae is set larger. This is because the more autonomous flying robots 200 that are interfering flying objects, the easier it is for the autonomous flying robots 200 to interfere with each other.

Also, the size of the local space may be set further considering the size of each autonomous flying robot 200 that is an interfering flying object. This is because the larger the size of the autonomous flying robot 200, which is the interfering flying object, the easier it is for the autonomous flying robots 200 to interfere with each other.

Also, if the cube overlaps an obstacle or other non-flyable area 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. 10, 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. Further, as shown in FIG. 11, when a cube L3 centered at the interference position I3 is included in a cube L4 centered at the interference position I4, a rectangular parallelepiped L34 including the cubes L3 and L4 and a cube L4 is equal, so the cube L4 is set as the local space.

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

For the interfering vehicles 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 interfering vehicle, 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.

Also, a constraint condition may be set such that the distance between the interfering flying objects is at least 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.

where N is the number of autonomous flying robots 200 serving as interfering flying objects, K is the number of discrete times, and a _i [k] represents the acceleration of the i-th autonomous flying robot 200 at time k. .

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 processing, the individual routes of the interfering flying objects are corrected using the calculated avoidance routes.

For example, as shown in FIGS. 12A and 12B, with respect to individual routes Ra, Rd, and Re of autonomous flying robots 200a, 200d, and 200e, which are interfering flying objects, the route in the local space is the autonomous flying robot Individual routes Ra′, Rd′, Re′ after correction are obtained by correcting avoidance routes ra, rd, and re of 200a, 200d, and 200e.

<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 flow charts shown in FIGS. 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. 13 is executed.

In the route search processing of the management device 100 shown in FIG. 13, 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). Details of the local space setting process will be described later.

Then, the processing of step 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 route correction unit 142 calculates an avoidance route in which the interfering flying objects do not interfere with each other in the local space of interest by performing optimization calculations (step S112).

The route correction means 142 performs an individual route correction process for correcting the route information based on the calculated avoidance route (step S114), and ends the route search process.

The process of step S108 is implemented by the local space setting process shown in FIG.

In the local space setting process of the management device 100 shown in FIG. 14, the local space setting means 138 first determines whether or not there are a plurality of interference positions (step S120). If there are a plurality of interference positions, the local space setting means 138 obtains the distance between each interference position (distance between interference positions) (step S122). Then, the local space setting means 138 determines whether or not the distance between interference positions is equal to or less than a predetermined threshold Th2 (step S124). If the distance between the interference positions is equal to or less than the threshold Th2, the local space setting means 138 integrates the intermediate position between the two interference positions as a new interference position (step S126), and returns to step S122.

On the other hand, when there is only one interference position, or when the distance between the interference positions is greater than the threshold Th2, the processing of steps S128 to S132 of loop 4 is performed for each interference position. One interference position is selected to be processed in loop 4 . Hereinafter, the interference position selected in loop 4 will be referred to as "interference position of interest".

The local space setting means 138 reads the body information (weight, size, maximum acceleration) of the interfering flying object corresponding to the interference position of interest (step S128).

Then, the local space setting means 138 calculates the range of influence (cylinder) based on the aircraft information of each interfering flying object corresponding to the interfering position of interest (step S130). If the influence range has already been calculated and stored in the interference determination process, the value may be read out.

Then, the local space setting means 138 determines the size of the local space in consideration of the volume of the cylinder indicating the range of influence of each interfering flying object (step S132). For example, the volume of a cylinder indicating the range of influence of each interfering flying object is obtained, and the sum of the volumes of the cylinders is obtained. The volume of the local space is determined so that the sum of the obtained volumes is a predetermined proportion (eg, 5%) of the volume of the local space.

The local space setting means 138 determines the size of the solid in the local space so as to obtain the obtained volume of the local space, and sets the local space at a position where the approximate center of the local space is the interference position (step S134), the local space setting process is terminated.

The size of the local space may be determined directly from the values exemplified as the values used to calculate the range of influence without calculating the range of influence of the interfering flying object. For example, the size of the local space may be determined according to the body weight, maximum acceleration, size, and number of the autonomous flying robots 200 that are interfering flying objects.

As described above, in the movement control system 1000 according to the embodiment of the present invention, the local space including the interference position obtained from the flight paths of each of the plurality of autonomous flying robots is A local space with a size determined by at least one of the maximum acceleration and the weight of the airframe is set, and an avoidance route in which the interfering flying objects do not interfere with each other in the local space is calculated for each interfering flying object, and the flight path is calculated. By correcting, it is possible to obtain flight paths that do not interfere with each other with a small 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 certain autonomous flying robot and each position on the individual route of another autonomous flying robot.

For example, a cylinder representing the range of wind influence is placed at each position on the individual path of an autonomous flying robot, and similarly, a cylinder representing the range of wind influence 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, the size of the local space may be changed according to the wind speed. For example, the management device 100 is connected to a fixed anemometer (not shown) installed on the ground, wall surface, or the like via a closed network, and based on the wind speed (wind strength) in the flight space, You can modify the size of For example, if the wind speed in the flight space is high, the size of the local space may be corrected so that the local space is larger than normal for safety reasons.

Also, the local space may be set within a range that does not interfere with the flight path of the autonomous flying robot other than the interfering flying object. The shape of the local space is preferably a cube, but if the local space obtained interferes with the flight path of the autonomous flying robot other than the interfering flying object, the avoidance path is calculated for the flight path of the autonomous flying robot as well. need arises. Therefore, if the obtained local space (cube) interferes with the flight path of an autonomous flying robot other than the interfering flying object, it is preferable to set the local space as a rectangular parallelepiped having substantially the same volume and having a shape that does not interfere. be. Also, if the obtained local space (cube) interferes with the flight path of the autonomous flying robot other than the interfering flying object, the local space ( Alternatively, it may be set as a local space translated to a position that does not interfere with the flight path of the autonomous flying robot other than the interfering flying object.

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.

Also, although the case of using an autonomous flying robot as an example of a flying object has been described, the present invention is not limited to this, and flying objects other than autonomous flying robots may also be used.

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 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 (6)

A flight plan calculation device that obtains flight plans that do not interfere with each other for a plurality of flying objects that fly in a three-dimensional space,
Based on the flight plan from the current position to the target position for each of the plurality of flying objects, an interfering flying object and an interfering position, which are the flying objects that are in an interference relationship due to the spatial proximity between the flight plans, are determined. an interference determination means for obtaining
local space setting means for setting a local space containing the interference position in the three-dimensional space, the size of which is determined by at least one of acceleration and weight of each of the interference flying objects;
flight plan correction means for calculating an avoidance plan for each of the interfering flying objects in which the interfering flying objects do not interfere with each other in the local space, and correcting the flight plan;
A flight plan calculation device comprising: 2. The flight plan calculation apparatus according to claim 1, wherein said local space setting means determines the size of said local space further using at least one of the size and the number of said interfering flying objects. 3. The flight plan calculation apparatus according to claim 1, wherein said local space setting means sets said local space that does not interfere with said flight plan of said flying object other than said interfering flying object. The flight plan calculation device according to any one of claims 1 to 3, wherein the local space setting means sets the local space such that the interference position is substantially at the center of the local space. The local space setting means sets the local space to be larger as the wind speed measured by the wind speed measuring means for measuring the wind speed in the three-dimensional space increases. A flight plan calculator as described. A program for determining a flight plan of a plurality of flying objects that fly in a three-dimensional space without interfering with each other,
the computer,
Based on the flight plan from the current position to the target position for each of the plurality of flying objects, an interfering flying object and an interfering position, which are the flying objects that are in an interference relationship due to the spatial proximity between the flight plans, are determined. Interference determination means for obtaining
local space setting means for setting a local space containing the interference position in the three-dimensional space, the size of the local space being determined by at least one of acceleration and weight of each of the interfering flying objects; A program for functioning as flight plan correction means for calculating an avoidance plan in which the interfering flying objects do not interfere with each other in a local space for each of the interfering flying objects and correcting the flight plan.

Images