JP7141403B2 - Laser scanner with real-time online self-motion estimation - Google Patents

Description

The present invention relates to mobile mapping systems.

(Priority claim)
This application is filed on October 10, 2017, entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION," PCT Application No. PCT/US2017/055938. (Attorney Docket No. KRTA-0008-WO) and is a continuation-in-part application thereof.

PCT Application No. PCT/US2017/055938 is a PCT application entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION" filed March 7, 2017 No. PCT/US2017/021120 (Attorney Docket No. KRTA-0005-WO) and is a continuation-in-part application thereof. This application also claims priority to PCT Application No. PCT/US2017/021120. PCT Application No. PCT/US2017/055938, entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION", filed Oct. 11, 2016 It further claims priority to Provisional Application No. 62/406,910 (Attorney Docket No. KRTA-0002-P02).

PCT Application No. PCT/US2017/021120, entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION", filed March 11, 2016 It claims the benefit of Provisional Application No. 62/307,061 (Attorney Docket No. KRTA-0001-P01).

No. 62/451,294, entitled "LIDAR AND VISION-BASED EGO-MOTION ESTIMATION AND MAPPING," filed Jan. 27, 2017. (Attorney Docket No. KRTA-0004-P01).

(Cross reference to related applications)
The disclosures of PCT/US2017/055938, PCT/US2017/021120, and US Provisional Patent Application Nos. 62/406,910, 62/307,061, and 62/451,294 , which are incorporated herein by reference in their entireties for all purposes.

An autonomous mobile device may need information about the terrain on which it operates. Such devices may rely either on a predetermined map presenting the terrain and any obstacles that may be found therein. Alternatively, the device may have the ability to map its terrain while stationary or in motion, including a computer-based mapping system having one or more sensors that provide real-time data. A mobile computer-based mapping system can estimate its change in position over time (odometer) and/or generate a 3D map representation, such as a point cloud of a 3D space.

An exemplary mapping system can include various sensors that provide data from which maps can be constructed. Some mapping systems may use a stereoscopic camera system as one such sensor. These systems benefit from the baseline between the two cameras as a reference for determining the scale of motion estimation. Binocular systems are preferred over monocular systems because monocular systems may not be able to determine the scale of the image without receiving data from additional sensors or making assumptions about device motion. It is from. In recent years, RGB-D cameras have become popular in the research field. Such cameras can provide depth information associated with individual pixels, thus helping determine scale. However, some methods involving RGB-D cameras can only use image areas with depth information coverage, resulting in large image areas, especially in open spaces where depth can only be acquired sparsely. may be wasted.

In other examples of mapping systems, an IMU may be coupled to one or more cameras such that scale constraints can be provided from IMU acceleration. In some examples, a monocular camera can be tightly or loosely coupled to an IMU using a Kalman filter. Other mapping systems may use optimization methods to solve for mobile system motion.

Alternative mapping systems can include the use of laser scanners for motion estimation. However, difficulties in using such data may arise from the scanning speed of the laser. While the system is moving, unlike fixed position laser scanners, the laser spot is affected by the relative movement of the scanner. This movement effect may therefore be a factor in the arriving laser spots arriving at the system at different times. As a result, there may be scan distortion due to external motion of the laser when the scan speed is slow relative to the motion of the mapping system. This motion effect can be compensated for by the laser itself, but this compensation may require an independent motion model to provide the required correction. As an example, motion can be modeled as a constant velocity or as a Gaussian process. In some examples, the IMU can provide a motion model. Such methods align space-time patches formed by laser point clouds to estimate sensor motion and correct for IMU bias with offline batch optimization.

A similar problem of motion distortion may be found when using rolling shutter cameras. In particular, image pixels may be received continuously over time, resulting in image distortion caused by extrinsic motion of the camera. In some examples, the visual odometry method can use an IMU to compensate for the rolling shutter effect given the pixel readout time.

In some examples, GPS/INS technology can be used to determine the location of the mobile mapping device. However, high accuracy GPS/INS solutions may not be practical when the application is GPS rejection, lightweight, or cost sensitive. It is recognized that accurate GPS mapping requires line-of-sight communication between the GPS receiver and at least four GPS satellites (although five are considered preferred). In some environments, for example, an urban environment that may include flyovers and other obstacles, it may be difficult to receive undistorted signals from four satellites.

That is, some technical techniques associated with fusing data from optical devices with other motion-measuring devices to generate a robust map of the terrain surrounding an autonomous mapping device, especially while the mapping device is in motion. It will be appreciated that challenges exist. Disclosed below are methods and systems for mapping devices that can acquire optical mapping information and generate robust maps with reduced distortion.

According to an exemplary and non-limiting embodiment, a method receives data from an IMU device at a first computing module at a first frequency to generate a first estimate for a mobile mapping system based at least in part on the received IMU data. calculating a position; and receiving a first position estimate and visual-inertial data at a second calculation module at a second frequency to operate the mobile based at least in part on the first position estimate and visual-inertial data. calculating a second position estimate of the mapping system; receiving the second position estimate and the laser scan data at a third calculation module at a third frequency to generate at least a portion of the second position estimate and the laser scan data; and calculating a third estimated position of the mobile mapping system based on the target.

According to another exemplary and non-limiting embodiment, a mobile mapping system receives data on a first frequency from an IMU device and determines a first estimated position of the mobile mapping system based at least in part on the received IMU data. a first computation module adapted to compute and receive a first estimated position and visual-inertial data at a second frequency and based at least in part on the first estimated position and visual-inertial data; a second computation module adapted to compute a second position estimate of the mapping system; and receiving the second position estimate and laser scan data at a third frequency to convert the second position estimate and laser scan data. and a third calculation module adapted to calculate a third estimated position of the mobile mapping system based at least in part.

1 is a block diagram of an embodiment of a mapping system; FIG. Figure 2 shows an embodiment of a block diagram of three computational modules and their respective feedback features of the mapping system of Figure 1; FIG. 2 illustrates an embodiment of a Kalman filter model for refining location information into a map; FIG. 10 illustrates an embodiment of a factor graph optimization model for refining location information into a map; FIG. 13 illustrates an embodiment of a visual-inertial odometry subsystem; FIG. 4 illustrates an embodiment of a scan matching subsystem; FIG. 10 illustrates an embodiment of a global map with coarse detail resolution; FIG. 10 illustrates an embodiment of a sub-region map with fine detail resolution; FIG. 4 illustrates an embodiment of multi-threaded scan alignment; FIG. 3 illustrates an embodiment of single-threaded scan alignment; FIG. 10 shows an embodiment of a block diagram of three computational modules in which feedback data from the visual-inertial odometry unit is suppressed due to data degradation; FIG. 10 shows an embodiment of a block diagram of three computational modules in which feedback data from the scan matching unit is suppressed due to data degradation; FIG. 10 shows an embodiment of a block diagram of three computational modules in which the feedback data from the visual-inertial odometry unit and the scan matching unit are partially suppressed due to data degradation; FIG. 10 illustrates an embodiment of an estimated trajectory for a mobile mapping device; FIG. 2 illustrates bi-directional information flow according to an exemplary non-limiting embodiment; 1 illustrates a dynamically reconfigurable system according to exemplary non-limiting embodiments; FIG. 1 illustrates a dynamically reconfigurable system according to exemplary non-limiting embodiments; FIG. FIG. 4 illustrates priority feedback for IMU bias correction according to an exemplary non-limiting embodiment; FIG. 2 shows a two-layer voxel representation of a map according to an exemplary non-limiting embodiment; FIG. 2 shows a two-layer voxel representation of a map according to an exemplary non-limiting embodiment; FIG. 4 illustrates multi-threaded processing of scan matching according to an exemplary non-limiting embodiment; FIG. 4 illustrates multi-threaded processing of scan matching according to an exemplary non-limiting embodiment; 1 illustrates an exemplary, non-limiting embodiment of a SLAM system; FIG. 1 illustrates an exemplary, non-limiting embodiment of a SLAM system; FIG. FIG. 2 illustrates an exemplary, non-limiting embodiment of a SLAM enclosure; FIG. 10 illustrates an exemplary, non-limiting embodiment of a point cloud indicating confidence levels; FIG. 10 illustrates an exemplary, non-limiting embodiment of a point cloud indicating confidence levels; FIG. 10 illustrates an exemplary, non-limiting embodiment of a point cloud indicating confidence levels; FIG. 3 illustrates an exemplary non-limiting embodiment of different confidence level metrics; 1 illustrates an exemplary, non-limiting embodiment of a SLAM system; FIG. FIG. 2 illustrates an exemplary, non-limiting embodiment of timing signals for a SLAM system; FIG. 2 illustrates an exemplary, non-limiting embodiment of timing signals for a SLAM system; FIG. 2 illustrates an exemplary, non-limiting embodiment of SLAM system signal synchronization; FIG. 3 illustrates an exemplary, non-limiting embodiment of cooperative air-to-ground mapping; FIG. 3 illustrates an exemplary, non-limiting embodiment of a sensor pack; FIG. 1 shows an exemplary, non-limiting embodiment of a block diagram of a laser-visual-inertial odometry and mapping software system; FIG. 10 illustrates an exemplary, non-limiting embodiment of comparison of scans involved in odometry estimation and localization; FIG. 10 illustrates an exemplary, non-limiting embodiment of a comparison of scan registration accuracy in stereotaxy; FIG. 10 illustrates an exemplary, non-limiting embodiment of a horizontal orientation sensor test; FIG. 10 illustrates an exemplary, non-limiting embodiment of a vertical orientation sensor test; FIG. 10 illustrates an exemplary, non-limiting embodiment of an accuracy comparison between a horizontal orientation sensor test and a downward tilt sensor test; 1 illustrates an exemplary, non-limiting embodiment of an aircraft having a sensor pack; FIG. FIG. 10 illustrates an exemplary, non-limiting embodiment of sensor trajectories; FIG. 10 illustrates an exemplary, non-limiting embodiment of autonomous flight results; FIG. 10 illustrates an exemplary, non-limiting embodiment of autonomous flight results over long-term operation;

In one general aspect, the present invention relates to a mobile computer-based mapping system that estimates position change over time (odometer) and/or generates a 3D map representation of a 3D space, such as a point cloud. A mapping system may include multiple sensors including, but not limited to, an inertial measurement unit (IMU), a camera, and/or a 3D laser scanner. The mapping system is also in communication with and processes the outputs from the sensors to estimate changes in position of the system over time and/or to generate a map representation of the surrounding environment. A computer system having at least one processor configured may be included. The mapping system can enable high-frequency low-latency online real-time self-motion estimation with dense and accurate 3D map registration. Embodiments of the present disclosure may include simultaneous localization and mapping (SLAM) systems. The SLAM system is GPS-independent and can include a multi-dimensional (eg, 3D) laser scanning and ranging system that enables simultaneous localization and mapping in real-time. SLAM systems can generate and manage data on highly accurate point clouds resulting from reflections of laser scanning from objects in the environment. Movement of any of the points in the point cloud aligns the points in the point cloud with respect to location so that the SLAM system can maintain an accurate understanding of the system's location and orientation as it navigates through the environment. is accurately tracked over time using .

In one embodiment, the position and motion resolution of the mobile mapping system can be refined sequentially in a series of coarse-to-fine updates. In a non-limiting example, a separate computational module can be used to update the position and motion of the mobile mapping system from coarse resolution with a fast update rate to fine resolution with a slower update rate. For example, an IMU device may provide data to the first computational module for predicting the motion or position of the mapping system at a high update rate. A visual-inertial odometry system can provide data to the second computational module to improve the motion or positional resolution of the mapping system at low update rates. In addition, the laser scanner can provide data to the third computational scan alignment module to further refine the motion estimate and align the maps at a lower update rate. In one non-limiting example, the data from the computation module configured to process fine resolution position and/or motion data is the data from the computation module configured to process coarse resolution position and/or motion data. can give feedback to In another non-limiting example, the computing module incorporates fault tolerance to address sensor degradation issues by automatically bypassing computing modules associated with sensor sourcing failures, incorrect data, incomplete data, or non-existent data. be able to. Thus, the mapping system can operate in the presence of highly dynamic motion and in a darker, uneven and structure-free environment.

In contrast to existing map generation techniques, which are mostly offline batch systems, the mapping system disclosed herein operates in real-time and can generate maps while in motion. This feature provides two practical advantages. First, users are not limited to scanners fixed on tripods or other non-stationary mounts. Instead, the mapping system disclosed herein can be associated with mobile devices, thereby expanding the range of environments that can be mapped in real time. Second, real-time features can give users feedback about the current map area while data is being collected. Online generated maps can also assist robots or other devices for autonomous navigation and obstacle avoidance. In some non-limiting embodiments, such navigation functionality can be built into the mapping system itself. In an alternate non-limiting embodiment, map data can be provided to additional robots with navigation capabilities that may require externally sourced maps.

There are several potential applications for sensors such as 3D modeling, scene mapping, and environment inference. The mapping system can provide the point cloud map to other algorithms that take the point cloud as input for further processing. Moreover, the mapping system can work both indoors and outdoors. Such embodiments do not require external lighting and can operate in the dark. Embodiments with a camera may require external lighting, but can handle rapid motion and can color the laser dot cloud with the image from the camera. The SLAM system can build and maintain point clouds in real-time as the user moves through the environment, e.g., walks, cycles, drives, flies, and combinations thereof. The map is built in real time as the mapper progresses through the environment. The SLAM system can track thousands of features as points. As the mapper moves, these points are tracked to allow motion estimation. Thus, the SLAM system operates in real-time and without reliance on external localization techniques such as GPS. In an embodiment, multiple (most often very large) environmental features, such as objects, are used as points for triangulation, and the system calculates position and orientation as it moves through the environment. Numerous location and orientation calculations are performed and updated to maintain an accurate current estimate of . In embodiments, fixed features (such as walls, doors, windows, furniture, and fixtures) are identified using relative motion of features within the environment so that the fixed features can be used in position and orientation calculations. It can be distinguished from moving features (people, vehicles, and other moving items). Underwater SLAM systems can use blue-green lasers to reduce attenuation.

The mapping system design follows the consideration that the drift in ego-motion estimation has a frequency lower than that of the module itself. Therefore, the three calculation modules are arranged in descending order of frequency. The high frequency module is dedicated to dealing with extreme motion, and the low frequency module cancels drift with respect to the preceding modules. Furthermore, this sequential processing favors computation in that the forward modules do not carry as much computation and run at a high frequency, giving the backward modules sufficient time for complete processing. Thus, the mapping system can achieve a high level of accuracy while operating online in real time.

Additionally, the system can be configured to address sensor degradation. If the camera does not work (e.g. due to dark, dramatic lighting changes, or uneven environment) or if the laser does not work (e.g. due to unstructured environment), the corresponding module can be bypassed and the rest of the system can be alternated to ensure functionality. In particular, in some exemplary embodiments, the proposed pipeline automatically determines degraded subspaces within the problem state space and partially solves the problem within the well-conditioned subspaces. As a result, the combination of "sound" parts from each module forms the final solution. As a result, the resulting combination of modules used to generate the outputs is not simply a linear or non-linear combination of module outputs. In some exemplary embodiments, the output reflects detours across one or more modules in combination with linear or non-linear combinations of the remaining functional modules. Results from testing the system through numerous experiments have shown that the system can produce high accuracy over several kilometers of navigation and robustness with respect to environmental degradation and extreme motion.

The modularized mapping system disclosed below is configured to process data from range, visual, and inertial sensors for motion estimation and mapping by using a multi-layer optimization structure. A modular mapping system
・The ability to dynamically reconfigure calculation modules,
Fully or partially bypassing failure modes within the computational module and combining data from the remaining modules in a manner that addresses sensor and/or sensor data degradation, thereby induced data degradation and mobile mapping in the environment. Ability to deal with extreme motion of the system, and Ability to cooperatively integrate computational modules to provide real-time performance;
High accuracy, robustness, and low drift can be obtained by incorporating features that can include

Disclosed herein is a mapping system for online self-motion estimation using 3D lasers, cameras, and IMUs. Additionally, the motion estimation aligns the laser points to build a map of the moving environment. In many real-world applications, self-motion and mapping must be done in real time. In autonomous navigation systems, maps can be crucial for motion planning and obstacle avoidance, while motion estimation is important for vehicle control and steering.

FIG. 1 depicts a simplified block diagram of a mapping system 100 according to one embodiment of the invention. Although specific components are disclosed below, such components are provided by way of example only and not limitation with respect to other equivalent or similar components. Exemplary systems include an IMU system 102 such as the Xsens® MTi-30 IMU, a camera system 104 such as the IDS® UI-1220SE monochromatic camera, and a Velodyne PUCK™ VLP-16 and a laser scanner 106, such as a laser scanner. IMU 102 provides inertial motion data derived from one or more of an xyz accelerometer, roll-pitch-yaw gyroscope, and magnetometer and inertial data at a first frequency; can be done. In some non-limiting examples, the first frequency can be approximately 200 Hz. Camera system 104 may have a resolution of 752×480 pixels, a horizontal field of view (FOV) of 76°, and a frame capture rate at the second frequency. In some non-limiting examples, the frame capture rate can operate at a second frequency of approximately 50 Hz. The laser scanner 106 has a horizontal FOV of 360° and a vertical FOV of 30° and is capable of receiving 0.3 million points per second at a third frequency representing the laser rotation speed. In some non-limiting examples, the third frequency can be approximately 5 Hz. As depicted in FIG. 1, a laser scanner 106 can be connected to a motor 108 incorporating an encoder 109 to measure the motor rotation angle. In one non-limiting example, laser motor encoder 109 can operate at a resolution of approximately ^0.25 °.

IMU 102, camera 104, laser scanner 106, and laser scanner motor encoder 109 may be in data communication with computer system 110, which includes one or more processors 134 and associated memory 103A and can be any computing device having sufficient processing power and memory to perform the desired odometry and/or mapping. For example, a laptop computer with a 2.6 GHz i7 quad-core processor (2 threads on each core for a total of 8 threads) and embedded GPU memory can be used. Additionally, the computer system may include one or more types of primary or dynamic memory 120, such as RAM, and one or more types of secondary or storage memory 160, such as hard disk or flash ROM. and Although specific computational modules (IMU module 122, visual-inertial odometry module 126, and laser scanning module 132) are disclosed above, such modules are merely exemplary modules having the functionality described above and are not limited to such modules. It must be recognized that it is not Similarly, the types of computing device 110 disclosed above are merely examples of the types of computing devices that can be used with the sensors described above for the purposes disclosed herein, and are in no way limiting.

