WO2022239318A1 - 移動体、移動制御方法、およびプログラム - Google Patents

移動体、移動制御方法、およびプログラム Download PDF

Info

Publication number
WO2022239318A1
WO2022239318A1 PCT/JP2022/003738 JP2022003738W WO2022239318A1 WO 2022239318 A1 WO2022239318 A1 WO 2022239318A1 JP 2022003738 W JP2022003738 W JP 2022003738W WO 2022239318 A1 WO2022239318 A1 WO 2022239318A1
Authority
WO
WIPO (PCT)
Prior art keywords
normal vector
control information
moving body
obstacle
unit
Prior art date
Application number
PCT/JP2022/003738
Other languages
English (en)
French (fr)
Inventor
琢人 元山
航平 漆戸
正樹 半田
政彦 豊吉
真一郎 阿部
Original Assignee
ソニーグループ株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by ソニーグループ株式会社 filed Critical ソニーグループ株式会社
Publication of WO2022239318A1 publication Critical patent/WO2022239318A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions

Definitions

  • the present disclosure relates to a mobile body, a movement control method, and a program, and more particularly to a mobile body, a movement control method, and a program that can suppress erroneous determinations in obstacle detection.
  • Some mobile objects such as drones, have a function to slow down or stop when an obstacle that could collide with them is detected on the orbit in the direction of travel.
  • Patent Literature 1 discloses a technique for detecting a normal vector in pixel units from a polarized image transmitted through a plurality of polarizing filters with different polarization directions.
  • the present disclosure has been made in view of such circumstances, and is intended to suppress erroneous determinations in obstacle detection.
  • a moving object includes a normal vector estimating unit that estimates a normal vector based on sensor data obtained by sensing a traveling direction of the own object, and controls movement of the own object based on the normal vector. and a control information generator that generates control information for the mobile object.
  • a movement control method estimates a normal vector based on sensor data obtained by sensing an object in a moving direction of a moving body, and performs control for controlling movement of the moving body based on the normal vector.
  • a movement control method for generating information estimates a normal vector based on sensor data obtained by sensing an object in a moving direction of a moving body, and performs control for controlling movement of the moving body based on the normal vector.
  • the program of the present disclosure causes a computer to estimate a normal vector based on sensor data obtained by sensing an object in the traveling direction of a moving object, and control the movement of the moving object based on the normal vector. It is a program for executing processing for generating control information.
  • a normal vector is estimated based on sensor data obtained by sensing an object in the traveling direction of a moving object, and control information for controlling movement of the moving object is generated based on the normal vector. be done.
  • FIG. 1 is a diagram illustrating an overview of technology according to the present disclosure; It is a block diagram which shows the hardware structural example of a mobile body. 2 is a block diagram showing a functional configuration example of a moving object according to the first embodiment; FIG. It is a flow chart explaining a flow of obstacle detection processing. It is a flow chart explaining a flow of obstacle detection processing. It is a figure explaining division
  • FIG. 1 is a diagram illustrating an overview of technology according to the present disclosure; It is a block diagram which shows the hardware structural example of a mobile body. 2 is a block diagram showing a functional configuration example of a moving object according to the first embodiment; FIG. It is a flow chart explaining a flow of obstacle detection processing. It is a flow chart explaining a flow of obstacle detection processing. It is a figure explaining division
  • FIG. 1 is a diagram illustrating an overview of technology according to the present disclosure; It is
  • FIG. 4 is a diagram illustrating calculation of a collision risk for each collision determination area; It is a figure explaining the area of the real space of point cloud data. It is a figure explaining the setting of an obstacle area. It is a figure explaining correction
  • FIG. 11 is a block diagram showing an example of the functional configuration of a moving object according to the second embodiment; FIG. It is a flow chart explaining a flow of obstacle detection processing. It is a flowchart explaining the flow of a track
  • FIG. 4 is a diagram for explaining estimation of a representative normal vector; It is a figure explaining calculation of an angular acceleration.
  • FIG. 12 is a block diagram showing an example of the functional configuration of a moving object according to the third embodiment; FIG.
  • FIG. 10 is a flowchart for explaining the flow of superimposed image transmission processing;
  • FIG. 10 is a diagram showing an example of superimposing a normal vector image on an RGB image;
  • FIG. 4 is a diagram showing an example of a sensor;
  • 1 is a block diagram showing a functional configuration example of a mobile body control system;
  • FIG. It is a figure which shows the structural example of a computer.
  • Some mobile objects such as drones, have a function of decelerating or stopping when an obstacle that may collide with them is detected on the trajectory in the direction of travel.
  • a moving body 10 includes a distance measuring sensor 11 configured by a stereo camera or the like.
  • the moving body 10 can acquire the point cloud data Pc by the distance measurement sensor 11 .
  • the moving body 10 when the point cloud data Pc exists on the spatial region Sa through which the moving body 10 passes along the predicted future trajectory, the moving body 10 has a collision possibility It determines that a certain obstacle is detected, and slows down or stops.
  • the point cloud data Pc near the ground G determines whether an obstacle is detected due to fluctuations due to ranging errors and accuracy. It may be included in the collision detection area. Further, when the moving body 10 flies at a low altitude near the ground G, there is a possibility that the collision determination area overlaps with the ground G due to an error in the velocity vector v or the like. As a result, the ground G is detected as an obstacle that may collide, and the moving body 10 stops unintentionally. This can occur not only during low-altitude flight, but also during high-speed flight.
  • the moving body 10 to which the technology according to the present disclosure is applied estimates the normal vector n for the point cloud data Pc based on the sensor data obtained by sensing the traveling direction of the moving body 10. .
  • the sensor data is, for example, a polarization image captured by a polarization image sensor.
  • the moving body 10 determines whether to decelerate or stop by calculating the collision risk for each point cloud data Pc.
  • the collision risk for the point cloud data Pc is calculated according to how much the normal vector n for the point cloud data Pc and the velocity vector v of the moving body 10 are directed in the same direction.
  • FIG. 4 is a block diagram showing a hardware configuration example of the mobile body 10. As shown in FIG.
  • the moving object 10 includes moving objects such as drones, vehicles, and ships.
  • moving objects such as drones, vehicles, and ships.
  • the technology according to the present disclosure includes mobile objects that move by user's control (operation), drones that fly autonomously, autonomous vehicles that move on land, autonomous vessels that move on or in water, and autonomous vehicles that move indoors. It can be applied to autonomous mobile robots such as vacuum cleaners.
  • the moving body 10 includes a sensor 20 , a communication section 21 , a control section 22 , a movement control section 23 , a movement mechanism 24 and a storage section 25 .
  • the sensor 20 includes various sensors including the distance measuring sensor 11 described above, and senses each direction around the moving body 10 including the traveling direction of the moving body 10 . Sensor data obtained by sensing is supplied to the control unit 22 .
  • the communication unit 21 is composed of a network interface and the like, and performs wireless or wired communication with a controller for operating the mobile object 10 and other arbitrary devices.
  • the communication unit 21 may directly communicate with a device that is a communication partner, or may perform network communication via a base station such as Wi-Fi (registered trademark), 4G, 5G, or a repeater. good.
  • the control unit 22 is composed of a CPU (Central Processing Unit), memory, etc., and controls the communication unit 21, the movement control unit 23, and the storage unit 25 by executing a predetermined program. For example, the controller 22 controls the movement controller 23 based on sensor data from the sensor 20 .
  • CPU Central Processing Unit
  • memory etc.
  • the movement control unit 23 is configured by a circuit such as a dedicated IC or FPGA (Field-Programmable Gate Array), and controls driving of the movement mechanism 24 according to the control of the control unit 22.
  • a circuit such as a dedicated IC or FPGA (Field-Programmable Gate Array)
  • the movement mechanism 24 is a mechanism for moving the mobile body 10, and includes a flight mechanism, a travel mechanism, a propulsion mechanism, and the like.
  • the moving body 10 is configured as a drone, and the moving mechanism 24 is configured by a motor, propeller, etc. as a flight mechanism.
  • the mobile mechanism 24 is configured by wheels as a traveling mechanism, and when the mobile body 10 is configured as an autonomous ship, the mobile mechanism 24 is a propulsion mechanism. It consists of a screw propeller.
  • the movement mechanism 24 is driven under the control of the movement control unit 23 to move the moving body 10 .
  • control unit 22 drives the moving mechanism 24 by controlling the movement control unit 23 according to the control signal from the controller received by the communication unit 21, for example. Thereby, the moving body 10 moves according to the operation of the controller by the user.
  • the storage unit 25 is composed of a non-volatile memory such as a flash memory, etc., and stores various information under the control of the control unit 22 .
  • FIG. 5 is a block diagram showing a functional configuration example of the moving body 10 of the first embodiment to which the technology according to the present disclosure is applied.
  • the moving body 10 shown in FIG. 5 includes two polarization cameras 30-1 and 30-2.
  • the polarization cameras 30-1 and 30-2 have polarization image sensors 51-1 and 51-2, respectively, and also function as stereo cameras.
  • the polarization image sensors 51-1 and 51-2 are configured by forming polarizers in a plurality of directions on photodiodes of pixels.
  • the polarization image sensors 51-1 and 51-2 are equipped with polarizers in four directions, and can acquire polarization images in four directions.
  • the polarization image sensors 51-1 and 51-2 are one of various sensors forming the sensor 20 of FIG.
  • the polarization image sensors 51-1 and 51-2 are simply referred to as the polarization image sensors 51 when they are not distinguished from each other.
  • the moving body 10 further includes a normal vector estimation unit 52, luminance image construction units 53-1 and 53-2, parallelization processing units 54-1 and 54-2, calibration data 55, and a normal vector correction unit 56. Prepare.
  • the normal vector estimation unit 52 estimates a normal vector for each pixel position of the polarization image based on the polarization image acquired by the polarization image sensor 51-1.
  • the polarization image used for estimating the normal vector may be the polarization image acquired by the polarization image sensor 51-2.
  • the normal vector estimating unit 52 obtains the relationship between the luminance and the polarization angle from the polarization direction and the luminance of the polarization image based on the polarization images having three or more polarization directions, and determines the azimuth angle ⁇ that provides the maximum luminance. determine. Further, the normal vector estimating unit 52 calculates the degree of polarization ⁇ using the maximum luminance and the minimum luminance obtained from the relationship between the luminance and the polarization angle, and based on the characteristic curve showing the relationship between the degree of polarization and the zenith angle, Determine the zenith angle ⁇ corresponding to the degree of polarization ⁇ . In this way, the normal vector estimator 52 estimates the azimuth angle ⁇ and the zenith angle ⁇ as the normal vector of the object for each pixel position based on the polarized images with three or more polarization directions.
  • the luminance image constructing units 53-1 and 53-2 generate luminance images from two viewpoints based on luminance values of pixels of the polarization images from two viewpoints obtained by the polarization image sensors 51-1 and 51-2, respectively. Configure.
  • the parallelization processing units 54-1 and 54-2 perform parallelization processing by stereo rectification on the two-viewpoint luminance images constructed by the luminance image construction units 53-1 and 53-2, respectively.
  • the parallelization process is performed using the internal parameters, external parameters, and distortion coefficients of the polarization cameras 30-1 and 30-2 held as calibration data 55.
  • FIG. The parallelization process corrects the two-viewpoint luminance image to a parallelized luminance image.
  • the normal vector correction unit 56 corrects the normal vector estimated for each pixel position of the polarization image according to the correction of the luminance image by the parallelization processing by the parallelization processing units 54-1 and 54-2.
  • the normal vector correction also uses the internal parameters, external parameters, and distortion coefficients of the polarization cameras 30-1 and 30-2.
  • the moving object 10 further includes a parallax estimation unit 57, a visual odometry unit 58, a GPS sensor 59, an IMU 60, a barometer 61, a geomagnetic sensor 62, and a self-position estimation unit 63.
  • the parallax estimation unit 57 estimates the parallax on the luminance image by stereo matching using the luminance image after parallelization processing. Based on the estimated parallax, the parallax estimation unit 57 outputs a parallax map composed of point cloud data representing the distance (depth) to the subject.
  • the visual odometry unit 58 estimates the trajectory of the vehicle itself (the mobile object 10) by visual odometry based on the luminance image after parallelization processing, and supplies it to the self-position estimation unit 63.
  • a GPS (Global Positioning System) sensor 59 acquires the GPS information of its own device (mobile body 10) and supplies it to the self-position estimation unit 63.
  • An IMU (Inertial Measurement Unit) 60 detects the three-dimensional angular velocity and acceleration of its own device (moving body 10 ) and supplies them to a self-position estimator 63 .
  • the barometer 61 measures air pressure and supplies it to the self-position estimation unit 63 .
  • the geomagnetic sensor 62 detects geomagnetism and supplies it to the self-position estimation unit 63 .
  • Each of the GPS sensor 59, IMU 60, barometer 61, and geomagnetic sensor 62 is also one of the various sensors that make up the sensor 20 of FIG.
  • the self-position estimation unit 63 performs sensor fusion using an extended Kalman filter based on data obtained from the visual odometry unit 58, GPS sensor 59, IMU 60, barometer 61, and geomagnetic sensor 62. Thereby, the self-position estimator 63 can calculate the self-position and velocity vector of the self-machine (moving body 10).
  • the mobile body 10 further includes an obstacle collision determination section 64 and a flight controller 65 .
  • the obstacle collision determination unit 64 Determine the probability of collision with 10 obstacles.
  • the obstacle collision determination unit 64 has a division unit 64a, a calculation unit 64b, and a setting unit 64c.
  • the dividing unit 64a divides the space area in the traveling direction of the moving body 10 into small areas that are continuous in the traveling direction. Hereinafter, the divided small areas are referred to as collision determination areas.
  • the calculator 64b calculates the risk of collision with an obstacle for each collision determination region divided by the divider 64a based on the normal vector, the point cloud data, and the velocity vector.
  • the setting unit 64c sets an obstacle area in which an obstacle may exist, based on the collision risk for each collision determination area calculated by the calculation unit 64b.
  • the obstacle collision determination unit 64 generates control information for controlling the movement of the own machine (moving body 10) based on the distance from the own machine (moving body 10) to the obstacle area. That is, the obstacle collision determination unit 64 has a function as a control information generation unit that generates control information for controlling the movement of the moving body 10 based on the collision risk.
  • the flight controller 65 corresponds to the movement control section 23 of FIG. 4, and controls the movement of the moving body 10 based on the control information generated by the obstacle collision determination section 64 (control information generation section).
  • the obstacle collision determination unit 64 generates control information and the flight controller 65 controls the movement of the mobile body 10 based on the control signal input from the controller for operating the aircraft (the mobile body 10). You can also The controller can not only input control signals for controlling the movement of the moving body 10 in real time, but can also input, for example, destinations and movement routes as control signals. In this case, the flight controller 65 controls the movement of the mobile body 10 so that the mobile body 10 moves autonomously based on the destination and movement route input as the control signal.
  • step S11 the polarization cameras 30-1 and 30-2 (polarization image sensors 51-1 and 51-2) start capturing polarization images.
  • step S12 the normal vector estimator 52 estimates the normal vector based on the polarization image captured by the polarization camera 30-1 or the polarization camera 30-2.
  • step S13 the luminance image constructing units 53-1 and 53-2 respectively construct luminance images from the two-viewpoint polarization images captured by the polarization cameras 30-1 and 30-2.
  • step S14 the parallelization processing units 54-1 and 54-2 perform parallelization processing on the two-viewpoint luminance images constructed by the luminance image construction units 53-1 and 53-2, respectively.
  • step S15 the normal vector correction unit 56 corrects the normal vector in accordance with the parallelization processing by the parallelization processing units 54-1 and 54-2.
  • step S16 the parallax estimation unit 57 estimates parallax from the luminance image after parallelization processing.
  • step S17 the self-position estimator 63 determines the self-position of the vehicle (moving body 10) based on the data obtained by the visual odometry unit 58, the GPS sensor 59, the IMU 60, the barometer 61, and the geomagnetic sensor 62. Calculate the velocity vector.
  • the dividing unit 64a of the obstacle collision determination unit 64 divides the space area in the traveling direction (velocity vector direction) into the collision determination area according to the accuracy of the distance measurement from the vehicle itself (moving body 10). split into
  • the dividing unit 64a divides the space through which the moving body 10 passes based on the distance Dr corresponding to the distance resolution in the optical axis direction Ax of the ranging sensors (polarization cameras 30-1 and 30-2 as stereo cameras).
  • the area Sa is divided into collision determination areas Cd. Since the distance resolution of the ranging sensor becomes less accurate as the distance from the machine becomes greater, the distance Dr increases as the distance from the machine becomes greater.
  • the length of the collision determination area Cd in the direction of the velocity vector v (depth direction) is set according to the distance Dr. That is, the length of the collision determination area Cd in the direction of the velocity vector v increases as the distance from the aircraft increases.
  • step S19 the calculation unit 64b extracts point cloud data included in the spatial region in the traveling direction (velocity vector direction) from the parallax map (point cloud data).
  • step S20 the calculation unit 64b calculates the collision risk for each collision determination area based on the normal vector of the extracted point cloud data.
  • the spatial region Sa in the direction of the velocity vector v is divided into 1st, 2nd, 3rd, .
  • a normal vector n is shown for the data Pc.
  • the collision risk Rk of the k -th collision determination area Cd is expressed by the following equation (1).
  • R area in Expression (1) is a value proportional to the area of the real space corresponding to one pixel of each point cloud data. As shown in FIG. 10, when the distance to the point cloud data is d, and the horizontal and vertical focal lengths of the polarization camera 30-1 or the polarization camera 30-2 are f x and f y , respectively, R area is represented by the following formula (2).
  • Equation (2) the product of d/ fx and d/ fy is proportional to the real space area corresponding to one pixel of point cloud data, and ⁇ area is a fixed weight parameter.
  • R count in Equation (1) is a value (weight) uniformly set for each point cloud data, and is represented by Equation (3) below.
  • ⁇ count is the total number of point cloud data included in the collision determination area Cd, and when there is an obstacle at close range, R area proportional to the area of the real space described above becomes smaller. It is a value to prevent too much.
  • R normal in equation (1) is a value (gain) calculated based on the normal vector n for each point cloud data, and is calculated using the inner product of the velocity vector v and the normal vector n for each point cloud data. , is represented by the following equation (4).
  • ⁇ normal is a fixed weight parameter.
  • the absolute value of the inner product of the velocity vector v and the normal vector n of each point cloud data increases as the moving direction of the moving body 10 and the surface of the subject face each other.
  • the traveling direction of the mobile object 10 and the normal direction of the ground surface are perpendicular to each other.
  • the absolute value of the inner product of the normal vector n becomes smaller.
  • the velocity vector v and the normal vector n of each point cloud data are both vectors in a fixed coordinate system with the moving body 10 as a reference.
  • step S20 the setting unit 64c determines whether there is a possibility that an obstacle exists based on the calculated collision risk for each collision determination area. Set an obstacle area.
  • the setting unit 64c sets the collision determination area Cd, in which the collision risk Rk is higher than the predetermined threshold value Rth and is closest to the own machine (moving body 10), as the obstacle area Ob.
  • the k - th collision determination area Cd whose collision risk Rk is higher than the predetermined threshold value Rth and which is closest to the own machine (moving body 10) is set as the obstacle area Ob.
  • the obstacle collision determination unit 64 determines the stoppable distance (braking distance) required to stop the own machine (moving body 10) from the current speed based on the speed vector v. Calculate The stopping distance may include a margin, for example 5m.
  • step S23 the obstacle collision determination unit 64 determines whether the distance to the obstacle area is shorter than the calculated stoppable distance. If it is determined that the distance to the obstacle area is shorter than the stoppable distance, the process proceeds to step S24.
  • the obstacle collision determination unit 64 when it is determined that the distance to the obstacle area is shorter than the stoppable distance, the obstacle collision determination unit 64 generates control information for decelerating the flight controller to a speed at which the flight controller can stop before the obstacle. output to 65.
  • step S24 the flight controller 65 decelerates the moving body 10 based on the control information generated by the obstacle collision determination section 64.
  • the control information for decelerating the moving body 10 to a speed at which it can be stopped before the obstacle is generated to decelerate the moving body 10, but the control information for stopping before the obstacle is generated. and the moving body 10 may be stopped.
  • step S24 is skipped.
  • the movement of the aircraft is controlled using the normal vector estimated based on the polarized image obtained by imaging the traveling direction of the aircraft.
  • the user can perform more natural manual steering.
  • Second Embodiment> In the second embodiment, as shown in FIG. 12, when the ground G is included in the obstacle area Ob in the space area Sa through which the moving body 10 flying at high speed passes, not only is the speed reduced, but also , and the corrected trajectory Ct to avoid the collision with the obstacle.
  • FIG. 12 is a block diagram showing a functional configuration example of the mobile body 10 of the second embodiment to which the technology according to the present disclosure is applied.
  • a moving body 10A shown in FIG. 12 basically has the same configuration as the moving body 10 of FIG. However, in the moving body 10A of FIG. 12, the obstacle collision determination unit 64 has a representative normal vector calculation unit 64d and an angular acceleration calculation unit 64e in addition to the division unit 64a, the calculation unit 64b, and the setting unit 64c. there is
  • the representative normal vector calculation unit 64d calculates a representative normal vector (hereinafter referred to as representative method line vector).
  • the angular acceleration calculator 64e predicts the velocity vector when the own machine (moving body 10A) reaches the obstacle area, and calculates the velocity vector (predicted velocity vector) and the representative normal vector calculator 64d. Angular acceleration is calculated so that the representative normal vectors are orthogonal.
  • the obstacle collision determination unit 64 can generate control information for correcting the trajectory of the aircraft so that the predicted velocity vector and the representative normal vector are orthogonal.
  • step S23 in the flowchart of FIG. 14 is executed in the same manner as the flow of the obstacle detection processing by the moving body 10 of FIG. 5 described with reference to the flowcharts of FIGS. 6 and 7.
  • step S23 when it is determined in step S23 that the distance to the obstacle area is shorter than the stoppable distance, the process proceeds to step S50.
  • step S50 the obstacle collision determination unit 64 executes trajectory correction processing.
  • FIG. 15 is a flowchart describing the flow of trajectory correction processing.
  • step S51 the representative normal vector calculation unit 64d of the obstacle collision determination unit 64 calculates the representative normal vector of the obstacle area.
  • FIG. 16A shows the normal vector n for the point cloud data Pc included in the obstacle area Ob.
  • Fig. B shows the distribution of the normal vector n in the obstacle area Ob.
  • the representative normal vector calculation unit 64d analyzes the distribution of the normal vectors n, and determines the vector in the most dominant direction in the obstacle area Ob as the representative normal vector Rn shown in FIG.
  • step S52 the angular acceleration calculator 64e calculates the predicted velocity vector , the angular acceleration at which the representative normal vector is orthogonal is calculated.
  • the predicted velocity vector Pv and the representative normal vector Rn are orthogonal to each other at a position a certain distance p away from the center of the obstacle area Ob in the direction of the representative normal vector Rn. Angular acceleration that realizes flight along the corrected trajectory Ct is calculated.
  • step S53 the obstacle collision determination section 64 determines whether or not the angular acceleration calculated by the angular acceleration calculation section 64e exceeds a predetermined value. If it is determined that the calculated angular acceleration does not exceed the predetermined value, the process proceeds to step S54.
  • step S54 the obstacle collision determination unit 64 determines whether or not point cloud data corresponding to an object that may collide exists on the corrected trajectory (corrected trajectory). If it is determined that there is no point cloud data corresponding to an object that may collide on the corrected trajectory, the process proceeds to step S55. At this time, the obstacle collision determination unit 64 outputs the angular acceleration calculated by the angular acceleration calculation unit 64e to the flight controller 65 as control information for correcting the trajectory of the own aircraft.
  • step S55 the flight controller 65 corrects the trajectory of the aircraft by controlling the attitude of the mobile body 10A based on the angular acceleration output by the obstacle collision determination unit 64 as control information. After that, the process returns to step S12 (FIG. 6), and the processes of steps S12 to S23 and S50 are repeated.
  • step S53 if it is determined in step S53 that the calculated angular acceleration exceeds a predetermined value, or if it is determined in step S54 that point cloud data corresponding to an object with a possibility of collision exists on the corrected trajectory, The process proceeds to step S56.
  • the obstacle collision determination unit 64 generates control information for decelerating the vehicle to a speed at which it can stop before the obstacle, and outputs the control information to the flight controller 65 .
  • step S56 the flight controller 65 decelerates the moving body 10A based on the control information generated by the obstacle collision determination section 64. After that, the process returns to step S12 (FIG. 6), and the processes of steps S12 to S23 and S50 are repeated. Also here, control information for stopping before the obstacle is generated, and the moving body 10A may be stopped.
  • the trajectory of the aircraft is corrected using the normal vector estimated based on the polarized image obtained by imaging the traveling direction of the aircraft. Even so, it is possible to avoid collisions with obstacles.
  • the normal vector image generated based on the estimated normal vector is superimposed on the image captured by the FPV camera, thereby assisting the user during remote operation of the mobile object 10.
  • FIG. 18 is a block diagram showing a functional configuration example of the moving body 10 of the third embodiment to which the technology according to the present disclosure is applied.
  • a moving body 10B shown in FIG. 18 has the same configuration as the moving body 10 in FIG. , a superimposition unit 115 and a transmission unit 116 .
  • the obstacle collision determination unit 64 includes a representative normal vector calculation unit 64d and an angular acceleration calculation unit 64e in addition to the division unit 64a, the calculation unit 64b, and the setting unit 64c. may
  • the FPV camera 100 has a gimbal mechanism and can capture images from various angles.
  • the RGB image sensor 111 of the FPV camera 100 is configured by arranging R, G, and B color filters on pixels in, for example, a Bayer array, and captures an RGB image.
  • the posture estimation unit 112 estimates the current posture of the FPV camera 100 based on the RGB image captured by the RGB image sensor 111, with the origin position of the camera coordinate system of the FPV camera 100 as a reference.
  • the coordinate transformation unit 113 transforms the coordinate system of the normal vector map composed of the normal vectors for each pixel position of the polarization image into the camera coordinate system of the FPV camera 100 .
  • the parallax map from the parallax estimation unit 57, the self-position from the self-position estimation unit 63, the orientation information of the polarization cameras 30-1 and 30-2, the polarization camera 30- 1,30-2 and current attitude information are used.
  • the normal vector image generation unit 114 generates a normal vector image colored according to the direction of the normal vector based on the coordinate-transformed normal vector map. That is, a normal vector image transformed into the camera coordinate system of the FPV camera 100 is generated.
  • the superimposing unit 115 generates a superimposed image by superimposing the normal vector image generated by the normal vector image generating unit 114 on the RGB image captured by the RGB image sensor 111 .
  • the transmission unit 116 corresponds to the communication unit 21 in FIG. 4, and transmits the superimposed image generated by the superimposing unit 115 to the controller for inputting the control signal of its own device (moving body 10B).
  • FIG. 19 is a flowchart for explaining the flow of superimposed image processing. The processing of FIG. 19 is executed in parallel with the obstacle detection processing described with reference to FIGS. 6, 7 and 14. FIG.
  • step S111 the FPV camera 100 (RGB image sensor 111) captures an RGB image.
  • step S112 the posture estimation unit 112 estimates the current posture of the FPV camera 100 based on the captured RGB image, with the origin position of the camera coordinate system of the FPV camera 100 as a reference.
  • step S ⁇ b>113 the coordinate transformation unit 113 transforms the coordinate system of the normal vector map into the camera coordinate system of the FPV camera 100 .
  • step S114 the normal vector image generation unit 114 generates a normal vector image colored according to the direction of the normal vector based on the coordinate-transformed normal vector map.
  • step S ⁇ b>115 the superimposing unit 115 superimposes the normal vector image generated by the normal vector image generating unit 114 on the RGB image captured by the RGB image sensor 111 .
  • the RGB image 130 shown in FIG. 20 is an image captured in a dark environment or a low-contrast image, for the user who remotely operates the moving object 10B while viewing the RGB image 130, the subject reflected in the RGB image 130 Unevenness is difficult to see.
  • the normal vector image generating unit 114 generates a normal vector image 140 colored according to the direction of the normal vector based on the normal vector map, and the superimposing unit 115 adds the normal vector image 140 to the RGB image 130.
  • a superimposed image 150 is generated by superimposing the vector image 140 .
  • step S116 the transmitting unit 116 transmits the superimposed image generated by the superimposing unit 115 to the controller for inputting the control signal of the own device (moving body 10B).
  • the process returns to step S111, and the above-described processes are repeated.
  • the FPV camera 100 may not be provided, and only the normal vector image may be transmitted to the controller.
  • the sensor 20 that realizes normal vector estimation and distance measurement is composed of the polarization image sensors 51-1 and 51-2 that constitute a two-viewpoint stereo camera.
  • the sensor 20 may be composed of a polarization image sensor 51 and RGB image sensors 211-1 and 211-2 that constitute a two-viewpoint stereo camera. .
  • the sensor 20 may be composed of a polarization image sensor 51 and a ranging sensor 231 such as a LiDAR (Light Detection and Ranging) or ToF (Time of Flight) sensor. good.
  • a polarization image sensor 51 such as a LiDAR (Light Detection and Ranging) or ToF (Time of Flight) sensor. good.
  • a ranging sensor 231 such as a LiDAR (Light Detection and Ranging) or ToF (Time of Flight) sensor. good.
  • the sensor 20 can realize normal vector estimation and range finding.
  • the normal vector is estimated based on the polarization image acquired by the polarization image sensor.
  • the normal vector based on sensor data obtained by sensing the traveling direction of the own machine (moving body 10) with a predetermined sensor.
  • a predetermined sensor For example, it is possible to estimate a normal vector based on data obtained by performing predetermined processing on depth information acquired by a range finding device such as a general stereo camera or LiDAR.
  • these configurations are controlled by the control unit 331 of the information processing device 320 when the movement of the moving body 310 is controlled by the information processing device 320 configured on the cloud, for example. may be implemented.
  • the control unit 331 estimates a normal vector based on the sensor data transmitted from the moving body 310, and generates control information for controlling movement of the moving body 310 based on the normal vector. The generated control information is transmitted to mobile 310 .
  • the moving body 10 to which the technology according to the present disclosure is applied has been described as having the effect when flying at a low altitude near the ground. The effect can be obtained even when moving along the .
  • the series of processes described above can be executed by hardware or by software.
  • a program that constitutes the software is installed in the computer.
  • the computer includes, for example, a computer built into dedicated hardware and a general-purpose personal computer capable of executing various functions by installing various programs.
  • FIG. 23 is a block diagram showing an example of the hardware configuration of a computer that executes the series of processes described above by a program.
  • a CPU 501 In the computer, a CPU 501 , a ROM (Read Only Memory) 502 and a RAM (Random Access Memory) 503 are interconnected by a bus 504 .
  • a bus 504 In the computer, a CPU 501 , a ROM (Read Only Memory) 502 and a RAM (Random Access Memory) 503 are interconnected by a bus 504 .
  • An input/output interface 505 is further connected to the bus 504 .
  • An input unit 506 , an output unit 507 , a storage unit 508 , a communication unit 509 and a drive 510 are connected to the input/output interface 505 .
  • the input unit 506 consists of a keyboard, mouse, microphone, and the like.
  • the output unit 507 includes a display, a speaker, and the like.
  • a storage unit 508 includes a hard disk, a nonvolatile memory, or the like.
  • a communication unit 509 includes a network interface and the like.
  • a drive 510 drives a removable medium 511 such as a magnetic disk, optical disk, magneto-optical disk, or semiconductor memory.
  • the CPU 501 loads a program stored in the storage unit 508 into the RAM 503 via the input/output interface 505 and the bus 504 and executes the above-described series of programs. is processed.
  • the program executed by the computer (CPU 501) can be provided by being recorded on removable media 511 such as package media, for example. Also, the program can be provided via a wired or wireless transmission medium such as a local area network, the Internet, or digital satellite broadcasting.
  • the program can be installed in the storage unit 508 via the input/output interface 505 by loading the removable medium 511 into the drive 510 . Also, the program can be received by the communication unit 509 and installed in the storage unit 508 via a wired or wireless transmission medium. In addition, the program can be installed in the ROM 502 or the storage unit 508 in advance.
  • the program executed by the computer may be a program that is processed in chronological order according to the order described in this specification, or may be executed in parallel or at a necessary timing such as when a call is made. It may be a program in which processing is performed.
  • the technology according to the present disclosure can be configured as follows. (1) a normal vector estimation unit for estimating a normal vector based on sensor data obtained by sensing the traveling direction of the aircraft; and a control information generator that generates control information for controlling movement of the mobile object based on the normal vector. (2) Further comprising a calculation unit that calculates a collision risk with an obstacle based on the normal vector, The moving body according to (1), wherein the control information generation unit generates the control information based on the collision risk. (3) (2) The moving body according to (2), wherein the calculation unit calculates the collision risk based on the normal vector of the point cloud data according to the distance from the vehicle.
  • (4) further comprising a dividing unit that divides the space area in the direction of travel into collision determination areas that are continuous in the direction of travel; (3) The moving body according to (3), wherein the calculator calculates the collision risk for each collision determination area based on the normal vector of the point cloud data included in the collision determination area. (5) (4) The moving body according to (4), wherein the dividing unit divides the spatial region into the collision determination regions according to the accuracy of distance measurement from the own device. (6) The calculation unit calculates the collision risk for each of the collision determination areas using the inner product of the normal vector and the speed vector of the own aircraft, which are obtained for the point cloud data included in the collision determination area. (4) or the moving body according to (5).
  • the calculation unit calculates a value corresponding to a sum of products of the inner product of each of the point cloud data included in the collision determination area and a value proportional to the area of the real space corresponding to the point cloud data, and calculates the value according to the collision detection area.
  • the moving object according to (6) which is calculated as the collision risk of the determination area.
  • a setting unit that sets the collision determination area closest to the own aircraft to an obstacle area where the collision risk is higher than a predetermined threshold and the obstacle may exist,
  • the moving body according to any one of (4) to (7), wherein the control information generating section generates the control information based on a distance from the own body to the obstacle area.
  • control information generating unit generates the control information for decelerating the own aircraft when the distance from the own aircraft to the obstacle area is shorter than the stoppable distance of the own aircraft. body.
  • control information generation unit further comprising a representative normal vector calculation unit for calculating a representative normal vector in the obstacle region based on the normal vector for the point cloud data included in the obstacle region;
  • the control information generation unit generates the control information for correcting the trajectory of the aircraft so that the predicted velocity vector when the aircraft reaches the obstacle area is orthogonal to the representative normal vector.
  • a normal vector image generator that generates a normal vector image based on the normal vector;
  • the moving object according to any one of (1) to (13), further comprising a superimposing unit that superimposes the normal vector image on an RGB image captured by an FPV (First Person View) camera.
  • the normal vector image generator generates the normal vector image colored according to the direction of the normal vector.
  • the moving body according to (14) or (15), wherein the normal vector image generation unit generates the normal vector image converted into the coordinate system of the FPV camera.
  • a transmitting unit that transmits the superimposed image to a controller for inputting a control signal for the own object.

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Traffic Control Systems (AREA)

