Online Observability-Constrained Motion Suggestion via Efficient Motion Primitiv

Zheng Rong, Shun’an Zhong and Nathan Michael

(1.School of Information and Electronics, Beijing Instituteof Technology, Beijing 100081, China; 2.Institute of Electronics, Chinese Academy of Sciences, Beijing 100190, China; 3.Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA)

A reliable state estimation is essential for an autonomous vision-based navigation system such as a mobile robot to enable accurate localization and environment perception with respect to undefined environments[1].Reliable perception systems depend on the availability of sufficient informative sensor observations and environmental information to ensure a continued accurate performance[2]. State estimation methodologies assume the existence and preservation of observability (the ability of the system states to be reconstructed from system output)[3]. However, this preserved observability condition may be violated in environments with limited or scarce information, resulting in increased estimation uncertainty and even divergence, especially for monocular vision-based autonomous systems.

In recent years, observability analysis techniques have received significant attention. Observability analysis provides a tool to evaluate the observability conditions with respect to the current system state and environment. Observability-constrained active control techniques leverage observability analysis to quantify the implications of sensor observations on a state estimate accuracy toward an informed trajectory selection[3], faster estimator convergence[4], and optimal sensor placement[5]. Observability-constrained vision navigation systems[6-8]detect the unobservable direction and reduce the estimation inconsistency by explicitly prohibiting spurious information gain. Trajectory is optimized to guarantee the sensing observability and controlling stability of the system[9]. Sampling trajectories are selected to maximize observability to localize the robot and construct the map of the environment[10-11]. Path planning has also been incorporated with visibility constraints towards the goal of less exploration time in unknown environments[12]. However, these studies focused on the observability analysis of the current system states, without considering the future observability and impact of future motions on the system observability. Local observability prediction can be of great value in many problematic situations where the reconstruction of the state is difficult and need to be sensed actively, such as robot localization in an unknown environment[9]. By characterizing the impact of future motion on system observability, this technique enables informed selection of future motions to avoid the potential observability degradation, and consequently to improve state estimation performance.

In this work, an online methodology is proposed, seeking to predict the local observability conditions and suggest observability-constrained motion directions toward enabling robust state estimation and safe operation for an autonomous system in unknown environments.The formulation leverages efficient numerical observability analysis and motion primitive technique to realize the local observability prediction, and accordingly suggest the future motion directions to avoid the potential estimation degradation due to observability deficiency. Following the prior work[13], the empirical observability Gramian (EOG)[14]is employed to enable a real-time local observability evaluation. A motion primitive[15]technique is utilized to enable local path sampling and observability-constrained motion suggesting. We assess the implication of potential motions on the system observability and resulting state estimation performance by evaluating the observability of potential trajectories and seek to preserve the estimation consistency by explicit motion direction selection.

The proposed approach is specialized to a monocular visual-inertial state estimation framework to assess the viability of the proposed approach to correctly predict the observability of future motions and effectively making motion suggestions to avoid the potential state estimation degeneracy of the perception system. Monocular visual-inertial state estimation methodology is a representative vision-based perception strategy that enables autonomous operation with resource-constrained systems such as a micro aerial vehicle and commodity devices.This choice of sensing modalities also represents a particularly challenging state estimation formulation due to the lack of the direct metric depth observation[16]. It is thereby essential for such a system to assure a full-state observability to achieve the state estimation accuracy.

1 Methodology

This section briefly summarizes relevant concepts related to monocular visual-inertial state estimator and EOG-based observability analysis based on prior works[2,13,15,17-19], and introduces a method to predict the observability of future motions and propose motion direction suggestions.

1.1 EOG-based observability evaluation for mono V-I estimator

The optimization-based monocular visual-inertial state estimation problem is formulated with respect to a sliding window that containsnvisual keyframes and a local map containingmlandmarks observed by the key frames, which is solved via a recursive optimization strategy. The full state parameters to be estimated are formulated as a vector X∈10n+m+3,

(1)

(2)

where zimuand zcamare inertial measurement unit (IMU) and camera measurements, rimuand rcamare the residuals of IMU and camera measurements, Pimuand Pcamare the corresponding residual covariance. An IMU pre-integration technique[20]is employed to represent the IMU measurement. The resulting integrated system is treated as sensor outputs in the optimization and EOG state-output simulation.