As illustrated in FIG. 1, the mapping system 100 incorporates a computational model that includes separate computational modules that sequentially reconstruct motion in a coarse-to-fine fashion (see also FIG. 2). Starting with motion prediction from the IMU 102 (IMU prediction module 122), a vision-inertia tight coupling method (vision-inertia odometry module 126) estimates motion and locally aligns the laser point. A scan matching method (scan matching refinement module 132) then further refines the estimated motion. In addition, scan alignment refinement module 132 aligns point cloud data 165 to build a map (voxel map 134). Maps can also be used as part of the navigation system 136, which is an optional mapping system. It can be appreciated that the navigation system 136 can be included as a computing module within the on-board computer system, primary memory, or can include an entirely separate system.

It can be appreciated that each computing module can process data from each one of the sensor systems. Thus, the IMU prediction module 122 generates a coarse map from the data derived from the IMU system 102 and the vision-inertial odometry module 126 processes further refined data from the camera system 104 to scan-match refine. Transformation module 132 processes the finest resolution data from laser scanner 106 and motor encoder 109 . Additionally, each of the finer-grained resolution modules further processes the data provided by the coarser-grained modules. A visual-inertial odometry module 126 refines the mapping data computed by and received from the IMU prediction module 122 . Similarly, scan match refinement module 132 further processes the data provided by vision-inertial odometry module 126 . As disclosed above, each of the sensor systems acquires data at different rates. In one non-limiting example, the IMU 102 can update data acquisition at a rate of approximately 200 Hz, the camera 104 can update data acquisition at a rate of approximately 50 Hz, and the laser scanner 106 can update data acquisition at a rate of approximately 5 Hz. You can update the data acquisition with These speeds are non-limiting and can, for example, reflect the data acquisition speed of each sensor. It can be appreciated that coarser-grained data can be acquired at a faster rate than finer-grained data, and can also be processed at a faster rate than finer-grained data. Although specific frequency values have been disclosed above for data acquisition and processing by various computational modules, neither absolute frequencies nor their relative frequencies are limiting.

Mapping data and/or navigation data can also be considered to include coarse level data and fine level data. Thus, in primary memory (dynamic memory 120) may be stored coarse position data within voxel map 134, which may be accessible to any of computation modules 122, 126, 132. . The files of detailed map data as point cloud data 165 that the scan match refinement module 132 can generate are transferred through the processor 150 into secondary memory 160, such as a hard disk, flash drive, or other more permanent memory. can be stored.

Not only are the calculation modules for the finer-grained calculations using the coarser-grained data, but both the visual-inertial odometry module 126 and the scan matching refinement module 132 (fine-grained position information and mapping) More refined mapping data can be fed back to IMU prediction module 122 via respective feedback paths 128 and 138 as a basis for updating the IMU position prediction. In this manner, the coarse position data and the mapping data can be refined in resolution in turn, with the refined resolution data serving as a feedback reference for the coarse resolution calculations.

FIG. 2 is a block diagram of three computational modules and their respective data paths. The IMU prediction module 122 may receive IMU location data 223 from the IMU (102 in FIG. 1). A visual-inertial odometry module 126 receives model data from the IMU prediction module 122, as well as visual data from one or more individually tracked visual features 227a, 227b from the camera (104 in FIG. 1). be able to. A laser scanner (106 in FIG. 1) generates data relating to laser-determined landmarks 233a, 233b that can be provided to the scan registration refinement module 132 in addition to the position data supplied by the vision-inertial odometry module 126. can be made A position estimation model from the visual-inertial odometry module 126 can be fed back 128 to refine the position model computed by the IMU prediction module 122 . Similarly, refined map data from the scan match refinement module 132 can be fed back 138 to apply additional corrections to the position model computed by the IMU prediction module 122 .

As depicted in FIG. 2 and disclosed above, the modularized mapping system can sequentially recover and refine motion-related data in a coarse-to-fine fashion. In addition to the above, the data processing of each module may be determined by the speed of data acquisition and processing of each of the devices outputting data to the module. Starting with motion estimation from the IMU, the visual-inertial tight coupling method can estimate and locally align the laser point. The scan matching method then further refines the estimated motion. The scan-match refinement module can also align the point clouds to build the map. As a result, the mapping system is time-optimized to process each refinement process as data becomes available.

FIG. 3 illustrates a standard Kalman filter model based on data derived from the same type of sensor depicted in FIG. As illustrated in FIG. 3, the Kalman filter model updates location data and/or mapping data upon receipt of any data from any one of the sensors regardless of the resolution performance of the data. In this case, for example, the visual-inertial odometry data can be used to update position information whenever such data becomes available, regardless of the state of position information estimation based on IMU data. Therefore, the Kalman filter model does not take advantage of the relative resolution of each type of measurement. FIG. 3 depicts a block diagram of a standard Kalman filter-based method for optimizing position data. The Kalman filter sequentially updates the position models 322a-322n as it is fed data. Thus, starting with an initial position prediction model 322a, the Kalman filter can predict 324a a subsequent position model 322b, which can be refined based on received IMU mechanized data 323. The position prediction model can be updated 322b according to the IMU mechanized data 323 in a prediction stage 324a, followed by an update stage in which individual visual features or laser landmarks are seeded.

FIG. 4 depicts position optimization based on the factor graph method. In this manner, the attitude of the mobile mapping system at the first time 410 can be updated to the attitude at the second time 420 after receiving the data. A factor graph optimization model combines constraints from all sensors during each refinement computation. In this case, IMU data 323, feature data 327a, 327b, and the like from the cameras, and laser landmark data 333a, 333b, etc. are all used for each update stage. It will be appreciated that such methods increase the computational complexity for each position refinement stage due to the large amount of data required. Furthermore, since sensors may provide data at independent rates that may differ by orders of magnitude, the entire refinement stage is time bound to the data acquisition time for the slowest sensor. As a result, such models may not be suitable for fast real-time mapping. The modularized system depicted in FIGS. 1 and 2 sequentially restores motion in a coarse-to-fine fashion. In this manner, the degree of motion refinement is determined by the availability of each type of data.

Assumptions, Coordinates, and Problem Assumptions and Coordinate Systems

As depicted in FIG. 1 above, the sensor system of the mobile mapping system can include laser 106, camera 104, and IMU 102. As shown in FIG. A camera can be modeled as a pinhole camera model with known internal parameters. External parameters between all three sensors can be calibrated. The relative pose between the camera and the laser and the relative pose between the laser and the IMU can be determined according to methods known in the art. A single coordinate system can be used for the camera and laser. In one non-limiting example, a camera coordinate system can be used and all laser points can be projected into the camera coordinate system in preliminary processing. In one non-limiting example, the IMU coordinate system can be parallel to the camera coordinate system, in which case the IMU measurements can be rotationally corrected when acquired. A coordinate system can be defined as follows.
The camera coordinate system {C} can have its origin at the optical center of the camera, in which the x-axis points to the left, the y-axis points up, and the z-axis points forward to the main axis of the camera. match,
The IMU coordinate system {I} can be originated at the IMU measurement center, in which the x-, y-, and z-axes are parallel to {C} and oriented in the same direction, and The world coordinate system {W} can be the coordinate system that coincides with {C} at the starting pose.

Landmark positions are not necessarily optimized according to some exemplary embodiments. This leaves six unknowns in the state space, thus keeping computational intensity low. The method of the present disclosure includes laser range finding to provide accurate depth information to features to ensure motion estimation accuracy, while jointly further optimizing feature depth. Only some portion of these measurements need be optimized since further optimization may result in reduced return values in certain circumstances.

According to an exemplary non-limiting embodiment, calibration of the system of the present description can be based at least in part on the mechanical geometry of the system. In particular, the rider can be calibrated relative to the motor shaft using mechanical measurements from the CAD model of the system with respect to the geometric relationship between the rider and the motor shaft. Such calibrations obtained relative to the CAD model have been shown to give high accuracy and drift without the need to perform additional calibrations.

MAP Estimation Problem The state estimation problem can be formulated as a maximum a posteriori probability (MAP) estimation problem. Define χ={x _i }(iε{1;2;...,m}) as the system state set and U={u _i }(iε{1;2;...,m}) as the control input set. , and Z={z _k }(kε{1;2; . . . ,n}) as the set of landmark measurements. Given the proposed system, Z can consist of both visual features and laser landmarks. The joint probability of the system is defined as
formula (1)

where P(x ₀ ) is the preceding state of the first system state, P(x _i |x _i-1 ), u _i ) represents the motion model, and P(z _k |x _ik ) is Represents a landmark measurement model. For each problem formulated as equation (1), there is a corresponding Bayesian network representation of that problem. MAP estimation is to maximize Eq. Under the assumption of zero mean Gaussian noise, this problem is equivalent to the least squares problem.
formula (2)

r _xi and r _zk in the above equations are the residual errors associated with the mathematical model and the landmark measurement model respectively.

A standard approach to solving Equation 2 is to combine all sensor data, eg, visual features, laser landmarks, and IMU measurements, into a large factor graph optimization problem. Conversely, the proposed data processing pipeline formulates multiple small optimization problems and solves these problems in a coarse-to-fine fashion. The optimization problem can be restated as:
Problem: Given the data from the laser, camera, and IMU, solve the problem by formulating it as equation (2) to determine the pose of {C} with respect to {W}, then use the estimated pose to Align the points and construct a map of the moving environment with {W}.

式（４）

式中の

は、｛Ｗ｝から｛Ｃ｝への回転行列であり、

は、｛Ｃ｝と｛Ｉ｝との間の平行移動ベクトルである。 IMU Prediction Subsystem IMU Mechanization This subsection describes the IMU prediction subsystem. Since the system considers {C} as the base sensor coordinate system, the IMU can be characterized with respect to {C}. As disclosed above in the subsection entitled "Assumptions and Coordinate Systems", {I} and {C} are parallel coordinate systems. ω(t) and a(t) are two 3×1 vectors representing the angular velocity and acceleration, respectively, of {C} at time t. We can denote the corresponding biases as b _ω (t) and b _a (t), and let n _ω (t) and n _a (t) be the corresponding noise. Define the vector, bias, and noise terms in {C}. Additionally, g can be written as a constant gravitational vector in {W}. The IMU measurement term follows the equation below.
Formula (3)

Formula (4)

in the ceremony

is the rotation matrix from {W} to {C},

is the translation vector between {C} and {I}.

は、回転中心（｛Ｃ｝の原点）が｛Ｉ｝の原点と異なることに起因する遠心力を表すことに注意されたい。視覚－慣性ナビゲーション方法の一部の例は、この遠心力項を排除するために運動を｛Ｉ｝においてモデル化する。深度情報が既知の視覚特徴部と未知の視覚特徴部との両方を用いる本明細書に開示する計算方法では、深度が未知の特徴部を｛Ｃ｝から｛Ｉ｝へと変換する段階は容易ではない（下記を参照されたい）。その結果、本明細書に開示するシステムは、代わりに運動の全てを｛Ｃ｝においてモデル化する。実際には、この項の効果を最大限に低減するためにカメラとＩＭＵとが互いの近くに装着される。 Section:

Note that represents the centrifugal force due to the center of rotation (origin of {C}) being different from the origin of {I}. Some examples of visual-inertial navigation methods model motion in {I} to eliminate this centrifugal force term. In the computational method disclosed herein that uses both visual features with known depth information and visual features with unknown depth information, the step of transforming features with unknown depth from {C} to {I} is straightforward. not (see below). As a result, the system disclosed herein models all motion in {C} instead. In practice, the camera and IMU are mounted close to each other to minimize the effect of this term.

The IMU bias can be a slowly varying variable. As a result, the most recently updated bias for the motion integral is used. Equation 3 is first time-integrated. Translation is then obtained from the acceleration data using the resulting orientation twice with Equation 4 for time integration.

Bias Correction IMU bias correction can be applied by feedback from either the camera or the laser (see 128, 138 in FIGS. 1 and 2, respectively). Each feedback term contains estimated piecewise motion over a short amount of time. These biases can be modeled as constant during piecewise motion. Starting with Equation 3, b _ω (t) can be calculated by comparing the estimated heading with the IMU integral. The updated b _ω (t) is used in another round of integration to recalculate the translation and compare it to the estimated translation to calculate b _a (t).

To reduce the effects of high frequency noise, a sliding window is used while maintaining a known number of biases. Non-limiting examples of the number of biases used within the sliding window can include 200 to 1000, with a recommended number of biases of 400 based on an IMU speed of 200 Hz. A non-limiting example of the number of biases in the sliding window for an IMU rate of 100 Hz is 100 to 500, with a typical value of 200 biases. A bias of average counts from a sliding window is used. In this implementation, the length of the sliding window serves as a parameter for determining the bias update rate. Alternative methods of modeling bias are known in the art, but implementations of the present disclosure are used to keep the IMU processing module as a separate and distinct module. The sliding window method can also allow dynamic reconfiguration of the system. In this manner, the IMU can be coupled with either the camera or the laser or both the camera and the laser as desired. For example, when the camera fails, the IMU bias can instead be corrected by the laser alone.

A sliding window can be used while maintaining a certain number of biases to reduce the effects of high frequency noise. In such cases, the average count bias from a sliding window can be used. In such implementations, the length of the sliding window serves as the parameter that determines the bias update rate. In some cases the bias can be modeled as a random walk and the bias updated by an optimization process. However, a non-standard implementation is preferred to keep the IMU processing in a separate module. This implementation favors dynamic reconfiguration of the system, ie the IMU can be coupled with either a camera or a laser. When the camera fails, the IMU bias can be corrected by the laser instead. As described, inter-module communication within a sequential modularized system is utilized to fix the IMU bias. This communication allows IMU bias to be corrected.

According to exemplary and non-limiting embodiments, IMU bias correction can be made by utilizing feedback from either a camera or a laser. The camera and laser each contain estimated piecewise motion over a short amount of time. When calculating the biases, the methods and systems described herein model the biases to be built during piecewise motion. Further beginning with Equation 3, the methods and systems described herein can compute b _ω (t) by comparing the estimated heading to the IMU integral. The updated b _ω (t) is used in another round of integration to recalculate the translation and compare it to the estimated translation to calculate b _a (t).

In some embodiments, the IMU output includes angular velocity with a relatively constant error over time. The resulting IMU bias is related to the fact that the IMU will always have some deviation from the ground truth. This bias can change over time. This bias is relatively constant and not of high frequency. The sliding window described above is a specified period of time during which IMU data is evaluated.

Referring to FIG. 5, a system diagram of the vision-inertial odometry subsystem is presented. The method combines vision with an IMU. Both vision and IMU constrain the optimization problem of estimating piecewise motion. At the same time, the method associates depth information with visual features. Depth can be obtained from the laser point when the feature is located within an area where laser range measurements can be obtained. Otherwise, depth can be calculated from triangulation using the estimated motion sequence so far. As a final option, the method can also use features for which neither depth is known by formulating the constraints differently. This may lead to features that may not always have laser range coverage, or triangulation due to not always being tracked long enough or located in the direction of camera motion. It fits into features that cannot be done. These three alternatives can be used alone or in combination to construct the registered point cloud. In some embodiments the information is discarded. Eigenvalues and eigenvectors can be computed and used to identify and specify impairments in the point cloud. When there is a degradation in a particular direction in the state space, we can discard the solutions in that direction in the state space.

Vision-Inertial Odometry Subsystem A block system diagram of the vision-inertial odometry subsystem is depicted in FIG. The optimization module 510 uses pose constraints 512 from the IMU prediction module 520 along with camera constraints 515 based on optical feature data with or without depth information for motion estimation 550 . A depth map registration module 545 may include registration and depth association of tracked camera features 530 with depth information obtained from laser points 540 . The depth map registration module 545 can also incorporate motion estimates 550 obtained from previous computations. The method tightly couples vision with the IMU. Each of these provides constraints 512, 515, respectively, to optimization module 510, which estimates piecewise motion 550. FIG. At the same time, the method associates depth information with visual features as part of depth map registration module 545 . Depth is obtained from the laser point when the feature is located within an area where laser range measurements can be obtained. Otherwise, depth is computed from triangulation using the estimated motion sequence so far. As a final option, the method can also use features for which neither depth is known by formulating the constraints differently. This applies to features that do not have laser range coverage or cannot be triangulated due to not being tracked long enough or located in the camera motion direction.

と表記する。

は、システム状態を表す式１内の

及びｘとは異なることに注意されたい。２つの理由、すなわち、１）深度関連付けがある程度の処理量を要し、深度関連付けをキー－フレームのみにおいて計算することによって計算強度を低減することができること、及び２）深度マップはフレームｃにおいて取得可能ではない可能性があり、位置合わせは既存の深度マップに依存することからレーザ点を位置合わせすることができない可能性があることからキー－フレームにおける特徴部を深度に関連付けることができる。｛Ｃ_c｝における正規化特徴部は、

と表記する。 Camera constrained visual-inertial odometry is a key-frame based method. A new key-frame is determined 535 when a certain number of features are missed or when the image overlap falls below a certain ratio. In this case, the upper right superscript l (lεZ ⁺ ) can indicate the last key-frame and c (cεZ ⁺ ) and c>k can indicate the current frame. As disclosed above, the method combines features of known and unknown depth. A depth-associated feature at key-frame l can be denoted as X _l =[x _l , y _l , z _l ] ^T in {C _l }. Correspondingly, features with unknown depth are instead computed using normalized coordinates

is written as

is in Equation 1, which represents the system state.

and x are different. There are two reasons: 1) the depth association takes some amount of processing and computational intensity can be reduced by computing the depth association only at the key-frames, and 2) the depth map is acquired at frame c. Features in key-frames can be related to depth since it may not be possible and laser points may not be aligned because alignment relies on existing depth maps. The normalized features in {C _c } are

is written as

及び

は、３×３の回転行列及び３×１の平行移動ベクトルとし、この場合

∈ＳＯ（３）及び

∈

が成り立ち、

及び

はＳＥ（３）変換を形成する。フレームｌとｃの間の運動関数を次式のように書くことができる。
式（５）

as well as

be a 3x3 rotation matrix and a 3x1 translation vector, where

εSO(3) and

∈

is established,

as well as

form the SE(3) transform. The motion function between frames l and c can be written as:
Formula (5)

が成り立つ。

を代入し、式５内の１番目及び２番目の行を３番目の行と組み合わせてｄ_cを排除すると以下に続く式を生じる。
式（６）

式（７）

X _c has unknown depth. Let d _c be the depth,

holds.

and combining the first and second rows in Equation 5 with the third row to eliminate d _c yields the following equation.
Formula (6)

Formula (7)

及び