Abstract

本開示は、障害物検出における誤判定を抑制することができるようにする移動体、移動制御方法、およびプログラムに関する。 法線ベクトル推定部は自機の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、制御情報推定部は、法線ベクトルに基づいて、自機の移動を制御するための制御情報を生成する。本開示に係る技術は、例えば、ドローンなどの移動体に適用することができる。

Description

移動体、移動制御方法、およびプログラム
　本開示は、移動体、移動制御方法、およびプログラムに関し、特に、障害物検出における誤判定を抑制することができるようにする移動体、移動制御方法、およびプログラムに関する。
　ドローンなどの移動体の中には、進行方向の軌道上に衝突可能性のある障害物を検出した場合、減速したり停止したりする機能を備えているものがある。
　しかしながら、ドローンが地面の近くを低空飛行した場合、地面が衝突可能性のある障害物として検出され、ドローンが意図せず停止してしまう。
　ところで、特許文献１には、偏光方向の異なる複数の偏光フィルタを透過した偏光画像から画素単位で法線ベクトルを検出する技術が開示されている。
特開２０１５－１１４３０７号公報
　進行方向の軌道上で法線ベクトルを検出することができれば、障害物検出における判定の精度を高められると考えられる。
　本開示は、このような状況に鑑みてなされたものであり、障害物検出における誤判定を抑制することができるようにするものである。
　本開示の移動体は、自機の進行方向をセンシングしたセンサデータに基づいて、法線ベクトルを推定する法線ベクトル推定部と、前記法線ベクトルに基づいて、前記自機の移動を制御するための制御情報を生成する制御情報生成部とを備える移動体である。
　本開示の移動制御方法は、移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報を生成する移動制御方法である。
　本開示のプログラムは、コンピュータに、移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報を生成する処理を実行させるためのプログラムである。
　本開示においては、移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルが推定され、前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報が生成される。
移動体の障害物の検出について説明する図である。 障害物の誤検出の例について説明する図である。 本開示に係る技術の概要について説明する図である。 移動体のハードウェア構成例を示すブロック図である。 第１の実施形態の移動体の機能構成例を示すブロック図である。 障害物検出処理の流れについて説明するフローチャートである。 障害物検出処理の流れについて説明するフローチャートである。 衝突判定領域の分割について説明する図である。 衝突判定領域毎の衝突リスクの算出について説明する図である。 点群データの実空間の面積について説明する図である。 障害物領域の設定について説明する図である。 軌道の補正について説明する図である。 第２の実施形態の移動体の機能構成例を示すブロック図である。 障害物検出処理の流れについて説明するフローチャートである。 軌道補正処理の流れについて説明するフローチャートである。 代表法線ベクトルの推定について説明する図である。 角加速度の算出について説明する図である。 第３の実施形態の移動体の機能構成例を示すブロック図である。 重畳画像送信処理の流れについて説明するフローチャートである。 法線ベクトル画像のＲＧＢ画像への重畳の例を示す図である。 センサの例を示す図である。 移動体制御システムの機能構成例を示すブロック図である。 コンピュータの構成例を示す図である。
　以下、本開示を実施するための形態（以下、実施形態とする）について説明する。なお、説明は以下の順序で行う。
　１．従来技術の課題と本開示に係る技術の概要
　２．移動体のハードウェア構成
　３．第１の実施形態（法線ベクトルに基づいた移動制御）
　４．第２の実施形態（法線ベクトルに基づいた軌道補正）
　５．第３の実施形態（法線ベクトル画像のＲＧＢ画像への重畳）
　６．変形例
　７．コンピュータの構成例
＜１．従来技術の課題と本開示に係る技術の概要＞
　ドローンなどの移動体の中には、進行方向の軌道上に衝突可能性のある障害物を検出した場合、減速したり停止したりする機能を備えているものがある。
　例えば、図１に示されるように、ドローンとして構成される移動体１０が、ユーザの操縦により、速度ベクトルｖで空中を飛行しているとする。移動体１０は、ステレオカメラなどで構成される測距センサ１１を備えている。移動体１０は、測距センサ１１により点群データＰｃを取得することができる。
　図中、一点鎖線の枠で囲まれているように、将来の予測軌道に沿って移動体１０が通過する空間領域Ｓａ上に点群データＰｃが存在する場合、移動体１０は、衝突可能性のある障害物が検出されたと判定し、減速したり停止したりする。
　しかしながら、図２に示されるように、移動体１０が地面Ｇの近くを低空飛行した場合、測距誤差や精度による揺らぎによって、地面Ｇ近くの点群データＰｃが、障害物の検出を判定する衝突判定領域に含まれるおそれがある。また、移動体１０が地面Ｇの近くを低空飛行した場合、速度ベクトルｖの誤差などにより、衝突判定領域が地面Ｇと重なるおそれがある。その結果、地面Ｇが衝突可能性のある障害物として検出され、移動体１０が意図せず停止してしまう。これは、低空飛行時に限らず、高速飛行時にも起こり得る。
　そこで、図３に示されるように、本開示に係る技術を適用した移動体１０は、自機の進行方向をセンシングしたセンサデータに基づいて、点群データＰｃについての法線ベクトルｎを推定する。センサデータは、例えば偏光イメージセンサにより撮像される偏光画像とされる。そして、移動体１０は、推定された法線ベクトルｎに基づいて、各点群データＰｃについての衝突リスクを算出することで、減速したり停止したりするか否かを判定する。点群データＰｃについての衝突リスクは、点群データＰｃについての法線ベクトルｎと、移動体１０の速度ベクトルｖが、どの程度同じ方向を向いているかに応じて算出される。
　これにより、移動体１０が低空飛行している場合や高速飛行している場合であっても、障害物検出における誤判定を抑制することを実現する。
＜２．移動体のハードウェア構成＞
　図４は、移動体１０のハードウェア構成例を示すブロック図である。
　移動体１０には、ドローンや車両、船舶などの移動物体が含まれる。以下においては、本開示に係る技術を、空中を飛行するドローンに適用した例について説明する。本開示に係る技術は、ユーザの操縦（操作）により移動する移動体の他、自律飛行するドローン、陸上を移動する自律走行車両、水上または水中を移動する自律航行船舶、屋内を移動する自律移動掃除機などの自律移動ロボットに適用することが可能である。
　移動体１０は、センサ２０、通信部２１、制御部２２、移動制御部２３、移動機構２４、および記憶部２５を備えている。
　センサ２０は、上述した測距センサ１１をはじめとする各種のセンサを含み、移動体１０の進行方向を含む移動体１０の周囲の各方向をセンシングする。センシングにより得られたセンサデータは、制御部２２に供給される。
　通信部２１は、ネットワークインタフェースなどで構成され、移動体１０を操縦するためのコントローラや、その他の任意の装置との間で、無線または有線による通信を行う。例えば、通信部２１は、通信相手となる装置と、直接通信を行ってもよいし、Ｗｉ－Ｆｉ（登録商標）や４Ｇ，５Ｇなどの基地局や中継器を介したネットワーク通信を行ってもよい。
　制御部２２は、ＣＰＵ（Central Processing Unit）やメモリなどで構成され、所定のプログラムを実行することにより、通信部２１、移動制御部２３、および記憶部２５を制御する。例えば、制御部２２は、センサ２０からのセンサデータに基づいて、移動制御部２３を制御する。
　移動制御部２３は、専用ＩＣやＦＰＧＡ（Field-Programmable Gate Array）などの回路により構成され、制御部２２の制御に従い、移動機構２４の駆動を制御する。
　移動機構２４は、移動体１０を移動させるための機構であり、飛行機構、走行機構、推進機構などが含まれる。この例では、移動体１０はドローンとして構成され、移動機構２４は飛行機構としてのモータやプロペラなどから構成される。また、移動体１０が自律走行車両として構成される場合、移動機構２４は走行機構としての車輪などから構成され、移動体１０が自律航行船舶として構成される場合、移動機構２４は推進機構としてのスクリュープロペラなどから構成される。移動機構２４は、移動制御部２３の制御に従って駆動し、移動体１０を移動させる。
　移動体１０においては、制御部２２が、例えば、通信部２１で受信されたコントローラからの制御信号に従い、移動制御部２３を制御することにより、移動機構２４を駆動させる。これにより、移動体１０は、ユーザによるコントローラの操作に従って移動する。
　記憶部２５は、フラッシュメモリなどの不揮発性メモリなどにより構成され、制御部２２の制御に従い、各種の情報を記憶する。
　以下、障害物検出における誤判定の抑制を実現する移動体１０の実施形態について説明する。
＜３．第１の実施形態＞
（移動体の機能構成例）
　図５は、本開示に係る技術を適用した第１の実施形態の移動体１０の機能構成例を示すブロック図である。
　図５に示される移動体１０は、２台の偏光カメラ３０－１，３０－２を備える。偏光カメラ３０－１，３０－２は、それぞれ偏光イメージセンサ５１－１，５１－２を有しており、ステレオカメラとしても機能する。
　偏光イメージセンサ５１－１，５１－２は、複数方向の偏光子がそれぞれ画素のフォトダイオード上に形成されて構成される。例えば、偏光イメージセンサ５１－１，５１－２には４方向の偏光子が搭載されており、４方向の偏光画像を取得することができる。偏光イメージセンサ５１－１，５１－２は、図４のセンサ２０を構成する各種のセンサの１つとされる。以下において、偏光イメージセンサ５１－１，５１－２をそれぞれ区別しない場合、単に、偏光イメージセンサ５１という。
　移動体１０は、法線ベクトル推定部５２、輝度画像構成部５３－１，５３－２、平行化処理部５４－１，５４－２、キャリブレーションデータ５５、および法線ベクトル補正部５６をさらに備える。
　法線ベクトル推定部５２は、偏光イメージセンサ５１－１により取得された偏光画像に基づいて、偏光画像の画素位置毎に法線ベクトルを推定する。法線ベクトルの推定に用いられる偏光画像は、偏光イメージセンサ５１－２により取得された偏光画像であってもよい。
　具体的には、法線ベクトル推定部５２は、偏光方向が３方向以上の偏光画像に基づいて、偏光方向と偏光画像の輝度から輝度と偏光角の関係を求め、最大輝度となる方位角φを判別する。また、法線ベクトル推定部５２は、輝度と偏光角の関係から得られた最大輝度と最小輝度を用いて偏光度ρを算出し、偏光度と天頂角の関係を示す特性曲線に基づいて、偏光度ρに対応する天頂角θを判別する。このようにして、法線ベクトル推定部５２は、偏光方向が３方向以上の偏光画像に基づいて、被写体の法線ベクトルとして、方位角φと天頂角θを画素位置毎に推定する。
　輝度画像構成部５３－１，５３－２は、それぞれ、偏光イメージセンサ５１－１，５１－２により取得された２視点の偏光画像の画素毎の輝度値に基づいて、２視点の輝度画像を構成する。
　平行化処理部５４－１，５４－２は、それぞれ、輝度画像構成部５３－１，５３－２により構成された２視点の輝度画像に対して、ステレオレクティフィケーションによる平行化処理を行う。平行化処理は、キャリブレーションデータ５５として保持されている、偏光カメラ３０－１，３０－２の内部パラメータ、外部パラメータ、および歪係数を用いて行われる。平行化処理により、２視点の輝度画像は、平行化された輝度画像に補正される。
　法線ベクトル補正部５６は、平行化処理部５４－１，５４－２による平行化処理による輝度画像の補正に応じて、偏光画像の画素位置毎に推定された法線ベクトルを補正する。法線ベクトルの補正にも、偏光カメラ３０－１，３０－２の内部パラメータ、外部パラメータ、および歪係数が用いられる。
　移動体１０は、視差推定部５７、ビジュアルオドメトリ部５８、ＧＰＳセンサ５９，ＩＭＵ６０、気圧計６１、地磁気センサ６２、および自己位置推定部６３をさらに備える。
　視差推定部５７は、平行化処理後の輝度画像を用いたステレオマッチングにより、輝度画像上の視差を推定する。視差推定部５７は、推定された視差に基づいて、被写体までの距離（デプス）を表す点群データから構成される視差マップを出力する。
　ビジュアルオドメトリ部５８は、平行化処理後の輝度画像に基づいて、ビジュアルオドメトリにより自機（移動体１０）の軌道を推定し、自己位置推定部６３に供給する。
　ＧＰＳ（Global Positioning System）センサ５９は、自機（移動体１０）のＧＰＳ情報を取得し、自己位置推定部６３に供給する。ＩＭＵ（Inertial Measurement Unit）６０は、自機（移動体１０）の三次元の角速度と加速度を検出し、自己位置推定部６３に供給する。気圧計６１は、気圧を計測し、自己位置推定部６３に供給する。地磁気センサ６２は、地磁気を検出し、自己位置推定部６３に供給する。ＧＰＳセンサ５９，ＩＭＵ６０、気圧計６１、地磁気センサ６２それぞれもまた、図４のセンサ２０を構成する各種のセンサの１つとされる。
　自己位置推定部６３は、ビジュアルオドメトリ部５８，ＧＰＳセンサ５９，ＩＭＵ６０、気圧計６１、地磁気センサ６２それぞれにより得られたデータに基づいて、拡張カルマンフィルタを用いたセンサフュージョンを行う。これにより、自己位置推定部６３は、自機（移動体１０）の自己位置と速度ベクトルを算出することができる。
　移動体１０は、障害物衝突判定部６４とフライトコントローラ６５をさらに備える。
　障害物衝突判定部６４は、法線ベクトル補正部５６からの法線ベクトル、視差推定部５７からの視差マップ（点群データ）、および自己位置推定部６３からの速度ベクトルに基づいて、移動体１０の障害物との衝突可能性を判定する。
　障害物衝突判定部６４は、分割部６４ａ、算出部６４ｂ、および設定部６４ｃを有している。
　分割部６４ａは、移動体１０の進行方向の空間領域を、その進行方向に連続する小領域に分割する。以下においては、分割された小領域を、衝突判定領域という。算出部６４ｂは、法線ベクトル、点群データ、および速度ベクトルに基づいて、分割部６４ａにより分割された衝突判定領域毎に、障害物との衝突リスクを算出する。設定部６４ｃは、算出部６４ｂにより算出された衝突判定領域毎の衝突リスクに基づいて、障害物の存在可能性のある障害物領域を設定する。
　そして、障害物衝突判定部６４は、自機（移動体１０）から障害物領域までの距離に基づいて、自機の移動を制御するための制御情報を生成する。すなわち、障害物衝突判定部６４は、衝突リスクに基づいて、移動体１０の移動を制御するための制御情報を生成する制御情報生成部としての機能を有する。
　フライトコントローラ６５は、図４の移動制御部２３に対応し、障害物衝突判定部６４（制御情報生成部）により生成された制御情報に基づいて、移動体１０の移動を制御する。
　なお、自機（移動体１０）を操縦するためのコントローラから入力される制御信号に基づいて、障害物衝突判定部６４が制御情報を生成したり、フライトコントローラ６５が移動体１０の移動を制御したりすることもできる。コントローラは、移動体１０の移動をリアルタイムに制御するための制御信号を入力するだけでなく、例えば、目的地や移動経路などを制御信号として入力することもできる。この場合、フライトコントローラ６５は、制御信号として入力された目的地や移動経路に基づいて、移動体１０を自律移動させるように、移動体１０の移動を制御する。
（障害物検出処理）
　図６および図７のフローチャートを参照して、図５の移動体１０による障害物検出処理の流れについて説明する。
　ステップＳ１１において、偏光カメラ３０－１，３０－２（偏光イメージセンサ５１－１，５１－２）は、偏光画像の撮像を開始する。
　ステップＳ１２において、法線ベクトル推定部５２は、偏光カメラ３０－１または偏光カメラ３０－２により撮像された偏光画像に基づいて法線ベクトルを推定する。
　ステップＳ１３において、輝度画像構成部５３－１，５３－２は、それぞれ、偏光カメラ３０－１，３０－２により撮像された２視点の偏光画像から輝度画像を構成する。
　ステップＳ１４において、平行化処理部５４－１，５４－２は、それぞれ、輝度画像構成部５３－１，５３－２により構成された２視点の輝度画像に対して平行化処理を行う。
　ステップＳ１５において、法線ベクトル補正部５６は、平行化処理部５４－１，５４－２による平行化処理に合わせて法線ベクトルを補正する。
　ステップＳ１６において、視差推定部５７は、平行化処理後の輝度画像から視差を推定する。
　ステップＳ１７において、自己位置推定部６３は、ビジュアルオドメトリ部５８，ＧＰＳセンサ５９，ＩＭＵ６０、気圧計６１、地磁気センサ６２それぞれにより得られたデータに基づいて、自機（移動体１０）の自己位置と速度ベクトルを算出する。
　図７のステップＳ１８に進み、障害物衝突判定部６４の分割部６４ａは、自機（移動体１０）からの測距精度に応じて、進行方向（速度ベクトル方向）の空間領域を衝突判定領域に分割する。
　図８を参照して、衝突判定領域の分割について説明する。
　例えば、分割部６４ａは、測距センサ（ステレオカメラとしての偏光カメラ３０－１，３０－２）の光軸方向Ａｘについての距離分解能に応じた距離Ｄｒに基づいて、移動体１０が通過する空間領域Ｓａを、衝突判定領域Ｃｄに分割する。測距センサの距離分解能は、自機から遠くなるほど精度が低くなることから、距離Ｄｒは、自機から遠くなるほど大きくなる。衝突判定領域Ｃｄの速度ベクトルｖ方向（奥行き方向）の長さは、距離Ｄｒに応じて設定される。すなわち、衝突判定領域Ｃｄの速度ベクトルｖ方向の長さは、自機から遠くなるほど大きくなる。
　図７のフローチャートに戻り、ステップＳ１９において、算出部６４ｂは、視差マップ（点群データ）から、進行方向（速度ベクトル方向）の空間領域に含まれる点群データを抽出する。
　ステップＳ２０において、算出部６４ｂは、抽出した点群データについての法線ベクトルに基づいて、衝突判定領域毎の衝突リスクを算出する。
　図９を参照して、衝突判定領域毎の衝突リスクの算出について説明する。
　図９においては、速度ベクトルｖ方向の空間領域Ｓａが、１，２，３，・・・，ｋ，ｋ＋１番目の衝突判定領域Ｃｄに分割されており、衝突判定領域Ｃｄそれぞれに含まれる点群データＰｃについての法線ベクトルｎが示されている。
　ここで、ｋ番目の衝突判定領域Ｃｄに含まれる点群データの数ｉをＮ個とすると、ｋ番目の衝突判定領域Ｃｄの衝突リスクＲｋは、以下の式（１）で表される。
Figure JPOXMLDOC01-appb-M000001
　式（１）におけるＲareaは、各点群データ１画素に対応する実空間の面積に比例する値である。図１０に示されるように、点群データまでの距離をｄとし、偏光カメラ３０－１または偏光カメラ３０－２の水平方向・垂直方向の焦点距離をそれぞれｆｘ，ｆｙとすると、Ｒareaは、以下の式（２）で表される。
Figure JPOXMLDOC01-appb-M000002
　式（２）において、ｄ／ｆｘとｄ／ｆｙの積が、点群データ１画素に対応する実空間の面積に比例し、ωareaは、固定の重みパラメータである。
　式（１）におけるＲcountは、各点群データに対して一律に設定された値（重み）であり、以下の式（３）で表される。
Figure JPOXMLDOC01-appb-M000003
　式（３）において、ωcountは、衝突判定領域Ｃｄに含まれる点群データの総数であり、至近距離に障害物があった場合に、上述した実空間の面積に比例するＲareaが小さくなりすぎることを防ぐための値である。
　式（１）におけるＲnormalは、各点群データについての法線ベクトルｎに基づいて算出される値（ゲイン）であり、速度ベクトルｖと各点群データの法線ベクトルｎの内積を用いて、以下の式（４）で表される。
Figure JPOXMLDOC01-appb-M000004
　式（４）において、ωnormalは、固定の重みパラメータである。また、速度ベクトルｖと各点群データの法線ベクトルｎの内積の絶対値は、移動体１０の進行方向と被写体の表面が向かい合うほど大きくなる。一方で、移動体１０が地面の近くを低空飛行している場合であっても、移動体１０の進行方向と地面表面の法線方向とは直交することから、速度ベクトルｖと各点群データの法線ベクトルｎの内積の絶対値は小さくなる。
　なお、速度ベクトルｖと各点群データの法線ベクトルｎは、いずれも移動体１０を基準とした固定座標系におけるベクトルとされる。
　以上のようにして、衝突判定領域毎の衝突リスクが算出されると、ステップＳ２０において、設定部６４ｃは、算出された衝突判定領域毎の衝突リスクに基づいて、障害物の存在可能性のある障害物領域を設定する。
　図１１を参照して、障害物領域の設定について説明する。
　図１１においては、１番目からｋ＋１番目の衝突判定領域Ｃｄに対して、移動体１０からの衝突判定領域Ｃｄそれぞれまでの距離Ｄと、衝突判定領域Ｃｄそれぞれの衝突リスクＲｋが示されている。
　設定部６４ｃは、衝突リスクＲｋが所定の閾値Ｒthより高く、かつ、自機（移動体１０）から最も近い衝突判定領域Ｃｄを、障害物領域Ｏｂに設定する。図１１の例では、衝突リスクＲｋが所定の閾値Ｒthより高く、かつ、自機（移動体１０）から最も近いｋ番目の衝突判定領域Ｃｄが、障害物領域Ｏｂに設定される。
　図７のフローチャートに戻り、ステップＳ２１において、障害物衝突判定部６４は、速度ベクトルｖに基づいて、現在の速度から、自機（移動体１０）の停止に必要な停止可能距離（制動距離）を算出する。停止可能距離には、例えば５ｍなどのマージンが含まれてもよい。
　ステップＳ２３において、障害物衝突判定部６４は、障害物領域までの距離が、算出した停止可能距離より短いか否かを判定する。障害物領域までの距離が停止可能距離より短いと判定された場合、ステップＳ２４に進む。
　例えば、図１１の例において、障害物領域Ｏｂまでの距離Ｄobが停止可能距離より短い場合、現状のままでは障害物領域Ｏｂに存在し得る障害物に衝突するおそれがある。そこで、障害物領域までの距離が停止可能距離より短いと判定された場合、障害物衝突判定部６４は、障害物より手前で停止可能な速度まで減速させるための制御情報を生成し、フライトコントローラ６５に出力する。
　ステップＳ２４において、フライトコントローラ６５は、障害物衝突判定部６４により生成された制御情報に基づいて、移動体１０を減速させる。ここでは、障害物より手前で停止可能な速度まで減速させるための制御情報が生成されることで、移動体１０を減速させるようにしたが、障害物より手前で停止させるための制御情報が生成され、移動体１０を停止させるようにしてもよい。
　一方、図１１の例において、障害物領域Ｏｂまでの距離Ｄobが停止可能距離より長い場合、障害物領域Ｏｂに存在し得る障害物より手前で停止可能であるので、ステップＳ２４はスキップされる。
　その後、ステップＳ１２に戻り、ステップＳ１２乃至Ｓ２４の処理が繰り返される。
　以上の処理によれば、自機の進行方向を撮像した偏光画像に基づいて推定された法線ベクトルを用いて、自機の移動が制御されるので、移動体１０が低空飛行している場合であっても、障害物検出における誤判定を抑制することが可能となる。その結果、ユーザはより自然なマニュアル操縦を行うことができるようになる。
＜４．第２の実施形態＞
　第２の実施形態においては、図１２に示されるように、高速飛行する移動体１０が通過する空間領域Ｓａにおいて、地面Ｇが障害物領域Ｏｂに含まれるような場合、単に減速させるだけでなく、補正軌道Ｃｔにより障害物との衝突を回避することを実現する。
（移動体の機能構成例）
　図１２は、本開示に係る技術を適用した第２の実施形態の移動体１０の機能構成例を示すブロック図である。
　図１２に示される移動体１０Ａは、基本的には図５の移動体１０と同様の構成を備える。但し、図１２の移動体１０Ａにおいて、障害物衝突判定部６４は、分割部６４ａ、算出部６４ｂ、および設定部６４ｃに加え、代表法線ベクトル算出部６４ｄと角加速度算出部６４ｅを有している。
　代表法線ベクトル算出部６４ｄは、設定部６４ｃにより設定された障害物領域に含まれる点群データについての法線ベクトルに基づいて、その障害物領域における代表的な法線ベクトル（以下、代表法線ベクトルという）を算出する。
　角加速度算出部６４ｅは、自機（移動体１０Ａ）が障害物領域に到達したときの速度ベクトルを予測し、その速度ベクトル（予測速度ベクトル）と、代表法線ベクトル算出部６４ｄにより算出された代表法線ベクトルが直交するような角加速度を算出する。
　これにより、障害物衝突判定部６４は、予測速度ベクトルと代表法線ベクトルが直交するように、自機の軌道を補正する制御情報を生成することができる。
（障害物検出処理）
　図１４のフローチャートを参照して、図１２の移動体１０Ａによる障害物検出処理の流れについて説明する。
　なお、図１４のフローチャートにおけるステップＳ２３までの処理は、図６および図７のフローチャートを参照して説明した、図５の移動体１０による障害物検出処理の流れと同様にして実行される。
　すなわち、ステップＳ２３において、障害物領域までの距離が停止可能距離より短いと判定された場合、ステップＳ５０に進む。ステップＳ５０において、障害物衝突判定部６４は、軌道補正処理を実行する。
　図１５は、軌道補正処理の流れについて説明するフローチャートである。
　ステップＳ５１において、障害物衝突判定部６４の代表法線ベクトル算出部６４ｄは、障害物領域の代表法線ベクトルを算出する。
　図１６を参照して、代表法線ベクトルを算出について説明する。
　図１６のＡ図には、障害物領域Ｏｂに含まれる点群データＰｃについての法線ベクトルｎが示されている。
　同Ｂ図には、障害物領域Ｏｂにおける法線ベクトルｎの分布が示されている。代表法線ベクトル算出部６４ｄは、法線ベクトルｎの分布を解析し、障害物領域Ｏｂにおいて最も支配的な方向のベクトルを、同Ｃ図に示される代表法線ベクトルＲｎに決定する。
　以上のようにして、障害物領域の代表法線ベクトルが算出されると、ステップＳ５２において、角加速度算出部６４ｅは、自機（移動体１０Ａ）が障害物領域に到達したときの予測速度ベクトルと、代表法線ベクトルが直交する角加速度を算出する。
　具体的には、図１７に示されるように、障害物領域Ｏｂの中心から代表法線ベクトルＲｎの方向に一定距離ｐだけ離れた位置において、予測速度ベクトルＰｖと代表法線ベクトルＲｎが直交するような補正軌道Ｃｔの飛行を実現する角加速度が算出される。
　ステップＳ５３において、障害物衝突判定部６４は、角加速度算出部６４ｅにより算出された角加速度が、所定値を超えるか否かを判定する。算出された角加速度が所定値を超えないと判定された場合、ステップＳ５４に進む。
　ステップＳ５４において、障害物衝突判定部６４は、補正後の軌道（補正軌道）上に、衝突可能性のある物体に対応する点群データが存在するか否かを判定する。補正軌道上に衝突可能性のある物体に対応する点群データが存在しないと判定された場合、ステップＳ５５に進む。このとき、障害物衝突判定部６４は、角加速度算出部６４ｅにより算出された角加速度を、自機の軌道を補正する制御情報として、フライトコントローラ６５に出力する。
　ステップＳ５５において、フライトコントローラ６５は、障害物衝突判定部６４により制御情報として出力された角加速度に基づいて、移動体１０Ａの姿勢を制御することで、自機の軌道を補正する。その後、ステップＳ１２（図６）に戻り、ステップＳ１２乃至Ｓ２３，Ｓ５０の処理が繰り返される。
　一方、ステップＳ５３において、算出された角加速度が所定値を超えると判定されるか、ステップＳ５４において、補正軌道上に衝突可能性のある物体に対応する点群データが存在すると判定された場合、ステップＳ５６に進む。このとき、障害物衝突判定部６４は、障害物より手前で停止可能な速度まで減速させるための制御情報を生成し、フライトコントローラ６５に出力する。
　ステップＳ５６において、フライトコントローラ６５は、障害物衝突判定部６４により生成された制御情報に基づいて、移動体１０Ａを減速させる。その後、ステップＳ１２（図６）に戻り、ステップＳ１２乃至Ｓ２３，Ｓ５０の処理が繰り返される。ここでも、障害物より手前で停止させるための制御情報が生成され、移動体１０Ａを停止させるようにしてもよい。
　以上の処理によれば、自機の進行方向を撮像した偏光画像に基づいて推定された法線ベクトルを用いて、自機の軌道が補正されるので、移動体１０Ａが高速飛行している場合であっても、障害物との衝突を回避することが可能となる。
＜５．第３の実施形態＞
　ジンバル機構を有するＦＰＶ（First Person View）カメラをドローンに搭載し、ＦＰＶカメラにより撮像された画像を、ユーザが操作するコントローラに送信することで、ユーザがその画像を見ながらドローンを遠隔操作することができる。しかしながら、ＦＰＶカメラにより撮像された画像では、ドローンが飛行する周辺環境の凹凸を視認しづらい場合がある。
　第３の実施形態においては、推定された法線ベクトルに基づいて生成された法線ベクトル画像を、ＦＰＶカメラにより撮像された画像に重畳することで、移動体１０の遠隔操作時のユーザの補助を実現する。
（移動体の機能構成例）
　図１８は、本開示に係る技術を適用した第３の実施形態の移動体１０の機能構成例を示すブロック図である。
　図１８に示される移動体１０Ｂは、図５の移動体１０と同様の構成に加え、ＲＧＢイメージセンサ１１１を有するＦＰＶカメラ１００、姿勢推定部１１２、座標変換部１１３、法線ベクトル画像生成部１１４、重畳部１１５、および送信部１１６を備える。なお、図１８の移動体１０Ｂにおいて、障害物衝突判定部６４が、分割部６４ａ、算出部６４ｂ、および設定部６４ｃに加え、代表法線ベクトル算出部６４ｄと角加速度算出部６４ｅを有していてもよい。
　ＦＰＶカメラ１００は、ジンバル機構を有し、様々なアングルで撮像することができる。ＦＰＶカメラ１００が有するＲＧＢイメージセンサ１１１は、画素上にＲ，Ｇ，Ｂのカラーフィルタが例えばベイヤ配列で配置されて構成され、ＲＧＢ画像を撮像する。
　姿勢推定部１１２は、ＲＧＢイメージセンサ１１１により撮像されたＲＧＢ画像に基づいて、ＦＰＶカメラ１００のカメラ座標系の原点位置を基準として、ＦＰＶカメラ１００の現在の姿勢を推定する。
　座標変換部１１３は、偏光画像の画素位置毎の法線ベクトルから構成される法線ベクトルマップの座標系を、ＦＰＶカメラ１００のカメラ座標系に変換する。法線ベクトルマップの座標変換には、視差推定部５７からの視差マップ、自己位置推定部６３からの自己位置、偏光カメラ３０－１，３０－２の姿勢情報、ＦＰＶカメラ１００の偏光カメラ３０－１，３０－２との相対位置と現在の姿勢情報が用いられる。
　法線ベクトル画像生成部１１４は、座標変換された法線ベクトルマップに基づいて、法線ベクトルの向きに応じて色付けされた法線ベクトル画像を生成する。すなわち、ＦＰＶカメラ１００のカメラ座標系に変換された法線ベクトル画像が生成される。
　重畳部１１５は、ＲＧＢイメージセンサ１１１により撮像されたＲＧＢ画像に、法線ベクトル画像生成部１１４により生成された法線ベクトル画像を重畳した重畳画像を生成する。
　送信部１１６は、図４の通信部２１に対応し、重畳部１１５により生成された重畳画像を、自機（移動体１０Ｂ）の制御信号を入力するためのコントローラに送信する。
（重畳画像送信処理）
　図１９は、重畳画像処理の流れについて説明するフローチャートである。図１９の処理は、図６および図７や図１４を参照して説明した障害物検出処理と並行して実行される。
　ステップＳ１１１において、ＦＰＶカメラ１００（ＲＧＢイメージセンサ１１１）は、ＲＧＢ画像を撮像する。
　ステップＳ１１２において、姿勢推定部１１２は、撮像されたＲＧＢ画像に基づいて、ＦＰＶカメラ１００のカメラ座標系の原点位置を基準として、ＦＰＶカメラ１００の現在の姿勢を推定する。
　ステップＳ１１３において、座標変換部１１３は、法線ベクトルマップの座標系を、ＦＰＶカメラ１００のカメラ座標系に変換する。
　ステップＳ１１４において、法線ベクトル画像生成部１１４は、座標変換された法線ベクトルマップに基づいて、法線ベクトルの向きに応じて色付けされた法線ベクトル画像を生成する。
　ステップＳ１１５において、重畳部１１５は、ＲＧＢイメージセンサ１１１により撮像されたＲＧＢ画像に、法線ベクトル画像生成部１１４により生成された法線ベクトル画像を重畳する。
　図２０を参照して、重畳画像の生成について説明する。
　図２０に示されるＲＧＢ画像１３０が、暗い環境で撮像された画像や、コントラストの低い画像である場合、ＲＧＢ画像１３０を見ながら移動体１０Ｂを遠隔操作するユーザにとって、ＲＧＢ画像１３０に映る被写体の凹凸を視認しづらい。
　そこで、法線ベクトル画像生成部１１４が、法線ベクトルマップに基づいて、法線ベクトルの向きに応じて色付けされた法線ベクトル画像１４０を生成し、重畳部１１５が、ＲＧＢ画像１３０に法線ベクトル画像１４０を重畳した重畳画像１５０を生成する。
　これにより、被写体の凹凸を視認しやすい画像が得られる。
　図１９のフローチャートに戻り、ステップＳ１１６において、送信部１１６は、重畳部１１５により生成された重畳画像を、自機（移動体１０Ｂ）の制御信号を入力するためのコントローラに送信する。処理はステップＳ１１１に戻り、上述した処理が繰り返される。
　以上の処理によれば、被写体の凹凸を視認しやすい画像を得ることができ、移動体１０Ｂの遠隔操作時のユーザの補助を実現することが可能となる。
　なお、本実施形態において、ＦＰＶカメラ１００を設けず、法線ベクトル画像のみがコントローラに送信されるようにしてもよい。
＜６．変形例＞
　以下においては、上述した実施形態の変形例について説明する。
　上述した実施形態においては、法線ベクトルの推定と測距を実現するセンサ２０が、２視点のステレオカメラを構成する偏光イメージセンサ５１－１，５１－２により構成されるものとした。
　これに限らず、図２１のＡ図に示されるように、センサ２０が、偏光イメージセンサ５１と、２視点のステレオカメラを構成するＲＧＢイメージセンサ２１１－１，２１１－２により構成されてもよい。
　同様に、図２１のＢ図に示されるように、センサ２０が、偏光イメージセンサ５１と、ＬｉＤＡＲ（Light Detection and Ranging）やＴｏＦ（Time of Flight）センサなどの測距センサ２３１により構成されてもよい。
　いずれの構成であっても、センサ２０は、法線ベクトルの推定と測距を実現することができる。
　また、上述した実施形態においては、偏光イメージセンサにより取得された偏光画像に基づいて、法線ベクトルが推定されるものとした。
　これ以外にも、所定のセンサにより自機（移動体１０）の進行方向がセンシングされたセンサデータに基づいて、法線ベクトルを推定することもできる。例えば、一般的なステレオカメラやＬｉＤＡＲなどの測距用のデバイスにより取得されたデプス情報に対して所定の処理を施したデータに基づいて、法線ベクトルを推定することが可能である。
　上述した実施形態において、法線ベクトルを推定する構成（法線ベクトル推定部５２）から、障害物との衝突を判定する構成（障害物衝突判定部６４）までは、図４の制御部２２により実現される。
　これらの構成は、図２２に示される移動体制御システムにおいて、移動体３１０の移動が、例えばクラウド上に構成される情報処理装置３２０により制御される場合、情報処理装置３２０が備える制御部３３１により実現されてもよい。この場合、制御部３３１が、移動体３１０から送信されたセンサデータに基づいて法線ベクトルを推定し、その法線ベクトルに基づいて移動体３１０の移動を制御するための制御情報を生成する。生成された制御情報は、移動体３１０に送信される。
　以上の構成においても、移動体３１０が低空飛行している場合の、障害物検出における誤判定を抑制することが可能となる。
　本開示に係る技術を適用した移動体１０は、地面の近くを低空飛行している場合において、その効果を奏するものとして説明したが、本開示に係る技術を適用した移動体１０は、例えば壁面に沿って移動している場合においても、その効果を奏することができる。
＜７．コンピュータの構成例＞
　上述した一連の処理は、ハードウェアにより実行することもできるし、ソフトウェアにより実行することもできる。一連の処理をソフトウェアにより実行する場合には、そのソフトウェアを構成するプログラムが、コンピュータにインストールされる。ここで、コンピュータには、専用のハードウェアに組み込まれているコンピュータや、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のパーソナルコンピュータなどが含まれる。
　図２３は、上述した一連の処理をプログラムにより実行するコンピュータのハードウェアの構成例を示すブロック図である。
　コンピュータにおいて、ＣＰＵ５０１，ＲＯＭ（Read Only Memory）５０２，ＲＡＭ（Random Access Memory）５０３は、バス５０４により相互に接続されている。
　バス５０４には、さらに、入出力インタフェース５０５が接続されている。入出力インタフェース５０５には、入力部５０６、出力部５０７、記憶部５０８、通信部５０９、およびドライブ５１０が接続されている。
　入力部５０６は、キーボード、マウス、マイクロフォンなどよりなる。出力部５０７は、ディスプレイ、スピーカなどよりなる。記憶部５０８は、ハードディスクや不揮発性のメモリなどよりなる。通信部５０９は、ネットワークインタフェースなどよりなる。ドライブ５１０は、磁気ディスク、光ディスク、光磁気ディスク、または半導体メモリなどのリムーバブルメディア５１１を駆動する。
　以上のように構成されるコンピュータでは、ＣＰＵ５０１が、例えば、記憶部５０８に記憶されているプログラムを、入出力インタフェース５０５およびバス５０４を介して、ＲＡＭ５０３にロードして実行することにより、上述した一連の処理が行われる。
　コンピュータ（ＣＰＵ５０１）が実行するプログラムは、例えば、パッケージメディア等としてのリムーバブルメディア５１１に記録して提供することができる。また、プログラムは、ローカルエリアネットワーク、インターネット、デジタル衛星放送といった、有線または無線の伝送媒体を介して提供することができる。
　コンピュータでは、プログラムは、リムーバブルメディア５１１をドライブ５１０に装着することにより、入出力インタフェース５０５を介して、記憶部５０８にインストールすることができる。また、プログラムは、有線または無線の伝送媒体を介して、通信部５０９で受信し、記憶部５０８にインストールすることができる。その他、プログラムは、ＲＯＭ５０２や記憶部５０８に、あらかじめインストールしておくことができる。
　なお、コンピュータが実行するプログラムは、本明細書で説明する順序に沿って時系列に処理が行われるプログラムであっても良いし、並列に、あるいは呼び出しが行われたとき等の必要なタイミングで処理が行われるプログラムであっても良い。
　本開示の実施形態は、上述した実施形態に限定されるものではなく、本開示の要旨を逸脱しない範囲において種々の変更が可能である。
　本明細書に記載された効果はあくまで例示であって限定されるものではなく、他の効果があってもよい。
　さらに、本開示に係る技術は以下のような構成をとることができる。
（１）
　自機の進行方向をセンシングしたセンサデータに基づいて、法線ベクトルを推定する法線ベクトル推定部と、
　前記法線ベクトルに基づいて、前記自機の移動を制御するための制御情報を生成する制御情報生成部と
　を備える移動体。
（２）
　前記法線ベクトルに基づいて、障害物との衝突リスクを算出する算出部をさらに備え、
　前記制御情報生成部は、前記衝突リスクに基づいて、前記制御情報を生成する
　（１）に記載の移動体。
（３）
　前記算出部は、前記自機からの距離に応じた点群データについての前記法線ベクトルに基づいて、前記衝突リスクを算出する
　（２）に記載の移動体。
（４）
　前記進行方向の空間領域を前記進行方向に連続する衝突判定領域に分割する分割部をさらに備え、
　前記算出部は、前記衝突判定領域に含まれる前記点群データについての前記法線ベクトルに基づいて、前記衝突判定領域毎の前記衝突リスクを算出する
　（３）に記載の移動体。
（５）
　前記分割部は、前記自機からの測距精度に応じて、前記空間領域を前記衝突判定領域に分割する
　（４）に記載の移動体。
（６）
　前記算出部は、前記衝突判定領域に含まれる前記点群データについて求められる、前記法線ベクトルと前記自機の速度ベクトルとの内積を用いて、前記衝突判定領域毎の前記衝突リスクを算出する
　（４）または（５）に記載の移動体。
（７）
　前記算出部は、前記衝突判定領域に含まれる前記点群データそれぞれの、前記内積と前記点群データに対応する実空間の面積に比例する値との積の総和に応じた値を、前記衝突判定領域の前記衝突リスクとして算出する
　（６）に記載の移動体。
（８）
　前記衝突リスクが所定の閾値より高く、かつ、前記自機から最も近い前記衝突判定領域を、前記障害物の存在可能性のある障害物領域に設定する設定部を備え、
　前記制御情報生成部は、前記自機から前記障害物領域までの距離に基づいて、前記制御情報を生成する
　（４）乃至（７）のいずれかに記載の移動体。
（９）
　前記制御情報生成部は、前記自機から前記障害物領域までの距離が、前記自機の停止可能距離より短い場合、前記自機を減速させる前記制御情報を生成する
　（８）に記載の移動体。
（１０）
　前記障害物領域に含まれる前記点群データについての前記法線ベクトルに基づいて、前記障害物領域における代表法線ベクトルを算出する代表法線ベクトル算出部をさらに備え、
　前記制御情報生成部は、前記自機が前記障害物領域に到達したときの予測速度ベクトルと、前記代表法線ベクトルが直交するように、前記自機の軌道を補正する前記制御情報を生成する
　（８）または（９）に記載の移動体。
（１１）
　前記制御情報生成部は、前記制御情報として、前記予測速度ベクトルと前記代表法線ベクトルが直交するような角加速度を算出する
　（１０）に記載の移動体。
（１２）
　前記制御情報生成部は、前記角加速度が所定値を超えず、かつ、補正後の前記軌道上に衝突可能性のある物体に対応する前記点群データが存在しない場合に、前記軌道を補正する前記制御情報を生成する
　（１１）に記載の移動体。
（１３）
　前記制御情報生成部は、前記角加速度が前記所定値を超えるか、または、補正後の前記軌道上に前記物体に対応する前記点群データが存在する場合、前記自機を減速させる前記制御情報を生成する
　（１２）に記載の移動体。
（１４）
　前記法線ベクトルに基づいて、法線ベクトル画像を生成する法線ベクトル画像生成部と、
　ＦＰＶ（First Person View）カメラにより撮像されたＲＧＢ画像に、前記法線ベクトル画像を重畳した重畳画像を生成する重畳部をさらに備える
　（１）乃至（１３）のいずれかに記載の移動体。
（１５）
　法線ベクトル画像生成部は、前記法線ベクトルの向きに応じて色付けされた前記法線ベクトル画像を生成する
　（１４）に記載の移動体。
（１６）
　法線ベクトル画像生成部は、前記ＦＰＶカメラの座標系に変換された前記法線ベクトル画像を生成する
　（１４）または（１５）に記載の移動体。
（１７）
　前記自機の制御信号を入力するためのコントローラに、前記重畳画像を送信する送信部をさらに備える
　（１４）乃至（１６）のいずれかに記載の移動体。
（１８）
　前記センサデータは、偏光イメージセンサにより撮像された偏光画像である
　（１）乃至（１７）のいずれかに記載の移動体。
（１９）
　移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、
　前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報を生成する
　移動制御方法。
（２０）
　コンピュータに、
　移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、
　前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報を生成する
　処理を実行させるためのプログラム。
　１０　移動体，　２０　センサ，　２２　制御部，　３０－１，３０－２　偏光カメラ，　５１，５１－１，５１－２　偏光イメージセンサ，　５２　法線ベクトル推定部，　６４　障害物衝突判定部，　６４ａ　分割部，　６４ｂ　算出部，　６４ｃ　設定部，　６４ｄ　代表法線ベクトル算出部，　６４ｅ　角加速度算出部，　６５　フライトコントローラ