The system observability is analyzed given the monocular vision-based sliding window state estimation formulation by the efficient EOG computation[13]. The EOG provides insight into the sensitivity of the system outputs with respect to the perturbed system states and captures the ability of the estimator to reconstruct the system states given the sensor outputs by numerically simulating the system state-output behaviors. Crucially, computation of the EOG is independent of state estimation and is therefore a viable observability analysis technique, which makes it possible to perform the observability prediction even in a degraded estimation scenario.Benefiting from the additive property, the EOG can be efficiently computed by partitioning the system output into sub-vectors and exploiting the underlying formulation sparsity of the EOG. The full EOG W is readily computed by perturbing the system initial conditions x∈ndirectly in positive and negative directions and simulating the state-output 2ntimes.

(3)

1.2 Generation and observability evaluation of motion primitive

Motion primitives are incorporated with the EOG evaluation to enable a motion extrapolation and subsequent local observability prediction. The EOG is computed for motion primitive end points based on the current state to enable the observability prediction locally.

A motion primitive generation strategy[15]is employed to compute primitives based on a discretized set of initial and final conditions with different trajectory durations. While the approach extends readily to three dimensions, in this work,for simplicity of presentation, we compute motion primitives for a system in two dimensions. The computation is decoupled into translation and heading trajectory generation with higher-order end point constraint bounds that correspond to the expected rates of motion exhibited during the simulation and experimental studies. After the computation of the motion primitive library, a look-up-table is generated to enable efficient online queries to find the appropriate end point states for observability evaluation.

1.3 Trajectory observability evaluation

To predict the local observability condition and suggest next-step motion direction, a trajectory tree with specified number of levels and branches is generated to realize extrapolation. As shown in Fig.1, a 3-level extrapolation frame with 3-branch motion primitives is generated, and totally 27 potential trajectories in the local region based on the current state are created. The observability conditions of potential trajectories composed of a series of motion primitive end points are evaluated by synthesizing the observability measure of involved end points. For simplicity of representation and without loss of sensitivity due to wide camera FOV, we compute each motion primitive with three spreading branches. The orientation of the system is instantaneously in the direction of the motion.

Fig.1 Example of a 3-level extrapolation frame with 3-branch motion primitives

Using the trajectory tree with multiple level extrapolation, the observability evaluation enables local observability prediction with a certain degree to avoid a sudden dead end from which the state estimator may not recover. The observability cost for each trajectory is computed using a weighted strategy, i.e. the closer steps use the larger weighting factor, based on the fact that the closest step has more accurate observability evaluation and is more crucial in near-term motion execution than later steps.Thus for a specified trajectory withNsteps (end points), the observability measureKcan be computed using all the observability measureskiof end points and preset weighting factorswi

(4)

As the observability prediction for the first step is more accurate than later steps, a more conservative strategy can be applied by executing one-step motion according to the multi-level evaluation result, without loss of foresight. Under this scenario, it is preferable to evaluate the observability measure for a motion direction instead of a specified trajectory. The observability cost for each motion direction can be computed using all the observability measures of the involved motion primitives.

(5)

in which,Nis the number of levels,Mis the number of motion primitives in each level of one motion direction,k(i,j)is the observability measure of thejthmotion primitive inithlevel, and the motion primitives in the same level use identical weighting factorwi. Considering the example in Fig. 1, the future motion for current stateSis spread out with three motion directionsS1,S2,S3, and the observability cost of each direction can be evaluated using the involved 13 end points. For example, the observability cost of the second directionis computed as

(6)

Fig.2 Motion suggestions for a predefined simulation scenario and straight trajectory

Thus the observability constrained motion direction can be suggested according to the resulting observability measures, and then can be incorporated with other motion constraints to yield an informed motion planning for the next step. Note that in the actual application of this strategy, the system doesn’t have to wait for the completion of the one-step execution to start the next motion suggestion. Instead, a denser motion suggestion can be generated and followed after each state estimation of a new coming frame, so that the new sensor observations can be added into the system and the environment information can be updated to yield a more accurate local observability prediction and consequently better motion suggestion.

1.4 Observability-constrained motion direction suggestion

