JP7769346B1 - MOBILE BODY, MEASUREMENT SYSTEM USING MOBILE BODY, AND MEASUREMENT METHOD USING MOBILE BODY - Google Patents

Abstract

[Problem] To enable autonomous movement while avoiding surrounding obstacles and carrying out the intended measurement task even in an environment where autonomous movement is difficult.
[Solution]
A mobile body detects objects located in its vicinity. The mobile body generates and updates map data that models the space extending in the direction of movement into a three-dimensional space. The mobile body generates three-dimensional spatial map data including the position, size, and movement speed of the detected objects and continuously updates the map data while moving. The mobile body also identifies static and dynamic objects based on the updated three-dimensional spatial map data, calculates a movement path by connecting multiple polynomial curves into a single curve based on the predicted movement of the dynamic objects, and continuously determines a movement path that avoids static and dynamic objects while moving. The mobile body autonomously moves toward a measurement start position according to the determined movement path, takes measurements using a predetermined measurement device, analyzes the measurement results, and outputs analysis results.
[Selected Figure] Figure 13

Description

Article 30, Paragraph 2 of the Patent Act applies. Presented at the 2023 IEEE International Conference on Robotics and Automation (ICRA), held in London, UK, for five days from May 29 to June 2, 2023. [Publications, etc.] Presented at the 78th Annual Academic Conference of the Japan Society of Civil Engineers, Reiwa 5 National Convention, held in Hiroshima, Japan, for five days from September 11 to September 15, 2023.

The present invention relates to measurement technology using moving objects.

Unmanned aerial vehicles (UAVs), a type of mobile object, have become easier to operate thanks to improvements in attitude control, flight control, and other technologies. Small, low-cost models of these unmanned aircraft are also on the rise. Against this backdrop, in recent years, attempts have been made to utilize them in a variety of industrial fields. For example, in the construction and civil engineering fields, unmanned aircraft are sometimes used to inspect structures such as bridges and buildings (see Patent Document 1).

Japanese Patent Application Laid-Open No. 2019-127245

In the fields of construction and civil engineering, unmanned aerial vehicles are often used at work sites inside tunnels. For example, when inspecting the interior of a tunnel, if unmanned aerial vehicles can measure the work area, there is an advantage in that the inspection can be carried out 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 movement is difficult to predict. For this reason, the interior of a tunnel can be considered a difficult environment for unmanned aerial vehicles to navigate autonomously. Thus, when using unmanned aerial vehicles to perform measurement work inside a tunnel, it is desirable for the unmanned aerial vehicle to be able to move autonomously while avoiding surrounding obstacles and perform the desired measurements.

An aspect of the present invention aims to provide technology that enables autonomous movement while avoiding surrounding obstacles and performing the desired measurement task, even in environments where autonomous movement is difficult.

One aspect of the present invention is a mobile body that moves autonomously 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 the space extending in the direction of movement of the mobile body in three-dimensional space. The map data generation unit generates three-dimensional spatial map data that includes the position of detected objects, the size of the objects, and the 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. Based on the results of predicting the movement of dynamic objects, the movement path determination unit calculates a movement path by connecting multiple polynomial curves into a single curve 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 moving body is moving.

According to one aspect of the present invention, a mobile object can move autonomously while avoiding surrounding obstacles, even in environments where autonomous movement is difficult, and perform the intended measurement task.

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

First, in the practical embodiment of the present invention, an example of a work site where a measurement system using an unmanned aerial vehicle (UAV) is used for "inspection work (excavation shape measurement work) near the tunnel face (near the very tip of the tunnel excavation)" will be described. At the tunnel excavation site, the excavation shape is visually confirmed. Specifically, a worker enters the area directly below the face (danger zone) and visually checks to identify any insufficient excavation areas, using, for example, a laser pointer. In response, the excavator operator excavates again. After excavation, the worker checks to see if the insufficient excavation has been resolved. As such, at excavation sites, skin collapse and face collapse are anticipated, and ensuring worker safety is desirable. Therefore, with the aim of ensuring worker safety, measuring excavation shape using an unmanned aerial vehicle is an effective alternative to manual measurement work. From the perspective of such industrial utility, this embodiment cites an example of application to measuring the excavation shape inside a tunnel.

Embodiments of the present invention will be described below 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 modified as appropriate without departing from the technical concept of the present invention.

First Embodiment
[System configuration]
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 a measurement site. Specifically, it is an unmanned aerial vehicle 3.

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 resolve 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 heavy equipment that the operator Op rides on, but may also be 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 flying vehicle. 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, avoiding collisions with obstacles (static and dynamic obstacles) that exist between takeoff and return to its original position, using autonomous movement control described below, to perform the intended measurement work. In the following description, the unmanned aerial vehicle 3 will be referred to as a UAV3 for convenience.

The measurement system 1 includes a terminal device 2. The terminal device 2 is carried and used, for example, by an operator Op of a 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 below) 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.

The input device 21 receives input operations from, for example, the 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, etc.

The output device 22 receives output signals from the terminal device 2 and outputs data, thereby providing information to, for example, the operator Op. 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 material. The display device may be a flat panel display such as a liquid crystal display (LCD) or an organic electroluminescence display (OELD).

The UAV3 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.

The UAV3 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 to rotate 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 the measurement device 36 (described below), 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.

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 the 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 location) 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.

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

The measurement device 36 includes an imaging device 37 and an inertial measurement unit (IMU), etc., and senses the surroundings of the UAV3 and the aircraft itself to measure the flight environment and flight status. The imaging device 37 captures images 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 body, an accelerometer that detects acceleration (linear motion), etc., and measures the movement of the moving body. 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.

The UAV 3 is equipped with an expansion device (expansion board) 4 that performs autonomous movement control, as described below. 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, as described below. 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 input signals (detection results) from the detection device 41. The autonomous flight control device 40 outputs control signals (drive commands) to the flight control device 30 according to the calculation results (determined movement path). After arriving at the measurement start position, the autonomous flight control device 40 outputs control signals (drive commands) to the flight control device 30 according to the specified measurement path. As a result, the measurement device 42 measures the excavation shape while flying along the measurement path.

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 use the technical features of the depth sensor to generate and update visually supported spatial map data (three-dimensional spatial map data) described below. The visually supported spatial map data is map data that models three-dimensional space, enabling the identification of static and dynamic obstacles and the 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 its destination, 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.