Claims (20)

  1. 　自機の進行方向をセンシングしたセンサデータに基づいて、法線ベクトルを推定する法線ベクトル推定部と、
    　前記法線ベクトルに基づいて、前記自機の移動を制御するための制御情報を生成する制御情報生成部と
    　を備える移動体。
  2. 　前記法線ベクトルに基づいて、障害物との衝突リスクを算出する算出部をさらに備え、
    　前記制御情報生成部は、前記衝突リスクに基づいて、前記制御情報を生成する
    　請求項１に記載の移動体。
  3. 　前記算出部は、前記自機からの距離に応じた点群データについての前記法線ベクトルに基づいて、前記衝突リスクを算出する
    　請求項２に記載の移動体。
  4. 　前記進行方向の空間領域を前記進行方向に連続する衝突判定領域に分割する分割部をさらに備え、
    　前記算出部は、前記衝突判定領域に含まれる前記点群データについての前記法線ベクトルに基づいて、前記衝突判定領域毎の前記衝突リスクを算出する
    　請求項３に記載の移動体。
  5. 　前記分割部は、前記自機からの測距精度に応じて、前記空間領域を前記衝突判定領域に分割する
    　請求項４に記載の移動体。
  6. 　前記算出部は、前記衝突判定領域に含まれる前記点群データについて求められる、前記法線ベクトルと前記自機の速度ベクトルとの内積を用いて、前記衝突判定領域毎の前記衝突リスクを算出する
    　請求項４に記載の移動体。
  7. 　前記算出部は、前記衝突判定領域に含まれる前記点群データそれぞれの、前記内積と前記点群データに対応する実空間の面積に比例する値との積の総和に応じた値を、前記衝突判定領域の前記衝突リスクとして算出する
    　請求項６に記載の移動体。
  8. 　前記衝突リスクが所定の閾値より高く、かつ、前記自機から最も近い前記衝突判定領域を、前記障害物の存在可能性のある障害物領域に設定する設定部を備え、
    　前記制御情報生成部は、前記自機から前記障害物領域までの距離に基づいて、前記制御情報を生成する
    　請求項４に記載の移動体。
  9. 　前記制御情報生成部は、前記自機から前記障害物領域までの距離が、前記自機の停止可能距離より短い場合、前記自機を減速させる前記制御情報を生成する
    　請求項８に記載の移動体。
  10. 　前記障害物領域に含まれる前記点群データについての前記法線ベクトルに基づいて、前記障害物領域における代表法線ベクトルを算出する代表法線ベクトル算出部をさらに備え、
    　前記制御情報生成部は、前記自機が前記障害物領域に到達したときの予測速度ベクトルと、前記代表法線ベクトルが直交するように、前記自機の軌道を補正する前記制御情報を生成する
    　請求項８に記載の移動体。
  11. 　前記制御情報生成部は、前記制御情報として、前記予測速度ベクトルと前記代表法線ベクトルが直交するような角加速度を算出する
    　請求項１０に記載の移動体。
  12. 　前記制御情報生成部は、前記角加速度が所定値を超えず、かつ、補正後の前記軌道上に衝突可能性のある物体に対応する前記点群データが存在しない場合に、前記軌道を補正する前記制御情報を生成する
    　請求項１１に記載の移動体。
  13. 　前記制御情報生成部は、前記角加速度が前記所定値を超えるか、または、補正後の前記軌道上に前記物体に対応する前記点群データが存在する場合、前記自機を減速させる前記制御情報を生成する
    　請求項１２に記載の移動体。
  14. 　前記法線ベクトルに基づいて、法線ベクトル画像を生成する法線ベクトル画像生成部と、
    　ＦＰＶ（First Person View）カメラにより撮像されたＲＧＢ画像に、前記法線ベクトル画像を重畳した重畳画像を生成する重畳部をさらに備える
    　請求項１に記載の移動体。
  15. 　法線ベクトル画像生成部は、前記法線ベクトルの向きに応じて色付けされた前記法線ベクトル画像を生成する
    　請求項１４に記載の移動体。
  16. 　法線ベクトル画像生成部は、前記ＦＰＶカメラの座標系に変換された前記法線ベクトル画像を生成する
    　請求項１４に記載の移動体。
  17. 　前記自機の制御信号を入力するためのコントローラに、前記重畳画像を送信する送信部をさらに備える
    　請求項１４に記載の移動体。
  18. 　前記センサデータは、偏光イメージセンサにより撮像された偏光画像である
    　請求項１に記載の移動体。
  19. 　移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、
    　前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報を生成する
    　移動制御方法。
  20. 　コンピュータに、
    　移動体の進行方向の物体をセンシングしたセンサデータに基づいて、法線ベクトルを推定し、
    　前記法線ベクトルに基づいて、前記移動体の移動を制御するための制御情報を生成する
    　処理を実行させるためのプログラム。
