Information processing device, information processing method, and program

Abstract

To provide an information processing apparatus that, even when the reliability of a sensor used for position and attitude measurement is reduced due to an impact and vibration, stably achieves the position and attitude measurement.SOLUTION: An information processing method includes: acquiring input information from a sensor mounted on a movable body (S302); acquiring vibration information including the magnitude and direction of vibration applied to the movable body (S303); and based on the input information after correction corrected by using the vibration information, acquiring the position and attitude of the movable body (S304).SELECTED DRAWING: Figure 3

Description

[Technical field]

[0001] The present invention relates to a technique for acquiring the position and orientation of a moving object. [Background technology]

[0002] Autonomous vehicles such as automated guided vehicles (AGVs) are used in factories and logistics warehouses. A method for estimating the position and orientation of such autonomous vehicles is known in which a camera or LiDAR (Laser Imaging Detection and Ranging) is used as a sensor to measure the environment around the vehicle and obtain the time-based position and orientation difference from the results. Another method is to calculate the position and orientation value by comparing it with environmental map data.

[0003] When a moving body creates environmental information indicating the shape and position of objects in the environment in which the moving body travels based on information obtained from a sensor, the accuracy of acquiring the relative position between the moving body and surrounding objects may decrease due to vibration of the moving body. Patent Document 1 discloses a technology that determines the reliability of sensor measurement values ​​based on the difference between the measured value of acceleration applied to the moving body and the value assumed as the acceleration, and removes sensor information at times with low reliability. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2009-174898 A Summary of the Invention [Problem to be solved by the invention]

[0005] However, in the method described in Patent Document 1, when the sensor information is used for position and orientation measurement, there is a problem that the accuracy of the position and orientation measurement decreases or the measurement is interrupted at times when the reliability is low.

[0006] The present invention has been made in view of the above-mentioned problems, and has an object to provide an information processing apparatus that can stably obtain the position and orientation of a moving object, and also to provide a method and a program for the same. [Means for solving the problem]

[0007] The information processing device according to the present invention has the following configuration: an input information acquiring means for acquiring input information from a sensor mounted on a moving body, and a vibration information acquiring means for acquiring vibration information including the magnitude and direction of vibration received by the moving body; When the sensor is subjected to vibration exceeding a threshold value, the input information acquired by the input information acquisition means is The apparatus further comprises a position and orientation acquisition means for acquiring a position and orientation of the moving body based on input information corrected using the vibration information. Effect of the Invention

[0008] According to the present invention, it is possible to stably obtain the position and orientation of a moving object. [Brief description of the drawings]

[0009] [Figure 1] FIG. 1 is a diagram showing a configuration of a mobile system according to a first embodiment; [Diagram 2] FIG. 1 is a block diagram showing a functional configuration of an information processing apparatus according to a first embodiment; [Diagram 3] FIG. 1 is a flowchart showing the flow of information processing according to a first embodiment. [Figure 4] FIG. 1 is a flowchart showing details of the flow of impact vibration information acquisition processing according to the first embodiment. [Diagram 5] FIG. 1 is a schematic diagram showing a result of feature point tracking in the first embodiment; [Figure 6] FIG. 11 is a flowchart showing the details of the flow of position and orientation calculation processing according to the first embodiment. [Figure 7] FIG. 1 is a diagram showing a configuration of a mobile system according to a second embodiment; [Figure 8] FIG. 11 is a flowchart showing the flow of information processing according to the second embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] Hereinafter, the embodiments of the present invention will be described in detail with reference to the drawings. Note that the following embodiments do not limit the scope of the present invention, and all of the combinations of features described in the following embodiments are not necessarily essential to constitute the present invention.

[0011] [First embodiment] DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A moving body system, a position and orientation measurement system, an information processing device, an information processing method, and a computer program according to a first embodiment of the present invention will be described in detail below with reference to the drawings.

[0012] In the first embodiment, an example will be described in which a mobile object equipped with a camera (image sensor) as a sensor moves within an environment, and the position and orientation of the mobile object are estimated. Here, the environment refers to the area in which the sensor moves and the three-dimensional space around it, and the position and orientation are values ​​of six degrees of freedom that combine three-dimensional position information and orientation information of three degrees of freedom. In the following description, the vertical direction of the environment is the z-axis, and the mobile object basically travels on the xy plane.