The measuring device 42 is a sensor that detects the shape of the measurement target, and in this embodiment is an image sensor (e.g., an RGB camera). The RGB camera captures images as RGB data. In this embodiment, the RGB camera captures images of the excavation site to measure the excavation shape. In the following description, the measuring device 42 will be referred to as the RGB camera 42 for convenience.

Figure 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 top of the main body 32 of the UAV 3. Therefore, a depth camera 41 and an RGB camera 42 are provided on the top of the main body 32 of the UAV 3. Note that the depth camera 41 and the RGB 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).

[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 non-volatile 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, etc. Each function provided by each of the above-described devices 20, 30, and 40 is stored as a program in the storage 103. The processor 101 provides each function by reading the program from the storage 103, loading it into the memory 102, and executing information processing. The program may be distributed to the computer system 100 via a public line. While 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).

[Tunnel excavation work]
FIG. 4 is a schematic diagram showing the flow of tunnel excavation work 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). The operator Op carries the terminal device 2 and operates the work vehicle M while checking the confirmed work plan, etc. After the operator Op completes the predetermined work, he stops operating the work vehicle M. After stopping the 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, measuring the excavation shape, and when it arrives at the measurement end position, it again autonomously moves to the predetermined position while avoiding obstacles.

Figure 5 is a flowchart showing the processing of the measurement system 1 according to this embodiment. Figure 6 is a diagram showing an example of the movement of the UAV 3 according to this embodiment. The processing shown in Figure 5 is a detailed description of the processing executed in step S3 described above. In the measurement system 1, the processing shown in Figure 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 Figure 6.

The UAV3 takes off from a predetermined position (step S11). Upon starting flight, the UAV3 acquires surrounding images including depth data using the depth camera 41 and begins generating visual support spatial map data Map based on the UAV3's own position (current position) as shown in FIG. 6 (step S2). The process of generating (updating) the visual support spatial map data Map is continuously executed while the UAV3 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 UAV3 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 UAV3 autonomously moves while avoiding the static obstacles So and dynamic obstacles Do according to the calculated movement route Rt (step S13).

When UAV3 arrives at the measurement start position near the face, it begins measuring the excavation shape (step S14). The measurement results are stored in UAV3. The excavation shape measurement process will be described later using Figure 7. When UAV3 arrives at the measurement end position near the face, it moves autonomously along the calculated movement path Rt while avoiding static obstacles So and dynamic obstacles Do (step S15). Then, 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.

The UAV 3 analyzes the stored measurement results (step S17). The process of analyzing the measurement results will be described later using Figure 8. The UAV 3 then 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 areas where excavation is insufficient via the output device 22 (step S18). The process of visualizing areas where excavation is insufficient will be described later using Figure 9.

Figure 7 is a flowchart showing the excavation shape measurement process (step S14) by the UAV 3 according to this embodiment. As shown in Figure 7, the UAV 3 performs measurement while flying, for example, a zigzag path near the excavation face (step S21). As it moves, the UAV 3 captures images of the excavation site using the RGB camera 42 and stores the 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 its measurement movement. On the other hand, if the UAV 3 determines that it has arrived at the measurement end position (step S23: YES), it terminates the measurement process. In this way, the UAV 3 flies along a predetermined measurement path 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 consecutive images of the excavation site as measurement results.

Figure 8 is a flowchart showing the analysis process (processing of step S17) of measurement results by the UAV 3 according to this embodiment. As shown in Figure 8, the UAV 3 acquires multiple images (multiple RGB data) of the measurement results from a predetermined storage area (step S31). The UAV 3 then performs SfM (Structure from Motion) analysis 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 many 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 as the analysis results to the terminal device 2.

Figure 9 is a flowchart showing the process for visualizing areas of insufficient excavation (the process of step S18) according to this embodiment. Figure 10 is a diagram showing the visualization of areas of insufficient excavation according to this embodiment. As shown in Figure 9, the terminal device 2 acquires analysis results from the UAV 3 (step S41). The terminal device 2 acquires three-dimensional model data 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 areas of insufficient excavation from the acquired analysis results (step S42). The terminal device 2 approximates the three-dimensional model data of the excavation shape and the design model data to a specific function using the least squares method, and overlays 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 Figure 10, the areas of insufficient excavation are displayed as area C in the heat map. This allows the operator Op to identify areas of insufficient excavation.

[Function Overview]
In tunnels, where communication with the outside is impossible, UAVs must avoid static obstacles with complex structures and dynamic obstacles whose movements are difficult to predict. Therefore, in order to autonomously navigate through such complex spaces, UAVs must recognize the travel space in real time and generate a travel path Rt that avoids static obstacles So and dynamic obstacles Do. One example of a technology for recognizing the travel space in real time is a method for detecting obstacles using a vision-based algorithm. For example, a bounding box of an obstacle is extracted using geometric information from an image. However, this method cannot distinguish between static obstacles So and dynamic obstacles Do. Another example of a technology for generating a travel path Rt that avoids static obstacles So and dynamic obstacles Do is a path planning method based on a single map representation, such as a geometric map, an occupancy map, or a Euclidean signed distance field map (ESDF). However, while this method is effective in static environments, it cannot simultaneously represent and handle static obstacles So and dynamic obstacles Do due to limitations in map representation. Furthermore, in order to avoid dynamic obstacles Do, whose movement is difficult to predict, high-frequency real-time route generation (real-time route planning) is required. However, in the environment of a UAV 3 equipped with an expansion 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.

Figure 11 is a diagram showing an overview of the 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 Figure 11. The UAV 3 generates visual support spatial map data Map shown in Figure 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 vehicle's own position Cp and track and predict the movement of the dynamic obstacles Do. The UAV 3 uses the depth image (depth map) from the depth camera 41 to model and represent static obstacles So in voxel units and dynamic obstacles Do using bounding boxes, and maps these onto the grid of the 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 vehicle's own position Cp as occupied voxels. The UAV3 tracks the movement of the dynamic obstacle Do using specific filtering. While the UAV3 is moving, it updates the map data based on the latest depth image captured by the depth camera 41 at its own position Cp. This allows the UAV3 to identify static obstacles So with complex structures and dynamic obstacles Do whose movement is difficult to predict, and track and predict the movement of the dynamic obstacles Do, even in an environment where communication with the outside world is not possible.

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

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

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

The transmitter/receiver unit 200 transmits and receives data to and from other devices other than the terminal device 2, such as the UAV 3. The transmitter/receiver unit 200 is a function of the communication device 23 shown in Figure 1.