After computing the observability cost for all the motion directions, the following strategy in Algorithm 1 can be used to propose the motion suggestion. An example of motion suggestion along a predefined trajectory in a simulated environment is shown in Fig.2. The corresponding online observability prediction results in three directions (left, forward, or right) is shown in Fig.3. Different motion directions are suggested due to the different landmark distribution in three stages. A threshold is used to identify the “forbidden direction”. Motion planner prefers the motion direction with a higher observability measure.

Fig.3 Observability prediction and resulting motion direction suggestion

Algorithm1Strategy of motion direction suggestion

Assume there aremmotion directions to be evaluated.

Initial condition:

Kmax=0;

imax=1;

forbidden_directions.clear();

Loop i: from 1 tom

ifKi

end.

ifKi>Kmax, thenKmax=Ki,imax=i;

end.

end loop.

Results:

·Forbidden_directionsindicates the directions in which the system is believed to experience an observability-deficient condition and prone to estimation degradation or failure;

·imaxindicates the most-suggested direction. Even in the worst case whereKmax

·Other directions are moderately suggested, in which the observability condition is believed to be able to provide sufficient information to ensure reasonable state estimation performance.

A reasonable extrapolation range is essential to make sure the system can make prediction with sufficient foresight and take action in advance to avoid an irreversible degraded situation. An extrapolation with too short distance cannot provide sufficient predictive information, while a large extrapolation distance cannot ensure the prediction accuracy as the system cannot get sufficient environment information and consequently cannot predict the sensor observation accurately. For example, in a confined environment the camera observation may change significantly along the trajectory and the system can hardly predict the feature observation for a far end point. The extrapolation distance is controlled by the velocity of the final state and extrapolation time duration. In this work, we assume the constant linear velocity magnitude and choose different extrapolation duration according to the scenario type. In a confined scenario, smaller duration is used, as the effective sensor observation prediction distance is limited by the environment. In an open scenario, a larger extrapolation duration can be used. The extrapolation width is controlled by the branch number and the heading separation between the branches. As a camera with wide FOV is employed in this study, the configuration of three branches and 45-degree separation is reasonable without any loss of sensitivity and computing efficiency.

2 Evaluation and Analysis

The proposed motion direction suggesting approach is evaluated given the optimization-based monocular visual-inertial state estimation formulation through simulation and real-world experiments. The perception system including a monocular camera and IMU (simulated and real) is carried along trajectories in different environments. By the simulation analysis we seek to demonstrate the expected ability and efficacy of the proposed approach both on the local observability prediction and motion direction suggestion. Our current study seeks to verify the effectiveness of the proposed methodology in real-world scenarios.

2.1 Implementation detail

Fig.5 Predicted observability, actual observability, and estimation covariance of the two trajectories

The simulation and experimental analysis leverage the same algorithm implementations, including the optimization-based sliding-window state estimator and local observability prediction-based motion suggestion as presented in sect. 1.4. We employ a time-synchronized monocular camera and IMU in the experiment. The model accurately simulates the associate sensor characteristics and uncertainty. Ceres solver[22]is utilized to solve the optimization problem, and analytic Jacobians are employed to ensure run-time performance. The full sliding window size of the state estimator is set to 30, and the update rate of optimization and motion suggestion is 10 Hz.The motion primitives are generated with three directions and four levels for a reasonable extrapolation distance and width, and consequently 120 observability measure of end points are evaluated. The EOG-based observability evaluation is established with a fixed perturbation size of 0.001 and translational states perturbed. Note that we use different threshold in simulation and experiment to detect the observability deficiency due to different scenario model. From the actual experiment we find that for the similar environment model (for example outdoor open scenario) a well-chosen threshold can be used and doesn’t need further adjustment.

2.2 Simulation results

In the simulation two pre-defined trajectories are tested in the same scenario (Fig.4). Straight trajectory-1 experienced observability degradation due to feature deficiency, while trajectory-2 is always involved in a feature-rich area. The observability measures of three motion directions are evaluated to make the prediction, and the actual current observability measure is used as a reference (Fig.5).

Fig.4 Pre-defined simulation scenario and two trajectories