のｈ番目の行である。特徴部に対して深度が取得不能である時に、ｄ_lをキー－フレームｌにおける未知深度とする。Ｘ_l及びＸ_cにそれぞれｄ_k

及びｄ_c

を代入し、式５内の３つ全ての行を組み合わせてｄ_k及びｄ_cを排除し、別の制約を生じる。
式（８）

R(h) and t(h) (hε{1,2,3}) are

as well as

is the h-th row of . Let d _l be the unknown depth at key-frame l when the depth is unavailable for the feature. d _k for X _l and X _c respectively

and d _c

and combining all three rows in Equation 5 to eliminate d _k and d _c yields another constraint.
Formula (8)

をフレームａとｂとの間の次式の４×４の変換行列として定義することができる。
式（９）

Motion Estimation The motion estimation process 510 includes 1) from features with known depths, such as those in Equations 6-7, and 2) from features with unknown depths, such as those in Equation 8. and 3) from IMU prediction 520 to solve an optimization problem that combines three sets of constraints.

can be defined as the 4×4 transformation matrix between frames a and b:
formula (9)

及び

は、対応する回転行列及び平行移動ベクトルである。更に

は、指数マッピングを通じて

に対応する３×１のベクトルとし、この場合、

∈ｓｏ（３）が成り立つ。正規化項θ／｜｜θ｜｜は、回転の方向を表し、｜｜θ｜｜は回転角である。各

は、カメラの６ＤＯＦ運動を含む

と

とのセットに対応する。

as well as

are the corresponding rotation matrices and translation vectors. Furthermore

through exponential mapping

Let a 3×1 vector corresponding to , where

εso(3) holds. The normalization term θ/||θ|| represents the direction of rotation, and ||θ|| is the angle of rotation. each

contains the 6DOF motion of the camera

When

corresponds to the set with

を用いてＩＭＵ姿勢制約を定式化することができる。

で表記する最後の２つのフレームｃ－１とｃの間の予測変換は、ＩＭＵ機械化から取得することができる。フレームｃにおける予測変換は次式のように計算される。
式（１０）

The determined motion transformation between frames 1 and c-1, i.e.

can be used to formulate the IMU pose constraint.

The prediction transform between the last two frames c-1 and c, denoted by , can be obtained from the IMU mechanization. The predictive transform at frame c is calculated as follows.
Formula (10)

及び

を

に対応する６ＤＯＦ運動とする。ＩＭＵ予測平行移動

は、方位に依存することが認められるであろう。例として、方位は、式（４）内の回転行列

を通じての重力ベクトルの投影、従って、積分される加速度を決定することができる。

は、

の関数として定式化することができ、

としてＩＭＵ予測モジュール１２２（図１及び図２）によって供給される２００Ｈｚの姿勢、並びに視覚－慣性オドメトリモジュール１２６（図１及び図２）によって供給される５０Ｈｚの姿勢は両方共に姿勢関数であることが認められるであろう。

を計算する段階は、フレームｃにおいて開始することができ、加速度は、時間に関して逆に積分することができる。

は、式（５）内にある

に対応する回転ベクトルとし、

及び

が求めるべき運動である。制約は、次式のように表すことができる。
式（１１）

式中の

は、適切にカメラ制約に対して姿勢制約をスケール調整する相対共分散行列である。

as well as

of

6 DOF motion corresponding to . IMU prediction translation

will be recognized to be orientation dependent. As an example, the orientation is the rotation matrix

The projection of the gravitational vector through , and thus the integrated acceleration, can be determined.

teeth,

can be formulated as a function of

The 200 Hz attitude supplied by the IMU prediction module 122 (FIGS. 1 and 2) as would be accepted.

can start at frame c and the acceleration can be integrated inversely with respect to time.

is in equation (5)

Let the rotation vector corresponding to

as well as

is the exercise that should be sought. Constraints can be expressed as follows.
Formula (11)

in the ceremony

is the relative covariance matrix that appropriately scales the pose constraint to the camera constraint.

及び

を含む。従って、フルスケールＭＡＰ推定は実施されず、周囲問題を解くためにのみ用いられる。ランドマーク位置は最適化されず、従って、状態空間内では６つの未知数しか用いられず、それによって計算強度が低く保たれる。従って、本方法は、特徴部に精確な深度情報を与えて運動推定精度を保証するためのレーザ距離測定を含む。その結果、一括調節によって特徴部の深度の更に別の最適化を不要とすることができる。 In embodiments of the vision-inertial odometry subsystem, the pose constraint satisfies the motion model and the camera constraint satisfies the landmark measurement model in equation (2). The optimization problem can be solved by using Newtonian gradient descent adapted to a robust fitting framework for outlier feature removal. In this problem, the state space is

as well as

including. Therefore, full-scale MAP estimation is not performed and is used only to solve the ambient problem. Landmark positions are not optimized, so only 6 unknowns are used in the state space, which keeps the computational intensity low. Accordingly, the method includes laser ranging to provide accurate depth information for features to ensure motion estimation accuracy. As a result, the global adjustment may eliminate the need for further optimization of feature depth.

A depth-associated depth map registration module 545 aligns the laser point with the depth map using the previously estimated motion. A laser spot 540 inside the camera field of view is held for a certain amount of time. The depth map is downsampled to keep constant density and stored in a 2D KD-tree for fast indexing. Within the KD-tree, all laser points are projected onto the unit sphere around the camera center. A point is represented by two angular coordinates. Features can be projected onto this sphere when associating depth with features. Three laser points are determined on this sphere for each feature. In this case, the validity of these points can be by calculating the distance between these three points in Cartesian space. When the distance is greater than the threshold, the point is likely from a different object, such as a wall and an object in front of it, and the validation fails. Finally, the depth is interpolated from the three points assuming a local planar patch in Cartesian space.

Features that do not have laser range coverage can be triangulated using the image sequence in which they are tracked when they have not been tracked over a certain distance and located in the direction of camera motion. . Such a procedure can update the depth based on the Bayesian probability mode at each frame.

Scan Alignment Subsystem This subsystem further refines the motion estimation from the previous module by laser scan alignment. FIG. 6 depicts a block diagram of the scan registration subsystem. The subsystem receives laser points 540 that lie within the local point cloud and aligns these points using supplied odometry estimates 550 . Geometric features are then detected 640 from the point cloud and matched against the map. Scan matching, like many methods known in the art, minimizes the feature-to-map distance. However, odometry estimation 550 also provides pose constraint 612 in optimization 610 . The optimization involves processing pose constraints on the determined feature correspondence 615 , which is further processed using the laser constraints 617 to produce the device pose 650 . This pose 650 is processed by map alignment processing 655 to facilitate determining feature correspondences 615 . This implementation uses a voxel representation of the map. Further, the implementation can be dynamically configured to run in parallel on one to multiple CPU threads.

Upon receiving a laser constrained laser scan, the method first aligns the points from scan 620 into a common coordinate system. We can use m (mεZ ⁺ ) to denote the scan number. It should be appreciated that a camera coordinate system can be used for both the camera and the laser. A scan m can be associated with the camera coordinate system at the beginning of the scan denoted {C _m }. To locally align 620 the laser point 540, the odometry estimates 550 from the visual-inertial odometry can be taken as key points, and IMU measurements can be used to interpolate between the key points. .

Let P _m be the local registration point cloud from scan m. Two sets of geometric features from P _m : one on the sharp edge, ie the edge point denoted ε _m , and the other on the local plane, ie H _m and It is possible to extract plane points to be represented. This extraction is by calculation of the curvature in the local scan. Points that have already selected neighboring points, such as points that lie on the boundary of the occluded region and points that have local planes nearly parallel to the laser beam, are not taken. These points are likely to contain large amounts of noise or change position over time as the sensor moves.

The geometric features are then matched against the constructed current map. Let Q m- ₁ be the map point cloud after processing the last scan, and define Q _m-1 by {W}. The points in Q _{m -1} are separated into two sets containing edge points and plane points respectively. Voxels can be used to store maps cropped at a certain distance around the sensor. For each voxel, two 3D KD-trees can be constructed, one for edge points and one for plane points. By using KD-trees for individual voxels, point retrieval is reduced because, given a query point, only the particular KD-tree associated with a single voxel needs to be retrieved (see below). Speed up.

式中の_Xmは、ε_m又はＨ_m内の点であり、θ_m（θ_m∈ｓｏ（３））及びｔ_m（ｔ_m∈

）は、｛Ｗ｝における｛Ｃ_m｝の６ＤＯＦ姿勢を示す。 When aligning the scans, ε _m and H _m are first projected into {W} using the best available motion guess, then for each point in ε _m and H _m the nearest point is found from the corresponding set on the map. To verify the geometric distribution of the point set, the associated eigenvalues and eigenvectors can be examined. In particular, one large and two small eigenvalues indicate edge line segments, and two large and one small eigenvalues indicate local planar patches. An expression is formulated for the distance from a point to the corresponding point set when the match is valid.
Formula (12)

where _Xm is a point in _εm or _Hm , _θm ( _θm ∈ so(3)) and _tm ( _tm ∈

) indicates the 6DOF pose of {C _m } in {W}.

は、オドメトリ推定によって与えられる｛Ｃ_m-1｝から｛Ｃ_m｝への姿勢変換とする。式（１０）と同様に、｛Ｗ｝における｛Ｃ_m｝の予測姿勢変換は次式で与えられる。
式（１３）

Motion estimation scan matching is formulated into an optimization problem 610 that minimizes the overall distance described by equation (12). The optimization further includes pose constraints 612 from previous motions. Let T _m-1 be the 4×4 transformation matrix for the pose of {Cm-1} in {W}, where T _m-1 is generated by processing the last scan.

Let be the pose transformation from {C _m-1 } to {C _m } given by the odometry estimate. Similar to equation (10), the predicted pose transformation of {C _m } in {W} is given by
Formula (13)

及び

は、

に対応する６ＤＯＦ姿勢とし、

は、相対共分散行列とする。制約は次式で与えられる。
式（１４）

as well as

teeth,

6 DOF posture corresponding to

is the relative covariance matrix. The constraint is given by
Formula (14)

及び

を用いることができる。加速度の積分は、方位（式（１１）における

と同じ）に依存することから

は、θ_mの関数である。ＩＭＵ姿勢制約は次式で与えられる。
式（１５）

式中の

は、対応する相対共分散行列である。最適化問題では、式（１４）及び式（１５）は、１つの制約セットへと線形結合される。この線形結合は、視覚－慣性オドメトリの作動モードによって決定される。ロバストな当て嵌めフレームワークに適応されたニュートン勾配降下法によって解かれる最適化問題は、θ_m及びｔ_mを精緻化する。 Equation (14) implies that the previous motion is from the visual-inertial odometry assuming the camera is functional. Otherwise, the constraint is from IMU prediction. To represent the same term by IMU mechanization

as well as

can be used. The integral of the acceleration is the orientation (

) from depending on

is a function of θ _m . The IMU pose constraint is given by the following equation.
Formula (15)

in the ceremony

is the corresponding relative covariance matrix. For the optimization problem, equations (14) and (15) are linearly combined into one set of constraints. This linear combination is determined by the mode of operation of the vision-inertial odometry. An optimization problem solved by Newtonian gradient descent adapted to a robust fitting framework refines θ _m and t _m .

及び

を所与として、マップ上でセンサと一緒に移動する対応するＳ_m-1が存在する。センサがマップの境界に近づくと、境界の反対側にあるボクセル７２５がマップ境界７３０を拡張するように移動される。移動されたボクセル内の点は消去され、マップの切り取りが生じる。 Maps within voxels Points on maps are kept within voxels. A two-level voxel implementation is illustrated in FIGS. 7A and 7B. Μm _-1 represents the set of voxels 702, 704 on the first level map 700 after processing the last scan. Voxels 704 surrounding sensor 706 form a subset Μ _m- 1 and are denoted as S _m-1 . 6DOF sensor attitude

as well as

, there is a corresponding S _m-1 that moves with the sensor on the map. As the sensor approaches the boundary of the map, the voxels 725 on the opposite side of the boundary are moved to extend the map boundary 730 . Points within the moved voxels are erased, resulting in map clipping.

と表記するボクセルセットによって形成される。走査を整合させる前に、最良の運動推測を用いてε_m及びＨ_m内の点がマップ上に投影され、

（

）内に埋められる。ε_m及びＨ_mからの点で占有されたボクセル７０８は抽出されてＱ_m-1が形成され、走査整合のための３Ｄ ＫＤ木内に格納される。ボクセル７１０は、ε_m及びＨ_mからの点で占有されない。走査整合の完了時に、走査は、マップをなすボクセル７０８内に融合される。その後、マップ点は一定の密度を維持するようにダウンサイズされる。第１のレベルのマップ７００の各ボクセルが第２のレベルのマップ７５０の部分ボクセルよりも大きい空間容積に対応することは認識することができる。従って、第１のレベルのマップ７００の各ボクセルは、第２のレベルのマップ７５０内の複数の部分ボクセルを含み、第２のレベルのマップ７５０内の複数の部分ボクセル上にマッピングすることができる。 As illustrated in FIG. 7B, each voxel j (jεεS _m−1 ) in the second level map 750 has a smaller magnitude than that in the first level map 700.

is formed by a set of voxels denoted as . Before matching the scans, the points in ε _m and H _m are projected onto the map using the best motion estimate,

(

). Voxels 708 occupied by points from ε _m and H _m are extracted to form Q _{m -1} and stored in a 3D KD-tree for scan matching. Voxel 710 is unoccupied by points from ε _m and H _m . Upon completion of scan registration, the scans are fused into the voxels 708 that make up the map. The map points are then downsized to maintain a constant density. It can be appreciated that each voxel of the first level map 700 corresponds to a larger spatial volume than a partial voxel of the second level map 750 . Thus, each voxel in the first level map 700 includes multiple partial voxels in the second level map 750 and can be mapped onto multiple partial voxels in the second level map 750 . .

（

）に対応するボクセルは、走査整合のためのセンサの周囲のマップを取り出すために用いられる。マップは、センサがマップ境界に近づく時にのみ切り取られる。従って、センサがマップの内部を進む時に切り取りは必要ない。別の注目点は、一方が縁点に対し、もう一方が平面点に対する２つのＫＤ木がＳ_m-1内の各個別ボクセルに対して用いられる点である。上述したように、そのようなデータ構造は点検索を高速化することができる。この方式で、

（

）内の各個別ボクセルに対して２つのＫＤ木を用いるのとは対照的に複数のＫＤ木の間の検索が回避される。

（

）内の各個別ボクセルに対して２つのＫＤ木を用いる場合は、ＫＤ木の構築及び保守のためにより多くのリソースを必要とする。 As described above with respect to FIGS. 7A and 7B, two voxel levels (first level map 700 and second level map 750) are used to store map information. The voxels corresponding to M _{m -1} are used to maintain the first level map 700 and are used in the second level map 750

(

) are used to retrieve a map around the sensor for scan alignment. The map is clipped only when the sensor approaches the map boundary. Therefore, no clipping is required when the sensor travels inside the map. Another point to note is that two KD-trees are used for each individual voxel in S _{m -1} , one for edge points and one for plane points. As mentioned above, such a data structure can speed up point searches. In this method,

(

) avoids searching between multiple KD-trees as opposed to using two KD-trees for each individual voxel in .

(

) requires more resources for building and maintaining the KD-trees.

（

）がマップを綿密に検索するのに役立つことに起因する。これらのボクセルなしでは、Ｑ_m-1内により多くの点が含まれてＫＤ木内に組み込まれる。また、各ボクセルに対してＫＤ木を用いることにより、処理時間は、Μ_m-1内の全てのボクセルに対してＫＤ木を用いる場合と比較して若干短縮される。 Table 1 compares the CPU processing time using different voxel and KD-tree configurations. Times are averaged from multiple data sets collected from different types of environments covering closed and open structured areas and vegetated areas. It can be seen that the KD-tree construction and query results in about twice the processing time when using only one voxel level Μm _-1 . This is the second voxel level

(

) helps to search the map closely. Without these voxels, more points in Q _m-1 are included and incorporated into the KD-tree. Also, by using KD-trees for each voxel, the processing time is slightly reduced compared to using KD-trees for all voxels within .mu.m _-1 .

(Table 1)
Table 1. Comparison of average CPU processing time in KD tree operation

Parallel scan matching involves building a KD-tree and iteratively finding feature matches. This process takes time and occupies major computations in the system. A single CPU thread cannot guarantee the desired update frequency, but a multi-threaded implementation can accommodate complex processing problems. FIG. 8A illustrates the case where two matcher programs 812, 815 are executed in parallel. Upon receipt of the scan, manager program 810 arranges the scan to match the latest available map. In one example consisting of a collective environment with multiple structures and multiple visual features, registration is slow and may not be completed before the arrival of the next scan. The two matchers 812 and 815 are called alternately. In one matcher 812, Pm _813a , Pm _-2 813b, and a further _Pmk (for k=even) 813n are respectively Qm _-2 813a, Qm _-4 813a, and a further Q _mk (for k=even) matched with 813n. Similarly, in the second matcher 815, Pm ₊₁ 816a, Pm _-1 816b, and yet another _Pmk (for k=odd) 816n are coupled to Qm _-1 816a, Qm _-3 816a, and with yet another Q _mk (for k=odd) 816n. The use of this alternate processing can give twice the amount of time for processing. Alternatives consisting of clean environments with few structures and visual features are computationally light. In such an example (FIG. 8B), only a single matcher 820 may be invoked. Since no alternating processing is required, P _m , P _m-1 , are sequentially matched with Q _m-1 , Q _m-2 , . . . (see 827a, 827b, 827n) respectively . Although typically only two threads may be required, this implementation can be configured to use up to four threads.

Transform Integration The final motion estimation is the integration of the outputs from the three modules depicted in FIG. The 5 Hz scan-matched output produces the most accurate maps, while the 50 Hz visual-inertial odometry output and 200 Hz IMU prediction are integrated for high frequency motion estimation.

About Robustness The robustness of the system is determined by its ability to cope with sensor degradation. Always assume that the IMU is functioning reliably as the backbone in the system. Cameras can also be affected by dramatic lighting changes and fail in dark/smooth environments or when significant motion blur is present (causing loss of visual feature tracking). Lasers cannot cope with environments without structure, eg scenes dominated by a single plane. Alternatively, data sparseness due to extreme motion can cause laser data degradation. Such extreme exercise includes highly dynamic exercise. As used herein, "highly dynamic motion" means substantially abrupt rotational or linear displacements of the system or continuous rotational or translational motions having substantially large magnitudes. The self-motion determination system of the present disclosure can operate in the presence of highly dynamic motion and in dark, uneven and structure-free environments. In some exemplary embodiments, systems of the present invention can operate while undergoing rotational angular velocities as high as 360 degrees per second. In other embodiments, the system can operate at linear velocities up to and including 100 kph. In addition, these motions can be combined angular and linear motions.