The visualization unit 201 visualizes predetermined information to convey that information to the operator Op. Based on the analysis results obtained from the UAV 3, the visualization unit 201 superimposes a three-dimensional model of the analysis results on the design model and visualizes the difference in the excavation shape (insufficient excavation) using a heat map display. This allows the operator Op to understand the areas where excavation is insufficient. The visualization unit 201 is a function possessed by the control device 20 and output device 22 shown in Figure 1.

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 re-excavate 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 of the output device 22 shown in Figure 1.

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

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

[Movement control function]
The drive control unit 301 performs airframe control of the UAV 3 regarding flight by transmitting control signals to the drive unit 302, receiving detection data from the first detection unit 303, receiving control signals from the autonomous movement control unit 401, etc. 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 turning on the power supply 35, 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.

The drive unit 302 controls the drive device 31D to perform the specified behavior in accordance with, for example, a control signal from the drive control unit 301, and drives the propeller 31P to rotate. Note that the drive unit 302 is a function possessed by the flight device 31.

The first detection unit 303 acquires the sensing results from the various sensors as detection results and transfers the acquired detection results to the drive control unit 301 as detection data. The first detection unit 303 is a function possessed by the measurement device 36.

[Autonomous movement control function]
The autonomous movement control function is a function possessed by the expansion 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. Furthermore, the autonomous movement control unit 401 updates the map data while moving based on the latest detection data of the second detection unit 402 at the self-position Cp. 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 of the autonomous flight control device 40.

The second detection unit 402 acquires the sensing results from the various sensors as detection results and transfers the acquired detection results to the autonomous movement control unit 401 as detection data. 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 a single image frame data.

[Measurement and analysis function]
The measurement and analysis processing unit 403 performs a predetermined analysis process on the detection results, i.e., image data, input from the second detection unit 402, and transmits the analysis result data to the terminal device 2 via the transmission and reception unit 300. The measurement and 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 and analysis processing unit 403 performs a predetermined analysis process (e.g., 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 and analysis processing unit 403 may be realized by an integrated circuit for image analysis processing (ASIC: Application Specific Integrated Circuit).

Figure 13 is a functional block diagram of the autonomous movement control unit 401 according to this embodiment. The autonomous movement control unit 401 has the functions shown in Figure 13. The autonomous movement control unit 401 has a memory unit 410, a map data generation unit 420, a movement route determination unit 430, etc., and these functional units work in conjunction with each other to provide predetermined functions related to autonomous movement.

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

The history data 411 is data related to the movement history of the moving obstacle Do, and 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 specified period of time. These data are stored in association with each frame data of the multiple captured images.

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 the 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, and height) and movement speed (amount of change in position per unit time) of the obstacles. In this way, the visual support spatial map data Map is map data that models the space extending in the direction seen from the UAV3's own position Cp (line of sight or direction of movement) in three-dimensional space.

The parameter data 413 is various parameters such as optimization weighting coefficients and normalization coefficients used when optimizing a travel route. The various stored parameters are updated by the 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.

[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 moving obstacles Do from the vehicle's own position Cp and track and predict the movement of the moving obstacles Do. 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 map data generation unit 420 executes the process shown in FIG. 14 and causes the above-mentioned functional modules to operate in conjunction with each other, thereby generating and updating the visual support spatial map data Map.

Figure 14 is a flowchart showing the process of generating visual support space map data Map according to this embodiment. As shown in Figure 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). This process by the area proposal unit 421 will be described later using Figures 15, 16, and 17.

Next, the obstacle identification unit 422 references 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 identification unit 422 will be described later using Figures 18 and 19.

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

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, data on the dynamic obstacle Do's past bounding box (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, resulting 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 in the time-series image. The obstacle identification unit 422 then 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 in the deletion history data are recognized as previously occupied voxels (effectively utilized as movement history data) during the refinement process of the map data that is performed during the next update (when tracking and predicting movement). Note that the deletion history data is included in the history data 411, and is stored and updated in the memory unit 410 by the obstacle identification unit 422.

The obstacle movement prediction unit 423 calculates environmental probabilities for predicting the movement path based on a Markov chain (step S54). This processing by the obstacle movement prediction unit 423 will be described later using Figures 20 and 21.

[Obstacle area proposal]
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.

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 approximate position and size (length, width, and 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 recognition of the depth distance (distance to the object) for each pixel. The advantages of the three-dimensional bounding box Bb are as follows: First, the three-dimensional bounding box Bb can model objects with a simple shape, which can take on a variety of possible 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 of visual support spatial map data Map for recognizing dynamic environments. A second advantage is that the three-dimensional bounding box Bb can contain data such as the object's center position, vertical and horizontal lengths. Therefore, the three-dimensional bounding box Bb can recognize the object's position, size, directionality (object movement), etc. in three dimensions as seen from the self-position Cp.

The process of generating a three-dimensional bounding box Bb for 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 Figure 16(A). The moving obstacle Do has depth values that change 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 with 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 Figure 16(D). Through this process, the region proposal unit 421 can obtain data regarding the approximate length and width (vertical and horizontal data) of the moving obstacle Do.

Moreover, the moving obstacle Do is recognized in the depth image as a second two-dimensional bounding box Bbb as shown in Figure 16(B). As indicated by the dotted 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 in 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 moving obstacle Do (height data).

If I is the ^image frame number, o is the obstacle number, ^and T is the time, the moving obstacle Do can ^be expressed as a bounding box with the object's center coordinates: _PoI = [ _xoI , _yoI ] ^T and a size: _SoI = [ _woI , _hoI ^{] T} with a depth d. Furthermore, as shown in FIG ^. 16(C), this box can be projected onto the image frame and converted into a map ^frame . Specifically, if ^M is ^the map frame number, the bounding box Bb can be converted into a map frame with the center coordinates: _PoM = [ ^xoM , _yoM , _d0M ] and a size: S = [ _woM , _hoM , _l0M ]. 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 this box into a map frame.

Returning to the explanation of Figure 15, the area 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. As a result, the sizes of multiple three-dimensional bounding boxes Bb on the static map (on the grid of the occupancy map) 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, to improve robustness and accuracy, the area 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.

As shown in FIG. 17A, the region proposal unit 421 multiplies the size (w, h, l) of the bounding box Bb1 of the detected obstacle generated from the depth image by a preset expansion coefficient (normalization coefficient) C _inflate . This expands the bounding box Bb1 in the x, y, and z directions, resulting in the generation of an expanded bounding box Bb2. Note that each point cloud shown in FIG. 17 represents a voxel in the static map data. Next, the region proposal unit 421 searches for an occupied voxel corresponding to the position of the obstacle on the static map (on the grid of the occupancy map). As a result, as shown in FIG. 17B, the region proposal unit 421 determines and proposes the expanded bounding box Bb3, which has the smallest size among all the occupied voxels present 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.

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

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

18 , the obstacle identifying unit 422 associates the obstacle with its position for all detected obstacles and records the associated data as history data 411 for tracking and predicting movement (step S71). Specifically, the obstacle identifying unit 422 associates a group of obstacles detected in an image frame at time t as one data set ^t O ^C = { ^t o ₀ ^C , ^t o ₁ ^C , ... ^t _on ^C }. The obstacle identifying unit 422 also associates data t o 0 C of each three-dimensional bounding box Bb with data ^t - _{1 o i} ^C that is closest to data ^t o _i ^C among data t ^{- 1} O _n ^C at the previous time t-1. The criterion for determining ^which data is closest is based on the center distance between the three-dimensional bounding boxes Bb. The overlap rate r _i,j between three-dimensional bounding boxes Bb can be defined by the following equation:

Here, A _i,j is the overlapping area of the upper surfaces of the three-dimensional bounding boxes Bb between data ^t o _i ^C and data ^t-1 o _i ^C at time t, and A _i is the area of the upper surface of data ^t o _i ^C. The obstacle identification unit 422 acquires 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 }. Note that, in order to reduce the influence of the three-dimensional bounding box Bb of a partially detected obstacle, the obstacle identification 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 identification 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.