PCT/JP2022/003738 2021-05-10 2022-02-01 移動体、移動制御方法、およびプログラム WO2022239318A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2021079528A JP2022173682A (ja) 2021-05-10 2021-05-10 移動体、移動制御方法、およびプログラム
JP2021-079528 2021-05-10

Publications (1)

Publication Number Publication Date
WO2022239318A1 true WO2022239318A1 (ja) 2022-11-17

Family

ID=84029034

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2022/003738 WO2022239318A1 (ja) 2021-05-10 2022-02-01 移動体、移動制御方法、およびプログラム

Country Status (2)

Country Link
JP (1) JP2022173682A (ja)
WO (1) WO2022239318A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114740878A (zh) * 2022-05-17 2022-07-12 贵州聚原数技术开发有限公司 基于计算机图像识别的无人机飞行障碍物探测方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018159738A1 (ja) * 2017-03-03 2018-09-07 ソニーセミコンダクタソリューションズ株式会社 固体撮像装置および電子機器
WO2019026714A1 (ja) * 2017-08-04 2019-02-07 ソニー株式会社 情報処理装置、および情報処理方法、プログラム、並びに移動体
US10281916B1 (en) * 2016-09-21 2019-05-07 Amazon Technologies, Inc. Detection of transparent elements using specular reflection
WO2020262060A1 (ja) * 2019-06-27 2020-12-30 ソニー株式会社 移動体、制御方法、およびプログラム

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10281916B1 (en) * 2016-09-21 2019-05-07 Amazon Technologies, Inc. Detection of transparent elements using specular reflection
WO2018159738A1 (ja) * 2017-03-03 2018-09-07 ソニーセミコンダクタソリューションズ株式会社 固体撮像装置および電子機器
WO2019026714A1 (ja) * 2017-08-04 2019-02-07 ソニー株式会社 情報処理装置、および情報処理方法、プログラム、並びに移動体
WO2020262060A1 (ja) * 2019-06-27 2020-12-30 ソニー株式会社 移動体、制御方法、およびプログラム

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114740878A (zh) * 2022-05-17 2022-07-12 贵州聚原数技术开发有限公司 基于计算机图像识别的无人机飞行障碍物探测方法