Both the vision-inertial odometry module and the scan matching module formulate and solve the optimization problem according to equation (2). When failure occurs, it corresponds to a degraded optimization problem, ie some directional constraints of the problem are in bad condition and noise dominates in determining the solution. _In one non-limiting method, eigenvalues denoted λ1, λ2, . . . , _λ6 and eigenvectors denoted v1, _v2 , . can. Since the state space of the sensor contains 6 DOFs (6 degrees of freedom), there are 6 eigenvalues/eigenvectors. Without loss of generality, we can sort v ₁ , v ₂ , . . . , v ₆ in descending order. Each eigenvalue represents how well the solution is in the direction of its corresponding eigenvector. By comparing the eigenvalues to a threshold, we can separate the well-conditioned directions from the degraded directions in the state space. Let h (h=0; 1, . . . , 6) be the number of well-conditioned directions. Two matrices can be defined as follows.
Equation (16)

When solving an optimization problem, an initial guess can be used to initiate non-linear iterations. Using the continuous pipeline depicted in FIG. 2, the IMU prediction provides the initial guesses for the vision-inertial odometry, and the output of the vision-inertial odometry is taken as the initial guess for scan matching. For two additional modules (visual-inertial odometry and scan matching modules), let x be the solution and Δx the update of x in the non-linear iteration, where Δx solves the linearized system equations. calculated by During the optimization process, instead of updating x in all directions, x can be updated only in well-conditioned directions, keeping initial guesses in the degraded direction.
Equation (17)

は、ゼロ行列になり、先行モジュールの出力が保たれる。 In equation (17), the system solves for motion in order from coarse to fine, starting with the IMU prediction, and two additional modules determine/refine the motion as much as possible. Refinement can include all 6 DOFs when the problem is in good shape. Alternatively, refinement can include from 0 DOF to 5 DOF when the problem is only partially good. When the problem is completely degraded,

becomes a matrix of zeros, preserving the output of the preceding module.

及び

は、視覚－慣性オドメトリモジュールからの固有ベクトルを含む行列を表し、

は、サブシステム内の十分に条件付けされた方向を表し、

は、劣化方向を表す。組み合わせ制約は次式で与えられる。
式（１８）

Returning to the pose constraints described in equations (14) and (15), it will be appreciated that these two equations are linearly combined in the scan matching problem. As defined in equation (16),

as well as

represents the matrix containing the eigenvectors from the visual-inertial odometry module,

represents a well-conditioned direction in the subsystem, and

represents the direction of deterioration. The combinatorial constraint is given by the following equation.
Equation (18)

が成り立ち、式（１８）は、式（１４）にあるもののような視覚－慣性オドメトリからの姿勢制約で構成される。しかし、カメラデータが完全に劣化した時に、

はゼロ行列であり、式（１８）は、式（１５）によるＩＭＵ予測からの姿勢制約で構成される。 During normal times when the camera is functioning,

and Eq. (18) is constructed with pose constraints from the visual-inertial odometry as in Eq. (14). However, when the camera data is completely degraded,

is the zero matrix and equation (18) is constructed with the pose constraint from the IMU prediction according to equation (15).

Case Study of Camera Degradation As depicted in FIG. 9A, the IMU prediction 122 is well conditioned in the visual-inertial odometry problem when the visual features are poorly obtainable for the visual-inertial odometry problem. Bypass 924 the visual-inertial odometry module 126, shown in phantom, fully or partially depending on the number of directions. The scan alignment module 132 can then locally align the laser spots for scan alignment. The IMU prediction that bypasses is subject to drift. Laser feedback 138 compensates for camera feedback 128 and corrects for IMU velocity drift and bias only in directions where it is not available. In this case, camera feedback has a high priority when the camera data is not degraded, because higher frequencies make camera feedback more appropriate. Laser feedback is not used when sufficient visual features are found.

Case Study of Laser Degradation As shown in FIG. 9B, when the environmental structure is insufficient for scan matching 132 to refine the motion estimation, the visual-inertial odometry module 126 output maps laser points onto the map. Bypass 930 the scan registration module completely or partially as noted by the dashed lines to align. When there are well-conditioned directions in the scan registration problem, laser feedback includes motion estimates that are refined in these directions. Otherwise laser feedback is empty 138 .

Camera and Laser Degradation Case Study In a more complex example, both the camera and the laser degrade at least to some extent. FIG. 10 depicts such an example. A vertical bar with 6 rows represents a 6DOF pose where each row is the DOF (degree of freedom) corresponding to the eigenvector in Eq. (16). In this example, vision-inertial odometry and scan matching each update the 3DOF of motion and leave motion unchanged in the other 3DOF. IMU predictions 1022 a - 1022 f may include initial IMU predictions 1002 . The visual-inertial odometry updates 1004 some 3DOF (1026c, 1026e, 1026f) to produce refined predictions 1026a-1026f. Scan-matching updates 1006 some 3DOF (1032b, 1032d, 1032f) to produce further refined predictions 1032a-1032f. Camera feedback 128 includes camera updates 1028a-1028f and laser feedback 138 includes laser updates 1038a-1038f, respectively. Referring to FIG. 10, cells without shading (1028a, 1028b, 1028d, 1038a, 1038c, 1038e) do not contain any updated information from their respective modules. All updates 1080 a - 1080 f to the IMU prediction module are a combination of updates 1028 a - 1028 f from camera feedback 128 and updates 1038 a - 1038 f from laser feedback 138 . Camera updates (e.g. 1028f) have higher priority than laser updates (e.g. 1038f) in one or more of the degrees of freedom in which feedback can be obtained from both the camera (e.g. 1028f) and the laser (e.g. 1038f). can have

及び

をフレームｃ－１とｃの間に視覚－慣性オドメトリによって推定された６ＤＯＦ運動とし、この場合、

及び

が成り立つ。

及び

は、時間内挿後に走査整合によって推定された対応する項とする。

及び

は、視覚－慣性オドメトリモジュールからの固有ベクトルを含む式（１６）に定義された行列とすることができ、この場合、

は十分に条件付けされた方向を表し、

は劣化方向を表す。

及び

を走査整合モジュールからの同じ行列とする。次式は、組み合わせフィードバックｆ_Cを計算する。
式（１９）

式中のｆ_V及びｆ_sは、カメラフィードバック及びレーザフィードバックを表し、以下に続く式で与えられる。
式（２０）

式（２１）

However, in practice, the visual-inertial odometry module and the scan matching module may run at different frequencies, and each may have its own direction of degradation. IMU messages can be used to interpolate between poses from the scan-matched output. In this manner, piecewise motion can be produced that is time aligned with the visual-inertial odometry output.

as well as

be the 6 DOF motion estimated by visual-inertial odometry between frames c−1 and c, where

as well as

holds.

as well as

Let be the corresponding term estimated by scan matching after temporal interpolation.

as well as

can be the matrix defined in equation (16) containing the eigenvectors from the visual-inertial odometry module, where

represents a well-conditioned direction, and

indicates the direction of deterioration.

as well as

Let be the same matrix from the scan matching module. The following formula computes the combinatorial feedback f _C .
Equation (19)

f _V and f _s in the equations represent camera feedback and laser feedback and are given by the equations that follow.
formula (20)

Equation (21)

及び

は、次式のようにｆ_Cのヌル空間に投影することができる。
式（２２）

Note that f _C only contains motion sought within a subspace of the state space. Motion from IMU prediction, i.e.

as well as

can be projected onto the null space of f _C as
Equation (22)

及び

を用いることができる。方位

は、ｂ_ω（ｔ）にしか関連しないが、平行移動

は、ｂ_ω（ｔ）とｂ_a（ｔ）の両方に依存する。バイアスは、次式を解くことによって計算することができる。
式（２３）

To represent the IMU predicted motion formulated as a function of b _ω (t) and b _a (t) by combining equations (3) and (4)

as well as

can be used. Azimuth

is only related to b _ω (t), but the translation

depends on both b _ω (t) and b _a (t). Bias can be calculated by solving the following equation:
Equation (23)

及び

はゼロ行列である。相応に、ｂ_ω（ｔ）及びｂ_a（ｔ）は、ｆ_Cから計算される。劣化した時に、ＩＭＵ予測運動

及び

は、運動を求めることができない（例えば、図１０の組み合わせフィードバックの白色の行１０８０ａ）方向に用いられる。結果は、先に計算されたバイアスがこれらの方向に保たれることである。 When the system is functioning normally, f _C spans the state space and in Eq. (22)

as well as

is a zero matrix. Correspondingly, b _ω (t) and b _a (t) are calculated from f _C . IMU predictive motion when degraded

as well as

is used in directions where motion cannot be determined (eg, white row 1080a in the combinatorial feedback of FIG. 10). The result is that the previously calculated biases are kept in these directions.

experiment
Testing with Scanner The odometry and mapping sidewall system was validated against two sensor sets. In the first sensor set, a Velodyne Lidar™ HDL-32E laser scanner was attached to a UI-1220SE monochromatic camera and an Xsens® MTi-30 IMU. This laser scanner has a horizontal FOV of 360°, a vertical FOV of 40°, and is sensitive to 0.7 million points/second at a rotation rate of 5 Hz. The camera was configured for a resolution of 752 pixels by 480 pixels, a horizontal FOV of 76° and a frame rate of 50 Hz. The IMU frequency was set to 200 Hz. For the second sensor set, a Velodyne Lidar™ VLP-16 laser scanner was attached to the same camera and IMU. This laser scanner has a horizontal FOV of 360°, a vertical FOV of 30°, and is sensitive to 0.3 million points/second at a rotation speed of 5 Hz. Each sensor set was attached to a data collection vehicle that ran on streets and off-roads respectively.

Up to 300 Harris corners were tracked for both sensor sets. In order to distribute the visual features evenly, the image was separated into 5×6 identical sub-regions, each providing up to 10 features. To maintain the number of features in each subregion, new features were generated when tracking missed features.

This software runs Linux® running Robotic Operating System (ROS) on a laptop computer with a 2.6 GHz quad-core processor (2 threads on each core for a total of 8 threads) and an integrated GPU. work in the system. Two versions of the software were implemented with visual feature tracking running on GPU and CPU respectively. Processing times are shown in Table 2. The time spent by the visual-inertial odometry (126 in FIG. 2) does not vary significantly with respect to environment or sensor configuration. On the GPU version, the visual-inertial odometry consumes about 25% of the CPU threads running at 50 Hz. In the CPU version, the visual-inertial odometry occupies about 75% of the threads. The first set of sensors yields slightly longer processing times than the second set of sensors. This is due to the scanner sensing more points and the program needing more time to maintain the depth map and relate depth to visual features.

Scan matching (132 in FIG. 2) consumes a longer and variable processing time related to environment and sensor configuration. For the first sensor set, scan matching occupies approximately 75% of the threads running at 5 Hz when operating in a structured environment. However, within a vegetative environment, where more points are aligned on the map, the program typically consumes about 135% of threads. With the second set of sensors, the scanner senses fewer points. The scan matching module 132 uses approximately 50-95% of the threads depending on the environment. The time spent by IMU prediction (132 in FIG. 2) is negligible compared to the other two modules.

Accuracy Test A test was performed to evaluate the accuracy of the proposed system. These tests used the first set of sensors. The sensor was mounted on an off-road vehicle driving around the university campus. A campus map was constructed after a 2.7 km run within 16 minutes. The average velocity over the test was 2.8 m/s.

(Table 2)
Table 2. Average CPU Processing Time Using First and Second Sensor Sets

The estimated trajectories and aligned laser points were aligned on satellite imagery to assess motion estimation drift over the test. In this case, the laser spot on the ground was manually removed. By matching the trajectory with the street on the satellite imagery, an upper bound on the horizontal error was found to be <1:0 m. A vertical error of <2:0 m was also found by comparing buildings on the same floor. This gives a total relative position drift that is <0:09% of the distance traveled at the end. It will be appreciated that accuracy could not be guaranteed for the measurements, so only an upper bound on position drift was calculated.

Additionally, a more comprehensive test was performed with the same sensor mounted on a passenger car. A passenger car was driven over a 9.3 km journey on a structured road. The route passed through vegetated environments, bridges, hills, and busy streets, and finally returned to the starting position. Elevation varied over 70m along the route. Vehicle speed during the test was between 9 and 18 m/s, except for waiting for traffic lights. It was found that buildings seen at both the start and end of the route were registered twice. Two registrations occur due to motion estimation drift over the path length. Thus, the first registration corresponds to the vehicle at the beginning of the test and the second registration corresponds to the vehicle at the end of the test. Distance measurements were <20 m, resulting in a relative position error of <0:22% of traveled distance at termination.

Each module in the system contributes to the overall accuracy. FIG. 11 depicts estimated trajectories in an accuracy test. A first trajectory plot 1102 of the trajectory of the mobile sensor generated by the vision-inertial odometry system is with the IMU module 122 and the vision-inertial odometry module 126 (see FIG. 2). The configuration used in the first trajectory plot 1102 is similar to that depicted in FIG. 9B. A second trajectory plot 1104 is based on forwarding the IMU predictions from the IMU module 122 directly to the scan matching module 132 bypassing the vision-inertial odometry. This configuration is similar to that depicted in FIG. 9A. A third trajectory plot 1108 of the complete pipeline is based on the combination of the IMU module 122, the visual-inertial odometry module 126, and the scan matching module 132 (see FIG. 2), with minimal drift. do not have. The position errors for the first two configurations, trajectory plots 1102 and 1104, are approximately four times and two times greater, respectively.

The first trajectory plot 1102 and the second trajectory plot 1104 can be viewed as predicted system performance when individual sensor degradation is encountered. If the scan match degrades (see FIG. 9B), the system degrades to the mode indicated by the first trajectory plot 1102 . When vision deteriorates (see FIG. 9A), the system degrades to the mode indicated by second trajectory plot 1104 . If none of the sensors have degraded (see FIG. 2), the system incorporates all of the optimization functions and produces trajectory plot 1108 . In another example, the system may take the IMU prediction as an initial guess, but run at the laser frequency (5Hz). The system generates a fourth trajectory plot 1106. FIG. The resulting accuracy is only marginally better than the second trajectory plot 1104 using the IMU coupled directly to the laser past the visual-inertial odometry. This result shows that the functionality of the camera is underutilized when solving the problem with all the constraints superimposed.

Another accuracy test of the system involved a mobile sensor traveling at the original 1x velocity and an accelerated 2x velocity. Every other frame of data was omitted for all three sensors when traveling at 2x velocity, resulting in more extreme motion throughout the test. The results are listed in Table 3. Three configurations were evaluated at each speed. At 2× speed, the accuracy of the visual-inertial odometry configuration and the IMU+scan matching configuration compared to the accuracy at 1× speed decreases at a significant rate of 0.54% and 0.38% of distance traveled. However, the full pipeline reduces accuracy by a very small percentage of only 0.04%. This result shows that the camera and laser compensate each other to maintain overall accuracy. This is especially true when the exercise is extreme.

Table 3. Relative position error as a percentage of distance traveled

(Table 3)
(The error at 1x velocity corresponds to the trajectory in Figure 11)

Referring to FIG. 12, an exemplary, non-limiting embodiment of bi-directional information flow is shown. As illustrated, three modules solve the problem step by step from coarse to fine, including an IMU prediction module, a visual-inertial odometry module, and a scan matching refinement module. The data processing flow is left-to-right and passes through each of the three modules, whereas the feedback flow is right-to-left and corrects for IMU bias.

Referring to Figures 13a and 13b, an exemplary, non-limiting embodiment of a dynamically reconfigurable system is shown. As illustrated in FIG. 13a, the IMU prediction (partially) bypasses the visual-inertial odometry module to align the laser spot locally when the visual features are insufficient for visual-inertial odometry. In contrast, as illustrated in FIG. 13b, the visual-inertial odometry output (partially) bypasses the scan-match refinement module to map laser points onto the map when the environmental structure is insufficient for scan-match. Align.

Referring to FIG. 14, an exemplary, non-limiting embodiment of priority feedback for IMU bias correction is shown. As illustrated, vertical bars represent 6 DOF poses and each row is one DOF. When degraded, starting with the left IMU prediction where all 6 rows labeled 'IMU', the visual-inertial odometry makes an update where the row is labeled 'camera' at 3DOF, followed by Scan Match makes an update that will cause the row to be labeled "LASER" in another 3 DOF. Camera feedback and laser feedback are combined like the vertical bar on the left. Camera feedback has a higher priority and the "laser" row from laser feedback is filled only if the "camera" row from camera feedback does not exist.

（（図１５ｂ）の全てのボクセル

）によって形成される。走査整合の前に、レーザ走査は、最良の運動推測を用いてマップ上に投影することができる。走査からの点によって占有された

（

）内のボクセルをクロスハッチングで標記している。続いて走査整合のためのクロスハッチング表示ボクセル内のマップ点が抽出されて３Ｄ ＫＤ木内に格納される。 Referring to Figures 15a and 15b, an exemplary and non-limiting embodiment of a two-layer voxel representation of a map is shown. A voxel M _m-1 on the map (all voxels in FIG. 15a) and a voxel S _m-1 surrounding the sensor (dotted fill voxels) are illustrated. S _m-1 is a subset of M _m-1 . When the sensor approaches the map boundary, the voxels on the opposite side of the boundary (bottom row) are moved to extend the map boundary. Points within the moved voxels are cleared and the map is clipped. As illustrated in FIG. 15b, each voxel j (jεS _m−1 ) (dot-filled representation voxel in FIG. 15a) is a set of voxels with small magnitudes

(All voxels in (Fig. 15b)

). Prior to scan registration, the laser scan can be projected onto the map using the best motion estimate. occupied by points from the scan

(

) are marked with cross hatching. The map points in the cross-hatched voxels for scan matching are then extracted and stored in the 3D KD-tree.

Referring to FIG. 16, an exemplary, non-limiting embodiment of multi-threaded processing of scan matching is shown. Illustratively, a manager program calls multiple matcher programs running on separate CPU threads to match scans to the latest available maps. FIG. 16a shows the two-thread case. Scans P _m , P _m-1 , . . . are aligned with Q _m , Q _m-1 , . In comparison, FIG. 16b shows the one-thread case, where P _m , P _m-1 , . . . are aligned with Q _m , Q _m-1 , . This implementation is dynamically configurable with up to four threads.

