Moving body, measurement system using moving body, and measurement method using moving body

Abstract

To make it possible to achieve autonomous movement while averting an obstacle existing in the vicinity even in an environment where autonomous movement is hard to do.SOLUTION: A moving body detects an object located in the vicinity. The moving body generates and updates map data in which a space spreading in a moving direction is modeled into a three-dimensional space. At this time, the moving body generates three-dimensional space map data containing a position, size, and moving speed of a detected object, and continuously updates the map data during movement. Further, the moving body discriminates a static object and dynamic object on the basis of the updated three-dimensional space map data, calculates a movement route, which is obtained by coupling plural polynomial curves in order to create one curve, on the basis of a result of prediction of a motion of the dynamic object, and continuously determines the movement route, along which the static object and dynamic object can be averted, during movement. The moving body autonomously moves toward a measurement initiation position according to the determined movement route, performs measurement using a predetermined measurement instrument, analyzes a result of the measurement, and outputs a result of the analysis.SELECTED DRAWING: Figure 13

Description

[Technical Field]

[0001] The present invention relates to a measurement technique using a moving object. [Background technology]

[0002] Unmanned aerial vehicles (UAVs), which are a type of mobile object, have become easier to operate thanks to improvements in technologies related to attitude control, flight control, and the like. Furthermore, the number of small, low-cost models of such unmanned aerial vehicles is also increasing. Against this background, in recent years, attempts have been made to utilize unmanned aerial vehicles in various industrial fields. For example, in the fields of construction and civil engineering, unmanned aerial vehicles are sometimes used to inspect structures such as bridges and buildings (see Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2019-127245 Summary of the Invention [Problem to be solved by the invention]

[0004] In the fields of architecture and civil engineering, unmanned aerial vehicles are often used in work sites inside tunnels. For example, when inspecting tunnels, the advantage of using unmanned aerial vehicles to measure work locations is that inspections can be performed without workers having to enter dangerous areas. However, the communication environment with the outside world (e.g., global navigation satellite systems) inside tunnels is extremely poor. Furthermore, when moving to the desired measurement location, unmanned aerial vehicles must avoid static obstacles with complex structures and dynamic obstacles, such as workers and heavy machinery, whose movements are difficult to predict. Therefore, the inside of tunnels can be considered a difficult environment for unmanned aerial vehicles to navigate autonomously. Thus, when using unmanned aerial vehicles to perform measurement work inside tunnels, it is desirable for the unmanned aerial vehicle to navigate autonomously while avoiding surrounding obstacles and performing the desired measurements.

[0005] An aspect of the present invention aims to provide a technology that enables a robot to move autonomously while avoiding surrounding obstacles and perform a target measurement task even in an environment where autonomous movement is difficult. [Means for solving the problem]

[0006] A mobile body according to one aspect of the present invention is a mobile body that autonomously moves and performs predetermined measurement tasks in an environment where its own position cannot be estimated through external communication. The mobile body has a detection unit, a drive control unit, an autonomous movement control unit, and a measurement and analysis unit. The detection unit detects objects located around the mobile body. The drive control unit controls the drive unit to perform body control of the mobile body. The autonomous movement control unit controls the drive control unit to autonomously move toward a measurement start position according to a determined movement path. The measurement and analysis unit analyzes measurement results obtained by a predetermined measurement device and outputs the analysis results. The autonomous movement control unit further has a map data generation unit and a movement path determination unit. The map data generation unit generates and updates map data that models a space extending in the direction of movement of the mobile body in a three-dimensional space. The map data generation unit generates three-dimensional spatial map data including the position of a detected object, the size of the object, and an amount of change in position per unit time that indicates the object's movement speed. The map data generation unit also continuously updates the three-dimensional spatial map data while the mobile body is moving. The movement path determination unit distinguishes between static and dynamic objects among the detected objects based on the updated three-dimensional spatial map data. The movement path determination unit calculates a movement path by connecting multiple polynomial curves into a single curve based on the results of predicting the movement of the dynamic object, and determines this as the movement path for autonomous movement. The movement path determination unit continuously calculates a movement path that avoids static and dynamic objects while the mobile body is moving. [Effects of the Invention]

[0007] According to one aspect of the present invention, a mobile body can move autonomously while avoiding surrounding obstacles and perform a target measurement task even in an environment where autonomous movement is difficult. [Brief explanation of the drawings]

[0008] [Figure 1] FIG. 1 is a diagram illustrating an example of a measurement system according to the first embodiment. [Figure 2] FIG. 2 is a diagram showing an example of the appearance of an unmanned aerial vehicle. [Figure 3] FIG. 3 is a diagram illustrating an example of a computer system. [Figure 4] FIG. 4 is a schematic diagram showing the flow of excavation work inside a tunnel according to the first embodiment. [Figure 5] FIG. 5 is a flowchart showing the processing of the measurement system according to the first embodiment. [Figure 6] FIG. 6 is a diagram showing an example of movement of the unmanned aerial vehicle according to the first embodiment. [Figure 7] FIG. 7 is a flowchart showing the process of measuring an excavation shape by the unmanned aerial vehicle according to the first embodiment. [Figure 8] FIG. 8 is a flowchart showing the process of analyzing the measurement results obtained by the unmanned aerial vehicle according to the first embodiment. [Figure 9] FIG. 9 is a flowchart showing the process of visualizing an insufficient excavation area according to the first embodiment. [Figure 10] FIG. 10 is a diagram showing how an insufficient excavation area is visualized according to the first embodiment. [Figure 11] FIG. 11 is a diagram showing an overview of an autonomous movement function that avoids obstacles according to the first embodiment. [Figure 12] FIG. 12 is a functional block diagram of the measurement system (terminal device and unmanned aerial vehicle) according to the first embodiment. [Figure 13] FIG. 13 is a functional block diagram of the autonomous movement control unit according to the first embodiment. [Figure 14] FIG. 14 is a flowchart showing the process of generating visual support spatial map data according to the first embodiment. [Figure 15] FIG. 15 is a flowchart showing the obstacle region suggestion process according to the first embodiment. [Figure 16] FIG. 16 is a diagram illustrating an example of obstacle detection (proposal candidate detection) according to the first embodiment. [Figure 17] FIG. 17 is a diagram showing an example of a map data refinement method according to the first embodiment. [Figure 18] FIG. 18 is a flowchart showing the obstacle tracking and identification process according to the first embodiment. [Figure 19] FIG. 19 is a diagram illustrating an example of a method for identifying a moving obstacle according to the first embodiment. [Figure 20] FIG. 20 is a flowchart showing the movement prediction process for a moving obstacle according to the first embodiment. [Figure 21] FIG. 21 is a diagram illustrating an example of a method for predicting the movement of a moving obstacle according to the first embodiment. [Figure 22] FIG. 22 is a flowchart showing the travel route optimization process according to the first embodiment. [Figure 23] FIG. 23 is a flowchart showing the optimization parameter calculation process according to the first embodiment. [Figure 24] FIG. 24 is a flowchart showing the process of calculating the parameters of a static obstacle according to the first embodiment. [Figure 25] FIG. 25 is a diagram illustrating an example of a method for calculating a collision cost and a gradient for a static obstacle according to the first embodiment. [Figure 26] FIG. 26 is a flowchart showing the process of calculating the parameters of a moving obstacle according to the first embodiment. [Figure 27] FIG. 27 is a diagram showing an example of a method for calculating a collision cost and a gradient for a moving obstacle according to the first embodiment. [Figure 28] FIG. 28 is a flowchart showing the iterative optimization process of a travel route according to the first embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0009] First, in an embodiment of the present invention, an example of a work site to which the present invention is applied will be described in which a measurement system using an unmanned aerial vehicle (UAV) is used for "inspection work (excavation shape measurement work) near the tunnel face (near the forefront of the tunnel excavation)" is used. At the excavation site in a tunnel, the excavation shape is visually confirmed. Specifically, a worker enters the area directly below the face (dangerous area) and points out any insufficient excavation discovered through visual inspection, for example, using a laser pointer. In response, the operator of the excavator excavates again. After excavation, the worker checks whether the insufficient excavation has been resolved. As such, at excavation sites, skin collapse and face collapse are expected, and ensuring the safety of workers is desirable. Therefore, for the purpose of ensuring worker safety, measuring the excavation shape using an unmanned aerial vehicle is effective as an alternative to manual measurement work. From the perspective of such industrial utility effects, this embodiment will cite an example of application to measurement work of the excavation shape in a tunnel.

[0010] Hereinafter, embodiments of the present invention will be described with reference to the drawings. Note that the present invention is not limited to the contents of the embodiments. The configuration of the present invention can be appropriately modified without departing from the technical concept of the present invention.

[0011] First Embodiment [System Configuration] Fig. 1 is a diagram showing an example of a mobile object measurement system 1 according to this embodiment. As shown in Fig. 1, at least one mobile object is used at the measurement site. Specifically, it is an unmanned aerial vehicle 3.

[0012] The heavy equipment M is a heavy equipment whose drive unit is operated by the operator Op to perform excavation. The heavy equipment M performs re-excavation to eliminate insufficient excavation based on the results of measurement of the excavation shape by the unmanned aerial vehicle 3. Note that the heavy equipment M is not limited to a heavy equipment that the operator Op rides on, but may also be an unmanned heavy equipment operated by remote control from the operator Op. In the following description, the heavy equipment M will be referred to as a work vehicle M for convenience. The unmanned aerial vehicle 3 is an unmanned mobile object that flies. The unmanned aerial vehicle 3 flies inside the tunnel toward the vicinity of the face where the excavation shape can be measured, measures the excavation shape, and returns to its original position from which it took off after measurement. The unmanned aerial vehicle 3 moves autonomously while avoiding collisions with obstacles (static obstacles and dynamic obstacles) that exist between takeoff and returning to its original position using autonomous movement control, which will be described later, to perform the intended measurement work. In the following description, the unmanned aerial vehicle 3 will be referred to as a UAV 3 for convenience.

[0013] The measurement system 1 includes a terminal device 2. The terminal device 2 is carried and used by, for example, an operator Op of the work vehicle M. The terminal device 2 may be installed on the work vehicle M. The terminal device 2 includes a control device 20, an input device 21, and an output device 22. The control device 20 includes a computer system 100 (described later) and a communication device 23. The control device 20 is communicatively connected to the input device 21 and the output device 22, and executes various processes of the terminal device 2. The communication device 23 communicates between the terminal device 2 and the UAV 3. The communication device 23 is, for example, a wireless communication device. Therefore, the terminal device 2 and the UAV 3 communicate wirelessly via the communication device 23. The communication device 23 communicates between the terminal device 2 and the UAV 3, for example, using an IP address routing function.

[0014] The input device 21 receives input operations from, for example, an operator Op of the work vehicle M, and generates input data. The input data generated by the input device 21 is output to the terminal device 2. The input device 21 is at least one of a computer keyboard, a button, a switch, a touch panel, and the like.

[0015] The output device 22 provides information to, for example, an operator Op by receiving an output signal from the terminal device 2 and outputting data. The output device 22 is at least one of a display device capable of displaying display data, an audio output device capable of outputting audio, and a printing device capable of outputting printed matter. The display device includes a flat panel display such as a liquid crystal display (LCD) or an organic electroluminescence display (OELD).