As described above, the obstacle identifying unit 422 generates a group of multiple obstacles detected in image frames at the same time as one data set ^t O ^C. Furthermore, the obstacle identifying unit 422 generates movement history data 411 as time-series data sets ^t H _i,k ^C of the same three-dimensional bounding box Bb based on the multiple data sets ^t O ^C for each elapsed time.

Next, the obstacle identifier 422 estimates the moving speed of the obstacle (step S72). The obstacle identifier 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. For ease of understanding the following explanation, it is assumed that all parameters described below exist within the map frame. The state vector of the obstacle can be expressed as X = [x, y, ...] ^T , and the measurement vector can be expressed as Z = [ _ox , _oy , _vx , _vy ] ^T . Here, _vx and _vy are the x- and y-directional velocities calculated based on the state change per unit time (two-dimensional position change per unit time) of the obstacle. Furthermore, assuming that A is a state transition model, Q is the covariance of the model noise, H is a 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 the measurement vector Z can be defined by the following equations.

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 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 determines whether the continuity coefficient C _con is less than a predetermined value T _con (step S74). If the continuity coefficient C _con is less than the predetermined value T _con (step S74: YES), the obstacle identifying unit 422 identifies the detected obstacle as a moving obstacle Do (step S75). On the other hand, if the estimated speed of the obstacle is less than the predetermined value (step S73: NO) or if the continuity coefficient C _con is equal to or greater than the predetermined value T _con (step S74: NO), the obstacle identifying unit 422 identifies the detected obstacle as a stationary obstacle So (step S76).

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, 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 has a configuration for filtering out static obstacles So that have been erroneously identified as moving obstacles Do (identification process of moving obstacles Do based on the continuity coefficient C _con : step S74).

Figure 19(A) shows history data 411 of the movement of a dynamic obstacle Do over six time periods. White dots: P ₁ -P ₆ and wavy arrows are the positions and trajectories of the obstacles recorded as movement history. Black dots: P ₁ ^GT -P ₆ ^GT are the true positions. Figure 19(B) shows history data 411 of the movement of a static obstacle So over six time periods. White dots: P ₁ -P ₆ and wavy arrows are the positions and trajectories of the obstacles recorded as movement history. Black dots: P ^GT are the true positions.

The obstacle identification unit 422 calculates a displacement vector (solid arrow) using a set of obstacle position data (e.g., _P1 at the first time and _P4 at the second time) from the movement history Pa defined by the following equation:
Next, the displacement vector D between each point can be defined by the following equation.

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

Assuming that an obstacle moves at a constant speed in a short period of time, the angle θ between the displacement vectors of a moving obstacle Do should be an acute angle (close to 0°) as shown in Fig. 19(A). On the other hand, as shown in Fig. 19(B), the angle θ between the displacement vectors of a static obstacle So should be an obtuse angle (close to 180°). Therefore, in this embodiment, this property is utilized and the continuity coefficient C _con is defined by the following equation:

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

As described above, the obstacle identification unit 422 uses a Kalman filter to estimate the movement speed of the detected obstacle, 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 identification unit 422 provisionally identifies the detected obstacle as a moving obstacle Do. The obstacle identification unit 422 performs processing to filter out erroneously identified static obstacles So, taking into account the possibility that a static obstacle So may have been erroneously identified as a moving obstacle Do.

[Prediction of moving obstacle paths]
Fig. 20 is a flowchart showing a process for predicting the movement of a moving obstacle Do according to this embodiment. Fig. 21 is a diagram showing an example of a method for predicting the movement of a moving obstacle Do according to this embodiment.

As shown in FIG. 20, the obstacle movement prediction unit 423 acquires a route library containing multiple route candidates that has been generated in advance through experiments (step S81). The obstacle movement prediction unit 423 calculates the probability distribution of the route candidates from the route library (step S82).

As shown in FIG. 21 , a path library is generated by collecting walking data P _path of a person in an experiment and fitting the collected walking data with a polynomial. Each vector in the path library ranges from a left-turn path (path 1 shown in FIG. 20 ) to a right-turn path (path 5 shown in FIG. 20 ) centered on a straight path. The obstacle movement prediction unit 423 acquires the path library generated in this manner. The obstacle movement prediction unit 423 then selects the most likely route by calculating the probability distribution P _path of each of the multiple route candidates included in the path library. To calculate the probability distribution P _path of the route, the initial state P _init of the Markov chain can be defined as the probability value of each route using the following equation:

Here, all values are discrete probability distributions (discrete Gaussian distributions). The probability values are obtained from a Gaussian kernel with P ^1/2 _init as the mean. This is because it is assumed that P ^1/2 _init corresponds to a straight route among multiple route candidates from a left turn route to a right turn route, and 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 by the following equation.

Humans tend to maintain their direction of movement as long as possible. Therefore, each row of the state transition matrix is a discrete Gaussian distribution obtained from a Gaussian kernel with P ^i,j _trans as the mean. To consider the interaction between the environment and obstacles, the obstacle movement prediction unit 423 uses static map data (occupancy map data) as a reference (Dist _i ) for each route in the route library 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 SoftMax function below to calculate the environmental probability P _env . The environmental probability P _env indicates the probability of selecting a specific route when considering the interaction between the environment and obstacles.