In embodiments, a real-time SLAM system may be used in combination with a real-time navigation system. In embodiments, the SLAM system may be used in combination with obstacle detection systems such as lidar or radar-based obstacle detection systems, vision-based obstacle detection systems, thermal-based systems, or the like. This obstacle detection may include detecting activity obstacles such as people, pets, or the like, such as by motion detection, heat detection, electric or magnetic field detection, or other mechanisms.

In embodiments, near-range features, such as objects that provide near-field reflections to the SLAM system, and far-range features, such as items that can be scanned through spaces between the near-range features or apertures within the near-range features. A point cloud established by scanning features of the environment to show spatial mapping, which can include spatial mapping, can be displayed on a screen-like surface forming part of SLAM. . For example, because different outdoor elements can be scanned through windows or doors at various points in the room, items in adjacent corridors can be scanned through such spaces or openings as the mapper moves around the room. In this case, the resulting point cloud may contain comprehensive mapping data of the immediate environment and partial mapping of far-field elements outside this environment. Thus, the SLAM system can include mapping of the space through the "pile fence" by identifying distant objects through spaces or openings (ie gaps in the fence) that are within the near range. Far-range data, for example, allows the mapper to move from an exhaustively mapped space (with well-understood orientation and position due to the density of the point cloud) to a only sparsely mapped space (such as a new room). A mapper that maintains a consistent location estimate as it moves can be used to help the system orient the SLAM as it moves from space to space. As the user moves from a near-field location to a far-field location, the SLAM system uses the relative density or sparsity of the point cloud to guide the mapper, e.g., through a user interface that forms part of SLAM. For example, the mapper can be directed from another space to a far portion that cannot be seen through the aperture.

In embodiments, the point cloud map from the SLAM system can be combined with mapping from other inputs such as cameras and sensors. For example, in the flying or spacecraft example, an airplane, drone, or other flying mobile platform can be used as reference data for the SLAM system (e.g., linking the point cloud resulting from the scan to a GPS reference location), or Possibly already equipped with other range finding and geolocation equipment capable of obtaining reference data from the scan for displaying further scan data, e.g. as an overlay on output from other systems. There is For example, conventional camera output can be shown together with point cloud data as an overlay, or vice versa.

In embodiments, the SLAM system may provide a point cloud containing data indicative of the return signal strength from each feature. This reflected strength can be used to help determine the effectiveness of the signal to the system, determine how features relate to each other, determine surface IR reflectance, and the like. can be done. In embodiments, this reflection intensity can be used as a basis for driving the point cloud display into the map. For example, SLAM systems can introduce some degree of color contrast (either automatically or under user control) to highlight the reflectance of the signal for a given feature, material, structure, or the like. ). Additionally, the SLAM system can be combined with other systems to extend the color and reflection coefficient information. In embodiments, one or more of the points in the point cloud are displayed using colors corresponding to parameters of the acquired data such as intensity parameters, density parameters, time parameters, and geospatial location parameters. can be done. Coloring of the point cloud may assist the user in understanding and analyzing the elements or features of the environment within which the SLAM system operates and/or the features of the acquisition process of the point cloud itself. can. For example, with a density parameter that indicates the number of points acquired within a geospatial area, a color representing an area where many data points Another color that indicates the presence of an image can be determined. Colors can also indicate, for example, the time it takes to transition through a series of colors as a scan is performed, resulting in a clear indication of the path to perform the SLAM scan. Coloring may be used to distinguish between various features (e.g., compare furniture in a space with walls), to provide artistic effects, to highlight areas of interest (e.g., to draw the attention of a scanning observer). It can also be done for display purposes, such as highlighting the relevant equipment for which you are asking.

In embodiments, the SLAM system can identify "shadows" (areas where the point cloud has relatively few data points from the scan) and highlight areas that require additional scanning. (eg, through a user interface). For example, such an area may blink or be drawn in a particular color within the visual interface of the SLAM system displaying the point cloud until the shadowed area has been sufficiently "painted" or covered by the laser scanning. Such an interface may include any indicator (visual, text-based, audio-based, or the like) to the user that highlights areas within the field of view that have not yet been scanned; Such indicators can be used to get the user's attention either directly or through an external device (such as the user's mobile phone). According to another exemplary and non-limiting embodiment, the system of the present invention uses SLAM's previously constructed point clouds and maps, etc., for comparison with the current scan to identify unscanned regions. External data of data stored in can be referenced.

In embodiments, the methods and systems disclosed herein use maps composed of point-cloud data indicating environmental features to determine precise position and orientation information, or based on reflected signals from laser scanning. includes a SLAM system that provides real-time positioning output at the operating point without requiring processing or computation by an external system to generate . In embodiments, the methods and systems disclosed herein may also include a SLAM system that provides real-time positioning information without requiring post-processing of data collected from laser scanning.

In embodiments, the SLAM system may be used in a variety of external applications such as vehicle navigation systems (such as for unmanned air vehicles, drones, mobile robots, unmanned underwater vehicles, autonomous vehicles, semi-autonomous vehicles, and many others). Can be integrated with the system. In embodiments, the SLAM system can be used to allow a vehicle to navigate within its own environment without relying on external systems such as GPS.

In embodiments, the SLAM system may determine a confidence level for the current estimate of position, orientation, or the like. The confidence level may be based on the density of points obtainable in the scan, the orthogonality of points obtainable in the scan, the geometry of the environment, or other factors, or a combination thereof. Confidence levels shall be attributed to position and orientation estimates at each point along the route of the scan so that segments of the scan may be referred to as low-confidence segments, high-confidence segments, or the like. can be done. Low-confidence segments may be highlighted for additional scanning, use of other techniques (such as making adjustments based on external data), or the like. For example, if the scan is done in a closed loop (the end point of the scan is the same as the start point at the first known location), any difference between the computed end location and the start location will be the start It can be solved by preferentially adjusting the location estimate for certain scan segments to restore agreement between location and end location. Location and location information in the low-trust segment can be preferentially adjusted compared to the high-trust segment. Thus, the SLAM system can use reliability-based error correction for closed-loop scanning.

In an exemplary and non-limiting embodiment of this confidence-based adjustment, the derivation of the piecewise smoothing and mapping (iSAM) algorithm, originally developed by Michael Kaess and Frank Dellaert of Georgia Tech (M. Kaess, A. Ranganathan, and F. Dellaert, "iSAM: Incremental Smoothing and Mapping", IEEE Trans. on Robotics (TRO), Vol. 24, No. 6, Dec. 2008, 1365- 1378 page, PDF ) can be used. The algorithm processes the map data in "segments" and iteratively refines the relative positions of these segments so as to optimize the residual error of alignment between segments. It allows closing the loop by adjusting all the data in the closed loop. Many segmentation points allow the algorithm to move the data significantly, whereas fewer segmentation points result in high stiffness.

When points for segmentation are selected based on the consistency confidence metric, this is taken advantage of in regions with low confidence so that the loop closure process does not spread local errors around the correct map section. Maps can be flexible and rigid in areas with high confidence. This can be further improved by weighting the segmentation points to assign a "flexibility" to each point and spreading the error based on this factor.

In embodiments, a confidence measure for an area or segment of the point cloud may be used to guide the user to perform additional scans, eg, to achieve an improved SLAM scan. In embodiments, the confidence measure may be based on a combination of point density, point orthogonality, etc., and may guide the user to enable more accurate scanning. Note that scanning attributes such as point density and point orthogonality can be determined in real-time as the scan progresses. Similarly, the system can sense the geometry of the scanning environment, which is likely to result in a low confidence measure. For example, a long corridor with smooth walls may not exhibit any irregularities to distinguish one scan segment from the next. In such instances, the system may assign a lower reliability measure to scan data acquired within such environments. The system uses lidar, cameras, and possibly other sensors to determine the decreasing reliability and guide the user through the scan with instructions (such as “slow down,” “turn left,” or the like). Various inputs can be used, such as In other embodiments, the system displays areas of lower confidence than desired to the user, such as through a user interface, while allowing the user to further scan the areas, volumes, or regions of low confidence. can be given assistance to

In embodiments, SLAM output may be fused with other content, such as output from cameras, and output from other mapping techniques. In embodiments, SLAM scanning may be performed in conjunction with capturing an audio track, such as by a companion application (optionally a mobile application) that captures time-encoded audio sounds corresponding to the scanning. In embodiments, the SLAM system collects data during a scan so that the mapping system can pinpoint when and where the scan occurred, including when and where the mapper acquired audio and/or sound. is time-encoded. In embodiments, temporal encoding is performed by such as inserting data into a map or scan that the user can access such as by clicking on an indicator on a map where audio is available. It can be used to localize the sound within a meaningful map area. In embodiments, other media formats such as photos, HD video, or the like can be captured and synchronized with scanning. These media formats can be accessed separately based on time information, or can be inserted at appropriate points within the map itself based on time synchronization of the scan output with time information about other media. . In embodiments, the user can use the temporal data to go back in time and see what has changed over time, such as based on multiple scans with different temporal encoding data. The scan can be further enhanced with other information such as date-stamped or time-stamped run data. From the above, a scan can be part of a multi-dimensional database of a scene or space, where the point cloud data can be any other data or media related to this scene, including time-based data or media. associated with. In embodiments, the computation allows backing up the scan, for example, returning to a given point in the scan and restarting at that point rather than having to restart a new scan starting at the origin. maintained throughout the sequence of steps or segments in a manner. This allows the partial scan information to be used as a starting point for continuing the scan, for example, when problems arise later in a scan that initially produced good output. In this case, the user can "discard" or "rewind" the scan back to a point and then resume the scan from this point. The system can maintain accurate position and location information based on point cloud features, and can maintain time information to allow temporal alignment with other time-based data. . Time-based data can be used to perform these edits, such as synchronizing scans or other media when, for example, a scan has been completed over a time interval and needs to be synchronized with other media captured over a different time interval. can also make it possible. The data in the point cloud can be tagged with a time stamp so that the time stamped data after the point in time to unwind to can be erased so that scanning can be restarted from the specified point. can. In embodiments, rewinding may be to a certain physical location, such as rewinding to a point in time and/or to certain geospatial coordinates.

Embodiments can fuse output from SLAM-based maps with other content such as HD video, including merging by coloring the point cloud and using it as an overlay. This fusion can include time synchronization between the SLAM system and other media capture systems. The content can be fused with video, still images of the space, CAD models of the space, audio content captured during the scan, metadata associated with the location, or other data or media.

In embodiments, the SLAM system can be integrated with other technologies and platforms, such as tools (eg, CAD) that can be used to manipulate point clouds. This integration includes combining scans with features modeled in CAD modeling tools, rapid prototyping systems, 3D printing systems, and other systems capable of using point clouds or solid model data. can be done. Scans can be provided as input to post-processing tools, such as coloring tools. Scans can be provided to mapping tools to add, for example, points of interest, metadata, media content, annotations, navigational information, semantic analysis to distinguish particular shapes and/or to identify objects. .

Output may be combined with output from other scanning and image capture systems such as ground penetrating radar, X-ray imaging, magnetic resonance imaging, computed tomography, infrared imaging, photography, video, SONAR, radar, lidar, etc. can be done. This combination can include integrating the output of the scan with navigation and mapping systems such as in-vehicle navigation systems, handheld mapping systems, mobile phone navigation systems, and others. Data from the scans can be used to provide position and orientation data, including X, Y, and Z position information, as well as pitch, roll, and yaw information, to other systems.

Data obtained from real-time SLAM systems are used in 3D motion capture systems, acoustic engineering applications, biomass measurements, aircraft construction, archaeology, architecture and civil engineering, augmented reality (AR), self-driving vehicles, autonomous mobile robotics applications, among others. Purification and processing, CAD/CAM applications, construction site management (e.g. progress checking), entertainment, exploration (space, mining, underwater, etc.), forestry (logging and management of other forestry products such as maple sugar), franchising. management and compliance (e.g. stores and restaurants), imaging applications for verification and compliance, indoor positioning, interior design, inventory inspection, landscape design, industrial space mapping for maintenance, trucking route mapping, military/intelligence applications, mobile mapping, oil pipeline monitoring and drilling, land and building evaluation and other real estate applications, retail indoor localization (e.g. combining real-time maps with inventory maps, etc.), security applications, stockpile monitoring (mineral , lumber, supplies, etc.), appraisal (including pre-scrutiny to make better estimates), UAV/drone, mobile robots, underground mapping, 3D modeling applications, virtual reality (including spatial coloring), warehouses It can be used for many different purposes including management, zoning/planning applications, autonomous missions, inspection applications, docking applications (including spacecraft and ships), caving, and video game/augmented reality applications. . In all usage scenarios, the SLAM system can operate as described herein in the mentioned usage areas.

According to an exemplary and non-limiting embodiment, the unit includes hardware synchronization of the IMU, cameras (visual) and LiDAR sensors. The unit can be operated for a period of time in the dark or in an unstructured environment. The processing pipeline can be modular. In the dark, the vision module can be bypassed. In an unstructured environment, the LiDAR module can be bypassed or partially bypassed. In an exemplary, non-limiting embodiment, all IMU, lidar, and camera data are time-stamped so that they can be aligned and synchronized in time. As a result, the system can function in an automated manner to synchronize image data and point cloud data. In some cases, the color data from the synchronized camera images can be used to color the chunk data pixels for display to the user.

The unit may contain four CPU threads for scan matching, and may run at 5 Hz using Velodyne data, for example. Movement of the unit during actuation can be of relatively high speed. For example, the unit can operate at an angular velocity of approximately 360 degrees/second and a linear velocity of approximately 30 m/s.

Units can be localized relative to previous spawn maps. For example, instead of building a map from scratch, the unit's software references a previously built map to map sensor attitudes and new maps within this old map framework (e.g., geospatial or other coordinates). can be generated. The unit can use localization to further extend the map. By expanding the new map within the old map frame, the new map can be further expanded and popped out of the old map. Thereby, an initial "backbone" scan is first generated, possibly post-processed to reduce drift and/or other errors, and then restarted from this map for local details such as side rooms within a building. A variety of modes of use are possible, including branching and chaining to add or increase point density within the region of interest. By taking this scheme, the backbone model can be generated with special care taken to limit global drift, and subsequent scans can be generated focused on capturing local detail. Also, multiple devices for faster large area capture can start from the same base map and perform detailed scans.

Furthermore, it is possible to start with a model generated by a different type of device or even restart from a model generated from a CAD drawing. For example, a fixed device with high global accuracy can build a base map from which a mobile scanner can resume and fill in details. In an alternative embodiment, the long-range device can scan the exterior and large interior areas of the building, and the short-range device can resume this scan to enter smaller corridors and rooms and add finer detail. can. Restarting from a CAD drawing can have great advantages in terms of quickly detecting discrepancies between the CAD and the as-built one.

Resume can also provide location-aligned time data. For example, multiple scans can be taken over time from the construction site to visually confirm progress. In other embodiments, multiple scans of the factory can aid tracking for asset management.

Resuming can alternatively be used simply to provide orientation data within the previous map. This can be useful in guiding robotic vehicles or in localizing new sensor data such as images, heat maps, sound within existing maps.

In some embodiments, the unit uses relatively high CPU usage in mapping mode and relatively low CPU usage in orientation mode suitable for long-term orientation/navigation. In some embodiments, the unit supports long-term operation by performing an internal reset from time to time. This internal reset is beneficial because some of the values generated during internal processing will rise over time. Over long periods of operation (e.g. days), these values can reach limits, such as the logical or physical limits of storage for the values in the computer, possibly causing processing to fail without a reset. be. In some cases, the system can automatically flush RAM memory to improve performance. In other embodiments, the system may downsample old scan data as may be necessary when performing real-time comparisons of newly acquired data with old and/or archived data. can.

In other exemplary embodiments, the unit can support flight applications and air-to-ground map fusion. In other embodiments, the unit may compute attitude outputs at the IMU frequency, eg, 100 Hz. In such cases, the software can generate maps as well as sensor poses. The sensor attitude conveys the position and orientation of the sensor with respect to the map being deployed. High frequency, accurate attitude output is useful for autonomous locomotion because vehicle motion control requires such data. The unit can further use the covariance and estimated reliability to fix the pose when the sensor is stationary.

Referring to Figures 17(a)-17(b), exemplary and non-limiting embodiments of SLAM are shown. The lidar is rotated to produce a substantially hemispherical scan. This rotation is accomplished by a mechanism having a DC motor driving a spur gear reduction to the rider mount by spur gear assembly 1704 . Spur gear reduction assembly 1704 allows the rider to be offset from motor 1708 . There are slip rings along the rotation of the rider to provide power and receive data from the rotating rider. An encoder 1706 for recording the orientation of the mechanism during scanning is also coincident with the rotation of the lidar. A thin section contact bearing supports the rider's rotating shaft. A counterweight on the rider's rotating plate balances the weight around the axis of rotation to keep the rotation smooth and constant. As depicted in the lidar drive mechanism and mounting diagram below, the mechanism has minimal wobble and rattle to allow maintenance of a constant speed for interpolation of scan point locations. designed not to Note that motor shaft 1710 is in physical communication with rider connector 1712 .

Referring to FIG. 18, an exemplary, non-limiting embodiment of SLAM enclosure 1802 is shown. SLAM enclosure 1802 is depicted in various views and perspectives. The dimensions are representative of embodiments and may be similar or different in size while maintaining the general characteristics and orientation of major components such as lidar, odometry cameras, tinted cameras, and user interface screens. It is not limited because it can be done.

In some embodiments, the unit includes a neck brace, shoulder brace, carry bag, or other wearable element or device (not shown), for example, to help the individual hold the unit while walking around. can be used. A unit or support element or device may include one or more stabilizing elements to reduce sway or vibration during scanning. In other embodiments, the unit can use a remote battery carried in a shoulder bag or the like to reduce the weight of the handheld unit, so the scanning device has an external power source.

In other embodiments, the cameras and lidar are arranged to maximize the field of view. A camera-laser arrangement poses a compromise. On the one hand the camera shields the FOV of the laser and on the other the laser shields the camera. In such an arrangement they both suffer some masking, but this masking does not significantly compromise the mapping quality. In some embodiments, the camera points in the same direction as the laser because visual processing is aided by the laser data. Laser range finding provides depth information for image features in the process.

In some embodiments, a reliability metric representing the reliability of spatial data can be used. Such reliability metric measurements can include, but are not limited to, point count, point distribution, point orthogonality, environment geometry, and the like. One or more reliability metrics for laser data processing (eg, scan matching) and image processing can be calculated. Referring to FIGS. 19(a)-19(c), illustrative and non-limiting examples are shown showing point clouds differentiated by laser alignment estimate confidence. In practice such images can be color coded, but as illustrated both trajectories and points are drawn as solid or dotted lines within groups based on the last confidence value at the time of recording. . In these examples, dark gray is bad and light gray is good. Values are thresholded such that all with values>10 are solid lines. Through experimentation, it was found that those with Velodyne<1 are unreliable, <10 are unreliable, and >10 are very good.