[0016] The UAV 3 includes a flight control device 30, a flight device 31, a main body 32, a position sensor 33, a communication device 34, a power source 35, and a measurement device 36.

[0017] The UAV 3 flies by rotating the propeller 31P. Therefore, the flight device 31 includes the propeller 31P and a drive unit 31D. The drive unit 31D generates a driving force for rotating the propeller 31P. The drive unit 31D includes an electric motor. The flight control device 30 is called a flight controller, and performs predetermined calculations based on measurement data from a measurement device 36 (described later) to control the attitude and flight of the aircraft. The flight control device 30 controls the aircraft by sending control signals to the drive unit 31D based on the calculation results. The main body 32 is supported by the flight device 31.

[0018] The position sensor 33 detects the position of the UAV 3. The position sensor 33 detects the position of the UAV 3 using a global navigation satellite system (GNSS). The global navigation satellite system includes a global positioning system (GPS), etc. The global navigation satellite system detects the absolute position of the UAV 3, which is defined by coordinate data such as latitude, longitude, and altitude. The global navigation satellite system detects the position of the UAV 3, which is defined in a global coordinate system. The global coordinate system is a coordinate system fixed to the Earth. The position sensor 33 includes a GPS receiver, etc., and detects the absolute position (coordinates) of the UAV 3. Note that the application environment assumed in this embodiment is inside a tunnel, an environment in which communication with the outside is not possible, i.e., an environment in which the UAV's own position (current position) cannot be estimated. Therefore, in this embodiment, the position sensor 33 is not functional. The communication device 34 communicates between the terminal device 2 and the UAV 3. The communication device 34 is, for example, a wireless communication device. Therefore, the terminal device 2 and the UAV 3 communicate wirelessly via the communication device 34.

[0019] The power supply 35 supplies power to the electric motor. The power supply 35 includes a rechargeable battery, etc. In the following description, the power supply 35 will be referred to as the battery 35 for convenience.

[0020] The measurement device 36 includes an imaging device 37 and an inertial measurement unit (IMU), etc., and senses the surroundings of the UAV 3 and the aircraft itself to measure the flight environment and flight status. The imaging device 37 captures an image of a subject and acquires image data. The imaging device 37 has an optical device and an image sensor. The optical device includes an optical lens, etc. The image sensor includes a CCD (Couple Charged Device) image sensor or a CMOS (Complementary Metal Oxide Semiconductor) image sensor, etc. The inertial measurement unit (not shown) includes an angular velocity meter (gyro) that detects the angular velocity (rotational motion) of the moving object, an accelerometer that detects acceleration (linear motion), etc., and measures the movement of the moving object. In addition to the accelerometer and angular velocity meter, the inertial measurement unit may also include a magnetometer (3-axis), a temperature sensor, a processor, etc.

[0021] The UAV 3 includes an expansion device (expansion board) 4 that performs autonomous movement control, which will be described later. The expansion device 4 includes an autonomous flight control device 40, a detection device 41, and a measurement device 42. The autonomous flight control device 40 includes a computer system 100, which will be described later. The autonomous flight control device 40 is communicatively connected to the detection device 41, the measurement device 42, and the flight control device 30, and performs various processes to autonomously move while avoiding obstacles and measure the excavation shape. Specifically, the autonomous flight control device 40 calculates and determines a movement path that avoids obstacles in real time based on an input signal (detection result) from the detection device 41. The autonomous flight control device 40 outputs a control signal (drive command) to the flight control device 30 according to the calculation result (determined movement path). After arriving at the measurement start position, the autonomous flight control device 40 outputs a control signal (drive command) to the flight control device 30 according to a predetermined measurement path. As a result, the measurement device 42 measures the excavation shape while flying along the measurement path.

[0022] The detection device 41 is a sensor that detects objects, and in this embodiment, it is a depth sensor (e.g., a depth-sensing camera). The depth sensor automatically detects nearby objects using measurement technologies such as stereo vision, time-on-flight, and structured light, and acquires detection data for the object (depth of the detection space: distance to the detected object) in real time. Therefore, in this embodiment, the autonomous flight control device 40 is configured to generate and update visually supported spatial map data (three-dimensional spatial map data) described below using the technical features of the depth sensor. The visually supported spatial map data is map data that models a three-dimensional space that enables identification of static and dynamic obstacles and tracking and prediction of the movement of dynamic obstacles. This allows the UAV 3 to predict the movement of obstacles from its own position, calculate a travel path that avoids the obstacles in real time, and autonomously travel to a destination location, even in an environment where external communication is not possible. In the following description, the detection device 41 is referred to as a depth camera 41 for convenience.

[0023] The measuring device 42 is a sensor that detects the shape of the measurement target, and in this embodiment is an image sensor (for example, an RGB camera). The RGB camera acquires a captured image as RGB data. In this embodiment, the RGB camera captures an image of the excavation location to measure the excavation shape. In the following description, the measuring device 42 will be referred to as the RGB camera 42 for convenience.

[0024] FIG. 2 is a diagram showing an example of the appearance of a UAV 3 according to this embodiment. In this embodiment, the extension device 4 is installed on the upper part of the main body 32 of the UAV 3. Therefore, a depth camera 41 and an RGB camera 42 are provided on the upper part of the main body 32 of the UAV 3. Note that the depth camera 41 and the RBG camera 42 may be cameras with different configurations, or may be an integrated camera. An example of an integrated camera is an RGB-D camera. An RGB-D camera is a camera that can acquire both color (RGB) and depth (D) data in real time. An RGB-D camera can output RGB data and depth data as a single image frame data (RGBD data).

[0025] [Basic configuration of computer system] FIG. 3 is a diagram illustrating an example of a computer system 100 according to this embodiment. The control device 20 of the terminal device 2, the flight control device 30 of the UAV 3, and the autonomous flight control device 40 constitute the computer system 100. The computer system 100 includes a processor 101, a memory 102, a storage 103, and an interface (I / F) 104. The processor 101 is, for example, a central processing unit (CPU). The memory 102 includes a nonvolatile memory such as a read-only memory (ROM) and a volatile memory such as a random access memory (RAM). The storage 103 includes a hard disk drive (HDD) and a solid state drive (SSD). The interface 104 includes an input / output circuit. The functions provided by the devices 20, 30, and 40 are stored as programs in the storage 103. The processor 101 provides each function by reading the programs from the storage 103, loading them into the memory 102, and executing information processing. The program may be distributed to the computer system 100 via a public line. Although Fig. 3 shows an example of the computer system 100 using a single processor, the present embodiment is not limited to this. The control device 20 of the terminal device 2 and the flight control device 30 and autonomous flight control device 40 of the UAV 3 may be configured, for example, by a computer system using two or more processors (multiprocessor).

[0026] [Tunnel excavation work] FIG. 4 is a schematic diagram showing the flow of excavation work inside a tunnel according to this embodiment. Tunnel excavation work is divided into several work processes. First, several personnel, including at least the site supervisor who manages the site and the operator Op who operates the work vehicle M, confirm the work plan for the excavation work (step S1). The operator Op operates the work vehicle M according to the confirmed work plan and performs the excavation work (step S2). At this time, the operator Op carries a terminal device 2 and operates the work vehicle M while checking the confirmed work plan, etc. After that, when the operator Op completes the predetermined work, he stops operating the work vehicle M. In response to the stop of operation of the work vehicle M, the UAV 3 performs the work of confirming the excavation shape (step S3). The UAV 3 autonomously moves from a predetermined position to a measurement start position near the face while avoiding obstacles. The UAV 3 then continues flying near the face to measure the excavation shape, and when it arrives at the measurement end position, it again autonomously moves to the predetermined position while avoiding obstacles.

[0027] FIG. 5 is a flowchart showing the processing of the measurement system 1 according to this embodiment. FIG. 6 is a diagram showing an example of movement of the UAV 3 according to this embodiment. The processing shown in FIG. 5 is a detailed processing executed in the process of step S3 described above. In the measurement system 1, the processing shown in FIG. 5 is executed between the terminal device 2 and the UAV 3 during on-site work. Furthermore, while processing is being executed in the measurement system 1, the UAV 3 moves as shown in FIG. 6.

[0028] The UAV 3 takes off from a predetermined position (step S11). When the UAV 3 starts flying, it acquires a surrounding image including depth data using the depth camera 41 and starts generating visual support spatial map data Map based on the UAV 3's own position (current position) as shown in FIG. 6 (step S2). The generation process (update process) of the visual support spatial map data Map is continuously executed while the UAV 3 is flying. As a result, the visual support spatial map data Map is updated to the latest data necessary for autonomous movement while avoiding obstacles. Based on the generated visual support spatial map data Map, the UAV 3 identifies static obstacles So and dynamic obstacles Do, tracks and predicts the movement of the dynamic obstacles Do, and calculates a movement route Rt that avoids the static obstacles So and dynamic obstacles Do. As a result, the UAV 3 autonomously moves while avoiding the static obstacles So and dynamic obstacles Do according to the calculated movement route Rt (step S13).

[0029] When the UAV3 arrives at the measurement start position near the face, it starts measuring the excavation shape (step S14). The measurement results are stored in the UAV3. The excavation shape measurement process will be described later with reference to FIG. 7. When the UAV3 arrives at the measurement end position near the face, it moves autonomously while avoiding static obstacles So and dynamic obstacles Do according to the calculated movement path Rt (step S15). Thereafter, the UAV3 lands at a predetermined position (step S16). At this time, the generation and update process of the visual support spatial map data Map also ends.

[0030] The UAV 3 analyzes the stored measurement results (step S17). The analysis process of the measurement results will be described later with reference to FIG. 8. Thereafter, the UAV 3 outputs the analysis results to the terminal device 2 via the communication device 34. Based on the input analysis results, the terminal device 2 visualizes the areas where excavation is insufficient via the output device 22 (step S18). The visualization process of the areas where excavation is insufficient will be described later with reference to FIG. 9.

[0031] FIG. 7 is a flowchart showing the process of measuring the excavation shape (the process of step S14) by the UAV 3 according to this embodiment. As shown in FIG. 7, the UAV 3 performs measurement while flying, for example, a zigzag route near the excavation face (step S21). While moving, the UAV 3 captures images of the excavation site using the RGB camera 42 and stores the acquired captured images (RGB data) as measurement results (step S22). The UAV 3 determines whether it has arrived at the measurement end position (step S23). If the UAV 3 determines that it has not arrived at the measurement end position (step S23: NO), it continues the measurement movement. On the other hand, if the UAV 3 determines that it has arrived at the measurement end position (step S23: YES), it ends the measurement process. In this way, the UAV 3 flies along a predetermined measurement route and captures images of the excavation site while moving its own position, thereby measuring the entire excavation face evenly. As a result, the UAV 3 acquires and stores multiple images of the excavation site that have been continuously captured as measurement results.