[0013] In this embodiment, the problem is that camera images are blurred by shocks or vibrations applied to a moving object, and the accuracy of measuring the position and orientation by tracking feature points in the camera images decreases. The information processing device performs image correction according to the magnitude and direction of the shock or vibration, thereby achieving stable position and orientation measurement.

[0014] (Configuration of a moving body system, a position and orientation measurement system, and an information processing device) 1 is a diagram showing an example of the configuration of a mobile body system 101 according to this embodiment. The mobile body system 101 is composed of an information processing device 103, a position and orientation measurement system 102 including a camera 104, a communication unit 105, a control device 106, and a wheel unit 107.

[0015] The information processing device 103 estimates the position and orientation of the moving object based on input information input from a camera 104. When the moving object moves autonomously, the information processing device 103 issues a movement instruction to a control device 106. The configuration of the information processing device 103 will be described in detail later.

[0016] The camera 104 is fixed on the mobile system 101 and is capable of continuously acquiring grayscale luminance images. For example, it is a stereo camera. For ease of explanation, it is assumed that the internal parameters of the camera, such as the focal length and angle of view, are known, and that the image is output without distortion or with distortion corrected. The camera 104 captures images at a frame rate of, for example, 30 times per second.

[0017] The communication unit 105 receives instructions from the user regarding the movement and rotation of the mobile body system 601 and the start and end of the position and orientation measurement process performed by the information processing device 103 .

[0018] The control device 106 drives the wheel section 107 based on instructions from the information processing device 103 and the communication section 105 to move and turn the mobile body system 101 .

[0019] The wheel portion 107 is a plurality of tires, some or all of which are coupled to a power source.

[0020] (Configuration of information processing device) The information processing device 103 has the functions of a general embedded PC device, and is composed of a CPU 111, a ROM 112, a RAM 113, a storage unit 114 such as an HDD or SSD, a general-purpose I / F 115 such as a USB, and a system bus 116.

[0021] The CPU 111 uses the RAM 113 as a work memory, executes an operating system (OS) and various computer programs stored in the ROM 112, the storage unit 114, etc., and controls each unit via a system bus 116. For example, the programs executed by the CPU 111 include programs for executing the processes described below.

[0022] In addition, a sensor for measuring position and orientation (a camera 104 in this embodiment), a communication unit 105, and a control device 106 are connected to the information processing device 103 via a general-purpose I / F 115.

[0023] (Functional configuration of information processing device) The functional configuration of the information processing device according to this embodiment will be described below. The processing of each unit described below is performed as software by loading a computer program from ROM 112 or the like onto RAM 113 and then executing the program by CPU 111. A part or all of the functional configuration may be configured as hardware such as ASIC or FPGA.

[0024] FIG. 2 is a block diagram showing the functional configuration of the information processing device 103.

[0025] A sensor input information acquisition unit 201 acquires sensor information from a sensor for measuring position and orientation (the camera 104 in this embodiment) via the general-purpose I / F 115. In this embodiment, image information is acquired.

[0026] The shock and vibration information acquisition unit 202 acquires information on the shock and vibration applied to the mobile system 101 based on sensor information and the like acquired via the general-purpose I / F 115 .

[0027] The position and orientation acquisition unit 203 calculates the position and orientation of the mobile system 101 at the time of acquiring the sensor information, based on the sensor information acquired by the sensor input information acquisition unit 201 and the shock and vibration information acquired by the shock and vibration information acquisition unit 202. The calculated position and orientation information is output to the control device 106 and used for automatic driving control of the mobile system 101. Alternatively, the position and orientation information can be output to a display device (not shown) and used for displaying the current position of the mobile system on a UI, etc. It is also possible to transmit the sensor information acquired by the sensor input information acquisition unit 201 to an external device via the communication unit 105, and have the position and orientation calculated by the external device be acquired by the position and orientation acquisition unit 205 via the communication unit 105.

[0028] (Position and orientation measurement processing) Next, a position and orientation measurement process according to an information processing method using the information processing device of this embodiment will be described with reference to Fig. 3 to Fig. 6. The process shown in Fig. 3 and the following flowcharts is realized by the CPU 111 reading out a program stored in the ROM 112 or the storage unit 114, calculating or processing information, and controlling each piece of hardware. Note that some or all of the steps shown in each flowchart may be realized by hardware such as an ASIC or an FPGA.