Using these metrics enables automated testing to solve model problems and off-line model corrections, for example when utilizing the loop closure tools discussed elsewhere herein. Additionally, the use of these metrics allows the user to be notified when a match is bad, possibly auto-pausing, discarding low-confidence data, and even notifying the user when scanning. FIG. 19(a) illustrates a scan of a building floor performed at a relatively slow pace. FIG. 19(b) illustrates a scan of the same building floor performed at a relatively fast pace. Note the slight spread of the fray when compared to the scans acquired from the slower scan pace, due in part to the speed at which the scans are performed. FIG. 19(a) illustrates a zoomed-in view of a potential failure spot of relatively low confidence.

Referring to FIG. 20, an exemplary and non-limiting embodiment of scan-to-scan match confidence metric processing is illustrated, showing visual feature tracking between a full laser scan and a map constructed from a previous full laser scan. The average number of parts can be calculated and presented visually. This metric can provide a useful but different confidence measure. In FIG. 20, the laser scanning reliability metric plot is presented in the left pane, and the average count of visual feature metrics for the same data is presented in the right pane. As before, dark gray lines indicate low confidence and/or low average number of visual features.

In some embodiments, loop closures can be used. For example, the unit can be activated as it walks around a room, cubicle, enters and leaves an office, and returns to its starting point. Ideally, the data mesh from the start and end points should be precisely meshed. In reality, some drift may exist. The algorithms described herein greatly dwarf such drift. A typical reduction is of the order of 10× over conventional methods (0.2% vs. 2%). This ratio reflects the error in the distance between the start and end points divided by the total distance traveled during the loop. In some embodiments, the software can recognize that it has returned to the starting point and re-fix it to the origin. Once complete, the variation can be captured and distributed across all of the collected data. In other embodiments, certain point cloud data whose reliability metrics indicate that the data reliability was poor can be determined and adjustments can be made in areas with low reliability.

In general, the system of the present invention can use explicit and implicit loop closures. In some cases, a user may indicate that the loop should be closed, such as through a user interface forming part of SLAM. As a result of this explicit loop closure, the SLAM merges the most recent scan data into the data acquired at the beginning of the loop in order to match the data acquired at the beginning and the data acquired at the end with each other to close the loop. can be aligned. In other embodiments, the system of the present invention can implement implicit loop closure. In such instances, the system of the present invention can operate in an automated manner, recognizing that it will actively rescan the location containing the origin or source area for the scan loop. .

In some embodiments, reliability-based loop closure may be used. First, the start and end points of a loop scan of an area containing multiple segments can be determined. The reliability of multiple segments can then be determined. Subsequently, adjustments can be made to the low quality segment rather than the high quality segment to match the start and end of the loop.

In other exemplary embodiments, multi-loop reliability-based loop closure may be implemented. In still other embodiments, confidence-based loop closures that add semantic adjustments can be used. For example, structural information can be derived from the attributes of the scanned element, ie the floor is flat, the corridor is straight, and so on.

In some cases, coloring of lidar point cloud data can be used. In some embodiments, rough coloring to collection points can be used in real time to help identify what has been captured. In other embodiments, off-line realistic coloring can be used. In other embodiments, each pixel in the camera can be mapped to a unique lidar pixel. For example, color data can be obtained from pixels in the colored camera that correspond to lidar data in the point cloud and appended to the lidar data.

According to an exemplary and non-limiting embodiment, the unit may employ a sequential multi-layer processing pipeline that solves for motion from coarse to fine. At each layer of optimization, the previous rough results are used as initial guesses to the optimization problem. The stages in the pipeline are:

1. We start with an IMU mechanism for motion prediction that provides high frequency updates (on the order of 200 Hz) but is subject to high levels of drift.

2. This estimation is then refined by visual-inertial odometry optimization at the camera frame rate (30-40 Hz), and this optimization problem uses the IMU motion estimation as an initial guess of pose change, tracking from the current camera frame. We adjust this pose change in an attempt to minimize the residual squared error in the motion from some feature to the keyframe.

3. This estimate is then further refined by laser odometry optimization at lower speeds determined by the "scan frame" speed. The scan data arrive continuously and the software divides them into multiple frames similar to the image frames at a constant rate, at which point the rate is such that one revolution of the lidar rotation mechanism is for each scan. It is the time it takes for a frame to be a complete data hemisphere. These data are stitched together using visual-inertial estimation of position change since points within the same scan frame are collected. In the lidar odometry pose optimization stage, the visual odometry estimate is taken as an initial guess and the optimization attempts to reduce residual errors in features tracked in the current scan frame that are matched to the previous scan frame.

4. In the final step, the current scan frame is matched against the entire map so far. Taking the laser odometry estimate as an initial guess, the optimization minimizes the residual squared error between features in the current scan frame and features in the map so far.

The resulting system enables high-frequency, low-latency self-motion estimation combined with dense, accurate 3D map registration. Additionally, the system can address sensor degradation by automatic reconfiguration that bypasses failed modules, since each stage can correct for errors in the previous stage. Thus, the system can operate in the presence of highly dynamic motion and in dark, uneven and structure-free environments. During experiments, the system demonstrated a relative position drift of 0.22% over 9.3km of navigation and is robust for running, jumping and even high speed driving on highways (up to 33m/s). demonstrated the sex.

Other key features of such systems can include the following.

Visual Feature Optimization with and without Depth: The software first tries to associate the depth of the tracked visual features with the laser data and then triangulates the depth between camera frames. You can try to determine by trying. The feature optimization software can then utilize all features in two different error calculations, one for features of known depth and one for features of unknown depth.

Laser feature determination : The software can extract laser scan features as the scanline data arrives rather than the entire scan frame. This extraction is much simpler, looking at the smoothness at that point defined by the relative distance between each point and the K closest points on either side of it, followed by the smoothest point , as planar features, and the sharpest points as edge features. This extraction also allows elimination of some points that may be bad features.

Map Alignment and Voxelization : Part of how well laser alignment works in real time is how the map and feature data are stored. Tracking the processor load of this stored data requires long-term scanning and selective voxelization into 3D elementary units to minimize the data stored while preserving what is needed for accurate alignment. Or important for downsampling. On-the-fly adjustment of this downsampling voxel size or basic unit based on processor load can improve the ability to maintain real-time performance in large maps.

Parallelism : Software can be configured so that it can take advantage of parallelism to maintain real-time performance when data arrives faster than the processor can process it. This parallelism is more significant with fast points/second lidar such as velodyne.

Robustness : The approach in which our system uses a separate optimization stage without including the previous estimation as part of the next estimation (apart from the initial estimation) has some intrinsic robustness. produce.

Confidence Metrics : Each optimization stage in the process can provide information about the reliability of the results of that stage itself. In particular, at each stage, the residual squared error remaining after optimization, the number of features to track between frames, etc. can be evaluated to give a confidence measure in the result.

The user can be presented with a reduced (eg, subsampled) version of the multispectral model given the data being acquired by the device. In one example, a cube of model data, each measuring 3 cm by 3 cm by 3 cm, can be represented as a single pixel in a reduced version presented on the user interface. The pixel selected for display can be the pixel closest to the center of the cube. A representative thumbnail view generated during SLAM operation is shown below. As will be explained, the decision to display a single pixel within a volume depends on the presence, or absence of any such points, of one or more points occupying a spatial cube of predetermined dimensions within the point cloud. represents a binary result indicating either In other exemplary embodiments, a selected pixel may be attributed with a value that indicates, for example, the number of pixels that are inside a predetermined cube represented by it. This attribute can be used when displaying the sub-sampled point cloud, for example by displaying each selected pixel using a color and/or intensity that reflects the value of this attribute.

A visual frame contains a single 2D color image from a tinted camera. A lidar segment includes a full 360 degree rotation of the lidar scanner. The vision frames and lidar segments are combined so that they can be matched with existing model data based on unit position data captured from associated sensors such as IMU and odometry (e.g. fast white/black) cameras. Synchronized.

Problems that cause low reliability metrics include, but are not limited to, reflections from glossy walls, glass, transparent glass, and narrow corridors. In some embodiments, the user of the unit pauses scanning, such as by pressing a pause button and/or requesting rewind to a predetermined point or requested number of seconds in the past. , and can be restarted.

According to an exemplary, non-limiting embodiment, unwinding during scanning may proceed as follows. First, it indicates that the user of the system wishes to rewind. This representation can be accomplished by manipulation of a user interface that forms part of SLAM. As a result of indicating that rewinding is desired, the system deletes or otherwise removes the portion of the scanned data points corresponding to the time span. Since all scanned data points are time-stamped, the system can effectively remove data points after a predetermined time, thus "rolling back" to a previous point in the scan. Images from the camera are collected and time-stamped during each scan as described herein. As a result, after removing the data points after the predetermined point in time, the system presents the user with a display of the image recorded at the predetermined point while displaying the scanned point cloud unwound to the predetermined point in time. be able to. The image serves as a metric to help the user of the system orient the SLAM to a position that closely matches the orientation and attitude of the SLAM at a previous predetermined time. Once the user is oriented close to the SLAM's past orientation at the predetermined time, the user can indicate a desire to resume scanning, such as by pressing a "go" button on the SLAM's user interface. . In response to a command to resume scanning, the SLAM can proceed to execute a processing pipeline that utilizes the new scan data to form an initial estimate of SLAM's position and orientation. During this process, the SLAM may not add new data to the scan, but uses the new scan data to determine the instantaneous confidence level of the user's position as well as how the newly acquired data corresponds to the previous scan data. A visual representation of the degree can be determined and displayed. Finally, once it is established that the SLAM location and orientation have been sufficiently determined for the previous scan data, scanning can continue.

As explained above, this rewind functionality is enabled in part by storing data. It is possible to estimate how many points are brought in each second and then how much to "unwind". The unit tells the user where he was x seconds ago and allows him to move to that location and take some scans to make sure he's in the right place. be able to. For example, the user can be told approximately where to go (or indicate where the user would like to resume). If the user is close enough, the unit can calculate where the user is and tell the user that they are close enough.

In other exemplary embodiments, the unit can operate in transitions between spaces. For example, when a user zips through a narrow doorway, there may not be enough data and time to determine the user's location within the new space. Specifically, in this example, the boundary of the door frame may prevent the rider from imaging a sufficient amount of the environment behind the door to establish the user's location before proceeding therethrough. One option is to detect a drop in this reliability metric and modify your behavior so that the operator slows down when approaching a narrow passage, e.g. flashing a visual indicator, changing the color of the screen. and the like to signal the operator.

Referring to FIG. 21, an exemplary, non-limiting embodiment of an overview of SLAM unit 2100 is shown. SLAM unit 2100 may include a timing server for generating multiple signals derived from the IMU 2106 pulse per second (PPS) signal. The generated signal can be used to synchronize data collected from various sensors within the unit. A microcontroller 2102 can be used to generate these signals and communicate with the CPU 2104 . Quadrature decoder 2108 can be built either within this microcontroller or on an external IC.

In some exemplary embodiments, IMU 2206 provides a rising edge PPS signal that is used to generate timing pulses for other parts of the system. The camera generates one rising edge signal as described above generated from the IMU PPS signal and two GPIO1 (one frame lasting) and GPIO2 (two frames lasting) illustrated with reference to FIG. Three signals can be received, including one falling edge signal.

As illustrated, each camera receives a trigger signal synchronized with an IMU PPS having a high frame rate of approximately 30 Hz or 40 Hz and a high resolution of approximately 0.5 Hz to 5 Hz.

Each IMS PPS can zero the internal counter of the microcontroller 2202 . The lidar sync output can trigger the following events.

• Reading the current encoder value through the quadrature decoder.

・Read current counter value.

Encoder values and counter values can be saved together and sent to the CPU. This phase can occur every 40 Hz defined by the lidar sync output illustrated with reference to FIG.

Another time synchronization technique can include IMU-based pulse-per-second synchronization that facilitates synchronizing sensors and computer processors. An exemplary, non-limiting embodiment of this type of synchronization is illustrated with reference to FIG.

IMU 2400 may be configured to send a pulse per second (PPS) signal 2406 to lidar 2402 . Each time a PPS is sent, computer 2404 is notified by recognizing a flag in the IMU data stream. Computer 2404 then follows up and sends the time string to lidar 2402 . Lidar 2402 synchronizes with PPS 2406 and encodes a time stamp into the lidar data stream based on the received time string.

Upon receiving the first PPS 2406, computer 2404 records its own system time. Starting with the second PPS, computer 2404 increases the recording time by one second and sends the resulting time string to lidar 2402, which subsequently corrects its own system time to track PPS 2506.

In this time synchronization scheme, IMU 2400 acts as a time server, but the initial time is obtained from the computer system time. The IMU 2400 data stream is associated with a timestamp based on the IMU's own clock and is initialized with the computer system time when the first PPS 2406 is sent. Thus, IMU 2400, lidar 2402, and computer 2404 are all time-synchronized. In embodiments, lidar 2402 may be a Velodyne lidar.

According to an exemplary and non-limiting embodiment, the unit includes a COM express board for scanning and a one-button interface.

According to an exemplary, non-limiting embodiment, the processing IMU, visual data sensor, and laser data sensor may be combined. The unit can operate for long periods of time in the dark or in an unstructured environment. In some embodiments, four CPU threads for scan matching, each running at 5 Hz with Velodyne data, can be used. As mentioned above, the movement of the unit may be fast, the unit can be localized relative to the previous map, and the localization can be used to extend the map. The unit exhibits relatively high CPU usage in mapping mode and relatively low CPU usage in localization mode, thus making it suitable for the long term.

According to exemplary and non-limiting embodiments, the methods and systems described herein enable coordination between ground-based and air-based mapping according to their respective characteristics. Ground-based mapping is not necessarily subject to space or time constraints. Generally, mapping devices carried by ground vehicles are suitable for large-scale mapping and can travel at high speeds. Congested areas, on the other hand, can be mapped with handheld deployments. However, ground-based mapping is limited by the altitude of the sensor, which makes top-down configurations difficult to achieve. As illustrated in Figure 25, ground-based experiments produce detailed maps of the building's perimeter, whereas the roof must be mapped from the air. Airborne mapping is limited due to short battery life when small air vehicles are used. It is also necessary to have a sufficient line of sight in space for the vehicle to operate safely.

The collaborative mapping described herein can utilize laser scanners, cameras, and low-quality IMUs to process data through multi-layered optimization. The resulting motion estimate can be fast (˜200 Hz) with low drift (typically <0.1% of distance traveled).

The high-precision processing pipeline described herein can be utilized to fuse ground-generated and air-generated maps in real-time or near-real-time. This fusion is partially accomplished by localization of one output from the ground-derived map relative to the output from the air-derived map.

The methods described herein accomplish cooperative mapping and further reduce airborne deployment complexity. A ground-based map is used to define the flight path and the vehicle performs autonomous missions. In some embodiments, the air vehicle can autonomously perform difficult flight tasks.

Referring to FIG. 26, an exemplary, non-limiting embodiment of a sensor/computer pack that can be utilized to enable collaborative mapping is shown. Processing software is not necessarily limited to a particular sensor configuration. Referring to FIG. 26(a), the sensor pack 2601 includes a laser scanner 2603 generating 0.3 million points/second, a camera 2605 with a resolution of 640×360 pixels and a frame rate of 50 Hz, and a low quality sensor at 200 Hz. It consists of IMU2607. An on-board computer processes data from the sensors in real-time for ego-motion estimation and mapping. Figures 26(b) and 26(c) illustrate the sensor field of view. An overlap is shared by the laser and camera, with which the processing software associates depth information from the laser to image features, as described more fully below.

According to exemplary and non-limiting embodiments, the software processes data from range sensors such as laser scanners, cameras, and inertial sensors. Instead of combining data from all sensors into a large full-scale problem, the methods and systems described herein parse the problem as multiple smaller problems, and divide these problems in a coarse-to-grain fashion. Solve in order. FIG. 27 illustrates a block diagram of the software system. In such systems, forward modules perform light processing to ensure that high-frequency motion estimation is robust against extreme motion. The modules behind it do a lot of processing and run at low frequencies to ensure the accuracy of the resulting motion estimates and maps.

The software begins with IMU data processing 2701 . This module runs at the IMU frequency to predict motion based on IMU mechanization. The results are further processed by the visual-inertial coupling module 2703 . Module 2703 tracks distinct image features through the image sequence and solves for motion in the optimization problem. In this case, the laser range measurements are registered on the depth map, thereby relating depth information to the tracked image feature. Since the sensor pack contains only a single camera, depth from the laser helps resolve scale ambiguity during motion estimation. The estimated motion is used to locally align the laser scan. In a third module 2705 these scans are aligned to further refine the motion estimation. Aligned scans are registered on the map while the scans are aligned to the map. To speed up processing, scan matching utilizes multiple CPU threads in parallel. To speed up point queries during scan matching, the map is stored in voxels. Since motion is estimated at different frequencies, a fourth module 2707 in the system obtains these motion estimates for integration. The output retains both high accuracy and low latency which is beneficial for vehicle control.

The modularized system further ensures robustness with respect to sensor degradation by choosing "sound" sensor modes when forming the final solution. For example, when the camera is in a low lighting environment or in a smooth environment, e.g. facing a clean white wall, or when the laser is in a symmetrical or extruded environment such as a long straight corridor. Sometimes the process generally fails to produce a reasonable motion estimate. The system can automatically determine degraded subspaces within the problem state space. When degradation occurs, the system only partially solves the problem within the well-conditioned subspace of each module. As a result, the "sound" parts are combined to produce a final reasonable motion estimate.

When a map is available, the method described above can be extended to make use of this map for localization. This extension is accomplished using a scan-matching method. This method extracts two types of geometric features, specifically edges and planar points, based on curvature within the local scan. Feature points are matched against the map. Edge points are matched to edge line segments and plane points are matched to local plane patches. Edge line segments and local plane patches are determined by examining the eigenvalues and eigenvectors associated with the local point sets on the map. Maps are stored in voxels to speed up processing. Localization solves an optimization problem that minimizes the total distance between feature points and their corresponding points. The optimization usually converges in 2-3 iterations due to the high accuracy odometry estimates used to give an initial guess to the localization.

