Integrity Risk-Based Model Predictive Control for Mobile Robots
Osama Abdul Hafez, Guillermo Duenas Arana, student member IEEE, and Matthew Spenko, member IEEE
Abstract— This paper presents a Model Predictive Controller (MPC) that uses navigation integrity risk as a constraint. Navigation integrity risk accounts for the presence of faults in localization sensors and algorithms, an increasingly important consideration as the number of robots operating in life and mission-critical situations is expected to increase dramatically in near future (e.g. a potential influx of self-driving cars). Specifically, the work uses a local nearest neighbor integrity risk evaluation methodology that accounts for data association faults as a constraint in order to guarantee localization safety over a receding horizon. Moreover, state and control-input constraints have also been enforced in this work. The proposed MPC design is tested using real-world mapped environments, showing that a robot is capable of maintaining a predefined minimum level of localization safety while operating in an urban environment.
INTRODUCTION
Traditionally, pose estimation performance is quantified using a covariance matrix or particle spread 1, 2. How-ever, these methods only take into account non-faulted cases, and thus may be insufficient for life- and mission-critical applications, such as self-driving cars or other co-robots 3. Faults, such as GNSS-clock errors 4 or mis-associating features extracted from the environment (e.g. from lidar, radar, or camera data) with the wrong landmark may occur regularly in mobile robot navigation. While some faults may be easily detected and removed, others remain undetected, and not accounting for the impact of those faults can lead to an increased safety risk.
Best services for writing your paper according to Trustpilot
To address this issue, prior work has taken inspiration from the aviation industry to evaluate a robot’s integrity risk, or the probability that a robot’s pose estimate lies outside pre-defined limits, a safety metric that considers both faulted and non-faults situations 5, 6, 7, 8, 9, 10. This paper builds upon that work to present a Model Predictive Controller (MPC) that uses navigation integrity risk as a constraint.
Several methods have been developed to predict the integrity risk in GNSS-based aviation applications 11,
However, these methods cannot be directly applied to mobile robots because ground vehicles operate under sky-obstructed areas where GNSS signals can be altered or blocked by buildings and trees. Therefore, additional sensors are required to localize ground vehicles, such as lidar. An integrity monitoring algorithm for lidar-based localization using an Extended Kalman Filter (EKF) coupled with a
*This work is supported by NSF Grant #1637899
O. A. Hafez, G. D. Arana, and M. Spenko are with the Mechanical, Materials, and Aerospace Engineering Department, Illinois Institute of Technology, Chicago, IL, USA [email protected], [email protected], [email protected]
global nearest neighbor data association algorithm has been developed in prior work 6, 9. However, the computational complexity of the global nearest neighbor algorithms restricts its online applicability. In response, a local nearest neighbor-based integrity risk evaluation methodology was developed
This paper uses that implementation and investigates the use of a local nearest neighbor integrity risk metric as a constraint for mobile robot based MPC to guarantee a predefined minimum level of localization safety.
There has been relatively little work in this area. 13, 14, 15, 16 employ the notion of “safe driving envelope” in designing MPCs for autonomous vehicles, but the approach concentrates mostly on collision avoidance, and presents neither rigorous proof of integrity nor practical safety levels.
uses the concept of “collision risk assessment” in de-signing a lidar-based predictive safety control algorithm for excavators. The approach is promising, but does not account for possible faults. Recently, there has been research on tube-based model predictive control, which tries to find the solution that keeps a robot as far as possible from obstacles or tracks a road’s center line in the presence of disturbances 18, 19, 20. But again, the approach assumes that all disturbances are zero mean random variables (non-faulted). In fact, most of the previous work uses state estimation error covariance as the basis for risk assessment, which is proven to not be sufficient 21, 22. In contrast, the MPC presented here generates control actions that guarantee a minimum level of localization safety during the receding horizon while taking into account possible faults in feature/landmark data associations.
The paper is organized as follows. Section II provides a mathematical background to the problem while Section III introduces the proposed model predictive controller. Results based on experimental data collected in an urban campus are discussed in Section IV. Finally, Section V presents conclusions and future work.
BACKGROUND
This section introduces the necessary mathematical nota-tion and explains the integrity monitoring algorithm used in the paper.
A. State Evolution Model
This work is motivated by its potential benefit and appli-cability to self-driving cars. As such, a no-slip kinematic bicycle model is used to model the robot (see Fig. 1). However, the work is general enough to be applied to any
where X and Y represent the center of mass position in the inertial frame, V is the rear wheel velocity, and Lr is the distance between the rear wheel and the center of mass. The vehicle state is given as x = X; Y; T , and the control inputs as u = V; T where is the steering angle.
The state evolution for the kinematic bicycle model is modeled as a non-linear function of the prior states, xk 2 Rm, and control-inputs, uk 2 Rn, with additive noise:
xk+1 = g(xk; uk) + wk where wk N (0; Wk) (2)
where g( ; ) is the discretized version of (1), uk is the control-input vector (determined by the MPC), and wk is white Gaussian noise with known covariance Wk.
800100649605
L ? Y Lr V ? X Fig. 1. Bicycle model schematic mobile robot motion model. The model is given as: 0 Y_ 1 = 0 Lr cos( ) tan( ) 1 (1) _ V cos( ) Lr sin( ) tan( ) L X V sin( ) + B L C _ V tan( ) @ A B C L @ A
where xk is the pose estimate mean after the EKF prediction step. Joerger et. al 6 proved that the innovations are normally distributed as:
i i;t ti;t i ti ti T i ; Yk k N yk (5) where Yk , Hk PkHk + Vk is the innovation covari- ance matrix. The innovation mean measures the separation between the expected measurement of landmark t and the expected measurement of the actual landmark corresponding to extracted feature i, which is unknown:
ti;t (6) yk , hti (xk) ht(xk) Thus, the innovation is only zero mean when the correct association between feature and landmark is selected, i.e. t = ti, in which case the r.h.s. of (6) becomes zero.
The next section briefly presents the local nearest neigh-bor data association process, which employs the innovation vectors as criteria to select correct associations.
D. Data Association
Given a set of nF extracted features and nL previously mapped landmarks, each feature i is associated to landmark t if the following criteria is met:
1 ; T where t = argmin 1 (7) k i;t kYi t k i;tkYi Threshold T is a user defined paramter and k i;tkY2 i 1 = i;tTYi 1 i;t is the weighted norm of the individual innovation vector of extracted feature i and landmark t. Note that an incorrect association occurs when the criteria in (7) is met for t = t , which means that feature i will be associated to 6 i a landmark, t , other than ti. B. Measurement Model In the next section, the integrity risk of the system is evaluated when incorrect associations occur. In feature-based navigation, raw sensor data is reduced E. Hazardous Misleading Information to a number of distinguishable features. This work assumes access to a map consisting of nL landmarks from which nkF In this work, the system’s integrity risk is evaluated as features are extracted at time k, such that nkF nL, where the probability of Hazardous Misleading Information (HMI). each feature is assumed to provide the same number of mea- HMI occurs when the estimate error in the state of interest surements, mF . Measurements belonging to each extracted exceeds a predefined alert limit (see Fig. 2), i.e.: feature zki 2 RmF are concatenated into the measurement HM I = T ^ > l vector zk 2 Rnk , which comprises a total of nk = nkF mF k k (8) measurements at time k. The feature extracted from landmark where 2 R m state of interest, l is t is denoted it, whereas the landmark from which feature i is a vector selects the ^ ^ has been extracted is denoted ti. The measurement model the permissible error limit, and k = xk xk is the EKF update estimate error. for a single extracted feature i is: The system’s integrity risk must be evaluated under both zki = hti (xk) + vki (3) faulted and fault-free hypotheses. This work focus on data where hti ( ) is the measurement function of landmark t and association faults, which occur when extracted features are i associated to the incorrect landmarks as explained in Sec- i vk is the white Gaussian noise with known covariance matrix tion II-D. An incorrect association event, IAk, at time such that vki N 0; Vki . k occurs as soon as one feature is incorrectly associated C. Innovation Vector while a correct association, CAk, indicates that all extracted features at k are correctly associated. Similarly, situations The discrepancy between a feature’s measurements and the where at least one incorrect association has occurred up to expected measurements from a landmark is the innovation. and including the current time are considered faulted while The innovation vector of feature i and landmark t is: only when all landmarks have been correctly associated i;t i (4) is it considered a fault-free condition. Then, denoting the k = zk ht(xk) 1228090636270
Fig. 2. Integrity risk representation for autonomous vehicle applications. The integrity risk is the probability of the car being outside the alert limit requirement box (blue shaded area) when it was estimated to be inside the box. If the primary concern is the lateral deviation, then the alert limit is the distance l between the edge of the car and the edge of the lane.
fault free condition as CAK = fCA1; : : : ; CAkg and its complementary event as IAK , the integrity risk is:
CONTROLLER DESIGN
Model predictive control has become increasingly popular in robotics 23, 24, 25, and a detailed description of the technique can be found in 26. In mobile robotics, most model predictive controllers predict future system states using a linear kinematic model 23. Model predictive con-trollers based on a linear time varying representation of the system have been applied to trajectory tracking in 27, 28. Nonlinear formulations for trajectory tracking have been shown in 29, 30.
A. Cost Function
In this work, the model predictive controller is designed to find the control input sequence that minimizes a quadratic cost function with a finite Receding Horizon N:
P (HM Ik) = P (HM Ik; CAK ) + P (HM Ik; IAK )(9)
where an uppercase time subscript K denotes all time up to and including time k. Rewriting (9) using the total probability theorem and bounding P (HM Ik j IAK ) by one, we define an upper bound on the integrity risk as:
P (HM Ik)1 + (P (HM Ik j CAK )1) P (CAK ) (10)
The first term in the right hand side of (10) is the integrity risk under fault-free conditions, which is evaluated by integrating the tails of the estimate pose distribution in the state of interest as:
P (HM Ik j CAK ) = P T ^k > l j CAK = 2 l ^k (11) T ^ where is the standard normal CDF and ^k = Pk is the standard deviation in the state of interest error. p The second term in (10) evaluates the probability of every landmark up to and including k being correctly associated,
which can be recursively computed as: P (CAK ) = P (CAk j CAK 1)P (CAK 1) (12)
where P (CA0) = 1. A lower bound on the probability of correct associations at k is presented in 8:
N J(k) = X( xkT+ijk R( kref+i)T Q R( kref+i) xk+ijk
i=1 uTj R uk+i 1jk) (15) k+i 1 k
ref is the difference between the where xk+ijk = xk+ijk xk+i predicted states given ^xk (estimated states at k using EKF) and the reference states (predicted tracking error) expressed
ref is the in the inertial frame; uk+i 1jk = uk+i 1jk uk+i 1 difference between the predicted optimal control given ^xk and the reference control determined by the path (predicted control error); R( kref+i) is a rotation matrix that transforms the predicted tracking error from the inertial frame to the path reference frame (x-axis is aligned with the tangent of the path); and R 2 Rn n and Q 2 Rm m are positive semi-definite weighting matrices.
MPC formulation requires converting the optimal control problem to a static optimization problem expressed as a function of the receding horizon control sequence. Therefore, every variable in the optimal control problem needs to be written in terms of the predicted control error over the horizon. The receding horizon predicted control and tracking error, respectively, are defined as:
Uk = uT uT ::: uT T (16) kjk k+1jk k+N 1jk T T T T (17) Xk = xk+1jk xk+2jk ::: xk+Njk P (CAk j CAK 1) 1 nkF + where Uk 2 RnN and Xk 2 RmN . It can be shown that n F ^ k 1 2 2 Xk can be expressed as a function of Uk and xkjk using m+mF yk Yk yk Yk i=1 4 (18) X 2 ti ti Xk = k x^k k + k Uk X max i 1 ; i 1 T the state evolution model (2): (13) j where X2 is the chi-squared CDF with a degrees of where k 2 RmN nN and k 2 RmN m are defined in a 23. By defining 2 RnN nN and k 2 RmN mN as a freedom and: T ti 1 = min ti;t (14) block matrices with R and R( kref+i) Q R( kref+i) (arranged i i 1 from i = 1; :::; N) as a diagonal elements, respectively, the yk Yk t6=ti yk Yk cost function can be expressed as: evaluates the minimum separation between landmark ti and the rest of the map.
The next section develops a model predictive controller that uses integrity risk as a constraint to guarantee localiza-tion safety.
T T Uk J(k) = Xk k Xk + Uk ( k ^xkjk + k Uk)T k( k ^xkjk + k Uk)
UTkUk
(19)
The cost function can be written in its standard form by where ck+r 2 Rmxyz , mxyz is the number of positioning expanding (19) then removing the terms that do not contain states, and L 2 Rmxyz m is the positioning states extraction Uk (as they don’t affect the optimal solution): matrix. The probabilistic bound vector at k over the receding 1 UkT Gk Uk + fkT Uk (20) horizon is defined as: J(k) = T Ck = ckT+1 ckT+2 ::: ckT+N (31) 2 where Gk 2 RnN nN and fk 2 RnN are defined as: By defining 2 RNmxyz Nm as a block matrix with Gk = 2( kT k k + ) (21) L as a diagonal elements, the positioning states inequality T (22) constraints can be written as a linear combination of the ^ optimization variable Uk using (18) as follows: fk = 2 k k k xkjk B. Control input and States constraints Xmin ref +Ck Xmax ref Ck Xk Xk Xk In this work, inequality constraints on control input states (32) have been enforced, expressed in terms of the optimization ref ^ + Ckk Uk variable Uk: Xmin Xk k xkjk (33) ref ^ Umin UkrefUk Umax Ukref Ck (23) Xmax Xk k xkjk ref is the reference control input over the horizon where Xkref is the reference states over the horizon, at which where Uk ^ is the at which the state evolution model is linearized. the state evolution model is linearized, and xk jk A covariance propagation model over the receding horizon difference between the EKF state estimate and the reference state at the beginning of the horizon. needs to be specified, in order to build a probabilistically valid states inequality constraint. EKF covariance propaga- C. Integrity Risk Constraint tion equations can be used to propagate uncertainty over Unlike control input and state constraints, integrity risk the horizon (without measurements) by using the linearized constraints cannot be expressed as a linear combination of the states and measurement matrices around the reference path. optimization variable Uk. Therefore, a proper formulation The linearized states and measurements matrices at epoch k must be defined in order to relate this complex metric to the and prediction step r is: control variation over the receding horizon. To this end, the Ak+r = @g(xk+r; uk+r) (24) integrity risk constraint is defined as: @xk+r ref ref P (HM I ) Ireq r = 1; 2; :::; N (34) uk+r j k;xk+r k k+rjk j 8 Hk+r = @h(xk+r) (25) where P (HM Ik+rjk) is the predicted integrity risk at epoch ref @xk+r k k and prediction step r given the estimated states and xk+r j covariance at k, and I req is the integrity risk requirement. where the size of h(xk+r) 2 n R k+r are determined by This requirement is intended to be determined by some the predicted number of landmarks in the field of view regulating agency. For example, the Federal Aviation Ad- (FoV) at epoch k and prediction step r. Kalman Filter-based ministrations (FAA) navigation integrity requirement for an covariance propagation during the horizon can be written as: aircraft precision approach is between 10 7 to 10 9 5. ^ T + Wk+r 1 The integrity risk constraint formulation as a function of Pk+rjk = Ak+r 1Pk+r 1jkAk+r 1 (26) the optimization variable, Uk, is illustrated in Algorithm T + Vk+r (27) 1. At the first prediction step, the non-linear state evolution Yk+r = Hk+rPk+rjkHk+r ^ = I kH T Y 1 Hk+r (28) model is used to predict the next states xk+1jk as a function Pk+r k Pk+r Pk+r k of the estimated states xk k = x^k (EKF state estimate ^ j j k+r k+r j with real measurements) andj the optimization variable ukjk. where Pk+rjk is the predicted covariance matrix at epoch k Then, the predicted (EKF) next state covariance matrix and prediction step r, given the covariance at k and assuming that all landmarks are detected from k to k + r. In this work, Pk+1jk is found by propagating the estimated states covari- ^ the state constraints are applied on the positioning part of ance matrix Pkjk = Pk (EKF states covariance matrix) using ^ (26). the states. For every Pk+rjk during the prediction horizon, The predicted measurements for every FoV landmark are the eigenvalue problem needs to be solved: computed. Subsequently, the predicted mis-association fault ^ T (29) mean between every pair of FoV landmarks is evaluated Pk+rjk = Qk+rDk+rQk+r followed by the minimum mean innovation norm for every where Dk+r are the eigenvalues (diagonal) matrix, and Qk+r FoV landmark. Thereafter, the upper-bound on the correct are a matrix its columns are the normalized eigenvectors. The ) is evaluated followed association probability P (CA w-sigma probabilistic bound on positioning states is defined ^ K+1jk as: ck+r = w max hdiag LT Dk1=+2rL i L by EKF covariance update Pk+1jk (without measurements) using (28). After that, the predicted integrity risk in the (30) first prediction horizon is evaluated as a function of the optimization variable ukjk, and augmented in the non-linear constraint vector c. Finally, the process repeats by propagat-
^
ing the predicted states xk+1jk and covariance matrix Pk+1jk in the second prediction horizon.
0151130
Algorithm 1 Integrity risk constraint formulation
Given
– Landmark map
0-298450
3: – Correct Association probability at k P (CAK )
4: – Predefined integrity risk requirement Ireq
^^
5: – States estimate and covarience matrix at k ( xk ; Pk )
6: -Linearizedstatesrecedinghorizonmatrices
fAk; :::; Ak+N 1g
– Linearized controls receding horizon matrices fBk; :::; Bk+N 1g
– Optimization variables f ukjk; :::; uk+N 1jkg
9: -recedinghorizonReferencecontrolinputs
ref ref
fukjk ; :::; uk+N 1jkg
for every prediction step r do
Prediction xk+rjk = g(xk+r 1jk; uk+r 1jk +
uref ) k+r 1jk 12: EKF covarience prediction Pk+rjk using (26)13: Calculate the predicted measurements for FoV land- marks 14: Evaluate the predicted mis-association fault mean be- ti;tj (ti 6= tj 8ij) tween every pair of FoV landmarks yk+r using (6)15: Find the minimum mean innovation norm for every FoV landmark ti Yi 1 using (14) yk+r j 16: Calculate k+r ) by evaluating CA P ( K+r k P (CAk+rjkjCAK+r 1jk) using (13) then substituting the result in (12)^
17:EKF covarience update Pk+rjk (without measure-ments) using (28)18:Compute the non-faulted integrity risk P (HM Ik+rjk j CAK+rjk) using (11)Find the integrity risk P (HM Ik+rjk) by substituting
j
P (HM Ik+rjkCAK+rjk) and P (CAK+rjk) in (10)Augment P (HM Ik+rjk) Ireq into the non-linear constraints vector c
end for
Return c
018415
IV. RESULTS
This section presents the results of applying the proposed MPC to a simulated mobile robot (modeled using eq. (1)) navigating in an experimentally mapped urban environment (see Fig. 3). A feature extractor, based on 31, is imple-mented to extract tree trunks and light posts. For life-critical co-robotics applications, ignoring the impact of misassocia-tions may result in an unpredictable positioning errors that are unbounded by the 3 covariance envelope (bounds the estimate error with 0:997 probability). For example, if the mobile robot were a self-driving car, the state estimate may be in the wrong lane, which is extremely dangerous. In the
Fig. 3. The experimentally mapped environment used for testing. Note the two features highlighted by the red rectangle on the right side.
6985-2449195
First element of MPC receding horizon Environment control sequence minimize cost funcon over receding horizon ; Robot subject to state, Probability control input, of correct and integrity risk associaon constraints Integrity monitor Feature range Map obtained a priori EKF and bearing using EKF-SLAM measurements Fig. 4. The proposed Control Scheme 13335-2426335
testing environment, landmarks are well-spaced (which leads to low P (HM I)) except for a light post and a column, as shown enclosed in red rectangle in the lower right side of Fig. 3. Mapping is done by fusing GPS positioning measurements, Inertial Measurement Unit (IMU), and lidar features using EKF-SLAM in a loose coupling fashion 32. The experimental setup is comprised of two Velodyne VLP-16 lidars, a STIM-300 IMU, and a Novatel SPAN-CPT GPS reciever.
The control scheme is illustrated in Fig. 4. A simu-lated lidar provides range and bearing measurements to the mapped landmarks (mF = 2). Lidar measurements are disturbed by Gaussian white noise (see Table I). Constant velocity is assumed, thus creating a path tracking problem (the penalization of longitudinal error in the path reference frame is set to zero). The path planner specifies the path to be a straight line from the current estimated state to the goal state. Therefore, the linearization, during the horizon, will be based on the path specified at the beginning of the horizon.
A comparison study between MPC with and without the integrity risk constraint has been conducted to investigate its impact on localization safety. Fig. 5 shows the robot’s trajec-
TABLE I
SIMULATION PARAMETERS
31115131445
Std. dev. lidar range, bearing 0:1 m, 2 X(m); Y (m); ( )min;max (10; 80) ( 2:5; 2:5) ( 45; 45) ( ); V (m=s)min;max ( 45; 45)(4; 4) Goal states X(m); Y (m); ( ) 0 1:5 0 R 5 0 Q 0 5 0 3 2 0 20 4 0 0 0 5 0:05 0 0 2 0 0 20 3 W 0 0:05 0 0 0 0:002 5 lidar range 15 m 4 Sampling time 0:1 s Prediction horizon 20 steps Alert limit 1 m Ireq 10 8 20 15 w/o integrity risk constraint w/ integrity risk constraint 10 Mapped landmarks using EKF-SLAM 5 (m) 0 Start Goal Y -5 -10 -15 -20 -100 -80 -60 -40 -20 0 20 31115-453771031115-2499360405130-2034540
X (m)
Fig. 5.Mobile Robot trajectory
tory for MPC with and without the integrity risk constraint. With the integrity risk constraint, the mobile robot turns to the left when it reaches X 50m, because the MPC predicts that staying on the original course would lead to an integrity risk that exceeds Ireq after 2 s (prediction horizon N dt) due to the chance of mis-associating the lamppost and column shown in Fig. 3 .
Specifically, the model predictive controller increases the distance between the two landmarks in the measurement space (range and bearing). Consequently, the probability of correct associations drops more for the case of MPC without the integrity-risk constraint than with the integrity risk con-straint ( 0:9999994 vs 0:9999999975 after passing the critical region), which is two orders of magnitude smaller. Therefore, the integrity risk exceeds the localization safety threshold without constraining the integrity ( 6 10 7 > Ireq), whereas constraining integrity preserves the integrity risk below the localization safety threshold ( 2:5 10 9 < Ireq), as shown in Fig. 6.
10-6
509270-109855
10-8
)
k
P(HMI
10-10
w/o integrity risk constraint
w/ integrity risk constraint
10-12 2 4 6 8 10 12 14 0 Time (s)
Fig. 6.Integrity Risk P (HMIk)
CONCLUSION AND FUTURE WORK
This paper investigates the application of integrity risk as a constraint in a model predictive controller for mobile robot tracking problems. The results show that the MPC is able to perform maneuvers needed to maintain a minimum level of localization safety. The work has implications in life- and mission-critical mobile robot applications, where not accounting for the probability of undetected faults can have a significant impact on safety. In future work, we will investigate the derivation of the integrity risk for obstacle avoidance problems.
REFERENCES
G. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba, “A Solution to the Simultaneous Localization and Map Building (SLAM) Problem,” IEEE Trans. on Robotics Automation, vol. 17, no. 3, pp. 229–241, 2001.
J. Leonard and H. Durrant-Whyte, Directed Sonar Sensing for Mobile Robot Navigation. Kluwer Academic Publishers, 1992.
N. A. Othman and H. Ahmad, “The analysis of covariance matrix for kalman filter based slam with intermittent measurement,” in Proc. Int. Conf. Systems, Control, and Informatics, 2013.
T. Walter and J. Blanch, “Characterization of gps clock and ephemeris errors to support araim,” 2015.
R. T. C. for Aeronautics Special Committee 159, “Minimum Aviation System Performance Standards for the Local Area Augmentation System (LAAS),” Document RTCA/DO-245, 2004.
M. Joerger, M. Jamoom, M. Spenko, and B. Pervan, “Integrity of laser-based feature extraction and data association,” in 2016 IEEE/ION PLANS, April 2016, pp. 557–571.
M. Joerger, G. D. Arana, M. Spenko, and B. Pervan, “Landmark selection and unmapped obstacle detection in lidar-based navigation,” Proceedings of the 30th International Technical Meeting of The Satellite Division of the Institute of Navigation (ION GNSS+ 2017), Sep 2017. Online. Available: http://par.nsf.gov/biblio/10072545G. D. Arana, M. Joerger, and M. Spenko, “Local nearest neighbor integrity risk evaluation for robot navigation,” ICRA, 2018.
M. Joerger and B. Pervan, “Quantifying safety of laser-based naviga-tion,” IEEE Transactions on Aerospace and Electronic Systems, pp. 1–1, 2018.
M. Joerger, G. Duenas Arana, M. Spenko, and B. Pervan, “A new approach to unwanted-object detection in gnss/lidar-based navigation,” Sensors, vol. 18, no. 8, 2018. Online. Available:
http://www.mdpi.com/1424-8220/18/8/2740R. G. Brown, “A baseline gps raim scheme and a note on the equivalence of three raim methods,” Navigation, vol. 39, no. 3, pp. 301–316, 1992.
B. W. Parkinson and P. Axelrad, “Autonomous GPS Integrity Moni-toring Using the Pseudorange Residual,” Navigation, vol. 35, no. 2, pp. 225–274, 1988.
J. Lee, B. Kim, J. Seo, K. Yi, J. Yoon, and B. Ko, “Automated driving control in safe driving envelope based on probabilistic prediction of surrounding vehicle behaviors,” SAE Int. J. Passenger Cars-Electronic and Electrical Systems, vol. 8, no. 2015-01-0314, pp. 207–218, 2015.
S. M. Erlien, S. Fujita, and J. C. Gerdes, “Shared steering control using safe envelopes for obstacle avoidance and vehicle stability,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 2, pp. 441–451, Feb 2016.
M. Brown, J. Funke, S. Erlien, and J. C. Gerdes, “Safe driving envelopes for path tracking in autonomous vehicles,” Control Engineering Practice, vol. 61, pp. 307 – 316, 2017. Online. Available: http://www.sciencedirect.com/science/article/pii/ S0967066116300831J. Suh, H. Chae, and K. Yi, “Stochastic model-predictive control for lane change decision of automated driving vehicles,” IEEE Transac-tions on Vehicular Technology, vol. 67, no. 6, pp. 4771–4782, June 2018.
K. Oh, S. Park, J. Seo, J.-G. Kim, J. Park, G. Lee, and K. Yi, “Development of a predictive safety control algorithm using laser scanners for excavators on construction sites,” Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, vol. 0, no. 0, p. 0954407018764046, 0. Online. Available: https://doi.org/10.1177/0954407018764046Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli, “A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles,” Vehicle System Dynamics, vol. 52, no. 6, pp. 802–823, 2014. Online. Available: https://doi.org/10.1080/00423114.2014.902537E. Kayacan, E. Kayacan, H. Ramon, and W. Saeys, “Robust tube-based decentralized nonlinear model predictive control of an autonomous tractor-trailer system,” IEEE/ASME Transactions on Mechatronics, vol. 20, no. 1, pp. 447–456, Feb 2015.
F. Ke, Z. Li, and C. Yang, “Robust tube-based predictive control for visual servoing of constrained differential-drive mobile robots,” IEEE Transactions on Industrial Electronics, vol. 65, no. 4, pp. 3437–3446, April 2018.
N. A. Othman and H. Ahmad, “The analysis of covariance matrix for kalman filter based slam with intermittent measurement,” in Proc. Int. Conf. Systems, Control, and Informatics, 2013.
M. V. A. Soloviev, C. Yang and C. Taylor, “Assured vision aided inertial localization,” in Proc. 27th Int. Tech. Meeting Satellite Div. Inst. Navigation (ION GNSS+ 2014), Tampa, Florida, 2014.
K. Kanjanawanishkul and A. Zell, “Path following for an omnidirec-tional mobile robot based on model predictive control,” in 2009 IEEE International Conference on Robotics and Automation, May 2009, pp. 3341–3346.
P. Bouffard, A. Aswani, and C. Tomlin, “Learning-based model predic-tive control on a quadrotor: Onboard implementation and experimental results,” in 2012 IEEE International Conference on Robotics and Automation, May 2012, pp. 279–284.
K. Alexis, C. Papachristos, G. Nikolakopoulos, and A. Tzes, “Model predictive quadrotor indoor position control,” in 2011 19th Mediter-ranean Conference on Control Automation (MED), June 2011, pp. 1247–1252.
D. Mayne, J. Rawlings, C. Rao, and P. Scokaert, “Constrained model predictive control: Stability and optimality,” Automatica, vol. 36, no. 6, pp. 789 – 814, 2000. Online. Available: http://www.sciencedirect.com/science/article/pii/S0005109899002149G. Klanar and I. krjanc, “Tracking-error model-based predictive control for mobile robots in real time,” Robotics and Autonomous Systems, vol. 55, no. 6, pp. 460 – 469, 2007. Online. Available: http://www.sciencedirect.com/science/article/pii/S0921889007000140W. F. Lages and J. A. V. Alves, “Real-time control of a mobile robot using linearized model predictive control,” IFAC Proceedings Volumes, vol. 39, no. 16, pp. 968 – 973, 2006, 4th IFAC Symposium on Mechatronic Systems. Online. Available: http://www.sciencedirect.com/science/article/pii/S1474667015342944R. Hedjar, R. Toumi, P. Boucher, and D. Dumur, “Finite horizon nonlinear predictive control by the taylor approximation : Application to robot tracking trajectory,” 2005.
D. Gu and H. Hu, “Receding horizon tracking control of wheeled mobile robots,” IEEE Transactions on Control Systems Technology, vol. 14, no. 4, pp. 743–749, July 2006.
C. Cabo, C. Ordoez, S. Garca-Corts, and J. Martnez, “An algorithm for automatic detection of pole-like street furniture objects from mobile laser scanner point clouds,” ISPRS Journal of Photogrammetry and Remote Sensing, vol. 87, pp. 47 – 56, 2014. Online. Available: http://www.sciencedirect.com/science/article/pii/S092427161300230XJ. Nieto, T. Bailey, and E. Nebot, “Scan-slam: Combining ekf-slam and scan correlation,” in Field and Service Robotics, P. Corke and S. Sukkariah, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 2006, pp. 167–178.