Firstly, the observability deficiency in trajectory-1 (left column) is correctly predicted (by 8.5 s) with a threshold of 0.3 applied on the predicted observability measure. The correctness of the prediction can be verified by the actual observability measure and the consequent state estimation performance degradation indicated by the increasing covariance. Secondly, during the duration [60 s, 100 s], left direction is strongly suggested, while the actual motion disobeyed the suggestion and entered into the observability-deficient area, which results in the consequent uncertainty with a degraded state estimation. On the contrary, trajectory-2 (right column) executed a motion following the suggested direction to avoid the feature-deficient area, yielding a reasonable observability measure along the trajectory and having a consistent estimated performance. The estimation performance can be checked by both the estimation covariance (Fig.5) and estimated path (Fig.6). The observability deficiency in trajectory-1 results in a big jump in estimated path and end point, while trajectory-2 yields a result with limited uncertainty.

Fig.6 Visualized estimation results, including estimated path and features

Note that in trajectory-1, the prediction of the observability degeneracy occurred 8 s earlier than the actual occurrence of the degeneracy, permitting a motion planner to take actions before entering into a pathological scenario. Although trajectory-2 is much longer than trajectory-1, the state estimation along trajectory-2 preserved a better performance with the well selected motion direction. Ideally, the best trajectory can be selected as the motion direction that keeps the highest observability value, but in actual application the motion planning should be constrained by both the observability and user goal. Note that trajectory-2 is generated based on the direction suggestion in trajectory-1 and actual feature distribution model in the simulated environment.

2.3 Experimental results

In the experiment, the system moves in a hallway with different observability conditions representing degraded scenarios such as white wall, narrow space, sharp turning, and dark lighting. As shown in Fig.7,four representative cases and three prediction points are studied.

Fig.7 Experiment scenario illustrated by estimated path and environment features

Fig.8 Predicted observability in three directions, actual observability, and estimation covariance

Fig.9 Camera images overlaid with online motion direction suggestion in four representative cases

The observability measure in three motion directions are predicted and motion directions are suggested accordingly (Fig.8). The actual observability measure is given as a reference to verify the efficacy of the prediction, while the estimation covariance is used to evaluate the actual estimation performance. Four representative cases and three prediction points are marked. In case-1, forward motion is mostly suggested according to the highest observability prediction, and the actual motion followed this direction, which preserved a good observability condition with little state estimation uncertainty. In case-2, the potential degeneracy in forward motion is successfully predicted.While the system kept moving forward, as expected, it experienced an actual observability degeneracy, which results in an increased estimation covariance. Before the sharp turn in case-3, moving right is strongly recommended due to the highly confined environment. The suggestion is followed by an actual motion. As a result, the actual observability increased significantly and the estimation uncertainty decreased to a low level. In case-4, before the system entering the dark area the degeneracy in future motion is successfully predicted and the violation of the suggestion results in a significantly increased state uncertainty.

The four representative scenarios include two true-positive predictions (1, 3) and two true-negative predictions (2, 4). In the true positive cases, the system proposed “should go” direction (green arrows overlaid on the camera images), and the actual motion followed the suggestion and consequently yielded a good state estimation performance. On the contrary, in the true negative cases the system moved along the predicted “forbidden” direction (red arrows on the camera images), eventually entering the degenerate conditions and inducing significant state estimation degradation. The true-positive and true-negative cases demonstrate the correctness of the observability prediction in motion directions with informative and deficient sensor observation, respectively. The corresponding images (Fig.9) captured by the camera are given to exhibit the visual environment conditions. Green arrows (G) indicate the most suggested directions, blue (B) for the moderately suggested directions and red (R) for the forbidden directions. Note that the arrows indicating the motion suggestions are generated and overlaid on the camera image online.

Similar to the simulation test, the degenerate conditions are successfully predicted prior to the actual occurrence of the degeneracy. Three representative prediction points (A′,B′,C′), and the corresponding actual occurrence points (A,B,C) are checked,tA′=12.35 s,tA=12.85 s,tB′=78.75 s,tB=79.45 s,tC′=86.95 s,tC=88.35 s. The prediction is 0.5 s, 0.7 s, 1.4 s earlier than the degeneracy occurrence, respectively.

3 Conclusion

An online observability-constrained motion suggesting methodology is proposed in this paper. The proposed approach seeks to make informed motion suggestion for a representative monocular visual-inertial state estimation system to preserve the robust estimation performance. An efficient EOG-based observability evaluation technique and motion primitives are incorporated to enable a local observability prediction and real-time motion suggestion, which makes it possible to evaluate an observability for 120 end points locally within 25 ms using one thread on a 2.9 GHz Intel Core-i7 laptop.The approach is evaluated by both simulation and experiments. The results demonstrate the correctness of the local observability prediction and efficacy of the motion suggestion.