Localization does not necessarily process individual scans, but stacks some scans for batch processing. Due to the high accuracy of odometry estimation, scans are closely aligned in the local coordinate frame and drift can be neglected over short periods of time (eg, seconds). The comparison is illustrated with reference to FIG. 28(8.4), where FIG. 28(a) is the single scan matched in the previous section (scan matching was performed at 5 Hz). , FIG. 28(b) shows scans stacked over 2 seconds and aligned during localization (scan alignment was performed at 0.5 Hz). It can be seen that the stacked scan contains significantly more structural detail, contributing to localization accuracy and robustness to environmental changes. In addition, low frequency execution keeps CPU utilization for on-board processing to a minimum (localization consumes only about 10% CPU threads). Stacked scans contain significantly more structural detail, contributing to localization accuracy and robustness to environmental changes. Roughly, about 25% of the environment can be changing or dynamic and the system will continue to work well.

Localization is compared to particle filter-based implementations. Odometry estimation provides a motion model to the particle filter. The particle filter uses 50 particles. At each update stage, the particles are resampled based on low variance resampling. The comparison results are shown in Figure 29 and Table 8.1. In this case, the error is defined as the absolute distance from the localized scan to the map. During evaluation, the methods and systems described herein pick a subset of planes and use the distance between points in the localized scan to corresponding plane patches on the map. FIG. 29 shows an exemplary, non-limiting embodiment of the error distribution. When the particle filter is run at the same frequency (0.5 Hz) as the method described here, the resulting error is five times larger. In contrast, in Table 8.1, the CPU processing time is more than doubled. In another test, running the particle filter at 5 Hz helped reduce the error to slightly greater than the method of the present disclosure. However, the corresponding CPU processing time is increased by a factor of over 22. These results imply that particle filter-based methods do not always take full advantage of high-accuracy odometry estimation.

(Table 8)
Table 8.1 Comparison of CPU processing time in stereotactic. When running the particle filter at 5 Hz, high density data is processed at 25% of the real-time rate due to high CPU demands.

Referring to FIG. 30, an exemplary, non-limiting embodiment of sensor interrogation is shown when the sensor pack is held horizontally in a garage. FIG. 30(a) shows the constructed map and sensor trajectory. FIG. 30(b) is a single scan. In this scenario the scan contains sufficient structural information. When bypassing the camera processing module, the system generates the same trajectory as the full pipeline. On the other hand, the methods and systems described herein perform another test with the downward tilt sensor pack vertically toward the ground. The results are shown in FIG. In this scenario, the structural information within the scan is fairly sparse, as shown in FIG. 31(b). Processing fails without the camera and succeeds with the full pipeline. The result is important for high altitude flights where tilting of the sensor pack is required.

Referring to FIG. 32, there is shown an exemplary, non-limiting embodiment in which the sensor pack is held by an operator going through a circle at a speed of 1-2 m/s, with a total travel distance of 410 m. FIG. 32(a) shows the resulting map and sensor trajectory using the horizontal orientation sensor configuration. The sensor is started and stopped at the same position. This test produces a drift of 0.18m through the path, resulting in a relative position error of 0.04% compared to the distance traveled. The operator then repeats this path with two sensor packs held at 45° and 90° angles respectively. The resulting sensor trajectory is shown in FIG. 32(b). Clearly, the tilt introduces more drift, the relative position error is 0.6% at 45° (blue dashed curve) and 1.4% at 90° (red dash-dotted curve). . Ultimately, the drift is canceled by localization on the map of FIG. 32(a) and both configurations yield solid black curves.

An exemplary, non-limiting embodiment of a drone platform 3301 is shown in FIG. The aircraft weighs approximately 6.8 kg (including batteries) and can carry a maximum payload of 4.2 kg. The sensor/computer pack is mounted on the bottom of the aircraft and weighs 1.7 kg. A remote controller is shown in the lower right of this figure. The remote controller is operated by the safety pilot to disable autonomy as needed during autonomous missions. Note that this aircraft is built to have a GPS receiver (on top of the aircraft). GPS data is not necessarily used in mapping or autonomous performance.

In the first collaborative mapping experiment, the operator held the sensor pack and walked around the building. The results are shown in FIG. In FIG. 25(a), ground-based mapping is performed at 1-2 m/s over a 914 m stroke to provide detailed coverage of the building perimeter. As expected, building roofs are blank on this map. A drone is then remotely commanded to fly over the building. In FIG. 25(b), this flight is performed at 2-3 m/s and travels 269 m. This process uses localization with respect to the map of FIG. 25(a). In this approach, an aerial map is fused with a ground-based map (white dots). After the ground-based map is constructed, the take-off position of the drone is determined on the map. The sensor starting pose for airborne mapping is known and localization starts from there. FIG. 34 presents air- and ground-based sensor trajectories in top and side views.

Additionally, the methods and systems described herein implement autonomous flight to achieve aerial mapping. Referring to FIG. 35, a ground-based map is first constructed by hand-held mapping over a 672m stroke at 1-2m/s around the flight area. This map and sensor trajectory are shown in FIG. 35(a). Path points are then defined based on this map, and the drone follows these path points to perform aerial mapping. As shown in Figure 35(b), the curve is the flight path, the large points on the curve are the path points, and the points form the aerial map. In this experiment, the drone took off from a hangar on the left side of the figure, flew across the site, passed through another hangar on the right side, and then landed back in the first hangar. The speed is 4 m/s when traversing the site and 2 m/s when passing through the hangar. Figures 35(c) and 35(d) are two images taken by the on-board camera while the drone was flying towards and about to enter the hangar on the right. FIG. 35(e) shows the estimated speed during the mission.

Finally, the methods and systems described herein perform different experiments over longer distances. As shown in Figure 36, the ground-based mapping includes an off-road vehicle driven from left to right over a 1463m journey at 10m/s. Autonomous flight traverses this scene using ground-based maps and path points. After takeoff, the drone ascends at 15m/s to a height of 20m above the ground. After that, the drone descends to 2m above the ground and passes through the trees at 10m/s. The flight path is 1118 m long as indicated by curve 3601 in Figure 36(b). The two images are when the drone is flying high above the trees (see Figure 36(c)) and when the drone is flying low below the trees (see Figure 36(d)). to be photographed.

Exemplary and non-limiting embodiments allow wide area positioning data, such as from GPS, to be incorporated within the processing pipeline. Global positioning data can be useful for canceling ego-motion estimation drift over long distances of travel and for aligning maps within a global coordinate frame.

In some embodiments, GPS data can be recorded concurrently with mapping activity. As the system moves, there is some level of drift that causes the error to grow with distance. Typically, it may suffer only 0.2% drift, but nevertheless when traveling 1000 meters, the drift is 2 meters per 1000 meters of travel. At 10 km the drift grows to 20 meters and so on. This error cannot be corrected without closing the loop (returning to the beginning of the root in the traditional sense). By providing external information in the form of GPS coordinates, the system can understand where it is and correct its current position estimate. Although this correction is typically performed in a post-processing effort, the system of the present invention can perform such correction in real time or near real time.

Thus, using GPS provides a method that allows the loop to be closed. GPS, of course, also has some amount of error, but it is usually consistent within a given area, and many GPS systems today give better than 30 cm accuracy in position X and Y on a surface. can be done. Other more expensive and sophisticated systems can provide cm-level positioning.

This use of GPS provides the following important functions. 1. Orientation of the point cloud on the Earth. 2. Path-corrected so that the map is more accurate because its own position is known, and so that any data acquired at that position can be correlated to a series of similarly collected GPS points. Ability to align and "fix" maps with information. 3. Ability for the system to act as an IMU when GPS is lost.

According to exemplary and non-limiting embodiments, dynamic vision sensors can be utilized to further improve estimation robustness with respect to extreme motion. Dynamic vision sensors report data only for pixels that have illumination changes, yielding both high speed and low latency.

This high velocity (generally defined as greater than about 10 Hz) can quickly provide fast information to ego-motion and estimation systems, thereby improving the value for localization and subsequent mapping. . When a system can capture more data with less delay, the system becomes more accurate and more robust. Features identified and tracked by the dynamic vision sensor allow for more accurate estimation, since more features and faster updates allow for more accurate tracking and motion estimation.

A direct method can be used to achieve image registration with a dynamic vision sensor for self-motion estimation. In particular, direct methods match successive images for feature tracking from image to image. On the other hand, the feature tracking method disclosed herein outperforms the direct method.

Exemplary and non-limiting embodiments may implement parallel processing running on general-purpose GPUs or FPGAs, thus enabling processing of data in greater amounts and at higher frequencies. .

Parallel processing techniques can be used to track some features at the same time, or partition the data for a predetermined area or direction into multiple smaller problems. Parallel architectures can take the form of multiple cores, threads, processors, or even special forms such as graphics processing units (GPUs). Using a divide-and-conquer approach can significantly speed up the overall process, since each node in the parallel architecture is dedicated to its part of the problem at the same time. In general such speedups are not linear. That is, by dividing the problem into 16 parts and processes, each part does not necessarily individually speed up the process by a factor of 16. There is overhead in splitting, communicating, and then regrouping the results.

In contrast, when using linear processing, each subproblem or computation is processed once and the data is moved in and out of memory, the CPU, and then back to memory or storage. This process is slow, but the pipeline architecture, when configured correctly with systolic arrays and the like, can improve things as long as the pipeline is kept full.

According to an exemplary and non-limiting embodiment, loop closure can be introduced to remove drift in the ego-motion estimate by global smoothing.

For example, when we return to our starting point, we know that we are already there, so the position estimate can be corrected. The starting point is the origin and there should be accumulated error that becomes apparent at the end of the travel. Taking the difference between the origin (0,0,0) and the position (eg, (1,2,3)) where you would be when you return to the beginning will give you an error value. However, this value is possibly evenly accumulated over the travel.

For example, when we travel 100 meters back to the starting point and find that we are off by 1 meter in xyz, then the problem arises of determining where in the overall distance to apply this error. One approach is to spread this error over the entire distance, in this case correcting the travel by 1 cm at each meter increment to stay on the path and, more importantly, return error-free to where you started. can be done. This correction is global smoothing.

Another approach is to use some other error measure while moving and apply the correction only where this error measure is high. For example, the covariance matrix gives a good metric for map quality and can be used to a priori distribute this error over the entire travel. In other words, locations with poor quality matches appearing in the covariance matrix can be used to distribute the error unevenly across this move according to the ratio of the error over the move. This variance will act to correct for errors in detailed areas where low quality values are associated with particular locations.

The following clauses provide additional statements regarding the embodiments disclosed herein.

Clause 1. obtaining a lidar point cloud comprising a plurality of points each attributed with at least geospatial coordinates and a segment; A method comprising assigning a level and adjusting geospatial coordinates of each of at least a portion of a plurality of points attributed with the same segment based at least in part on the confidence level.

Clause 2. The method of clause 1, wherein the reliability level is the reliability level of loop closure.

Article 3. The method of Clause 1, wherein adjusting the geospatial coordinates is responsive to the determined loop closure error.

Article 4. The method of clause 1, wherein the segment whose geospatial coordinates are to be adjusted has a lower confidence level than at least one other segment.

Section 5. initiating using SLAM to acquire a lidar point cloud including a plurality of points having a starting location and each attributed with at least geospatial coordinates and a segment; A method comprising moving a loop and determining a scan end point when the SLAM is proximate to the starting location.

Clause 6. 7. The method of clause 6 including receiving an indication from the SLAM user that determining the scan end point has completed traversing the loop.

Article 7. 7. The method of clause 6 wherein determining the scan end point is based on points within the segment indicating proximity to the start location.

Article 8. 7. The method of clause 6, wherein points in the segment other than the segment containing the starting location are attributed geospatial coordinates that are proximal to the starting location.

Article 9. obtaining a lidar point cloud comprising a plurality of points each attributed with at least geospatial coordinates and a time stamp; and geospatial coordinates that have time stamps that are temporally close to the time stamp of each point to be colored and that are closest to the geospatial coordinates of the points to be colored and coloring at least a portion of the plurality of points using color information derived from an image having a.

Clause 10. 10. The method of Clause 9, wherein coloring is performed in one of real time and near real time.

Clause 11. deriving a motion estimate for the SLAM system using an IMU forming part of the SLAM system; refining the motion estimate by a visual-inertial odometry optimization process to generate a refined estimate; Refining the refined estimate by minimizing at least one residual squared error between at least one feature in the current scan and at least one previously scanned feature by a laser odometry optimization process. a method comprising:

Clause 12. 12. The method of clause 11, wherein deriving the motion estimate includes receiving IMU updates at a frequency of about 200 Hz.

Article 13. 13. The method of clause 12, wherein the IMU update frequency is between 190 Hz and 210 Hz.

Article 14. 12. The method of clause 11, wherein refining the motion estimate comprises refining the motion estimate at a rate equal to the frame rate of a camera forming part of the SLAM system.

Article 15. 15. The method of clause 14, wherein the frame rate is between 30Hz and 40Hz.

Article 16. 12. The method of clause 11, wherein the laser odometry optimization process is performed at a scanning frame rate as the lidar rotating mechanism forming part of the SLAM scans a complete data hemisphere.

Article 17. acquiring a plurality of depth tracking visual features in a plurality of camera frames using a camera forming part of a SLAM system; A method comprising associating with a lidar derived point cloud and triangulating the depth of at least one visual feature between at least two camera frames.

Article 18. 18. The method of clause 17, wherein the associating and triangulating steps are performed on processors using parallel computers.

Article 19. A SLAM device including a microcontroller, an inertial measurement unit (IMU) adapted to generate a plurality of timing signals, and a timing server adapted to generate a plurality of synchronization signals derived from the plurality of timing signals. Clause SLAM device, wherein the synchronization signal operates to synchronize at least two sensors forming part of the SLAM device.

Clause 20. 21. The SLAM device of clause 20, wherein the at least two sensors are selected from the group consisting of lidar, camera and IMU.

Article 21. obtaining a lidar point cloud comprising a plurality of points each attributed with at least geospatial coordinates and a time stamp; and obtaining a plurality of images each attributed with at least geospatial coordinates and a time stamp. and at least one of a time stamp temporally close to the time stamp of each colored point and a geospatial coordinate geographically close to the geospatial coordinate of each colored point. A method comprising coloring at least a portion of the plurality of points using color information derived from the image, and displaying the colored portion of the plurality of points.

Article 22. 22. The method of clause 21, further comprising displaying output from the camera as an overlay over the displayed plurality of points.

Article 23. obtaining a lidar point cloud including a plurality of points each attributed with at least geospatial coordinates and a time stamp; and coloring at least a portion of the plurality of points using color information. , wherein each of the plurality of points is colored with a color corresponding to a parameter of the acquired lidar point cloud selected from the group consisting of intensity parameter, density parameter, time parameter, and geospatial location parameter.

Article 24. obtaining, using SLAM, a lidar point cloud comprising a plurality of near-field points derived from a corresponding near environment and a plurality of far-field points derived from a corresponding far-field environment, wherein the far-field points are , through one or more spaces between one or more elements located in the near environment, and using multiple far points as the SLAM moves from the near environment to the far environment. The method by which is determined.

Article 25. receiving feedback in the SLAM system from at least one of a camera and a laser, the feedback comprising a plurality of feedback terms; modeling a plurality of biases each associated with the plurality of feedback terms; but how to form a sliding window of bias.

Article 26. 26. The method of clause 25, wherein each of the plurality of feedback terms includes estimated piecewise motion of the SLAM system.

Article 27. 27. The method of clause 26, wherein each of the plurality of bias terms is modeled to be constant during piecewise motion.

Article 28. 26. The method of clause 25 including a bias between 200 and 1000 sliding windows.

Article 29. 29. The method of clause 28, wherein the sliding window contains about 400 biases.

Clause 30. 26. The method of clause 25, wherein the sliding window serves as a parameter for determining the update rate of multiple biases.

Article 31. 26. The method of clause 25, wherein the sliding window enables dynamic reconfiguration of the SLAM system.

Article 32. 26. The method of clause 25, further comprising performing IMU bias correction for multiple biases.

Article 33. 33. The method of clause 32, wherein implementing IMU bias correction includes utilizing data from at least one of a laser and a camera.

Article 34. 34. The method of clause 33, wherein the laser and camera form part of the SLAM system.

Article 35. 34. The method of clause 33, wherein each of the laser and camera includes estimated piecewise motion over time.

Article 36. 26. The method of clause 25, wherein the sliding window is an array formed of a predetermined number of biases, wherein biases are added and deleted on a first-in/first-out basis.

Article 37. receiving visual data from the camera and the IMU forming part of the SLAM system and inertial data from the IMU; and estimating the piecewise motion of the SLAM system using the visual data and the inertial data as constraints. and associating depth information with one or more visual features derived from the visual data.

Article 38. 38. The method of clause 37, wherein depth information is obtained from laser data.

Article 39. 39. The method of clause 38, wherein depth information can be calculated from triangulation using estimated piecewise motion when laser data is not available.

Clause 40. 38. The method of clause 37 utilized to construct a point cloud with registered depth information.

Article 41. 41. The method of clause 40, further comprising calculating one or more eigenvectors for the point cloud and using the one or more eigenvectors to specify degradation of the point cloud.

Article 42. 42. The method of clause 41, wherein the degradation of a direction of the state space indicated by at least one of the eigenvectors is utilized to discard the solution for that direction.

Article 43. determining the relative pose between a camera having a camera coordinate system and a laser having a laser coordinate system, both forming part of the SLAM system, by utilizing a single coordinate system for the camera and the laser; and determining the relative attitude between the laser and an IMU forming part of the SLAM system and having an IMU coordinate system.

Article 44. 44. The method of clause 43, wherein the step of utilizing a single coordinate system includes projecting multiple laser points into the camera coordinate system during preprocessing.

Article 45. 44. The method of clause 43, wherein the IMU coordinate system is substantially parallel to the camera coordinate system.

Article 46. 46. The method of clause 45, further comprising rotationally correcting the IMU data from the IMU as it is acquired.

Article 47. 44. The method of clause 43, wherein the camera coordinate system originates at the camera optical center with the x-axis pointing left, the y-axis pointing up, and the z-axis pointing forward and coinciding with the main axis of the camera.

Article 48. 44. The system of clause 43, wherein the IMU coordinate system originates at the IMU measurement center where the x-, y-, and z-axes are parallel to and point in the same direction.

Article 49. The system of clause 43, wherein the world coordinate system is the coordinate system that coincides with the initial attitude of the SLAM system.

Clause 50. Establishing a motion model of the mobile mapping system using one or more pose constraints and a mobile mapping system including one or more landmark positions using one or more camera constraints and solving for each of the motion model and the landmark measurements.

Article 51. 51. The method of clause 50, wherein each model solving step includes utilizing Newton gradient descent.