[0029] FIG. 3 is a flowchart showing the flow of the position and orientation measurement process in this embodiment.

[0030] In step S301, the information processing device 103 is initialized. The information processing device sets the initial position and orientation of the mobile system 101 with the current position as the origin in the environment and the current orientation as a predetermined direction (for example, the positive Y-axis direction). In addition, the sensor input information acquisition unit 201 acquires initial sensor information from each sensor (the camera 104 in this embodiment), and stores the information in the work memory as "sensor information at the previous time" acquired when step S302 is executed for the first time.

[0031] The subsequent processing from step S302 to step S304 is repeatedly executed until an instruction to end position and orientation measurement is given by a user or the like. In the following explanation, a series of processing from step S302 to step S304 will be explained as processing at a certain time t. Here, t is an integer value, and increases by 1 each time a series of processing is performed. In addition, the following explanation assumes a situation in which the mobile body system 101, which had been running on a flat surface until time t=1, is subjected to a large upward swing in the attitude of each sensor due to an impact at time t=2. Specific examples of situations in which an impact may occur include a situation in which the mobile body runs over a step on a sidewalk or the like, a situation in which the tires run over an obstacle such as a stone or branch, or a situation in which the mobile body is subjected to intermittent vibrations from a cobblestone road or the like.

[0032] In step S302, the sensor input information acquisition unit 201 acquires the latest sensor information from each sensor, and also acquires the sensor information at the immediately preceding time that is stored in the work memory.

[0033] In step S303, the shock and vibration information acquisition unit 202 acquires information about the shock and vibration applied to the mobile system 101. The process of step S303 will be described in detail later.

[0034] In step S304, the position and orientation acquisition unit 203 calculates the position and orientation of the mobile body system 101. Details of the process in step S304 will be described later.

[0035] Step S305 is a branch to determine whether or not to end the position and orientation measurement process. If the position and orientation measurement is to be ended due to a user instruction or the like, the process ends. If not, the process returns to step S302 and the next process is performed.

[0036] (Details of shock and vibration information acquisition process) Next, the shock and vibration information acquisition process shown in S303 will be described in detail. In this embodiment, the magnitude and direction of the shock or vibration received by the mobile system 101 are analyzed based on image data acquired by the camera 104. Fig. 4 is a flowchart showing the flow of the shock and vibration information acquisition process in this embodiment.

[0037] In step S401, the impact vibration information acquisition unit 202 acquires image data acquired by the camera 104 from the latest and immediately previous sensor information acquired in step S302.

[0038] In step S402, the impact vibration information acquisition unit 202 detects and tracks feature points between the two images acquired in step S401, and acquires motion vectors of feature points that have been successfully tracked.

[0039] Figure 5(a) shows the results of feature point tracking between an image captured at time t = 1 and the image captured just before (t = 0). The penalty points in the figure indicate successfully tracked feature points, and the arrows accompanying the penalty points indicate the amount of movement (motion vector) of the feature points from the previous time.

[0040] Fig. 5(b) illustrates the results of feature point tracking between the image captured at the next time t=2 and the image captured at the previous time (t=1). At time t=2, the image is blurred vertically due to an impact applied to camera 104 during shooting. As a result of the blurring of the image, some of the feature points that were successfully tracked in Fig. 5(a) have failed to be tracked.

[0041] FIG. 5(c) shows the result of tracking feature points between the image acquired at time t=2 and the image acquired at time (t=1) after image blurring has been corrected.

[0042] In step S403, the impact vibration information acquisition unit 202 compares the group of motion vectors acquired in step S402 with the group of motion vectors calculated at the previous time, and analyzes the magnitude and direction of the impact applied to the mobile system 101. Specifically, a difference vector is calculated between the motion vectors of the feature points successfully tracked at both times, and the average vector is acquired as the impact vector (magnitude and direction of the impact) at that time.