[0032] FIG. 8 is a flowchart showing the analysis process (processing of step S17) of the measurement results by the UAV 3 according to this embodiment. As shown in FIG. 8, the UAV 3 acquires multiple images (multiple RGB data) of the measurement results from a predetermined storage area (step S31). The UAV 3 performs SfM (Structure from Motion) analysis processing on the acquired multiple images (step S32). SfM is an analysis technique that generates a three-dimensional model of an object by estimating the capture position of each of the multiple images and analyzing the positional relationship between the images. The generated three-dimensional model is composed of a large amount of point cloud data. In this embodiment, this analysis technique is used to acquire a three-dimensional model of the excavation shape from multiple images of the excavation site. The UAV 3 outputs the point cloud data acquired in this manner to the terminal device 2 as the analysis result.

[0033] FIG. 9 is a flowchart showing the visualization process of insufficient excavation areas according to this embodiment (the process of step S18). FIG. 10 is a diagram showing the visualization of insufficient excavation areas according to this embodiment. As shown in FIG. 9, the terminal device 2 acquires analysis results from the UAV 3 (step S41). The terminal device 2 acquires data of a three-dimensional model of the excavation shape composed of point cloud data as the analysis result. The terminal device 2 generates and displays a heat map that visualizes the insufficient excavation areas from the acquired analysis results (step S42). The terminal device 2 approximates the data of the three-dimensional model of the excavation shape and the data of the design model to a specific function using the least squares method, and superimposes them to generate a heat map that visualizes the difference (insufficient excavation) between the actual excavation shape and the reference shape in the design. In FIG. 10, the insufficient excavation areas are displayed as area C in the heat map. This allows the operator Op to grasp the insufficient excavation areas.

[0034] [Feature Overview] In tunnels, where communication with the outside world is impossible, UAVs must avoid static obstacles with complex structures and dynamic obstacles whose movements are difficult to predict. Therefore, to autonomously navigate through such complex environments, UAVs must recognize the travel space in real time and generate a travel path (Rt) that avoids static obstacles (S) and dynamic obstacles (D). One example of a technology for recognizing the travel space in real time is a vision-based algorithm for detecting obstacles. For example, it extracts bounding boxes of obstacles using geometric information from images. However, this method cannot distinguish between static obstacles (S) and dynamic obstacles (D). Other examples of techniques for generating a travel path (Rt) that avoids static obstacles (S) and dynamic obstacles (D) include path planning methods based on a single map representation, such as a geometric map, an occupancy map, or a Euclidean signed distance field map (ESDF). However, while these methods are effective in static environments, they are unable to simultaneously represent and handle static obstacles (S) and dynamic obstacles (D). Furthermore, in order to avoid dynamic obstacles Do whose movements are difficult to predict, real-time, high-frequency route generation (real-time route planning) is required. However, in the environment of UAV3 equipped with extension device 4 and performing real-time route planning using this device, computational resources are limited, making it difficult to realize processing with high computational costs.

[0035] FIG. 11 is a diagram illustrating an overview of an autonomous movement function for obstacle avoidance according to this embodiment. Therefore, in the measurement system 1 according to this embodiment, the UAV 3 realizes autonomous movement as shown in FIG. 11. The UAV 3 generates visual support spatial map data Map shown in FIG. 11(A). The visual support spatial map data Map is three-dimensional dynamic map data (dynamic map) that can identify static obstacles So and dynamic obstacles Do from the UAV's own position Cp and track and predict the movement of the dynamic obstacles Do. The UAV 3 uses a depth image (depth map) from the depth camera 41 to model and represent the static obstacles So in voxels and the dynamic obstacles Do using bounding boxes, and maps these on the grid of an occupancy map. As a result, the UAV 3 generates map data that can recognize the positions (relative positions) of the static obstacles So and dynamic obstacles Do relative to the UAV's own position Cp as occupied voxels. The UAV 3 tracks the movement of the dynamic obstacles Do using specific filtering. While moving, the UAV 3 updates the map data based on the latest depth image captured by the depth camera 41 at its own position Cp. This allows the UAV 3 to identify static obstacles So with complex structures and dynamic obstacles Do whose movements are difficult to predict, and track and predict the movements of the dynamic obstacles Do, even in an environment where communication with the outside is impossible.

[0036] As shown in FIG. 11(B), the UAV 3 calculates in real time an optimal route Rt toward the destination location while avoiding static obstacles So and dynamic obstacles Do located ahead, according to the visual support spatial map data Map. To avoid the dynamic obstacle Do, whose movement is difficult to predict, the UAV 3 employs an optimization algorithm (B-spline curve optimization algorithm) specific to the present application, thereby reducing the computational cost (improving the computation speed) required to optimize the route Rt. Furthermore, the UAV 3 repeats optimization to adapt to dynamic environments in which the surrounding environment changes over time. This allows the UAV 3 to perform real-time calculations to determine the route plan within an appropriate computation time. Therefore, the UAV 3 can avoid the dynamic obstacle Do, whose movement is difficult to predict, even in a computational environment with limited resources.

[0037] 12 is a functional block diagram of the measurement system 1 according to this embodiment. The terminal device 2 and the UAV 3 included in the measurement system 1 according to this embodiment have the functions shown in FIG.

[0038] [Terminal Device] The terminal device 2 has a transmitting / receiving unit 200, a visualization unit 201, an instruction unit 202, etc., and provides predetermined functions by the respective functional units operating in cooperation with each other.

[0039] The transmitter / receiver 200 transmits and receives data to and from devices other than the terminal device 2, such as the UAV 3. The transmitter / receiver 200 is a function included in the communication device 23 shown in FIG.

[0040] The visualization unit 201 visualizes predetermined information to convey the information to the operator Op. Based on the analysis results acquired from the UAV 3, the visualization unit 201 visualizes the difference in the excavation shape (insufficient excavation) by superimposing a three-dimensional model of the analysis results on the design model and displaying it as a heat map. This allows the operator Op to grasp the areas where excavation is insufficient. The visualization unit 201 is a function possessed by the control device 20 and the output device 22 shown in FIG. 1.

[0041] The instruction unit 202 instructs the operator Op based on the visualized information. If the difference in the excavation shape is equal to or greater than a predetermined value, the instruction unit 202 instructs the operator Op to carry out re-excavation using the work vehicle (excavator) M. The instruction unit 202 may additionally display supplementary information or provide audio guidance to help the operator Op understand, for example, which part of the face needs to be re-excavated and how much excavation is required. The instruction unit 202 is a function possessed by the output device 22 shown in FIG. 1.

[0042] [UAV] The UAV 3 has a transceiver unit 300, a drive control unit 301, a drive unit 302, a first detection unit 303, etc., and the functional units work in cooperation to provide predetermined functions. Furthermore, the UAV 3 has an autonomous movement control unit 401, a second detection unit 402, a measurement analysis processing unit 403, etc. as extended functions, and the functional units work in cooperation to provide predetermined functions.

[0043] The transmitter / receiver 300 transmits and receives data to and from devices other than the UAV 3, such as the terminal device 2. The transmitter / receiver 300 is a function of the communication device 34 shown in FIG.

[0044] [Movement control function] The drive control unit 301 controls the UAV 3's flight by transmitting control signals to the drive unit 302, receiving detection data from the first detection unit 303, and receiving control signals from the autonomous movement control unit 401. The drive control unit 301 is a function of the flight control device 30. For example, when the drive control unit 301 receives a predetermined control signal after the power supply 35 is turned on, it controls takeoff from a predetermined position, etc. Based on the movement route data received from the autonomous movement control unit 401, the drive control unit 301 controls the flight of the UAV 3 along the route and landing at a predetermined position, etc.

[0045] The driving unit 302 controls the driving device 31D to perform the instructed behavior according to, for example, a control signal from the drive control unit 301, and drives the propeller 31P to rotate. Note that the driving unit 302 is a function that the flight device 31 has.

[0046] The first detection unit 303 acquires sensing results from various sensors as detection results, and transfers the acquired detection results as detection data to the drive control unit 301. The first detection unit 303 is a function that the measurement device 36 has.

[0047] [Autonomous movement control function] The autonomous movement control function is a function possessed by the extension device 4 mounted on the UAV 3. The autonomous movement control unit 401 performs autonomous flight control to avoid static obstacles So and dynamic obstacles Do based on the detection data of the second detection unit 402 and the visual support spatial map data Map. The autonomous movement control unit 401 identifies static obstacles So and dynamic obstacles Do from the self-position Cp based on the detection data of the second detection unit 402 and generates visual support spatial map data Map that can track and predict the movement of the dynamic obstacles Do. The autonomous movement control unit 401 also updates the map data based on the latest detection data of the second detection unit 402 at the self-position Cp while moving. The autonomous movement control unit 401 uses the latest visual support spatial map data Map to calculate in real time an optimal movement route Rt toward the destination position while avoiding static obstacles So and dynamic obstacles Do located ahead. The autonomous movement control unit 401 optimizes the movement route Rt using an optimization algorithm unique to the present application. The functions of the autonomous movement control unit 401 will be described later with reference to FIG. 12. The autonomous movement control unit 401 is a function that the autonomous flight control device 40 has.

[0048] The second detection unit 402 acquires sensing results from various sensors as detection results and transfers the acquired detection results as detection data to the autonomous movement control unit 401. The second detection unit 402 is a function of the depth camera 41 and the RGB camera 42 (or the RGB-D camera). The second detection unit 402 acquires depth data and RGB data from the depth camera 41 and the RGB camera 42, respectively, and transfers them to the autonomous movement control unit 401 as one image frame data.

[0049] [Measurement and analysis function] The measurement analysis processing unit 403 performs a predetermined analysis process on the detection results input from the second detection unit 402, i.e., image data, and transmits the analysis result data to the terminal device 2 via the transmission / reception unit 300. The measurement analysis processing unit 403 is a function of the autonomous flight control device 40. In this embodiment, multiple images of the excavation site are given as an example of the detection results. The measurement analysis processing unit 403 performs a predetermined analysis process (for example, SfM analysis process, etc.) on the multiple images. This makes it possible to acquire a three-dimensional model of the excavation shape composed of point cloud data. In order to reduce calculation costs, the measurement analysis processing unit 403 may be realized by an integrated circuit for image analysis processing (ASIC: Application Specific Integrated Circuit).

[0050] Fig. 13 is a functional block diagram of an autonomous movement control unit 401 according to this embodiment. The autonomous movement control unit 401 has the functions shown in Fig. 13. The autonomous movement control unit 401 has a storage unit 410, a map data generation unit 420, a movement route determination unit 430, etc., and these functional units operate in cooperation with each other to provide predetermined functions related to autonomous movement.

[0051] The various data according to this embodiment include history data 411, map data 412, parameter data 413, etc. The storage unit 410 stores these data in a predetermined storage area. The storage unit 410 is a function of the memory 102 and the storage 103 shown in FIG. 3.

[0052] The history data 411 is data relating to the movement history of the moving obstacle Do, which is used when tracking and predicting the movement of the moving obstacle Do. The history data 411 is, for example, position data and movement speed data of the moving obstacle Do in the visual support spatial map data Map (three-dimensional dynamic map data). The position data is time-series data indicating the movement position on the grid of the occupancy map, and the movement speed data is time-series data indicating the movement speed calculated from the amount of change in the movement position over a predetermined time. These data are stored in association with each frame data of the multiple captured images.