Article 52. 52. The method of clause 51, wherein Newton gradient descent is adapted to a robust fitting framework for outlier feature removal.

Article 53. 51. The method of clause 50, wherein solving is performed without optimizing multiple landmark locations.

Article 54. receiving an odometry estimate comprising a plurality of cardinal points from a visual-inertial odometry module of a mobile mapping system; receiving a plurality of IMU measurements from an IMU forming part of the mobile mapping system; aligning a plurality of laser points collected from the forming part lasers based at least in part on the IMU measurements.

Article 55. 55. The method of clause 54, wherein the aligning step comprises utilizing IMU measurements to interpolate between a plurality of cardinal points.

Article 56. 56. The method of clause 55, wherein the step of interpolating comprises selecting one or more geometric features to be used for tracking from a point cloud formed of key points.

Article 57. 57. The method of clause 56, wherein bad points in the point cloud are not selected based on geometry.

Article 58. 57. The method of clause 56, wherein the bad points in the point cloud are not selected based on a relationship between one or more points and surfaces in the point cloud.

Article 59. storing map information including point clouds in a plurality of first level voxels each occupying a first volume of the same size; storing map information in voxels at a second level, wherein the first volume is greater than the second volume; retrieving map information from the voxels; performing scan matching on the retrieved second level voxels; and maintaining.

Article 60. 60. The method of clause 59, wherein each first level voxel is mapped to a plurality of second level voxels.

Article 61. 59. The method of clause 59, wherein the second level voxels are stored in a 3D KD tree.

Article 62. 60. The method of clause 59, further comprising downsizing the point cloud to maintain a substantially constant point density.

Article 63. A method comprising providing a ground point cloud map, generating an air point cloud map, and fusing the ground point cloud map and the air point cloud map in real time or near real time.

Article 64. 64. The method of clause 63, wherein the airborne point cloud map is generated utilizing a drone including a mobile mapping system.

Article 65. The method of clause 63 in which the drone includes a laser scanner, camera, and low quality IMU.

Article 66. 66. The method of clause 65, wherein the data from the laser scanner, camera and IMU are processed by a multi-layered optimization process.

Article 67. 64. The method of clause 63, wherein the fusing step further comprises localizing at least one output from the ground derived map with respect to the airborne point cloud map.

Article 68. 65. The method of Clause 64, wherein generating the air origin cloud map includes defining a drone flight path and autonomously flying the drone according to the drone flight path.

While only a few embodiments of the present disclosure have been illustrated and described, many modifications and variations can be made to these embodiments without departing from the spirit and scope of the present disclosure as set forth in the following claims. It will be clear to those skilled in the art that modifications can be made. All patent applications and patents, both foreign and domestic, and all other publications cited herein are incorporated herein in their entirety to the fullest extent permitted by law. .

The methods and systems described herein may be partially or wholly deployed by a machine executing computer software, program code and/or instructions on a processor. The present disclosure may be implemented as a method on a machine, as a system or apparatus as part of or associated with a machine, or embodied in a computer readable medium on one or more of the machines. can be implemented as a computer program product that runs on In embodiments, the processor may be part of a server, cloud server, client, network infrastructure, mobile computer platform, fixed computer platform, or other computer platform. A processor can be any type of computing or processing device capable of executing program instructions, code, binary instructions, and the like. A processor may be a signal processor, digital processor, embedded processor, microcontroller, or coprocessor that directly or indirectly facilitates the execution of program code or program instructions stored therein (mathematics coprocessor, graphics coprocessor, and communication coprocessors, etc.). In addition, processors may enable the execution of multiple programs, threads, and code. Multiple threads can be executed concurrently to improve processor performance or to facilitate concurrent operation of applications. Depending on the implementation, the methods, program code, program instructions, etc. described herein may be implemented in one or more threads. A thread can spawn other threads that can have an assigned priority associated with it, and the processor executes these threads based on a priority based on instructions given in the program code or any other order. can do. A processor, or any machine that utilizes it, may include non-transitory memory that stores the methods, code, instructions, and programs described herein and elsewhere. A processor may access, through an interface, non-transitory storage media that may store the methods, code, and instructions described herein and elsewhere. Storage media associated with a processor for storing methods, programs, codes, program instructions, or other types of instructions that a computing or processing device can execute include CD-ROMs, DVDs, memory, hard disks, It may include, but is not limited to, one or more of flash devices, RAM, ROM, cache, and the like.

Processors can include one or more cores that can increase the speed and performance of multiprocessors. In embodiments, the processing may be dual-core processors that combine two or more independent cores (referred to as dies), quad-core processors, other chip-level multiprocessors, and the like.

The methods and systems described herein may be implemented partially or wholly by machines executing computer software on servers, clients, firewalls, gateways, hubs, routers, or other such computer and/or network connection hardware. can be deployed The software program may be associated with servers, which may include file servers, print servers, domain servers, Internet servers, intranet servers, cloud servers, and other variants such as secondary servers, host servers, distributed servers, and the like. can. Servers may access memory, processors, computer-readable media, storage media, ports (both physical and virtual), communication devices, and other servers, clients, machines, and devices through wired or wireless media. It may include one or more of such interfaces. A method, program, or code described herein and elsewhere may be executed by a server. Additionally, other devices required to perform the methods described in this application can be considered part of the infrastructure associated with the server.

Servers may provide interfaces to other devices including, without limitation, clients, other servers, printers, database servers, print servers, file servers, communication servers, distributed servers, social networks, and the like. In addition, this coupling and/or connection can facilitate remote execution of programs across networks. Networking of some or all of these devices may facilitate parallel processing of a program or method at one or more locations without departing from the scope of the present disclosure. Additionally, any of the devices attached to the server through the interface can include at least one storage medium capable of storing methods, programs, code, and/or instructions. A central repository can provide program instructions to be executed on different devices. In this implementation, the remote repository can serve as a storage medium for program code, instructions, and programs.

Software programs can be associated with clients, which can include file clients, print clients, domain clients, Internet clients, intranet clients, and other variations such as secondary clients, host clients, distributed clients, and the like. Clients may access memory, processors, computer-readable media, storage media, ports (physical and virtual), communication devices, and other clients, servers, machines, and devices through wired or wireless media. It may include one or more of such interfaces. Any method, program, or code described herein and elsewhere may be executed by a client. In addition, other devices required for execution of the methods described in this application can be considered part of the infrastructure associated with the client.

Clients may provide interfaces to other devices including, without limitation, servers, other clients, printers, database servers, print servers, file servers, communication servers, distributed servers, and the like. In addition, this coupling and/or connection can facilitate remote execution of programs across networks. Networking of some or all of these devices may facilitate parallel processing of a program or method at one or more locations without departing from the scope of the present disclosure. Additionally, any of the devices attached to the client through the interface may include at least one storage medium capable of storing methods, programs, applications, code, and/or instructions. A central repository can provide program instructions to be executed on different devices. In this implementation, the remote repository can serve as a storage medium for program code, instructions, and programs.

The methods and systems described herein can be partially or wholly deployed by a network infrastructure. A network infrastructure may include computing devices, servers, routers, hubs, firewalls, clients, personal computers, communication devices, routing devices, other active or passive devices, modules, and/or components known in the art. It can contain elements such as Computing devices and/or non-computing devices associated with the network infrastructure may include, among other components, storage media such as flash memory, buffers, stacks, RAM, ROM, and the like. The processes, methods, program code, instructions described herein and elsewhere may be performed by one or more of the network infrastructure elements. The methods and systems described herein incorporate features of Software as a Service (SaaS), Platform as a Service (PaaS), and/or Infrastructure as a Service (IaaS). It can be adapted for use with any type of private, community, or hybrid cloud computing network or cloud computing environment, including those with.

The methods, program code, and instructions described herein and elsewhere may be implemented over a cellular network having multiple cells of sender-controlled contact media content items. A cellular network may be either a frequency division multiple access (FDMA) network or a code division multiple access (CDMA) network. A cellular network may include mobile devices, cell sites, base stations, repeaters, antennas, radio towers, and the like. Cell networks may be GSM, GPRS, 3G, EVDO, mesh, or other network types.

The methods, program codes, and instructions described herein and elsewhere may be implemented on or by mobile devices. Mobile devices can include navigation devices, cell phones, mobile phones, mobile personal digital assistants, laptops, palmtops, netbooks, pagers, e-book readers, music players, and the like. These devices may include, among other components, storage media such as flash memory, buffers, RAM, ROM, and one or more computing devices. A computing device associated with the mobile device is operable to execute program code, methods, and instructions stored thereon. Alternatively, mobile devices can be configured to execute instructions in coordination with other devices. A mobile device can communicate with a base station that is interfaced with a server and configured to execute program code. Mobile devices may communicate over peer-to-peer networks, mesh networks, or other communication networks. The program code may be stored on a storage medium associated with the server and executed by a computing device embedded within the server. A base station may include a computing device and a storage device. A storage device may store program code and instructions that are executed by a computing device associated with the base station.

Computer software, program code, and/or instructions are computer components, devices, and recording media that hold digital data for use by the computer over some interval of time, and semiconductor storage known as random access memory (RAM); Mass storage for more permanent storage such as magnetic storage in forms such as optical discs, hard disks, tapes, drums, cards and other types, processor registers, cache memory, volatile memory, Non-volatile memory, optical storage such as CD, DVD, flash memory (e.g. USB stick or USB key), floppy disk, magnetic tape, paper tape, punch card, standalone RAM disk, Zip drive, removable mass storage. , off-line, etc., with removable media such as dynamic memory, static memory, read/write storage, variable storage, read-only storage, random access storage, sequential access storage, location addressable storage, file addressing stored on and/or thereon a machine-readable medium, which may include other computer memory such as readable storage, content-addressable storage, network-attached storage, storage area networks, barcodes, magnetic ink, etc. can be accessed.

The methods and systems described herein can transform physical and/or intangible objects from one state to another. Additionally, the methods and systems described herein can transform data representing physical and/or intangible objects from one state to another.

Elements described and depicted herein, including those in flow diagrams and block diagrams, throughout the figures imply logical boundaries between the elements. However, according to software or hardware engineering conventions, the depicted elements and their functions are programs stored internally as monolithic software structures, stand-alone software modules, or modules using external routines, code, services, etc. It can be implemented through computer-executable media on a machine having a sender-controlled contact media content item processor capable of executing instructions, and all such implementations are intended to be within the scope of the present disclosure. can be Examples of such machines include personal digital assistants, laptops, personal computers, mobile phones, other handheld computing devices, medical equipment, wired or wireless communication devices, transducers, chips, calculators, satellites, tablet PCs, electronic They may include, but are not limited to, books, gadgets, electronic devices, sender-controlled contact media content items, devices with artificial intelligence, computing devices, networked appliances, servers, and routers. Further, the elements depicted in the flow diagrams and block diagrams, or any other logical components, may be implemented on a machine capable of executing program instructions. Accordingly, unless explicitly stated otherwise or apparent from context, the foregoing drawings and description illustrate functional aspects of the system of the present disclosure, but no software implementation for implementing those functional aspects. No specific sequence should be inferred from these descriptions. Likewise, it will be appreciated that the various steps illustrated and described above may be modified and the order of steps may be adapted to a particular application of the technology disclosed herein. All such changes and modifications are intended to be within the scope of this disclosure. Thus, unless required by a particular application, explicitly stated, or otherwise apparent from the context, the depiction and/or description of the order of the various steps does not require a specific order of execution for those steps. Then it should not be understood.

The methods and/or processes and associated steps described above may be implemented in hardware, software, or any combination of hardware and software suitable for a particular application. The hardware may include general purpose computers and/or special purpose computing devices, or specific computing devices or particular aspects or components thereof. Processing may be implemented in one or more microprocessors, microcontrollers, embedded microcontrollers, programmable digital signal processors, or other programmable devices with internal and/or external memory. The processing may also or alternatively be embodied in an application specific integrated circuit, programmable gate array, programmable array logic, or any other device or combination of devices that may be configured to process electronic signals. can be It will further be appreciated that one or more of the processes can be implemented as computer-executable code that can be executed on a machine-readable medium.

The computer-executable code may be written in a structured programming language such as C, an object-oriented programming language such as C++, or on one of the above devices, as well as on processors, heterogeneous combinations of processor architectures, and with different hardware. Any other high-level or low-level capable of being stored, compiled, or interpreted for execution on a combination of software or on any other machine capable of executing program instructions programming languages (assembly languages, hardware description languages, and database programming languages and techniques).

Thus, in one aspect, the methods and combinations thereof described above can be embodied in computer-executable code that implements the steps of these methods when executed on one or more computing devices. In another aspect, the method may be embodied in a system that performs its steps, distributed in some manner across devices, or all of its functionality dedicated to stand-alone devices or other hardware. can be integrated within the software. In another aspect, means for performing the steps associated with the processes described above may include any of the hardware and/or software described above. All such permutations and combinations are intended to fall within the scope of the present disclosure.

While the present disclosure has been disclosed in conjunction with preferred embodiments which have been illustrated and described in detail, various modifications and improvements to these embodiments will become readily apparent to those skilled in the art. Accordingly, the spirit and scope of the present disclosure should not be limited by the foregoing examples, but should be construed in the broadest sense permitted by law.

The terms "a" and "an" and "the" and similar denotations in the context of describing the present disclosure (particularly in the context of the claims that follow) are, unless otherwise indicated herein, or shall be construed to encompass both singular and plural references unless the context clearly contradicts. The terms “comprising,” “having a sender-controlled contact media content item,” “including,” and “containing” are non-limiting terms, unless otherwise specified (i.e., “including shall be construed as "not limited to"). Recitation of value ranges herein is intended only to serve as a shorthand method for individually describing each separate value falling within the range, unless otherwise indicated herein. , each separate value is incorporated herein as if these values were individually set forth herein. All methods disclosed herein can be performed in any suitable order unless otherwise indicated herein and otherwise clearly contradicted by circumstances. The use of any and all examples or exemplary language (eg, "such as") provided herein is intended only to further clarify the disclosure of the present invention, unless otherwise claimed. No limitation is imposed on the scope of the disclosure of the present invention. No language in the specification should be construed as indicating any non-claimed element as essential to the practice of the disclosure.

While the foregoing written description will enable those skilled in the art to make and use their presently considered best mode, those skilled in the art will appreciate modifications to the specific embodiments, methods, and examples herein. , combinations, and equivalents. Accordingly, the present disclosure should be limited not by the embodiments, methods, and examples described above, but by all embodiments and methods that fall within the scope and spirit of the present disclosure.

All documents cited herein are hereby incorporated by reference.

2100 SLAM unit 2102 Microcontroller 2104 CPU
2106 IMUs
2108 quadrature decoder

Claims (28)

receiving data from an IMU device on a first frequency with a first computing module and computing a first estimated position of the mobile mapping system based at least in part on the received IMU data;
receiving the first estimated position and visual-inertial data at a second frequency with a second computing module; calculating a second position estimate;
receiving the second estimated position and laser scan data at a third computing module at a third frequency; calculating an estimated position of
A method , wherein operation of at least one computing module is bypassed . 2. The method of claim 1, wherein each of said first frequency, said second frequency and said third frequency are different from each other. 3. The method of claim 2, wherein said first frequency is higher than said second frequency, and said second frequency is higher than said third frequency. 2. The method of claim 1 , wherein the estimated positions computed by each of the remaining non-diverted modules are combined in a linear fashion. 2. The method of claim 1 , wherein the estimated positions calculated by each of the remaining non-diverted modules are combined in a non-linear fashion. 2. The method of claim 1 , wherein when the first computation module is bypassed, the second position estimate is computed from the visual inertial data without reference to the first position estimate. Method. 2. The method of claim 1 , wherein the third estimated position is calculated from the laser scan data and the first estimated position when the second computation module is bypassed. 2. The method of claim 1, further comprising determining an impairment subspace within the problem state space and calculating an estimated position within the impairment subspace. 9. The method of claim 8 , wherein the degraded subspace is a well-conditioned subspace. 2. The method of claim 1, wherein the estimated position calculated by at least one of the second computation module or the third computation module is received as input by a previous computation module. 2. The method of claim 1, wherein the mobile mapping system is adapted to operate at high angular velocities. 12. The method of claim 11 , wherein the high angular velocity comprises a rotational velocity of 360 degrees/second. 2. The method of claim 1, wherein the mobile mapping system is adapted to operate at high linear velocities. 14. The method of claim 13 , wherein the high linear velocity comprises a velocity of 100 kilometers per hour. A mobile mapping system,
a first computation module adapted to receive data on a first frequency from an IMU device and compute a first estimated position of the mobile mapping system based at least in part on the received IMU data;
receiving the first estimated position and the visual-inertial data at a second frequency and calculating a second estimated position of the mobile mapping system based at least in part on the first estimated position and the visual-inertial data; a second computing module adapted to
receiving the second estimated position and laser scan data at a third frequency and calculating a third estimated position of the mobile mapping system based at least in part on the second estimated position and the laser scan data; a third computational module that has become
A mobile mapping system , wherein operation of at least one computing module is bypassed . 16. The mobile mapping system of claim 15 , wherein each of said first frequency, said second frequency and said third frequency are different from each other. 17. The mobile mapping system of claim 16 , wherein said first frequency is higher than said second frequency and said second frequency is higher than said third frequency. 16. The mobile mapping system of claim 15 , wherein the estimated positions computed by each of the remaining non-diverted modules are combined in a linear fashion. 16. The mobile mapping system of claim 15 , wherein the estimated positions calculated by each of the remaining non-diverted modules are combined in a non-linear fashion. 16. The method of claim 15 , wherein when the first computation module is bypassed, the second position estimate is computed from the visual inertial data without reference to the first position estimate. mobile mapping system. 6. The mobile mapping system of claim 15 , wherein the third position estimate is calculated from the laser scan data and the first position estimate when the second computation module is bypassed. . 6. The mobile mapping system of claim 15 , further comprising determining an impairment subspace within the problem state space and calculating an estimated position within the impairment subspace. 23. The mobile mapping system of claim 22, wherein the degraded subspace is a well-conditioned subspace. 6. Mobile mapping according to claim 15 , wherein the estimated position calculated by at least one of the second computation module or the third computation module is received as input by a previous computation module. system. 16. The mobile mapping system of claim 15 , wherein the mobile mapping system is adapted to operate at high angular velocities. 26. The mobile mapping system of claim 25 , wherein the high angular velocity comprises a rotational velocity of 360 degrees/second. 16. The mobile mapping system of claim 15 , wherein the mobile mapping system is adapted to operate at high linear velocities. 28. The mobile mapping system of claim 27 , wherein the high linear velocity comprises a velocity of 100 kilometers per hour.

Images