[0043] (Details of position and orientation calculation process) Next, the position and orientation calculation process shown in S304 will be described in detail. In this embodiment, the position and orientation of the mobile system 101 is calculated based on image data acquired by the camera 104. The camera 104 is fixed on the mobile system 101, and the relative position and orientation thereof are known and stored in the RAM 113, and the position and orientation values ​​of the camera 104 and the mobile system 101 are mutually convertible. For ease of explanation, in the following explanation, it is assumed that the position and orientation values ​​of the camera 104 and the mobile system 101 are the same.

[0044] FIG. 6 is a flowchart showing the flow of the position and orientation calculation process in this embodiment.

[0045] Step S601 is a branch to determine whether image blur correction is necessary for calculating the position and orientation. Here, if the magnitude of the impact vector acquired in step S403 is equal to or greater than a preset threshold, it is determined that image correction is necessary, and the process proceeds to step S602. The threshold value for the magnitude of the impact vector is set so that it is determined that image correction is not necessary when the moving object is going up a slope or turning a curve. If image correction is not necessary, the process proceeds to S604. The method of determination here is not limited to the magnitude of the impact vector, and it is also possible to determine that image correction is necessary, for example, when the number or ratio of successfully tracked feature points is equal to or less than a threshold.

[0046] In step S602, the shock vibration information acquisition unit 202 performs image blur recovery processing on the latest image based on the shock vector acquired in step S403. Specifically, the acquired shock vector is first decomposed into vertical and horizontal components of the image, and the magnitude of the component in each direction is calculated. Next, the image is converted into spatial frequency components using a known two-dimensional Fourier transform. After that, for each spatial frequency, a recovery coefficient is acquired based on a two-dimensional lookup table prepared in advance, which inputs the vertical frequency and the magnitude of the vertical component of the shock vector, and the coefficients are multiplied. This recovers the frequency components reduced by the blur caused by the vibration in the vertical direction. Similarly, the frequency components are recovered in the horizontal direction. Finally, a two-dimensional inverse Fourier transform is performed on the spatial frequency component image with the frequency components restored, and a luminance image with the frequency components restored is acquired.

[0047] In step S603, the position and orientation acquisition unit 203 uses the image corrected in step S602 to perform detection and tracking of feature points between the two images again.

[0048] In step S604, the position and orientation acquisition unit 203 updates the current position and orientation of the mobile body system 101. Specifically, if the image has been corrected, the position and orientation acquisition unit 203 calculates a difference in position and orientation from the previous time of the mobile body system 101 based on the feature point detection result in step S603, or if not, based on the feature point detection result in step S402. Specifically, the calculation of the position and orientation difference is performed using a known bundle adjustment process. This is a process in which three-dimensional coordinates in space are assumed for at least eight or more feature points that have been successfully tracked, and the relative position and orientation between images and the three-dimensional coordinates of the feature points are optimized so that the difference (reprojection error) between the reprojection results on both images and the detected positions of the feature points on the images is reduced. In this case, if the three-dimensional coordinates of the feature points can be estimated from stereo distance measurement using a stereo camera or past feature point tracking results, they can be used as initial values ​​or fixed values ​​of the three-dimensional coordinates.

[0049] In step S605, the position and orientation acquisition unit 203 adds the position and orientation difference calculated in step S604 to the position and orientation at the previous time to calculate the position and orientation value at the latest time.

[0050] By the above processing, even if the camera used for position and orientation measurement is subjected to an impact, position and orientation measurement can be continued stably.

[0051] [Second embodiment] In the first embodiment, an example was described in which a single camera was used as a sensor for measuring position and orientation, and position and orientation measurement processing was performed according to impact information acquired from a camera image. In the present embodiment, an example will be described in which position and orientation measurement processing is performed by combining multiple sensors.

[0052] In this embodiment, position and orientation measurement is performed using an IMU and tire drive amount in addition to a camera as a sensor for position and orientation measurement. These multiple measurement methods have different resistance to vibration and impact depending on the principle of each method.