Also Published As

Publication number Publication date
JP2022173682A (ja) 2022-11-22

Similar Documents

Publication Publication Date Title
US10914590B2 (en) Methods and systems for determining a state of an unmanned aerial vehicle
Huh et al. Integrated navigation system using camera and gimbaled laser scanner for indoor and outdoor autonomous flight of UAVs
US10435176B2 (en) Perimeter structure for unmanned aerial vehicle
TWI827649B (zh) 用於ｖｓｌａｍ比例估計的設備、系統和方法
JP7263630B2 (ja) 無人航空機による３次元再構成の実行
JP6029446B2 (ja) 自律飛行ロボット
Zingg et al. MAV navigation through indoor corridors using optical flow
US20180032042A1 (en) System And Method Of Dynamically Controlling Parameters For Processing Sensor Output Data
US10254767B1 (en) Determining position or orientation relative to a marker
Eynard et al. UAV altitude estimation by mixed stereoscopic vision
JP6235213B2 (ja) 自律飛行ロボット
JP5990453B2 (ja) 自律移動ロボット
JP6195450B2 (ja) 自律飛行ロボット
Chambers et al. Perception for a river mapping robot
Lippiello et al. MAV indoor navigation based on a closed-form solution for absolute scale velocity estimation using optical flow and inertial data
Suzuki et al. Vision based localization of a small UAV for generating a large mosaic image
JP6138326B1 (ja) 移動体、移動体の制御方法、移動体を制御するプログラム、制御システム、及び情報処理装置
JP6934116B1 (ja) 航空機の飛行制御を行う制御装置、及び制御方法
Holz et al. Towards multimodal omnidirectional obstacle detection for autonomous unmanned aerial vehicles
US20210263533A1 (en) Mobile object and method for controlling mobile object
Celik et al. Mono-vision corner SLAM for indoor navigation
WO2022239318A1 (ja) 移動体、移動制御方法、およびプログラム
JP6469492B2 (ja) 自律移動ロボット
Ivancsits et al. Visual navigation system for small unmanned aerial vehicles
Rydell et al. Autonomous UAV-based forest mapping below the canopy

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22807025

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 18558540

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22807025

Country of ref document: EP

Kind code of ref document: A1