Finally, the transition state can be predicted by the following equation:
where * denotes element-wise multiplication.

As described above, the obstacle movement prediction unit 423 generates a route library containing multiple route candidates from the person's walking data. The obstacle movement prediction unit 423 selects the most likely route by calculating the probability distribution P _path of each route from the multiple route candidates included in the route library. At this time, the obstacle movement prediction unit 423 uses static map data as a reference (Dist _i ) and calculates the distance from the reference starting point to the collision point with the obstacle for each route in the route library based on the state transition matrix of the Markov chain. Based on the calculated distance for each route, the obstacle movement prediction unit 423 calculates an environmental probability P _env of selecting a specific route when considering the interaction between the environment and the obstacle. Based on the environmental probability P _env calculated in this way, the obstacle movement prediction unit 423 selects a future movement route of the dynamic obstacle Do from the multiple route candidates and sets it as a predicted route.

As described above, the map data generation unit 420 has 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.

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 on the movement history. The obstacle identification unit 422 identifies a moving obstacle Do from the detected obstacles based on the calculated speed. At this time, the obstacle identification unit 422 performs identification processing of the moving obstacle Do using a continuity coefficient C _con calculated based on the angle θ between the displacement vectors, in order to filter out moving obstacles Do that should originally be identified as static obstacles So. This enables the obstacle identification unit 422 to track the moving obstacle Do and further reduces erroneous identification of obstacles (improving the identification accuracy of moving obstacles Do).

The obstacle movement prediction unit 423 calculates a probability distribution P _path of route candidates from the route library. At this time, the obstacle movement prediction unit 423 sets the initial state P _init of the Markov chain as the probability value of each route, and calculates the distance to the obstacle for each route (route candidate) in the route library, taking into account the interaction between the environment and the obstacle, using a state transition matrix. The obstacle movement prediction unit 423 calculates an environmental probability P _env for predicting the movement route based on the calculated distance. As a result, the obstacle movement prediction unit 423 selects a future movement route of the moving obstacle Do from multiple route candidates based on the calculated environmental probability P _env . In this way, the obstacle movement prediction unit 423 predicts the movement route of the moving obstacle Do.

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 predicted movement 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. Therefore, the map data generation unit 420 can identify static obstacles So and dynamic obstacles Do from the self-position Cp and generate and update visual support spatial map data Map in real time, which can track and predict the movement of the dynamic obstacles Do. In other words, the map data generation unit 420 can generate and update three-dimensional spatial map data of the measurement site in real time. Therefore, even in an environment with extremely poor external communication, the UAV3 can recognize the latest state of the surrounding environment in real time by using the visual support spatial map data Map. As a result, the UAV3 can optimize its travel route Rt by taking into account future changes in the state of the surrounding environment (movement of the dynamic obstacle Do).