[0053] For example, the accuracy of a capacitance-type acceleration sensor used in some IMUs decreases when it is subjected to stronger acceleration or continuous vibration than expected. The method of integrating the driving force of the tire to calculate the amount of movement assumes movement on a plane, so the accuracy decreases due to unevenness of the ground, and it becomes difficult to calculate the correct amount of movement, especially when the tire floats and spins due to a vertical impact such as a step. In addition, it becomes difficult to calculate the correct amount of movement when the tire slips due to a strong lateral impact, but in this case, the relationship between the strength of the impact and the degree of decrease in the estimation accuracy of the amount of movement differs between the vertical and lateral directions. Although the camera generates image blur as described in the first embodiment, it does not have a mechanical structure that is directly affected by impacts and vibrations, so it can be said to be a sensor that is relatively resistant to vibrations and impacts.

[0054] In this embodiment, the contribution of a sensor that is estimated to have high position and orientation measurement accuracy depending on the occurrence conditions of vibration or impact is set relatively high, and the position and orientation measurement results from multiple sensors are synthesized according to the contribution, thereby achieving highly accurate position and orientation measurement.

[0055] (Configuration of moving body system and position and orientation measurement system) 7 is a diagram showing an example of the configuration of a mobile body system 701 according to this embodiment. A position and orientation measurement system 702 according to this embodiment is composed of an information processing device 103, a camera 104, and an IMU 704. The hardware configuration of the information processing device 103 and the camera 104 are similar to those in the first embodiment, and therefore description thereof will be omitted. In this embodiment, the camera 104, the control device 106, and the IMU 704 function as sensors for position and orientation measurement.

[0056] The IMU 704 is an inertial sensor fixed on the mobile system 701. Like the camera 104, the IMU 704 is connected to the information processing device 103 via the general-purpose I / F 115. The IMU 704 acquires three-dimensional acceleration and angular velocity applied to the mobile system 701. On the other hand, the IMU 704 has a characteristic that the accuracy of acquiring the acceleration and angular velocity decreases when a strong acceleration exceeding a threshold is applied.

[0057] For ease of explanation, it is assumed that the IMU 704 is attached to the center of the mobile system 701, and in the following explanation, it is assumed that the position, orientation, acceleration, and angular velocity of the mobile system 701 are identical to those of the IMU 704.

[0058] Moreover, both the camera 104 and the IMU 704 are fixed on the mobile system 701, and their relative positions and orientations are known and stored in the RAM 113. Therefore, the position and orientation information of the camera 104 and the position and orientation information of the IMU 704 (i.e., the position and orientation information of the mobile system 701) can be converted into each other. For ease of explanation, in the following explanation, it is assumed that the position and orientation information measured using the camera 104 indicates values ​​that have been converted into the position and orientation information of the IMU 704.

[0059] The functional configuration of the information processing device 103 according to this embodiment is similar to that of the first embodiment shown in Fig. 2, and therefore a detailed description thereof will be omitted. In addition, the basic flow of the position and orientation measurement process and the details of the impact vibration information acquisition process are similar to those of the first embodiment shown in Fig. 3 and Fig. 4, and therefore a detailed description thereof will be omitted.

[0060] (Details of position and orientation calculation process) The following describes in detail the position and orientation calculation process in S304 in this embodiment. In this embodiment, the position and orientation of the mobile body system 701 are calculated based on data acquired from the camera 104, the control device 106, and the IMU 704.

[0061] 8 is a flowchart showing the flow of the position and orientation calculation process in this embodiment. The processes from step S801 to step S803 are executed synchronously in parallel for each sensor.

[0062] In step S801, similarly to the first embodiment, the position and orientation acquisition unit 203 calculates a position and orientation difference value from the previous time based on the camera 104. This process is similar to the processes of steps S601 to S604 described in the first embodiment, and therefore a detailed description thereof will be omitted.

[0063] In step S802, the position and orientation acquisition unit 203 integrates the acceleration and angular velocity values ​​obtained from when the camera 104 acquires the previous image to when it acquires the latest image, and calculates a position and orientation difference value based on the IMU.

[0064] In step S803, the position and orientation acquisition unit 203 integrates the amount of tire movement obtained by the camera 104 from when the previous image is acquired to when the latest image is acquired, and calculates a position and orientation difference value based on the amount of tire movement. This position and orientation difference value is calculated on the assumption that the mobile system 101 is traveling on a flat surface. Therefore, this position and orientation difference value is composed of a movement vector on an xy plane that corresponds to a flat surface such as a floor, and an amount of rotation about the z axis perpendicular to the plane.