[0053] The map data 412 is visual support space map data Map, which is generated and stored by the map data generation unit 420 (described later). The stored map data is updated by the map data generation unit 420. The visual support space map data Map is three-dimensional dynamic map data shown in FIG. 11(A). Static obstacles So are modeled in voxel units and represented as three-dimensional objects, while dynamic obstacles Do are modeled using bounding boxes and represented as three-dimensional objects. The visual support space map data Map is map data of a virtual space in which these three-dimensional objects are mapped onto a grid of an occupancy map based on position information acquired as detection results. Furthermore, the visual support space map data Map contains data related to objects as three-dimensional object data, and therefore includes the position, size (length, width, height), and movement speed (amount of change in position per unit time) of the obstacle. Thus, the visual support space map data Map is map data that models the space extending in the direction (line of sight or movement direction) viewed from the UAV 3's own position Cp in a three-dimensional space.

[0054] The parameter data 413 is various parameters such as weighting coefficients and normalization coefficients for optimization used when optimizing a travel route. The various stored parameters are updated by a travel route determination unit 430, which will be described later. The initial values ​​of each parameter are set in advance by the user depending on the purpose.

[0055] [Visual support spatial map data generation function (update function)] The map data generation unit 420 generates and updates visual support spatial map data Map that can identify static obstacles So and dynamic obstacles Do from the vehicle's own position Cp and track and predict the movement of the dynamic obstacles Do. The map data generation unit 420 has an area proposal unit 421, an obstacle identification unit 422, an obstacle movement prediction unit 423, etc. The map data generation unit 420 executes the processing shown in Fig. 14 and causes the above functional modules to operate in conjunction with each other, thereby generating and updating the visual support spatial map data Map.

[0056] Fig. 14 is a flowchart showing the generation process of visual support space map data Map according to this embodiment. As shown in Fig. 14, the area proposal unit 421 detects static obstacles So and dynamic obstacles Do, merges the detected obstacles with static map data (occupancy map data), and proposes an area where the obstacles exist (step S51). The process by the area proposal unit 421 will be described later with reference to Figs. 15, 16, and 17.

[0057] Next, the obstacle identifying unit 422 refers to the history of the time-series positions of the detected obstacles (tracks the history) and identifies moving obstacles Do from among the detected obstacles through a predetermined filtering process (step S52). As a result, detected obstacles that have not been identified as moving obstacles Do can be identified as static obstacles So. This process by the obstacle identifying unit 422 will be described later with reference to FIGS. 18 and 19.

[0058] Next, the obstacle identification unit 422 inputs the time-series positions of the moving obstacle Do into the static map data, and deletes the occupied voxels (mapping objects) of the previous positions due to the movement of the moving obstacle Do from the static map data (step S53). The moving obstacle Do should not be included in the area where the static obstacle So exists, i.e., the static area, of the proposed area. Therefore, the movement history of the obstacle in the static area, i.e., the moving obstacle Do, should be removed (cleaned).

[0059] One removal method is to delete the occupied voxels at the previous position of the dynamic obstacle Do from the static region. However, when reflecting a dynamic environment that changes over time in map data, past bounding box data of the dynamic obstacle Do (movement history data 411) is required for tracking and predicting its movement. Therefore, as described above, simply deleting the occupied voxels would result in deleting the movement history data, which would result in a failure to search for the dynamic obstacle Do when tracking and predicting its movement. Therefore, in this embodiment, the obstacle identification unit 422 records the deletion history of the occupied voxels. Specifically, the obstacle identification unit 422 deletes the corresponding occupied voxels based on the position (past position) of the dynamic obstacle Do for each image frame of the time-series images. Then, the obstacle identification unit 422 records data related to the deleted occupied voxels in a hash table as deletion history data at the time of deletion. As a result, the voxels of the deleted history data are recognized as previously occupied voxels (effectively utilized as movement history data) in the refinement process of the map data that is performed at the next update (when tracking and predicting movement). Note that the deleted history data is included in the history data 411, and is stored and updated in the storage unit 410 by the obstacle identification unit 422.

[0060] The obstacle movement prediction unit 423 calculates environmental probabilities for predicting a movement path based on a Markov Chain (step S54). The processing by the obstacle movement prediction unit 423 will be described later with reference to FIGS. 20 and 21.

[0061] [Proposed obstacle area] Fig. 15 is a flowchart showing the obstacle area suggestion process according to this embodiment. Fig. 16 is a diagram showing an example of obstacle detection (suggestion candidate detection) according to this embodiment. Fig. 17 is a diagram showing an example of a map data refinement method according to this embodiment.

[0062] As shown in FIG. 15, the region proposal unit 421 acquires a depth image from the second detection unit 402 (depth camera 41) (step S61). Next, based on the acquired depth image, the region proposal unit 421 generates a three-dimensional bounding box Bb of the obstacle, including the rough position and size (length, width, height) of the static obstacle So and the dynamic obstacle Do (step S62). At this time, the region proposal unit 421 calculates U map data using a histogram of column depth values ​​in the depth image (depth map). The depth image is data that enables the depth distance (distance to an object) for each pixel to be recognized. The three-dimensional bounding box Bb has the following advantages. First, the three-dimensional bounding box Bb can model objects with a simple shape, which can take various shapes. Therefore, the computational cost required for generation is low and processing is fast. Therefore, the three-dimensional bounding box Bb is suitable for real-time generation and update processing of visual support spatial map data Map for recognizing dynamic environments. A second advantage is that the three-dimensional bounding box Bb can include data such as the center position, vertical and horizontal lengths of the object, etc. Therefore, the three-dimensional bounding box Bb can recognize the position, size, directionality (movement of the object), etc. of the object as seen from the self-position Cp in three dimensions.

[0063] The process of generating a three-dimensional bounding box Bb of an obstacle will be described below, taking the case where the detected obstacle is a moving obstacle Do as an example. Assume that the moving obstacle Do is represented by RGB data as shown in FIG. 16(A). The moving obstacle Do has a depth value that changes continuously in the depth image. In this case, if it is assumed that the color tone of the moving obstacle Do is significantly different from the color tone of the background, the region proposal unit 421 generates U map data by grouping regions of pixels having depth values ​​greater than a threshold, and can recognize the moving obstacle Do. As a result, the moving obstacle Do is recognized in the U map data as a first two-dimensional bounding box Bba as shown in FIG. 16(D). Through this process, the region proposal unit 421 can obtain data regarding the approximate length and width (length and width data) of the moving obstacle Do.

[0064] Furthermore, the dynamic obstacle Do is recognized in the depth image as a second two-dimensional bounding box Bbb as shown in Figure 16(B). As shown by the wavy lines in Figures 16(B) and (D), the region proposal unit 421 searches for pixels having depth values ​​near the first two-dimensional bounding box on the U map data corresponding to each column of the depth image. This allows the region proposal unit 421 to obtain data regarding the approximate height of the dynamic obstacle Do (height data).