[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 performs repeated optimization even after deriving a solution to the minimization problem, in order to be applicable to a dynamic environment in which the surrounding environment changes over time.

[Optimization of B-spline curves]
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 an obstacle exists on the travel route Rt drawn using the B-spline curve (step S92). At this time, the first optimization unit 431 references 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 an obstacle exists on the travel route Rt based on the positions and sizes of static obstacles So and dynamic obstacles Do, the predicted movements 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 the travel route Rt (step S94). Note that "unconstrained" means that there are no restrictions on the range of the parameters.

A B-spline curve of degree k is a single curve formed 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 route or target position is given, the first optimization unit 431 parameterizes the travel route Rt as a set of control points as shown in the following equation. The first control point and the last control point correspond to the start and end positions of the UAV3's travel.

The optimization parameter is a set S of the intermediate N-2(k-1) 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 parameter, 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 movement route Rt.
where α is a weighting coefficient for optimization. Thus, the objective cost function C _total (S) is a weighted combination for optimization of the path control limit cost C _control , the path smoothness cost C _smooth , the collision cost C _static with static obstacles So, and the collision cost C _dynamic with dynamic obstacles Do.

FIG. 23 is a flowchart showing the calculation process of optimization parameters (costs) according to this embodiment. As shown in FIG. 23, the first optimization unit 431 calculates parameters related to the control limit cost C _control (step S101). When the UAV 3 moves along the movement route Rt, the UAV 3 must move within a range that does not exceed its own control limit (a control limit value for stable movement). The control limit cost C _control limits the possible speed and acceleration to a range that does not exceed the control limit. The derivative of a B-spline curve can be expressed by another B-spline curve. Therefore, the control points V _i and A _i for the speed and acceleration can be defined by the following equations.

Here, δt is the elapsed time. Therefore, when the maximum speed: v _max and the acceleration: a _max are given, the control limit cost function can be defined by the following equation.
Note that when the velocity and acceleration exceed the control limits, an L2 norm is applied to the control limit penalty using a unit normalization factor λ.

Next, the first optimization unit 431 calculates parameters related to the smoothness cost _Csmooth (step S102). The smoothness cost _Csmooth is applied to prevent the generation of a jerky movement path Rt. When an acceleration control point _Ai and a jerk control point _Ji , which is a derivative of acceleration, are given, the smoothness cost function can be defined by the following equation:

The first optimization unit 431 calculates a parameter related to the collision cost C _static with the static obstacle So (the possibility of colliding with the static obstacle So) (step S103). This processing by the first optimization unit 431 will be described later with reference to FIGS. 24 and 25.

The first optimization unit 431 calculates a parameter related to the collision cost C _dynamic with the moving obstacle Do (the possibility of colliding with the moving obstacle Do) (step S104). This processing by the first optimization unit 431 will be described later with reference to FIGS. 26 and 27.

[Collision Cost Calculation for Static Obstacles]
Fig. 24 is a flowchart showing a process for calculating parameters of a static obstacle So according to this embodiment. Fig. 25 is a diagram showing an example of a method for calculating a collision cost C _static and a gradient for a static obstacle So according to this embodiment.

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 estimate the collision cost C _static and gradient for the static obstacle So. The collision cost and gradient of the control point, which are optimization parameters, can be estimated using the guide point. Note that the guide point is a point (collision avoidance guide point) where the control point of the B-spline curve is reset on the collision avoidance path Rts that has been searched and determined to avoid the obstacle.

24 , when a travel path Rtp that collides with a static obstacle So is given, the first optimization unit 431 identifies collision control points P ⁱ _ccp (step S111). At this time, the first optimization unit 431 uses, for example, a function that identifies collision control points P _ccp when the travel path (collision path) Rtp is input. As a result, the first optimization unit 431 acquires one collision data set S _col = {P ⁰ _ccp , P ¹ _ccp , ... P ⁿ _ccp } using the identified multiple collision control points P _ccp as output values of the function.

Next, the first optimization unit 431 performs a collision-avoidance route search (step S112). The first optimization unit 431 uses a predetermined route search algorithm such as A* or Dijkstra to search for a collision-free travel route (collision-avoidance route) Rts.

Next, the first optimization unit 431 sets new control points (sets collision avoidance guide points) on the searched route Rts (step S113). The first optimization unit 431 projects each collision control point P ⁱ _ccp in the collision dataset S _col onto a tangent (line segment) LS between the start point P ^S _swp and end point P ^E _swp of the collision avoidance route Rts determined by the search.

The first optimization unit 431 uses the projected point to calculate the direction angle θ _direct from (0,π), and uses the pair of the projection point and angle 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 point P ⁱ _guide shown in FIG. 25. This point is the guide point P _gp (a point reset to avoid collision of the control point) of the collision control point P ⁱ _ccp . Note that, as shown in FIG. 25, the direction angle θ _direct describes a semicircle, so this algorithm is circle-based.

Next, the first optimization unit 431 calculates the collision cost C _static and gradient for the static obstacle So (step S114). The collision cost C _static of the collision control point P ⁱ _ccp can be defined by the following formula using the acquired guide point P ⁱ _guide and a preset safety distance d _safe .

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 penalizes control points whose signed distance (first distance between the guide point and the obstacle) is smaller than the safe distance d _safe (second distance between the UAV 3 and the obstacle required to avoid collision). In this way, the first optimization unit 431 can calculate the gradient of each collision control point P ⁱ _ccp according to the chain rule based on the following formula.

Here, P^ _i is the initial collision control point P ⁱ _ccp . The direction of the negative gradient is the direction in which the collision control point P ⁱ _ccp is pushed out from the obstacle existence region into the collision avoidance region.

25 shows an optimized movement path Rto that smoothly turns to avoid the stationary obstacle So by using the circle-based guide point algorithm described above by the first optimization unit 431. In this way, the first optimization unit 431 determines the movement path Rto that avoids the stationary 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 above-mentioned circle-based guide point algorithm to approximate the collision cost C _static and gradient for the static obstacle So.

[Collision Cost Calculation for Dynamic Obstacles]
Fig. 26 is a flowchart showing a process for calculating parameters of a moving obstacle Do according to this embodiment. Fig. 27 is a diagram showing an example of a method for calculating a collision cost C _dynamic and a gradient for a moving obstacle Do according to this embodiment.

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

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 safe distance r being linearly reduced (r>r') from the current position _O0 of the moving obstacle Do to the predicted position _Ok . The first optimization unit 431 obtains the future position { _O1 , _O2 , ... _Ok } at the predicted time k using linear prediction, taking into account the current position _O0 and velocity _V0 of the moving obstacle Do.

Next, the first optimization unit 431 sets a collision region R (collision region _RA and collision region _RB ) from the current position _O0 of the moving obstacle Do to the predicted position _Ok (step S122). To construct a horizontal distance field, the first optimization unit 431 first draws a circle with a safety radius (safety distance) r centered at the current position _O0 . In this way, the first optimization unit 431 sets a circular first collision region _RA shown by a dotted line in FIG.

The reliability of future prediction decreases as the distance from the current position _O0 to the predicted position _Ok increases. Therefore, taking into account the low reliability of future prediction, the first optimization unit 431 linearly decreases the value of the distance from the current position _O0 to the predicted position _Ok so that the safe distance r becomes zero over the prediction time k. As a result, the first optimization unit 431 sets a substantially conical second collision region _R1B , as indicated by the dotted line in FIG. 27 . By setting two collision regions R with different shapes, a circular first collision region _R1A and a substantially conical second collision region _R1B , the first optimization unit 431 can prevent excessive avoidance behavior compared to a cylindrical collision region (a collision region derived by maintaining the safe distance r even for a predicted position with low reliability). The first optimization unit 431 can generate a travel route Rto that takes future prediction into account.

The first optimization unit 431 calculates the distance to the safe area of the moving position for each of the two differently shaped collision areas R to estimate the collision cost with the moving obstacle Do.

The first optimization unit 431 calculates the distance to the safety area of the moving position in the circular first collision area _RA and the approximately conical second collision area _RB (step S123). As shown in Fig. 27, the collision control point ^P1ccp (Pi _,c ) of the moving obstacle Do is _located within the circular first collision area _RA surrounded by the arc ACB and the line segments (first and second line segments) _AO0 and _BO0 . Therefore, the distance to the safety area (third distance from the collision control point to the safety area) Δd _i can be defined by the following equation.
27, the collision control point P ⁷ _ccp (P _i,p ) of the moving obstacle Do exists within the second collision area R _B , which is a substantially conical area surrounded by the line segments (first and second line segments) AO ₀ and BO ₀ and the line segments (third and fourth line segments) AO _k and BO _k . A straight line perpendicular to the line segment BO _k and passing through the intersection O' on the line segment O ₀ O _k is a straight line passing through the control point P _i,p . Therefore, the distance Δd _i to the safety area can be defined by the following equation.

Next, the first optimization unit 431 calculates the collision cost C _dynamic and gradient for the dynamic obstacle Do (step S124). The collision cost C _dynamic of the collision control point P ⁱ _ccp can be defined by the following equation using the distance Δd _i to the safety area.
Then, the gradient of each collision control point P ⁱ _ccp can be calculated according to the chain rule together with the equations (19) and (20) above.

In this way, the first optimization unit 431 sets a circular first collision area _RA with a safety radius r as the safety distance centered on the current position _O0 of the moving obstacle Do. Furthermore, taking into account the low reliability of future predictions, the first optimization unit 431 linearly decreases the value of the distance from the current position _O0 to the predicted position _Ok so that the safety distance r becomes zero with the prediction time 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 travel route Rto that avoids the moving obstacle Do by optimizing a B-spline curve.

[Iterative optimization]
The minimization problem of the objective cost function C _total (S) is unconstrained and includes multiple objectives. Therefore, solving this problem only once may not guarantee the safety of the movement route Rt of the UAV 3. Therefore, in this embodiment, the second optimization unit 432 repeatedly solves this problem by resetting the control points P _i of the B-spline curve on the collision avoidance route Rts that was searched and determined to avoid the obstacle, until the entire movement route Rt no longer collides with the detected obstacle.

The collision cost C _static with the static obstacle So and the collision cost C _dynamic with the dynamic obstacle Do are useful for the first optimization unit 431 to determine the collision avoidance route R to. However, through careful study by the inventors, it has been found that this affects the safety of the movement route R to of the UAV 3 for the following reasons.

The first reason is that it has been found that if the values of the weights α _static and α _dynamic of the collision costs C _static and C _dynamic shown in (13) above are not sufficiently large, it affects the safety of the travel route Rt. In this case, the second optimization unit 432 increases the values of the weights α _static and α _dynamic using a preset expansion coefficient λ.

The second reason is that when solving the minimization problem of the objective cost function C _total (S), there is a possibility that the control point P _i may be pushed toward a new obstacle after optimization, invalidating the collision cost and gradient calculated up to that point. It has been found that this affects the safety of the travel path Rt. Therefore, the control point P _i after optimization must be reset to avoid the obstacle again. Therefore, the second optimization unit 432 performs a process of setting a new control point. Therefore, the second optimization unit 432 repeatedly performs the process, checking each time whether there is a control point P _i that needs to be reset, and resetting the corresponding control point P _i to a new control point. The second optimization unit 432 repeatedly performs the above process until the entire travel path Rt no longer collides with detected obstacles.

FIG. 28 is a flowchart showing the iterative optimization process for the travel path Rt according to this embodiment. As shown in FIG. 28 , the second optimization unit 432 determines whether or not it is necessary to reset the control point P _i after optimization (step S131). If the second optimization unit 432 determines that it is necessary to reset the control point P _i (step S131: YES), it sets a new control point (step S132). At this time, the second optimization unit 432 again searches for a collision-free travel path (collision avoidance path) Rts and sets a new control point (sets a collision avoidance guide point) on the searched path Rts. Next, the second optimization unit 432 updates the objective cost function C _total (S) (step S133). At this time, the second optimization unit 432 _calculates the control limit cost C _control of the path, the smoothness cost C _smooth of the path, the collision cost C _static with the static obstacle So, and the collision cost C _dynamic with the dynamic obstacle Do based on the reset control point Pi. Next, the second optimization unit 432 updates the movement path Rt (step S134). At this time, the second optimization unit 432 again solves the minimization problem of the objective cost function C _total (S) and updates the movement path Rt to the optimized movement path Rto. Next, the second optimization unit 432 verifies the obstacle parameters (step S135). At this time, the second optimization unit 432 verifies whether the collision cost C _static with the static obstacle So and the collision cost C _dynamic with the dynamic obstacle Do are minimized.

On the other hand, if the second optimization unit 432 determines that resetting of the control point P _i is not necessary (step S131: NO), it determines whether the values of the weights α _static and α _dynamic of each collision cost C _static and C _dynamic are equal to or less than a predetermined value (step S136). If the second optimization unit 432 determines that the values of the weights α _static and α _dynamic of each collision cost C _static and C _dynamic are equal to or less than a predetermined value (step S136: YES), it updates the values of the weights α _static and α _dynamic (step S137). At this time, the second optimization unit 432 increases the values of the weights α _static and α _dynamic by multiplying the values of the weights α _static and α _dynamic by a preset expansion coefficient λ. Thereafter, the second optimization unit 432 proceeds to the process of step S133.

As described above, if it is necessary to reset the optimized control points P _i , the second optimization unit 432 performs the following processing until the entire travel route Rt does not collide with the detected obstacle. The second optimization unit 432 resets the control points P _i of the B-spline curve on the collision avoidance route Rts determined by searching for obstacle avoidance, and iteratively solves the minimization problem of the objective cost function C _total (S). In this way, the second optimization unit 432 determines in real time the optimal travel route Rto according to the dynamic environment in which the surrounding environment changes over time. Therefore, the UAV3 avoids the dynamic obstacle Do, whose movement is difficult to predict.

As described above, when an obstacle is present on the movement path Rt drawn using a B-spline curve, the movement path determination unit 430 calculates optimization parameters. At this time, when a rough path or a target position is given, the movement path determination unit 430 parameterizes the movement path Rt as a set S of control points P _i of the B-spline curve. The movement path determination unit 430 uses the set S as the optimization parameters. Therefore, the movement path determination unit 430 solves the minimization problem of the objective cost function C total (S), which is a combination of the control limit cost C _control of the path, the smoothness cost C _smooth of the path, the collision _cost C _static with the static obstacle So, and the collision cost C _dynamic with the dynamic obstacle Do, all weighted for optimization. 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 travel path determination unit 430 resets the control points P i of the B-spline curve on the collision avoidance path Rts determined by searching for obstacle avoidance until the entire travel path _Rt does not collide with the detected obstacle. The travel path determination unit 430 repeatedly solves the minimization problem of the objective cost function C _total (S). As a result, the UAV 3 calculates in real time an optimal travel path Rt that avoids static obstacles So and dynamic obstacles Do located ahead and heads toward the destination position. At this time, the UAV 3 uses the above-mentioned B-spline curve optimization algorithm to avoid dynamic obstacles Do, whose movement is difficult to predict, thereby reducing the calculation cost required for optimizing the travel path Rt (improving calculation speed). Furthermore, by repeating the optimization, the UAV 3 can autonomously travel along a highly safe travel path Rt while avoiding static obstacles So and dynamic obstacles Do, even in a dynamic environment where the surrounding environment changes over time. Furthermore, the UAV 3 performs real-time calculations to determine a path plan in an appropriate calculation time by repeatedly performing optimization, so that the UAV 3 can avoid a dynamic obstacle Do whose movement is difficult to predict, even in a resource-limited calculation environment.

[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 expansion device 4 that performs autonomous movement control. The expansion 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 (drive 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 (drive command) to the flight control device 30 according to the predetermined measurement path. As a result, the measurement device 42 measures the excavation shape while flying along the measurement path.

According to the measurement system 1 of this embodiment, the UAV 3 can autonomously move while avoiding surrounding obstacles, even in environments where autonomous movement is difficult, and perform the intended measurement work. Therefore, with the measurement system 1 of this embodiment, the UAV 3 can be effectively used as an alternative to workers measuring the excavation shape during inspection work near the tunnel face inside the tunnel. This means that 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.

The autonomous flight control device 40 also 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 at the measurement site and track and predict the movement 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.

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 UAV3's own position Cp within space, the UAV3 can recognize the latest state of the surrounding environment in real time by using the visual support spatial map data Map. As a result, the UAV3 can optimize its travel route Rt by taking into account future changes in the state of the surrounding environment (movement of dynamic obstacles Do).

Furthermore, the autonomous flight control device 40 uses the latest visual support spatial map data Map to calculate in real time an optimal movement path Rt that avoids static obstacles So and dynamic obstacles Do located ahead and heads toward the destination position. Specifically, the autonomous flight control device 40 uses a B-spline curve optimization algorithm to solve the minimization problem of an objective cost function C _total (S) that includes at least a collision cost C with the obstacles, with a set S of control points _Pi of the B-spline curve as an optimization parameter. In this way, the autonomous flight control device 40 determines an optimal movement path (collision avoidance path) Rto that avoids the static obstacles So and dynamic obstacles Do. Furthermore, the autonomous flight control device 40 resets the control points Pi of the B-spline curve on the collision avoidance path Rts that has been searched and determined to avoid the obstacles, until the entire movement path _Rt no longer collides with the detected obstacles. The autonomous flight control device 40 iteratively solves the minimization problem of the objective cost function C _total (S). As a result, the UAV 3 calculates in real time the optimal movement route Rt to the destination position while avoiding the static obstacles So and dynamic obstacles Do located ahead.

By using the above-mentioned B-spline curve optimization algorithm to avoid dynamic obstacles Do, whose movements are difficult to predict, the UAV3 can reduce the calculation cost required to optimize the movement route Rt (improving calculation speed). Furthermore, by repeating optimization, the 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 optimization, the UAV3 can perform real-time calculations to determine the path plan within an appropriate calculation time. Therefore, the UAV3 can avoid dynamic obstacles Do, whose movements are difficult to predict, even in a computational environment with limited resources.

[Modification]
In the above embodiment, the minimization problem of the objective cost function C _total (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.

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 taking into account future changes in the surrounding environment (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 of collisions with detected obstacles. However, even with the above configuration, there are limitations on the computational resources of the expansion device 4, and the UAV 3 may not be able to calculate the optimal travel route Rt 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 Rt for obstacle avoidance also change. In light 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, even if the calculation fails, the UAV3 will recalculate the optimal movement route Rto. This allows the UAV3 to improve the rate at which the intended measurement work is carried out.

While the above embodiment illustrates an example in which the work vehicle M is driven by direct operation of the operator Op, 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 from around the vehicle, work plan data, design model data, etc. In this case, the UAV3 can communicate 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 identified insufficient excavation area. The work vehicle M then transmits a notification of the completion of excavation work to the UAV3. Upon receiving the notification of the completion of excavation work, the UAV3 performs measurement work again. In this way, when the work vehicle M is an autonomously driven heavy machinery, the UAV3 and the work vehicle M can work together in an unmanned environment. In this case, the manager can monitor the UAV3 and work vehicle M in the unmanned environment using a remote camera and manage the work process.

In the above embodiment, a UAV3 was described using a multicopter (rotor-based aircraft) as an example, but the technology of the present invention is not limited to this. UAV3 includes fixed-wing aircraft and airship-type aircraft. Examples of mobile objects include not only vehicles and aircraft capable of moving on land or in the air, but also ships and submarines capable of moving on water or underwater. It is desirable to select these mobile objects appropriately depending on the work purpose and environment to which they are applied. For example, when performing underwater tunnel excavation work, an unmanned submarine can be used instead of an unmanned aircraft.

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 tunnel face inside the 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 aims to ensure safety at the work site, improve productivity, and/or improve construction accuracy, and is effective as an alternative to manual work.

1: Measurement system, 2: Terminal device, 3: UAV (unmanned aerial vehicle), 4: Expansion device;
301: Drive control unit;
401: Autonomous movement control unit;
410: Storage 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: visual support spatial map data, So: static obstacles, Do: dynamic obstacles, Rt: travel route.

Claims (13)

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 three-dimensional spatial 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 one curve based on a 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,
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 that defines 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;
calculating the first and second collision costs and solving the mathematical model to optimize the curve and determine the travel path;
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 two end points of the arc and the predicted movement position, and the first and second line segments, the second collision area being set so that the value of the safety distance decreases from the current position to the predicted movement position;
The optimization unit
A moving body that identifies the collision control point in each of the first and second collision regions and calculates the third distance. 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. 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. 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. the map data generating unit has an object identifying unit that identifies whether the detected object is the static object or the 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 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 between the third position and the fourth position is calculated;
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. 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, delete the corresponding second three-dimensional object 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. 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. 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 1 , 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. 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 1 , wherein the optimization of the curve is repeated until the entire moving path does not collide with the static object and the dynamic object. The optimization unit
2. The moving body according to claim 1, 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, multiplying the weighting coefficients by a predetermined coefficient to increase the values of the weighting coefficients; and updating the objective cost function based on the first and second collision costs using the increased weighting coefficients. 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. 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 three-dimensional spatial 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 one curve based on a 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,
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;
calculating the first and second collision costs and solving the mathematical model to optimize the curve and determine the travel path;
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 two end points of the arc and the predicted movement position, and the first and second line segments, the second collision area being set so that the value of the safety distance decreases from the current position to the predicted movement position;
The optimization unit
a moving body that identifies the collision control point in each of the first and second collision regions and calculates the third distance;
A measurement system using a moving body, comprising: a terminal device that visualizes the analysis results output from the moving body. 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 three-dimensional spatial 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 continuously updating the three-dimensional spatial map data while the moving body is moving;
a step of identifying static objects and dynamic objects among the detected objects based on the updated three-dimensional spatial map data, calculating a movement path by connecting a plurality of polynomial curves to form a single curve based on the result of predicting the movement of the dynamic objects, and 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;
analyzing the measurement results and outputting the analysis results;
The calculating step
determining the movement path that avoids the static object and the dynamic object by optimizing the curve;
The determining step includes:
a set of a plurality of control points of the polynomial curve is used as an optimization parameter, and a mathematical model is defined that defines a minimization problem of an objective cost function based on at least a first collision cost indicating the possibility that the moving body will collide with the static object and a second collision cost indicating the possibility that the moving body will collide with the dynamic object , by calculating and solving the first and second collision costs , thereby optimizing the curve and determining the movement path;
The determining step includes:
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 two end points of the arc and the predicted movement position, and the first and second line segments, the second collision area being set so that the value of the safety distance decreases from the current position to the predicted movement position;
The determining step includes:
a measurement method using a moving object, the method including identifying the collision control point in each of the first and second collision regions and calculating the third distance;