[0065] In step S804, the position and orientation acquisition unit 203 determines the contribution of each sensor to be used when synthesizing the position and orientation difference values ​​calculated based on each sensor. The calculation of the contribution is determined based on the impact vector calculated by the impact vibration information acquisition unit 203. Specifically, the acquired impact vector is decomposed into a vertical component and a horizontal component of the image, and the value of the contribution is determined based on the magnitude of the component in each direction and a calculation method set in advance for each sensor.

[0066] The contribution of each sensor is calculated using a predetermined function with the magnitude of impact or vibration on each axis as an argument. In this embodiment, the camera 104 is considered to be more resistant to vibration and impact than other sensors, so it is used as the reference sensor for position and orientation measurement, and the contribution of the camera 104 is fixed at 1.

[0067] The accuracy of the IMU 704 decreases when an acceleration of a certain strength or more is applied, regardless of the direction of the vibration or impact obtained from the image of the camera 104. Therefore, the contribution of the IMU 704 refers only to the magnitude of the impact vector, and uses a function such that the contribution is 1 when the magnitude of the impact vector is below a threshold α1, is 0 when the magnitude is above a threshold α2, and decreases linearly between the thresholds α1 and α2. The thresholds α1 and α2 are values ​​defined according to the vibration resistance performance of the IMU used.

[0068] The position and orientation difference measurement based on the tire drive amount employs a method of calculating the contribution degree for each of the vertical and horizontal components of the impact vector and accumulating them. For the vertical direction, the accuracy decreases due to unevenness of the ground, and the accuracy decreases significantly especially when the impact causes the tires to spin. Therefore, a function is used in which the contribution degree is 1 when the vertical component of the impact vector is 0, and 0 when it is equal to or greater than threshold value β1, and the contribution degree decreases linearly between 0 and threshold value β. The threshold value β is a value defined as the magnitude of the vertical component of the impact vector when an impact equivalent to gravitational acceleration 1G is applied to the mobile system 101 in the vertical direction.

[0069] For the lateral direction, impacts in the range where the tire does not slip are considered not to have a significant adverse effect on position and orientation measurement based on the drive amount, but if slippage occurs, accurate calculation of the position and orientation difference becomes impossible. Therefore, if the lateral component of the impact vector is less than the threshold γ, the contribution is set to 1, and if it is equal to or greater than the threshold γ, it is set to 0. The threshold γ is a value defined based on the assumed coefficient of friction between the tire and the ground.

[0070] In step S805, the position and orientation acquisition unit 203 calculates a position and orientation difference value of the mobile body system 701 based on the position and orientation difference values ​​based on each sensor calculated in steps S801 to S803 and the contribution of each sensor calculated in step S804. Basically, the position and orientation difference calculated by each sensor is separated into position differences on the x-, y-, and z-axes and rotation amounts about the x-, y-, and z-axes, and a weighted average value is calculated for each, with the contribution of each sensor used as a weighting value. Here, the position and orientation difference value based on the driving amount of the tires does not have information on the position difference in the z-axis direction and the rotation amounts about the x-, y-axes, and the weights for these values ​​are treated as 0.

[0071] In step S806, the position and orientation acquisition unit 203 adds the position and orientation difference calculated in step S805 to the position and orientation at the previous time to calculate the position and orientation value at the latest time, and determines the position and orientation.

[0072] By the above processing, even when a combination of multiple sensors is used to perform position and orientation measurement processing, it is possible to suppress deterioration in accuracy due to shock or vibration and perform stable position and orientation measurement.

[0073] [Variations] In the first embodiment, a method of adding a position and orientation difference calculated from the feature point tracking results of multiple images to an initial position and orientation value has been described as an example of a position and orientation measurement method using a camera, but the position and orientation measurement method using a camera is not limited to this. For example, known position and orientation measurement methods such as SLAM (Simultaneous Localization and Mapping) and SVO (Semi-Direct Monocular Visual Odometry) may be used.