[0065] If I is the image frame number, o is the obstacle number, and T is the time, the moving obstacle Do is the center coordinate of the object: P o I =[x o I ,y o I ] T and a size S with depth d o I =[w o I ,h o I ] T Furthermore, as shown in FIG. 16(C), the box can be projected onto the image frame and converted into a map frame. Specifically, when M is the map frame number, the bounding box Bb has center coordinates: P o M =[x o M ,y o M ,d o M ] and size: S = [w o M ,h o M ,l o M In this way, the region proposal unit 421 generates a three-dimensional bounding box Bb of the detected obstacle from the depth image and converts the box into a map frame.

[0066] Returning to the description of FIG. 15, the region proposal unit 421 performs a refinement process on the map data (step S63). The accuracy of the three-dimensional bounding box Bb of the detected obstacle generated by the process of step S62 is affected by noise from the depth image. Therefore, on the static map (on the grid of the occupancy map), the sizes of the multiple three-dimensional bounding boxes Bb may differ from one another (boxes of various sizes may exist on the map). Therefore, the accuracy of the boxes is not sufficient. Therefore, in this embodiment, in order to improve robustness and accuracy, the region proposal unit 421 normalizes the bounding box Bb of the detected obstacle generated from the depth image. In this embodiment, this process is referred to as a refinement process.

[0067] As shown in FIG. 17(A), the region proposal unit 421 applies a preset expansion coefficient (normalization coefficient) C to the size (w, h, l) of the bounding box Bb1 of the detected obstacle generated from the depth image. inflate 17(B) , the region proposal unit 421 determines and proposes the extended bounding box Bb3, which has the smallest size among all the occupied voxels in the data, as the region where the obstacle exists. In this way, the region proposal unit 421 normalizes the sizes of the multiple bounding boxes Bb of the detected obstacle generated from the depth image. As a result, noise is removed from the depth image, and the accuracy of the three-dimensional bounding box Bb is improved.

[0068] As described above, the region proposal unit 421 models the detected obstacle by generating a three-dimensional bounding box Bb. Furthermore, the region proposal unit 421 removes noise from the three-dimensional bounding box Bb by normalizing the size, thereby improving the accuracy of the modeling. The region proposal unit 421 proposes the three-dimensional bounding box Bb generated in this manner as a region on the static map (on the grid of the occupancy map) where an obstacle exists (a region on the spatial map that the UAV3 should avoid).

[0069] [Obstacle identification and tracking] Fig. 18 is a flowchart showing obstacle tracking and identification processing according to this embodiment. Fig. 19 is a diagram showing an example of a method for identifying a moving obstacle Do according to this embodiment.

[0070] 18, the obstacle identifying unit 422 associates the obstacle with the position of each detected obstacle and records it as history data 411 for tracking and predicting the movement (step S71). Specifically, the obstacle identifying unit 422 stores a group of multiple obstacles detected in an image frame at time t as one data set. t O C ={ t o0 C , t o1 C ,··· t o n C}. The obstacle identification unit 422 also associates the data of each three-dimensional bounding box Bb as t o0 C and the previous time t-1 t-1 O n C Of which, data t o i C The closest data t-1 o i C The criteria for determining the closest data is based on the center distance between the three-dimensional bounding boxes Bb. The overlap rate between the three-dimensional bounding boxes Bb: r i,j can be defined by the following formula:

number

[0071] where A i,j is the data at time t t o i C and data t-1 o i C The overlapping area of ​​the top surfaces of the three-dimensional bounding boxes Bb between A and Bb is i is the data t o i C The obstacle identification unit 422 calculates the movement history of each obstacle over time k based on the overlap rate: t H i,k C ={ t o i C , t-1 o i C ,··· t-k o i C}. In order to reduce the influence of the three-dimensional bounding box Bb of the partially detected obstacle, the obstacle identifying unit 422 determines whether the entire obstacle is included in the camera's imaging area (FOV: Field of View) in the image frame at elapsed time k. As a result, if the obstacle identifying unit 422 determines that the entire three-dimensional bounding box Bb of the detected obstacle is included in the imaging area, it determines the size of the three-dimensional bounding box Bb. This determination process is effective for an imaging device with a small imaging area.

[0072] As described above, the obstacle identifying unit 422 converts a group of obstacles detected in image frames at the same time into one data set. t O C Furthermore, the obstacle identifying unit 422 generates a plurality of data sets for each elapsed time. t O C Based on the same 3D bounding box Bb, the time series data sett H i,k C The movement history data 411 is generated as follows.

[0073] Next, the obstacle identification unit 422 estimates the moving speed of the obstacle (step S72). The obstacle identification unit 422 applies a Kalman filter to the map frame (a time-series representation area on a spatial map) to track the detected obstacle and estimate its moving speed. To make the following explanation easier to understand, it is assumed that all parameters described below exist within the map frame. The state vector of the obstacle is X=[x, y,...] T and the measurement vector is Z=[o x ,o y ,v x ,v y ] T where v x ,v y are the x- and y-direction velocities calculated based on the obstacle's state change per unit time (two-dimensional position change per unit time). Furthermore, if A is the state transition model, Q is the covariance of the model noise, H is the measurement model, R is the covariance of the measurement noise, and t is time, the model and measurement value of the state vector X and measurement vector Z can be defined by the following equations.

number

number

[0074] Next, the obstacle identifying unit 422 determines whether the estimated speed of the detected obstacle is equal to or greater than a predetermined value (step S73). The predetermined value for this determination is set by the user. If the estimated speed of the obstacle is equal to or greater than the predetermined value (step S73: YES), the obstacle identifying unit 422 calculates the continuity coefficient C con is the predetermined value T con The obstacle identifying unit 422 determines whether the continuity coefficient C con is the predetermined value T conIf the estimated speed of the obstacle is less than the predetermined value (step S73: NO), or if the continuity coefficient C con is the predetermined value T con In the above cases (step S74: NO), the detected obstacle is identified as a static obstacle So (step S76).

[0075] For example, if the estimated speed of a detected obstacle exceeds a user-defined threshold (predetermined value), an obstacle identification process is performed to identify the obstacle as a moving obstacle Do. However, in the above identification process, there is a possibility that some static obstacles So may be identified as moving obstacles Do due to measurement noise of the detection device 41. Therefore, in this embodiment, the obstacle identification unit 422 is configured to filter out static obstacles So that have been erroneously identified as moving obstacles Do (using a continuity coefficient C con Step S74: identification processing of a moving obstacle Do based on

[0076] FIG. 19(A) shows the movement history data 411 of the moving obstacle Do for six time periods. The white dots: P1-P6 and the wavy arrows indicate the positions and trajectories of the obstacles recorded as the movement history. Black dots: P1 GT -P6 GT is the true position. Figure 19(B) shows the history data 411 of the movement of the static obstacle So for six time periods. The white dots: P1-P6 and the wavy arrows are the positions and trajectories of the obstacles recorded as the movement history. The black dots: P GT is the true position.

[0077] The obstacle identifying unit 422 calculates a displacement vector (solid arrow) using a set of obstacle position data (for example, P1 at the first time and P4 at the second time) from the movement history Pa defined by the following equation.

number

number

[0078] Thereafter, the obstacle identifying unit 422 calculates the cosine value of the angle θ between each pair of displacement vectors D having successive indices.

number

[0079] Assuming that an obstacle moves at a constant speed in a short time, as shown in FIG. 19(A), in the case of a dynamic obstacle Do, the angle θ between the displacement vectors should be an acute angle (close to 0°). On the other hand, as shown in FIG. 19(B), in the case of a static obstacle So, the angle θ between the displacement vectors should be an obtuse angle (close to 180°). Therefore, in this embodiment, by utilizing this property, the continuity coefficient C con is defined by the following formula:

number

[0080] The obstacle identifying unit 422 identifies the detected obstacles with a continuity coefficient C con is the threshold (predetermined value) T con The smaller detected obstacle is temporarily recorded as a tentative identification result as a moving obstacle Do. The obstacle identification unit 422 records a plurality of past tentative identification results for each obstacle for a predetermined number of frames as identification history data. As a result, if the obstacle identification unit 422 finds that a predetermined number or more of similar recognition results exist in the past tentative identification results from the identification record, it finally identifies the corresponding detected obstacle as a moving obstacle Do.

[0081] As described above, the obstacle identifying unit 422 estimates the moving speed of the detected obstacle using a Kalman filter, and if the estimated speed is less than a predetermined value, identifies the detected obstacle as a static obstacle So. On the other hand, if the estimated speed is equal to or greater than a predetermined value, the obstacle identifying unit 422 provisionally identifies the detected obstacle as a moving obstacle Do. Taking into consideration the possibility that a static obstacle So has been erroneously identified as a moving obstacle Do, the obstacle identifying unit 422 performs processing to filter out the erroneously identified static obstacle So.

[0082] [Predicting the movement path of dynamic obstacles] Fig. 20 is a flowchart showing the movement prediction process of a moving obstacle Do according to this embodiment. Fig. 21 is a diagram showing an example of a movement prediction method of a moving obstacle Do according to this embodiment.

[0083] 20, the obstacle movement prediction unit 423 acquires a route library including a plurality of route candidates that has been generated in advance through experiments (step S81). The obstacle movement prediction unit 423 calculates a probability distribution of the route candidates from the route library (step S82).

[0084] As shown in Fig. 21, the walking data P path The obstacle movement prediction unit 423 acquires the route library generated in this way. Then, the obstacle movement prediction unit 423 calculates the probability distribution P of each route from among the multiple route candidates included in the route library. path The most likely route is selected by calculating the probability distribution P path To calculate the initial state of the Markov chain, P init can be defined as the probability value of each route using the following formula:

number

[0085] Here, all values ​​are discrete probability distributions (discrete Gaussian distributions). The probability values ​​are P 1 / 2 init is obtained from a Gaussian kernel with mean P 1 / 2 initThis is because, among multiple route candidates from a left turn route to a right turn route, corresponds to a straight route, and it is assumed that people tend to choose a route that is as close to a straight line as possible.Furthermore, the state transition matrix of the Markov chain can be defined as follows.

number

[0086] Humans tend to maintain the direction of movement as long as possible. Therefore, each row of the state transition matrix is ​​P i,j trans In order to take into account the interaction between the environment and the obstacle, the obstacle movement prediction unit 423 uses the static map data (occupancy map data) as a reference (Dist i ) and calculates the distance from the reference starting point (self-position Cp) to the collision point with the obstacle. Next, the obstacle movement prediction unit 423 inputs the calculated distance into the following SoftMax function to obtain the environmental probability P env Calculate the environmental probability P env indicates the probability of choosing a particular path when considering interactions with the environment and obstacles.

number

[0087] Finally, the transition state can be predicted by the following equation:

number

[0088] As described above, the obstacle movement prediction unit 423 generates a route library including multiple route candidates from the walking data of a person. The obstacle movement prediction unit 423 calculates the probability distribution P pathAt this time, the obstacle movement prediction unit 423 selects the most probable route by calculating the static map data as a reference (Dist i ) and calculates the distance from the reference starting point to the collision point with the obstacle. The obstacle movement prediction unit 423 calculates the environmental probability P env The obstacle movement prediction unit 423 calculates the environmental probability P env Based on this, the future movement route of the moving obstacle Do is selected from multiple route candidates and set as the predicted route.

[0089] As described above, the map data generation unit 420 includes an area proposal unit 421, an obstacle identification unit 422, and an obstacle movement prediction unit 423. The area proposal unit 421 generates a three-dimensional bounding box Bb that can represent the position and size of a detected obstacle based on the depth image, and maps it on a static map (on the grid of the occupancy map). In this way, the area proposal unit 421 proposes an area where either a static obstacle So or a dynamic obstacle Do exists.

[0090] The obstacle identification unit 422 records the time-series positions of the detected obstacles to generate history data 411 of the movement of the detected obstacles, and calculates the movement speed of the detected obstacles using a Kalman filter for the movement history. The obstacle identification unit 422 identifies a moving obstacle Do from among the detected obstacles based on the calculated speed. At this time, the obstacle identification unit 422 uses a continuity coefficient C calculated based on the angle θ between the displacement vectors to filter out the moving obstacle Do that should originally be identified as a static obstacle So. con This enables the obstacle identifying unit 422 to track the moving obstacle Do, and further reduces erroneous identification of the obstacle (improving the identification accuracy of the moving obstacle Do).

[0091] The obstacle movement prediction unit 423 calculates the probability distribution P of the route candidate from the route library. pathAt this time, the obstacle movement prediction unit 423 calculates the initial state P init is set as the probability value of each path, and the state transition matrix is ​​used to calculate the distance to the obstacle for each path (route candidate) in the path library, taking into account the interaction between the environment and the obstacle. The obstacle movement prediction unit 423 calculates the environmental probability P env As a result, the obstacle movement prediction unit 423 calculates the calculated environmental probability P env Based on this, the obstacle movement prediction unit 423 selects a future movement route of the moving obstacle Do from a plurality of route candidates.

[0092] As described above, the map data generation unit 420 can generate visual support spatial map data Map by modeling the position and size of detected obstacles and associating three-dimensional objects in voxel or bounding box format with static map data (occupancy map data). Furthermore, the map data generation unit 420 associates the identification results of static obstacles So and dynamic obstacles Do, the movement history of the identified dynamic obstacles Do, and the movement prediction results of the dynamic obstacles Do with the static map data (occupancy map data). The map data generation unit 420 updates the visual support spatial map data Map to the latest data according to the dynamic environment. Thus, the map data generation unit 420 can generate and update, in real time, visual support spatial map data Map that can identify static obstacles So and dynamic obstacles Do from the self-position Cp and track and predict the movement of the dynamic obstacles Do. In other words, the map data generation unit 420 can generate and update, in real time, three-dimensional spatial map data of the measurement site. Therefore, even in an environment where communication with the outside world is extremely poor, the UAV 3 can recognize the latest state of the surrounding environment in real time by using the visual support spatial map data Map. Therefore, the UAV 3 can optimize the movement route Rt by taking into account future changes in the state of the surrounding environment (movement of the dynamic obstacle Do).

[0093] [Route determination function (generation function)] The movement path determination unit 430 calculates and determines an optimal movement path Rt for avoiding static obstacles So and dynamic obstacles Do when the UAV 3 heads toward the measurement start position. The process of determining the movement path Rt is continuously executed while the UAV 3 is flying. The movement path determination unit 430 has two optimization units for optimizing the movement path Rt. Specifically, the movement path determination unit 430 has a first optimization unit 431 and a second optimization unit 432. The movement path determination unit 430 calculates and determines the optimal movement path Rt for the UAV 3 by operating the above-mentioned functional modules in cooperation with each other. The first optimization unit 431 executes the process shown in FIG. 22 to generate the optimal movement path Rt using a B-spline curve and perform optimization to solve the minimization problem of the objective cost function. The second optimization unit 432 is adapted to a dynamic environment in which the surrounding environment changes over time, so it repeatedly performs optimization even after deriving a solution to the minimization problem.

[0094] [B-spline curve optimization] FIG. 22 is a flowchart showing the optimization process for the travel route Rt according to this embodiment. As shown in FIG. 22, the first optimization unit 431 generates a B-spline curve based on the control points of one or more curves (step S91). The first optimization unit 431 determines whether or not an obstacle exists on the travel route Rt drawn using the B-spline curve (step S92). At this time, the first optimization unit 431 refers to the map data 412 stored in the storage unit 410, i.e., the visual support space map data Map. The first optimization unit 431 determines whether or not an obstacle exists on the travel route Rt based on the positions and sizes of static obstacles So and dynamic obstacles Do, the predicted movement of the dynamic obstacles Do, and the like. If an obstacle exists on the travel route Rt (step S92: YES), the first optimization unit 431 calculates optimization parameters (step S93). The first optimization unit 431 solves an unconstrained optimization problem (a mathematical formula (mathematical model) that defines a minimization problem of an objective cost function) based on the calculated optimization parameters, and determines a travel route Rt (step S94). Note that "unconstrained" means that there are no restrictions on the range of parameters.

[0095] A B-spline curve of degree k is a single curve created by connecting multiple k-1 degree polynomial curves using control points defined on a knot vector. Basis functions can be defined by knot values ​​and can be calculated recursively. When a rough path or target position is given, the first optimization unit 431 parameterizes the movement path Rt as a set of control points as shown in the following equation. The first and last control points correspond to the start and end positions of the UAV3.

number

[0096] The optimization parameters are the N-2(k-1) intermediate control points P i The first optimization unit 431 optimizes the B-spline curve according to a gradient-based formulation without constraints (performing unconstrained nonlinear optimization). Therefore, when the set S is used as the optimization parameters, the objective cost function can be defined by the following equation. The first optimization unit 431 solves the minimization problem of the objective cost function and determines the travel route Rt.

number

[0097] 23 is a flowchart showing the calculation process of the optimization parameters (costs) according to this embodiment. As shown in FIG. 23, the first optimization unit 431 calculates the control limit cost C control(Step S101). When the UAV 3 moves along the movement route Rt, it is necessary to move the UAV 3 within a range that does not exceed its own control limit (control limit value for stable movement). The control limit cost C control The B-spline curve derivative can be expressed by another B-spline curve. Therefore, the velocity and acceleration control points V i and A i can be defined by the following formula:

number

[0098] where δt is the elapsed time. Therefore, the maximum speed: v max and acceleration: a max Given this, the control margin cost function can be defined as follows:

number

[0099] Next, the first optimization unit 431 calculates the smoothness cost C smooth The parameters for the smoothness cost C are calculated (step S102). smooth is applied to prevent the generation of a jerky movement path Rt. i and the control point J of the jerk, which is the derivative of the acceleration. i Given and, the smoothness cost function can be defined as follows:

number

[0100] The first optimization unit 431 calculates the collision cost C staticA parameter related to (the possibility of colliding with the static obstacle So) is calculated (step S103). This processing by the first optimization unit 431 will be described later with reference to FIGS.

[0101] The first optimization unit 431 calculates the collision cost C dynamic A parameter related to (the possibility of colliding with a moving obstacle Do) is calculated (step S104). This processing by the first optimization unit 431 will be described later with reference to FIGS.

[0102] [Collision cost calculation for static obstacles] 24 is a flowchart showing the calculation process of the parameters of the static obstacle So according to this embodiment. static 10A and 10B are diagrams illustrating an example of a method for calculating a gradient.

[0103] The static obstacle So is modeled and represented by occupied voxels on a static map (on the grid of the occupancy map). Therefore, it is not possible to directly obtain the collision cost and gradient for optimization. Therefore, in this embodiment, the first optimization unit 431 uses a circle-based guide point algorithm to calculate the collision cost C static The collision cost and gradient of the control points, which are optimization parameters, can be estimated using guide points. Note that guide points are points (collision avoidance guide points) where the control points of the B-spline curve are reset on the collision avoidance path Rts determined by searching for obstacle avoidance.

[0104] As shown in FIG. 24, when a travel path Rtp that collides with a static obstacle So is given, the first optimization unit 431 calculates a collision control point P i ccp At this time, when the first optimization unit 431 receives, for example, a movement path (collision path) Rtp, it identifies a collision control point P ccp As a result, the first optimization unit 431 uses a function to identify the plurality of identified collision control points P ccpis the output value of the function, and one collision data set S col ={P 0 ccp ,P 1 ccp ,···P n ccp} to get

[0105] Next, the first optimization unit 431 searches for a collision-avoidance route (step S112). The first optimization unit 431 searches for a collision-free travel route (collision-avoidance route) Rts using a predetermined route search algorithm such as A* or Dijkstra.

[0106] Next, the first optimization unit 431 sets new control points (sets collision avoidance guide points) on the searched route Rts (step S113). col Each collision control point P in i ccp The starting point P of the collision avoidance route Rts determined by the search S swp and the end point P E swp Project it onto the tangent (line segment) LS.

[0107] The first optimization unit 431 uses the projected points to calculate a direction angle θ from (0, π). direct The projection point and angle are used to project a ray onto the collision avoidance path Rts determined by the search. The intersection of this ray and the collision avoidance path Rts determined by the search is the intersection P i guide The point in question is the collision control point P i ccp Guide point (point reset to avoid collision of control point) P gp As shown in FIG. 25, the direction angle θ direct Since describes a semicircle, this algorithm is circle-based.

[0108] Next, the first optimization unit 431 calculates the collision cost C static and the gradient is calculated (step S114). iccp Collision cost C static is the obtained guide point P i guide and the preset safety distance d safe and can be defined by the following formula:

number

[0109] Here, the signed distance (output value of the signDist function) defines positive and negative distances as control points on the outside and inside of the obstacle. The above formula uses a cubic function, and the signed distance (the first distance between the guide point and the obstacle) is the safe distance (the second distance between the UAV3 and the obstacle required to avoid a collision) d safe In this way, the first optimization unit 431 assigns a penalty to each collision control point P i ccp The gradient of can be calculated according to the chain rule based on the following formula:

number

[0110] where P^ i is the initial collision control point P i ccp The negative gradient direction is the direction of the collision control point P i ccp is pushed from the obstacle area to the collision avoidance area.

[0111] 25 shows an optimized movement path Rto that smoothly turns to avoid the static obstacle So by using the above circle-based guide point algorithm by the first optimization unit 431. In this way, the first optimization unit 431 determines the movement path Rto that avoids the static obstacle So by optimizing the B-spline curve. In addition, in order to reduce the calculation cost of the optimization process (to improve the calculation speed), the first optimization unit 431 uses the circle-based guide point algorithm to calculate the collision cost C staticand approximate the gradient.

[0112] [Collision cost calculation for dynamic obstacles] Fig. 26 is a flowchart showing the calculation process of the parameters of the moving obstacle Do according to this embodiment. Fig. 27 is a flowchart showing the calculation process of the collision cost C dynamic 10A and 10B are diagrams illustrating an example of a method for calculating a gradient.

[0113] Unlike a static obstacle So, a dynamic obstacle Do changes state and is uncertain from the viewpoint of object movement. Therefore, optimizing the travel path Rt using only the currently acquired detection, tracking, and prediction data for the dynamic obstacle Do is unreliable. Therefore, in this embodiment, the first optimization unit 431 applies a horizontal distance field to estimate the collision cost and gradient for the dynamic obstacle Do. This is a method for evaluating the distance to the safe zone of the travel position based on the predicted future position.

[0114] As shown in Fig. 26, the first optimization unit 431 identifies the predicted position of the moving obstacle Do (step S121). Fig. 27 shows a horizontal distance field. More specifically, Fig. 27 shows the horizontal distance field from the current position O0 of the moving obstacle Do to the predicted position O k The first optimization unit 431 calculates the future position of the moving obstacle Do at a prediction time k using linear prediction, taking into account the current position O0 and velocity V0 of the moving obstacle Do: {O1, O2, . . . O k} to get

[0115] Next, the first optimization unit 431 calculates the current position O0 of the moving obstacle Do from the predicted position O k Collision area R (collision area R A and the collision area R B ) is set (step S122). In order to construct a horizontal distance field, the first optimization unit 431 first draws a circle with a safe radius (safe distance) r centered on the current position O0. As a result, the first optimization unit 431 determines a circular first collision area R A Set.

[0116] The reliability of future predictions depends on the predicted position. k Therefore, taking into consideration the low reliability of future prediction, the first optimization unit 431 optimizes the distance from the current position O0 to the predicted position O1 so that the safe distance r becomes zero with the prediction time k. k As a result, the first optimization unit 431 linearly reduces the value of the distance to the second collision area R 1 , which is a substantially conical shape indicated by a dotted line in FIG. B The first optimization unit 431 sets a circular first collision area R A and the approximately conical second collision area R B By setting two collision regions R with different shapes, it is possible to prevent excessive avoidance behavior compared to a cylindrical collision region (a collision region derived by maintaining a safe distance r even if the predicted position is unreliable). The first optimization unit 431 can generate a movement route Rto that takes future predictions into consideration.

[0117] The first optimization unit 431 calculates the distance to the safety area of ​​the movement position according to each of the two collision areas R of different shapes, in order to estimate the collision cost with the moving obstacle Do.

[0118] The first optimization unit 431 defines a circular first collision area R A and the approximately conical second collision area R B 27, the distance to the safe area of ​​the moving position of the moving obstacle Do is calculated (step S123). 1 ccp (P i,c ) is the circular first collision area R surrounded by the arc ACB and the line segments (first and second line segments) AO0 and BO0. A Therefore, the distance to the safe area (the third distance from the collision control point to the safe area) Δd i can be defined by the following formula:

number

number

[0119] Next, the first optimization unit 431 calculates the collision cost C dynamic and the gradient is calculated (step S124). i ccp Collision cost C dynamic is the distance to the safe area Δd i It can be defined by the following formula using

number

[0120] In this way, the first optimization unit 431 calculates a circular first collision area R with the current position O0 of the moving obstacle Do as the center and the safety radius r as the safety distance. A In addition, taking into consideration the low reliability of future prediction, the first optimization unit 431 sets the distance from the current position O0 to the predicted position O1 so that the safe distance r becomes zero along with the prediction time k. k The first optimization unit 431 applies such a horizontal distance field to estimate the collision cost and gradient for the moving obstacle Do, and determines a moving path Rto that avoids the moving obstacle Do by optimizing a B-spline curve.

[0121] [Iterative optimization] Objective cost function C total The minimization problem of (S) has no constraints and includes multiple objectives. Therefore, solving this problem only once may not guarantee the safety of the movement path Rt of the UAV 3. Therefore, in this embodiment, the second optimization unit 432 optimizes the control points P of the B-spline curve on the collision avoidance path Rts that has been determined by searching for obstacle avoidance until the entire movement path Rt does not collide with the detected obstacle. i is reset and the problem is solved repeatedly.

[0122] Collision cost C against a static obstacle So static and the collision cost C against the dynamic obstacle Do. dynamic and help the first optimization unit 431 determine the collision avoidance route Rto. However, through careful study by the inventors, it has been found that this affects the safety of the movement route Rt of the UAV 3 for the following reasons.

[0123] The first reason is that the collision cost C static ,C dynamic Weight α static ,α dynamic It has been found that if the value of is not sufficiently large, it affects the safety of the travel route Rt. In this case, the second optimization unit 432 uses a preset expansion coefficient λ to set the weight α static ,α dynamic Increase the value of .

[0124] The second reason is that the objective cost function C total In solving the minimization problem of (S), the control point P i However, there is a possibility that the object will be pushed towards a new obstacle, and the collision cost and gradient calculated up to that point will become invalid. In this case, it was found that the safety of the movement path Rt will be affected. Therefore, the control point P after optimization i Therefore, the second optimization unit 432 performs a process of setting new control points. Therefore, the second optimization unit 432 performs the process repeatedly, and each time, the control points P that need to be reset are iCheck whether the control point P exists. i The second optimization unit 432 repeatedly executes the above process until the entire movement route Rt does not collide with the detected obstacle.

[0125] 28 is a flowchart showing the iterative optimization process of the movement route Rt according to this embodiment. As shown in FIG. 28, the second optimization unit 432 iteratively optimizes the optimized control points P i The second optimization unit 432 determines whether or not it is necessary to reset the control point P i If it is determined that the objective cost function C needs to be reset (step S131: YES), new control points are set (step S132). At this time, the second optimization unit 432 searches again for a collision-free movement path (collision avoidance path) Rts, and sets new control points (sets collision avoidance guide points) on the searched path Rts. Next, the second optimization unit 432 calculates the objective cost function C total (S) (step S133). At this time, the second optimization unit 432 updates the reset control point P i Based on this, the control marginal cost C control , the route smoothness cost C smooth , the collision cost C against a static obstacle So static , and the collision cost C for the dynamic obstacle Do dynamic Next, the second optimization unit 432 updates the travel route Rt (step S134). At this time, the second optimization unit 432 calculates the objective cost function C total The minimization problem of (S) is solved again, and the travel route Rt is updated to the optimized travel route Rto. Next, the second optimization unit 432 verifies the obstacle parameters (step S135). At this time, the second optimization unit 432 verifies the collision cost C static and the collision cost C against the moving obstacle Do dynamic Verify that is minimized.

[0126] On the other hand, the second optimization unit 432 optimizes the control point P i If it is determined that there is no need to reset the collision cost C static ,Cdynamic Weight α static ,α dynamic The second optimization unit 432 determines whether the value of each collision cost C is equal to or less than a predetermined value (step S136). static ,C dynamic Weight α static ,α dynamic If it is determined that the value of is equal to or less than the predetermined value (step S136: YES), the weight α static ,α dynamic (step S137). At this time, the second optimization unit 432 updates the value of the preset expansion coefficient λ with the weight α static ,α dynamic By multiplying the value of static ,α dynamic After that, the second optimization unit 432 proceeds to the process of step S133.

[0127] As described above, the second optimization unit 432 optimizes the optimized control point P i If it is necessary to reset the path Rt, the following process is performed until the entire path Rt does not collide with the detected obstacle. The second optimization unit 432 sets the control point P of the B-spline curve on the collision avoidance path Rts that has been searched and determined to avoid the obstacle. i and redefine the objective cost function C total The second optimization unit 432 repeatedly solves the minimization problem of (S). As a result, the second optimization unit 432 determines in real time the optimal movement route Rto according to the dynamic environment in which the surrounding environment changes over time. As a result, the UAV 3 avoids the dynamic obstacle Do whose movement is difficult to predict.

[0128] As described above, when an obstacle is present on the movement route Rt drawn by the B-spline curve, the movement route determination unit 430 calculates the optimization parameters. At this time, when a rough route or a target position is given, the movement route determination unit 430 calculates the movement route Rt by arranging the control points P of the B-spline curve. i The travel route determination unit 430 uses the set S as an optimization parameter. Therefore, the travel route determination unit 430 determines the controllable cost C control , the route smoothness cost C smooth, the collision cost C against a static obstacle So static , and the collision cost C for the dynamic obstacle Do dynamic are weighted and combined to optimize the objective cost function C total (S) is solved. As a result, the movement path determination unit 430 determines an optimal movement path (collision avoidance path) Rto that avoids the static obstacle So and the dynamic obstacle Do. Furthermore, the movement path determination unit 430 adds control points P of the B-spline curve to the collision avoidance path Rts that has been determined by searching for obstacle avoidance until the entire movement path Rt does not collide with the detected obstacles. i The travel route determination unit 430 resets the objective cost function C total The UAV 3 repeatedly solves the minimization problem of (S). As a result, the UAV 3 calculates in real time an optimal movement route Rt toward the destination location while avoiding static obstacles So and dynamic obstacles Do located ahead. The UAV 3 then uses the B-spline curve optimization algorithm described above to avoid dynamic obstacles Do, whose movement is difficult to predict, thereby reducing the computational cost required to optimize the movement route Rt (improving the calculation speed). Furthermore, by repeatedly performing optimization, the UAV 3 can autonomously move along a highly safe movement route Rt while avoiding static obstacles So and dynamic obstacles Do, even in a dynamic environment where the surrounding environment changes over time. Furthermore, by repeatedly performing optimization, the UAV 3 performs real-time calculations to determine the path plan within an appropriate calculation time. Therefore, the UAV 3 can avoid dynamic obstacles Do, whose movement is difficult to predict, even in a computational environment with limited resources.

[0129] [effect] As described above in detail, the measurement system 1 according to this embodiment provides the following advantages. The UAV 3 included in the system 1 includes an extension device 4 that performs autonomous movement control. The extension device 4 includes an autonomous flight control device 40, a detection device 41, and a measurement device 42. The detection device 41 is a depth camera that detects the distance to a detected object in real time. The autonomous flight control device 40 calculates and determines a movement path Rt that avoids obstacles in real time based on an input signal (detection result) from the detection device 41. The autonomous flight control device 40 outputs a control signal (driving command) to the flight control device 30 according to the calculation result (determined movement path Rt). After arriving at the measurement position, the autonomous flight control device 40 outputs a control signal (driving command) to the flight control device 30 according to a predetermined measurement path. As a result, the measurement device 42 measures the excavation shape while flying along the measurement path.

[0130] According to the measurement system 1 of this embodiment, the UAV 3 can autonomously move while avoiding surrounding obstacles and perform the intended measurement work, even in environments where autonomous movement is difficult. Therefore, in the measurement system 1 of this embodiment, the UAV 3 can be effectively used as an alternative to workers' measurement of the excavation shape during inspection work near the tunnel face. Therefore, workers do not need to enter the dangerous area near the face. This ensures the safety of workers and prevents accidents at the excavation site (improving the safety of inspection work near the face). Furthermore, the UAV 3 notifies the operator Op of areas where excavation is insufficient via the terminal device 2. Therefore, the measurement system 1 of this embodiment not only improves work safety, but also productivity and construction accuracy.

[0131] Furthermore, the autonomous flight control device 40 generates and updates three-dimensional spatial map data of the measurement site in real time using depth images from the depth camera 41. Specifically, the autonomous flight control device 40 generates visual support spatial map data Map that can identify static obstacles So and dynamic obstacles Do in the measurement site and track and predict the movements of the dynamic obstacles Do. Furthermore, the autonomous flight control device 40 updates the visual support spatial map data Map in real time in response to changes in the surrounding environment of the UAV 3.

[0132] In this way, the autonomous flight control device 40 can generate and update three-dimensional spatial map data of the measurement site in real time. Therefore, even in an environment where the communication environment with the outside world is extremely poor and it is difficult to recognize the UAV 3's own position Cp in space, the UAV 3 can recognize the latest state of the surrounding environment in real time by using the visual support spatial map data Map. Therefore, the UAV 3 can optimize the travel route Rt by taking into account future changes in the state of the surrounding environment (movement of the dynamic obstacle Do).

[0133] Furthermore, the autonomous flight control device 40 uses the latest visual support space map data Map to calculate in real time an optimal movement route Rt toward the destination position while avoiding static obstacles So and dynamic obstacles Do located ahead. Specifically, the autonomous flight control device 40 uses a B-spline curve optimization algorithm to calculate the control points P i The set S of obstacles is used as optimization parameters, and the objective cost function C is defined as total (S) is solved. As a result, the autonomous flight control device 40 determines an optimal movement path (collision avoidance path) Rto that avoids the static obstacle So and the dynamic obstacle Do. Furthermore, the autonomous flight control device 40 adds control points P of the B-spline curve to the collision avoidance path Rts that has been searched and determined to avoid the obstacles until the entire movement path Rt does not collide with the detected obstacles. i The autonomous flight control device 40 resets the objective cost function C total By repeatedly solving the minimization problem of (S), the UAV3 calculates in real time the optimal movement route Rt toward the destination position while avoiding the static obstacles So and the dynamic obstacles Do located ahead.

[0134] By using the above-described B-spline curve optimization algorithm to avoid dynamic obstacles Do whose movements are difficult to predict, UAV3 can reduce the computational cost required to optimize the movement route Rt (improving the calculation speed). Furthermore, by repeating the optimization, UAV3 can autonomously move along a highly safe movement route Rt while avoiding static obstacles So and dynamic obstacles Do, even in a dynamic environment where the surrounding environment changes over time. Furthermore, by repeatedly performing the optimization, UAV3 can perform real-time calculations to determine the path plan within an appropriate calculation time. Therefore, UAV3 can avoid dynamic obstacles Do whose movements are difficult to predict, even in a computational environment with limited resources.

[0135] [Variations] In the above embodiment, the objective cost function C total The minimization problem of (S) is mathematically modeled as an unconstrained optimization problem. This mathematical model can be determined by specifying an appropriate solution method according to the definition of the problem, and the technology of the present invention is not limited to a specific mathematical model.

[0136] In the above embodiment, the UAV 3 is configured to recognize the latest state of the surrounding environment in real time and optimize the travel route Rt by taking into account future changes in the surrounding environment (the movement of the dynamic obstacle Do). Furthermore, the UAV 3 is configured to repeatedly execute the optimization process until the entire travel route Rt is free from collisions with detected obstacles. However, even with the above configuration, there are limitations on the computational resources of the extension device 4, and the UAV 3 may not be able to calculate the optimal travel route Rto for obstacle avoidance. In such cases, the following modified embodiment is possible. The surrounding conditions of the UAV 3 change over time. Therefore, as the surrounding conditions change, the calculation conditions for the optimal travel route Rto for obstacle avoidance also change. In consideration of this, for example, if the optimization calculation fails, the UAV 3 attempts to land on the spot. The UAV 3 takes off again after a predetermined time has elapsed, updates the visual support spatial map data Map based on the detection results of the detection device 41, and further optimizes the travel route Rt. As a result, the UAV 3 recalculates the optimal travel route Rto even if the calculation fails. Therefore, UAV3 can improve the rate at which the desired measurement work is carried out.

[0137] In the above embodiment, an example was shown in which the work vehicle M was driven by direct operation of the operator Op, but the technology of the present invention is not limited to this. For example, the work vehicle M may be configured to drive autonomously based on sensing data around the vehicle, work plan data, design model data, etc. In this case, the UAV 3 can communicate data with the work vehicle M and transmit analysis data based on measurements to the work vehicle M. As a result, the work vehicle M recognizes the difference (insufficient excavation) between the actual excavation shape and the design reference shape based on the received analysis data and design model data. As a result, the work vehicle M excavates again in the recognized insufficient excavation area. The work vehicle M then transmits a notification of the completion of the excavation work to the UAV 3. Upon receiving the notification of the completion of the excavation work, the UAV 3 performs measurement work again. In this way, when the work vehicle M is an autonomously driven heavy machine, the UAV 3 and the work vehicle M can work together in an unmanned environment. In this case, a manager can monitor the UAV 3 and the work vehicle M in the unmanned environment using a remote camera and manage the work process.

[0138] In the above embodiment, the UAV 3 has been described using a multicopter (rotor-based aircraft) as an example, but the technology of the present invention is not limited to this. Examples of UAV 3 include fixed-wing aircraft and airship-type aircraft. Examples of mobile objects include not only vehicles and aircraft capable of moving on land and in the air, but also ships and submarines capable of moving on water and underwater. It is desirable to appropriately select these mobile objects depending on the purpose and environment of the work to be performed. For example, in underwater tunnel excavation work, an unmanned submarine can be used instead of an unmanned aircraft.

[0139] In the above embodiment, an example was given in which the measurement system 1 using a mobile object was applied to inspection work near the face inside a tunnel, but the technology of the present invention is not limited to this. The technology of the present invention can be applied to any work that can be performed using at least one mobile object. Furthermore, the technology of the present invention is effective as an alternative to manual work, with the aim of ensuring safety at the work site, improving productivity, and / or improving construction accuracy. [Explanation of symbols]

[0140] 1: Measurement system, 2: Terminal device, 3: UAV (Unmanned Aerial Vehicle), 4: Expansion device; 301: Drive control unit; 401: Autonomous movement control unit; 410: Memory unit, 411: History data, 412: Map data, 413: Parameter data; 420: map data generation unit, 421: area proposal unit, 422: obstacle identification unit; 430: movement path determination unit, 431: first optimization unit (B-spline curve optimization unit), 432: second optimization unit (iterative optimization unit); 100: Computer system, 101: Processor, 102: Memory, 103: Storage, 104: Interface. Map: Visually supported spatial map data, So: static obstacles, Do: dynamic obstacles, Rt: travel route.

Claims

1. A mobile body that moves autonomously and performs a predetermined measurement task in an environment where its own position cannot be estimated by communication with the outside, a detection unit that detects objects located in the vicinity; a drive control unit that controls the drive unit to perform aircraft control; an autonomous movement control unit that controls the drive control unit to autonomously move toward a measurement start position according to the determined movement path; a measurement analysis unit that analyzes the measurement results obtained by a predetermined measurement device and outputs the analysis results; The autonomous movement control unit a map data generation unit that generates map data that is a three-dimensional model of a space extending in the moving direction of the moving body, the three-dimensional spatial map data including the position of the detected object, the size of the object, and an amount of change in the position per unit time that indicates the moving speed of the object, and that continuously updates the three-dimensional spatial map data while the moving body is moving; a movement path determination unit that identifies static objects and dynamic objects from among the detected objects based on the updated three-dimensional spatial map data, calculates a movement path by connecting a plurality of polynomial curves into a single curve based on the result of predicting the movement of the dynamic objects, and determines the movement path as the movement path for the autonomous movement, and continuously calculates the movement path that avoids the static objects and the dynamic objects while the moving body is moving.

2. The three-dimensional spatial map data is Among the detected objects, the static objects include data of one or more first three-dimensional objects modeled in voxel units, and the dynamic objects include data of one or more second three-dimensional objects modeled in bounding boxes, each of the first and second three-dimensional objects is mapped onto a grid of an occupancy map, which is static map data, based on position data of the corresponding object, and the mapped three-dimensional objects indicate the existence area of ​​the object in the space extending in the direction of movement of the moving body, the detection unit includes a depth sensor, The map data generation unit The moving body according to claim 1 , wherein the positions and sizes of the static object and the dynamic object are identified based on a histogram of depth values ​​acquired from the depth sensor, and the first and second three-dimensional objects are generated.

3. the map data generation unit has an area proposal unit that indicates an area where the detected object exists, The region proposal unit multiplying the size of the three-dimensional object by a predetermined coefficient to expand the size of the three-dimensional object in a three-dimensional direction; The moving body according to claim 2 , wherein the three-dimensional object whose size is relatively small compared to the sizes of the other three-dimensional objects among the expanded three-dimensional objects is determined as the existence region of the object.

4. the map data generation unit has an object identification unit that identifies whether the detected object is a static object or a dynamic object, The object identification unit performing a Kalman filter process on map frame data corresponding to time-series data in the three-dimensional spatial map data; Estimating the speed of movement of the detected object; The moving body according to claim 1 , further comprising: determining whether the estimated moving speed is equal to or greater than a predetermined value; and identifying the detected object as the dynamic object when it is determined that the moving speed is equal to or greater than the predetermined value.

5. the map data generation unit has an object identification unit that identifies whether the detected object is a static object or a dynamic object, The object identification unit Identifying a first position at a first time and a second position at a second time of the same object from a movement history that records the time-series position data of the detected object, and calculating a first displacement vector between the identified first position and the identified second position; a third position at a third time and a fourth position at a fourth time for the same object are identified, and a second displacement vector that intersects with the first displacement vector is calculated from the third position to the fourth position; The moving body according to claim 1 , wherein whether or not the object is a dynamic object is determined based on an angle between the first displacement vector and the second displacement vector.

6. The map data generation unit When updating the three-dimensional spatial map data, among the plurality of second three-dimensional objects mapped on the grid of the occupancy map, a corresponding second three-dimensional object is deleted from the grid based on a past position of the dynamic object; The moving body according to claim 2 , wherein data of the deleted second three-dimensional object is recorded in a hash table.

7. the map data generation unit includes an object movement prediction unit that determines a predicted path of the dynamic object; The object movement prediction unit Acquire a route library including data on a plurality of route candidates, including at least a left-turn route, a straight route, and a right-turn route; The moving body according to claim 1, wherein a probability distribution based on a Markov chain is calculated for each of the plurality of route candidates included in the acquired route library, and the most likely route from the calculated probability distribution is determined as the predicted route of the dynamic object.

8. The travel route determination unit an optimization unit that determines the movement path that avoids the static object and the dynamic object by optimizing the curve, The optimization unit a set of control points of the polynomial curve as optimization parameters; a mathematical model defining a minimization problem of an objective cost function based on at least a first collision cost indicating a possibility that the moving body will collide with the stationary object and a second collision cost indicating a possibility that the moving body will collide with the dynamic object; The moving body according to claim 1 , wherein the first and second collision costs are calculated and the mathematical model is solved to optimize the curve and determine the moving path.

9. The optimization unit identifying a collision control point among the plurality of control points of the movement path relative to the stationary object; Searching for a collision avoidance path that does not collide with the stationary object based on the start point and end point of the collision control point; resetting the collision control points other than the start point and the end point on the collision avoidance path determined by searching; The moving body according to claim 8 , wherein the first collision cost is calculated based on a first distance between the control point and the static object after resetting and a second distance between the moving body and the static object for avoiding a collision.

10. The optimization unit a collision area is set within a range of a certain distance in the horizontal direction from the current position of the dynamic object to the predicted movement position; identifying a collision control point among the plurality of control points of the movement path for the dynamic object in the collision region; calculating a third distance from the collision control point to a safe area where the dynamic object does not collide with the collision control point; and calculating the second collision cost based on the third distance; The collision area is the first radius is a safe distance at which the moving body and the dynamic object do not collide with each other, a first collision area defined by an arc of the first radius centered at the current position of the dynamic object, and first and second line segments connecting two end points of the arc to the center; a second collision area defined by third and fourth line segments connecting the two end points of the arc and the predicted movement position, and the first and second line segments; The optimization unit The moving body according to claim 8 , wherein the collision control points are identified in the first and second collision regions, and the third distance is calculated.

11. The optimization unit determining whether or not to reset the control points after the previous optimization, and resetting the control points if it is determined that the control points should be reset; Optimizing the curve based on the reset control points and determining the movement path again; The moving body according to claim 8 , wherein the optimization of the curve is repeated until the entire moving path does not collide with the static object and the dynamic object.

12. The optimization unit 9. The moving body according to claim 8, further comprising: a determining unit that determines whether or not to reset the control points after a previous optimization; if it determines not to reset the control points; a determining unit that determines whether or not the weighting coefficients of the first and second collision costs are equal to or less than a predetermined value; if it determines that the weighting coefficients are equal to or less than the predetermined value, the moving body increases the value of the weighting coefficients by multiplying the weighting coefficients by a predetermined coefficient.

13. the mobile body comprises an extension device having at least one processor; The moving body according to claim 1 , wherein the extension device includes at least the autonomous movement control unit.

14. A measurement system using a mobile object that moves autonomously and performs a predetermined measurement task in an environment where its own position cannot be estimated by communication with the outside, a detection unit that detects an object located around the moving body; a drive control unit that controls a drive unit to perform body control of the moving body; an autonomous movement control unit that controls the drive control unit to autonomously move the moving body toward a measurement start position according to the determined movement path; a measurement analysis unit that analyzes the measurement results obtained by a predetermined measurement device and outputs the analysis results; The autonomous movement control unit a map data generation unit that generates map data that is a three-dimensional model of a space extending in the moving direction of the moving body, the three-dimensional spatial map data including the position of the detected object, the size of the object, and an amount of change in the position per unit time that indicates the moving speed of the object, and that continuously updates the three-dimensional spatial map data while the moving body is moving; a moving body having a movement path determination unit that identifies static objects and dynamic objects from among the detected objects based on the updated three-dimensional spatial map data, calculates a movement path by connecting a plurality of polynomial curves into one curve based on a result of predicting the movement of the dynamic object, and determines the movement path as the movement path for the autonomous movement, and continuously calculates the movement path that avoids the static objects and the dynamic objects while the moving body is moving; A measurement system using a moving body, comprising: a terminal device that visualizes the analysis results output from the moving body.

15. A measurement method using a mobile object that moves autonomously and performs a predetermined measurement task in an environment where its own position cannot be estimated by communication with the outside, detecting an object located in the vicinity of the moving body; generating map data that is a three-dimensional model of a space extending in the moving direction of the moving body, the three-dimensional map data including the position of the detected object, the size of the object, and an amount of change in the position per unit time that indicates the moving speed of the object, and continuously updating the three-dimensional map data while the moving body is moving; a step of identifying static objects and dynamic objects from among the detected objects based on the updated three-dimensional spatial map data, calculating a movement path by connecting a plurality of polynomial curves into one curve based on a result of predicting the movement of the dynamic objects, determining the movement path as the movement path of the autonomous movement, and continuously calculating the movement path that avoids the static objects and the dynamic objects while the moving body is moving; autonomously moving the moving body toward a measurement start position according to the determined movement path; a step of measuring with a predetermined measuring device while moving from the measurement start position to the measurement end position; and analyzing the measurement results and outputting the analysis results.

Images