The observability-constrained motion suggestion is an active strategy to ensure the state estimation performance of a vision-based autonomous system towards safe operation in an undefined environment. To achieve the goal, we will incorporate the proposed approach with the controlling system to enable observability-constrained planning in challenging operation environments.

[1] Lupton T, Sukkarieh S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics,2012, 28(1): 61-76.

[2] Hinson B T. Observability-based guidance and sensor placement[D]. Washington: University of Washington, 2014.

[3] Alaeddini A, Morgansen K A. Trajectory design for a nonlinear system to insure observability[C]∥2014 European Control Conference (ECC), Strasbourg, France, 2014.

[4] Hinson B T, Binder M K, Morgansen K A. Path planning to optimize observability in a planar uniform flow field[C]∥2013 American Control Conference (ACC), Washington, DC, USA, 2013.

[5] Qi J, Sun K, Kang W. Optimal PMU placement for power system dynamic state estimation by using empirical observability gramian[J]. IEEE Transactions on Power Systems,2015, 30(4): 2041-2054.

[6] Hesch J A, Kottas D G, Bowman S L, et al. Observability-constrained vision-aided inertial navigation, 2012-001[R]. Minneapolis: Dept of Comp Sci & Eng, MARS Lab, University of Minnesota, 2012.

[7] Kottas D G, Hesch J A, Bowman S L, et al. On the consistency of vision-aided inertial navigation[C]∥Experimental Robotics, Switzerland, 2013.

[8] Hesch J A, Kottas D G, Bowman S L, et al. Towards consistent vision-aided inertial navigation[M]. Berlin: Springer, 2013: 559-574.

[9] Alaeddini A, Morgansen K A. Trajectory optimization of limited sensing dynamical systems[EB/OL]. [2017-02-15].https:∥arxiv.org/abs/1611.08056.

[10] Lorussi F, Marigo A, Bicchi A. Optimal exploratory paths for a mobile rover[C]∥IEEE International Conference on Robotics and Automation (ICRA), Seoul, Korea, 2001.

[11] Devries L, Majumdar S J, Paley D A. Observability-based optimization of coordinated sampling trajectories for recursive estimation of a strong, spatially varying flowfield[J]. Journal of Intelligent & Robotic Systems,2013, 70(1-4): 527-544.

[12] Richter C, Roy N. Learning to plan for visibility in navigation of unknown environments[C]∥International Symposium on Experimental Robotics (ISER), Tokyo, Japan, 2016.

[13] Rong Z, Michael N. Detection and prediction of near-term state estimation degradation via online nonlinear observability analysis[C]∥2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Lausanne, Switzerland, 2016.

[14] Singh A K, Hahn J. On the use of empirical gramians for controllability and observability analysis[C]∥American Control Conference, Portland, Oregon, USA, 2005.

[15] Mueller M W, Hehn M, D’andrea R. A computationally efficient motion primitive for quadrocopter trajectory generation[J]. IEEE Transactions on Robotics,2015, 31(6): 1294-1310.

[16] Hansen P, Alismail H, Rander P, et al. Monocular visual odometry for robot localization in LNG pipes[C]∥2011 IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 2011.

[17] Lall S, Marsden J E, Glavaški S. Empirical model reduction of controlled nonlinear systems[C]∥IFAC World Congress, New York, USA, 1999.

[18] Qi J, Sun K, Kang W. Adaptive optimal PMU placement based on empirical observability Gramian[C]∥10th IFAC Symposium on Nonlinear Control Systems NOLCOS, Monterey, California, USA, 2016.

[19] Shen S J, Michael N, Kumar V. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs[C]∥IEEE International Conference on Robotics and Automation (ICRA) Washington, USA, 2015.

[20] Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration theory for fast and accurate visual-inertial navigation[EB/OL]. [2017-02-12].https:∥pdfs.semanticscholar.org/ed4e/9f89d1fbcf 50bea8c65b947b6397a61b4945.pdf.

[21] Krener A J, Ide K. Measures of unobservability[C]∥48th IEEE Conference on Decision and Control (CDC/CCC), Shanghai, China, 2009.

[22] Agarwal S, Mierle K. Ceres solver[EB/OL]. [2016-08-14].http:∥ceres-solver.org/.