[0074] A specific example of using SLAM will be described. The mobile system holds, as map information of the environment, information on a 3D point cloud that indicates shape features of objects in an environment in which a mobile body carrying a camera travels. The 3D point cloud information is the 3D coordinates in world coordinates of image feature points. Map information of the environment is generated in advance based on an image capturing the shape of an object existing in the environment, and is stored in a storage device. Based on the input image and the map information, the position and orientation acquisition unit 203 minimizes the difference (reprojection error) between the positions of feature points (edge ​​features that indicate shape) on the input image and the positions when the feature points on the map are projected onto the input image using the position and orientation of the camera when the input image was captured. This estimates the position and orientation of the camera.

[0075] Further, an environmental map creation system that uses these techniques to create an environmental map of the surroundings of a moving object at the same time as measuring the position and orientation is also within the scope of the present invention.

[0076] In the second embodiment, a method was described in which position and orientation measurements were performed based on the respective sensors of the camera, IMU, and tire drive amount, and the results were synthesized based on vibration and impact information, but the sensors that can be used are not limited to these. For example, LiDAR (Laser Imaging Detection and Ranging) may be added to the sensor, or a part of it may be replaced. In general, mirror rotation type LiDAR has a mechanical rotation mechanism, so its accuracy is easily reduced by impact, and it is better to set a lower contribution level than the camera when vibration or impact occurs. On the other hand, MEMS (Micro Electro Mechanical Systems) type LiDAR does not have a mechanical mechanism. Since image blurring like a camera is small, a higher contribution level than the camera is set for strong impacts.

[0077] Alternatively, position information obtained by GNSS (Global Navigation Satellite System) may be used, or position and orientation values ​​calculated from the results of observing the mobile system with a sensor such as a camera installed outside the mobile body may be used. For example, a method is conceivable in which the image of a security camera capturing a moving mobile system is processed on a server machine, the position and orientation information of the mobile system recognized in the image is calculated, and the calculated information is transmitted to the mobile system via a communication unit.

[0078] In the second embodiment, the contribution of each sensor is calculated based on a predetermined formula. However, the method of calculating the contribution is not limited to this, and any method may be used as long as it sets a relatively high contribution to a sensor with high position and orientation measurement accuracy estimated from the acquired vibration and impact information. For example, a two-dimensional lookup table using the magnitudes of the vertical and horizontal components of the impact vector as input may be used. Alternatively, the contribution may be determined based on not only the impact vector acquired at the relevant time, but also the history of the impact vector at each recent time.

[0079] Alternatively, the contribution degree may be determined by taking into account information other than vibration and impact information. For example, the tire drive amount and position and orientation measurement based on GNSS may be considered to have relatively lower accuracy than other sensors even in ideal conditions, and a process may be performed to set the contribution degree lower. Alternatively, in a vehicle having front and rear wheels, a certain amount of tire slip occurs during steering operation, so a process may be performed to lower the contribution degree of the position and orientation measurement based on the tire drive amount while control is being performed to turn the moving body.

[0080] In the first and second embodiments, the feature point tracking results of the camera image are used as the shock vibration acquisition means, but the shock vibration acquisition means available for the present invention is not limited to this. In the first and second embodiments, the direction of the shock or vibration is acquired in two axes in addition to the magnitude, but acquisition of the direction of the shock or vibration is not essential, and the format in which it is acquired is not limited to this. For example, the acceleration value may be acquired in three axes from the IMU. Alternatively, only the magnitude of the shock or vibration may be acquired.

[0081] Moreover, shock and vibration information may be acquired using information other than that of a sensor on the mobile system. For example, an image of an external device such as a security camera capturing a moving mobile system from a position where the mobile system can be observed may be processed on a server machine, and the shaking state of the mobile system recognized in the image may be analyzed and transmitted to the mobile system through a communication unit. The shaking state may be obtained by tracking feature points obtained by analyzing the image. The method described in step S402 may be used as a method for tracking the feature points. Alternatively, the magnitude of vibration or impact predicted in association with position and orientation information may be acquired from electronic map data stored on an information processing device. Specifically, position information of steps and rough roads recorded on a map is acquired, and position and orientation measurement is performed assuming that a predetermined magnitude of shock or vibration will occur at that location.

[0082] When performing blur correction processing on an image as in the first embodiment, if triaxial shock or vibration information is acquired, the triaxial information is projected onto the image plane and converted into biaxial shock or vibration information, thereby making it possible to perform blur correction processing in the same manner as in the first embodiment. Alternatively, if only information on the magnitude of the shock or vibration is acquired, blur correction processing is performed without distinguishing between directions on the image.

[0083] In addition, when the contribution of each sensor is set according to vibration or impact information as in the second embodiment, the contribution may be calculated using a function or lookup table according to the number of axes of the acquired vibration or impact information, or may be calculated based on vibration or impact information converted into a smaller number of axes.

[0084] In the first and second embodiments, a mobile body system having a plurality of tires as a wheel portion is described as an example, but the wheel portion of the mobile body system is not limited to this. For example, the mobile body system may be a flying object such as a multicopter, or a walking mobile body having a plurality of legs. The mobile body system may also be a manned mobile body that a user can board and directly operate. Furthermore, the mobile body system may be a mobile body equipped with a function of autonomously traveling on a route set in advance.

[0085] The present invention can also be realized by a process in which a program for implementing one or more of the functions of the above-mentioned embodiments is supplied to a system or device via a network or a storage medium, and one or more processors in a computer of the system or device read and execute the program. The present invention can also be realized by a circuit (e.g., ASIC) that implements one or more of the functions. [Explanation of symbols]

[0086] 101 Mobile Systems 102 Position and Orientation Measurement System 103 Information processing equipment 104 Camera 105 Communications Department 106 Control device 107 Wheel section 201 Sensor input information acquisition unit 202 Shock and vibration information acquisition unit 203 Position and orientation acquisition unit

Claims

1. An input information acquisition means for acquiring input information from a sensor mounted on the moving object; a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body; and a position and orientation acquisition means for acquiring a position and orientation of the moving body based on corrected input information obtained by correcting the input information acquired by the input information acquisition means using the vibration information when the sensor is subjected to vibration exceeding a threshold value.

2. An input information acquisition means for acquiring input information from a sensor mounted on a moving object; a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body; a position and orientation acquisition means for acquiring a position and orientation of the moving object based on input information corrected by using the vibration information, The information processing device, characterized in that the vibration information acquisition means acquires the vibration information by tracking feature points obtained from image information of the moving object captured by an external device capable of observing the moving object.

3. An input information acquisition means for acquiring input information from a sensor mounted on a moving object; a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body; a position and orientation acquisition means for acquiring a position and orientation of the moving object based on input information corrected by using the vibration information, the input information acquisition means acquires image information from an image sensor mounted on the moving object; the vibration information acquiring means acquires the vibration information by tracking feature points obtained from the image information.

4. The information processing device according to claim 3, characterized in that, when the proportion of the trackable feature points is equal to or less than a threshold value, the position and orientation acquisition means acquires the position and orientation of the moving body based on corrected input information obtained by correcting the input information using the vibration information.

5. The mobile device further includes a means for acquiring map information of an environment in which the mobile object moves, 5. The information processing apparatus according to claim 1, wherein the vibration information acquisition means acquires the vibration information based on environmental information included in the map information.

6. An input information acquisition means for acquiring input information from each of a plurality of sensors mounted on the moving object; a vibration information acquiring means for acquiring vibration information including a magnitude and a direction of the vibration received by the moving body; and a position and orientation determining means for determining a position and orientation of the moving body by weighting the position and orientation of the moving body acquired using each of the input information based on the vibration information.

7. The information processing device described in Claim 1, characterized in that the input information acquisition means acquires image information from an imaging sensor mounted on the moving body, and the position and orientation acquisition means performs blur correction processing on the image information as a correction for the input information.

8. An input information acquisition step of acquiring input information from a sensor mounted on the moving object; a vibration information acquiring step of acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body; and a position and orientation acquisition step of acquiring a position and orientation of the moving body based on corrected input information obtained by correcting the input information acquired in the input information acquisition step using the vibration information when the sensor is subjected to vibration exceeding a threshold value.

9. An input information acquisition step of acquiring input information from each of a plurality of sensors mounted on the moving object; a vibration information acquiring step of acquiring vibration information including a magnitude and a direction of vibration received by the sensor due to vibration of the moving body; and a position and orientation determination step of determining a position and orientation of the moving body by weighting the position and orientation of the moving body obtained using each of the input information based on the vibration information.

10. A program for causing a computer to function as each of the means of the information processing device according to any one of claims 1 to 7.

Images