New Constrained Initialization for Bearing-Only SLAM
2013 IEEE International Conference on Control System, Computing and Engineering, 29 Nov. - 1 Dec. 2013, Penang, Malaysia
New Constrained Initialization for Bearing-Only SLAM Saeed Mohammadloo
Mohammad Vali Arbabmir and Habib Ghanbarpour Asl
Young Researchers & Elite Club Garmsar Branch, Islamic Azad University Garmsar, Iran [email protected]
Aerospace Engineering Department Sharif University of Technology Tehran, Iran [email protected]
Abstract— In this paper we present a new landmark initialization technique for Bearing-Only Simultaneous Localization and Mapping (SLAM) algorithm. The initialization scheme is a type of the delayed constrained initialization method and is different from previous approaches. In our work, it is shown that the angle between sequential observations measured by a bearing-only sensor such as pan-tilt camera and the distance between the vehicle and landmark plays an important role in landmark localization accuracy. Considering this fact, a proper constrained function that utilizes the rotation angle of the camera is applied between multiple landmark location estimates and the best estimate is selected. The simulation results show that the proposed technique is capable to pick out the best initial estimate for landmark location and improves the accuracy of state estimates in Bearing-Only SLAM.
measurements from a landmark should be gathered [8]. However, as demonstrated in [9], the paths that the robot observes the landmarks have a significant effect on the quality of bearing-only observation. For more investigation, in section III by use of some simple geometric rules, we have demonstrated that the distance of a landmark from the observer and the angle between sequential measurements has an important effect on localization accuracy. This fact is a key issue in Bearing-Only SLAM. The problem of landmark initialization using only bearing measurements has been discussed in the recent literature. Munguia and Grau [10] have presented a general review on the previous approaches for landmark initialization in BearingOnly SLAM. They have included Delayed [11-12] and Undelayed [13-14] methods in their work that are the most common approaches for addressing the problem of initializing new landmarks in Bearing-Only SLAM. In this work, we have presented a new type of delayed landmark initialization method in Bearing-Only SLAM which uses a simple constraint and extracts precise initial location for landmarks. The following section briefly describes the Bearing-Only SLAM algorithm implemented by the standard EKF. Section III presents an analysis for the dependence of the accuracy of landmark initialization to the bearing-only measurements quality. In Section IV, we formulate the precise constrained bearing-only landmark initialization algorithm. Section V depicts the simulation results from the application of the proposed algorithm and section VI concludes the paper.
Index Terms— Bearing-Only SLAM, Constrained Initialization, Extended Kalman Filter
I. INTRODUCTION Simultaneous Localization and Mapping (SLAM) is the process of concurrently building a feature based map of the environment and using this map to obtain estimates of the location of the vehicle. The Extended Kalman Filter (EKF) is a usual mechanism for implementing the SLAM algorithm by which the information gathered by the vehicle can be consistently fused to yield bounded estimates of vehicle and feature locations in a recursive fashion. This algorithm first appeared in a seminal paper by Smith, Self and Cheeseman [1-2] and has been a centre point of robotic research, in recent several years [3], [4]. Typically, SLAM has been performed using range-bearing sensors, but there is another implementation for this algorithm called Bearing-Only SLAM which utilizes only bearing measurements using sensors such vision and Infra-Red sensors, which are small, reliable and cheap. Solving the Bearing-Only SLAM problem with vision as the only external sensor is now the goal of much of the effort in the area [5], [6], [7]. Bearing-Only SLAM is hindered by the landmark initialization problem, where the estimated location of a new landmark cannot be determined from a single measurement, and combined information from multiple measurements may be ill-conditioned. The problem with landmark initialization in Bearing-Only SLAM is that a single measurement is not adequate for finding the location of landmarks and at least two measurements are required. Therefore, sequential pairs of
978-1-4799-1506-4 ©2013 IEEE
II. BEARING-ONLY SLAM In this section, we will present a formalization of the bearing-only Extended Kalman Filter SLAM problem. In our work, we have considered the following assumptions: • An autonomous vehicle moves in an unknown environment. The motion of this vehicle is described by its nonlinear equations of motions. • The vehicle is equipped with an Inertial Measurement Unit (IMU) and a pan-tilt rotational camera (a bearingonly sensor). A typical IMU contains accelerometers and gyroscopes that provide information about the motion of a moving body. Using the camera, the vehicle recognizes the environmental context, detects the landmarks and provides bearing measurements from landmarks.
95
2013 IEEE International Conference on Control System, Computing and Engineering, 29 Nov. - 1 Dec. 2013, Penang, Malaysia
x l (k ) = x l (k − 1)
2-D Kinematic Model of the Vehicle An accurate vehicle model is an essential component of most localization schemes. Vehicle models can be of varying degrees of complexity. However, as is the standard for robotic navigation algorithms, we have utilized 2-D kinematic model of the vehicle. So we use a simplified two-dimensional strapdown navigation system for our vehicle. That is presented in Fig. 1.
(3)
where xl(k) is the landmark position vector at time step k. B. Sensor Model A camera is a projective sensor which measures the bearing of image features. The landmark observation process can also be modeled in state-space notation by a non-linear vector function in the discrete form of:
z(k ) = h(x (k )) + w(k )
(4)
where z is the observation made at time step k, h is a model of the observation of system states as a function of time steps and w is a random vector describing measurement corruption noise. Given the current vehicle position (xv, yv) and the position of an observed landmark (xl, yl), the observation of bearing, can be modeled as: ⎛ y (k ) − yv (k ) ⎞ ⎟ − ψ v (k ) z(k ) = arctan ⎜⎜ l ⎟ ⎝ xl (k ) − xv (k ) ⎠
Fig. 1. The vehicle states involving in the SLAM algorithm.
Our vehicle contains two accelerometers and a single axis rate gyroscope, all of which are attached rigidly to the body of the vehicle. The full sets of two dimensional strapdown navigation system equations are [15]:
C. Estimation Process Given the vehicle and observation models described in the previous section, the SLAM process consists of generating the best estimates for the system states given the information available to the system. The estimation process is accomplished using the standard form of EKF and is broken into two steps: 1) Prediction The vehicle position, heading and velocity are predicted forward in time steps using (2) with data supplied by the inertial sensors. Therefore, the state of the system at time k can be represented by the augmented state vector X(k), consisting of the states of the vehicle and the estimated position of the landmark. The state covariance P is propagated forward via:
ψ = ω zb + υ z v xi = ( f xb + υ x ) cosψ − ( f yb + υ y )sin ψ
(
)
v yi = ( f xb + υ x ) sin ψ + f yb + υ y cosψ
(1)
xi = v xi y i = v yi , where the measurements in the body axes of the vehicle in the plane of motion are denoted by the subscript b. It is assumed that navigation is required to take place with respect to a spacefixed reference frame. The measurements in this frame are denoted by the subscript i. ωzb is the measured angular rate and fxb and fyb are the measured specific forces. υx, υy and υz are dynamic noise of sensor measurements. vxi and vyi are the vehicle velocity and xi and yi are the position of vehicle. For implementation and simulation purposes, we describe a discrete time state model for vehicle using Eq. (1). Assuming that the control inputs at time k are u(k) = [ωzb(k) fxb(k) fyb(k)] and the states of the system are x(k) = [ψv(k) vxi(k) vyi(k) xv(k) yv(k)], the discrete equivalent of (1) in general form can be written as :
x(k) = f(x(k-1),u(k))
P − (k ) = ∇ X F(k )P + (k − 1)∇ X FT (k ) + ∇ u F(k )Q(k )∇ u F T (k )
(6)
where ∇ X F(k ) and ∇ u F(k ) are the jacobian of the state transition function with regard to the augment state vector ˆ + (k − 1) and the current input vector u(k) respectively. X 2) Update Assuming that we have already initialized the 2-D position of a feature, the state estimate is updated from further observation via: ˆ + (k ) = X ˆ − (k ) + K (k )ν (k ) X
(2)
(7)
The gain matrix K(k) and innovation υ(k) are calculated as :
B. Landmark Model In the context of SLAM, a landmark is a feature of the environment that can be consistently and reliably observed using the vehicle’s sensor. In the case of vision sensors, common objects such as corners are used as landmarks. For the SLAM algorithm, the feature states are usually assumed to be stationary. Therefore, the dynamic portion of the process model consists only of the vehicle model. This leads to the simple discrete landmark model:
978-1-4799-1506-4 ©2013 IEEE
(5)
(
)
ν (k ) = z (k ) − h Xˆ − (k )
(8)
K (k ) = P − (k )∇ X h T (k )S−1 (k ) S(k ) = ∇ X h (k )P − (k )∇ X h T (k ) + R (k )
(9) (10)
where ∇ x h (k ) is the jacobian of the observation model with regard to the predicted state vector Xˆ + (k − 1) and R(k) is the
96
New Constrained Initialization for Bearing-Only SLAM Saeed Mohammadloo
Mohammad Vali Arbabmir and Habib Ghanbarpour Asl
Young Researchers & Elite Club Garmsar Branch, Islamic Azad University Garmsar, Iran [email protected]
Aerospace Engineering Department Sharif University of Technology Tehran, Iran [email protected]
Abstract— In this paper we present a new landmark initialization technique for Bearing-Only Simultaneous Localization and Mapping (SLAM) algorithm. The initialization scheme is a type of the delayed constrained initialization method and is different from previous approaches. In our work, it is shown that the angle between sequential observations measured by a bearing-only sensor such as pan-tilt camera and the distance between the vehicle and landmark plays an important role in landmark localization accuracy. Considering this fact, a proper constrained function that utilizes the rotation angle of the camera is applied between multiple landmark location estimates and the best estimate is selected. The simulation results show that the proposed technique is capable to pick out the best initial estimate for landmark location and improves the accuracy of state estimates in Bearing-Only SLAM.
measurements from a landmark should be gathered [8]. However, as demonstrated in [9], the paths that the robot observes the landmarks have a significant effect on the quality of bearing-only observation. For more investigation, in section III by use of some simple geometric rules, we have demonstrated that the distance of a landmark from the observer and the angle between sequential measurements has an important effect on localization accuracy. This fact is a key issue in Bearing-Only SLAM. The problem of landmark initialization using only bearing measurements has been discussed in the recent literature. Munguia and Grau [10] have presented a general review on the previous approaches for landmark initialization in BearingOnly SLAM. They have included Delayed [11-12] and Undelayed [13-14] methods in their work that are the most common approaches for addressing the problem of initializing new landmarks in Bearing-Only SLAM. In this work, we have presented a new type of delayed landmark initialization method in Bearing-Only SLAM which uses a simple constraint and extracts precise initial location for landmarks. The following section briefly describes the Bearing-Only SLAM algorithm implemented by the standard EKF. Section III presents an analysis for the dependence of the accuracy of landmark initialization to the bearing-only measurements quality. In Section IV, we formulate the precise constrained bearing-only landmark initialization algorithm. Section V depicts the simulation results from the application of the proposed algorithm and section VI concludes the paper.
Index Terms— Bearing-Only SLAM, Constrained Initialization, Extended Kalman Filter
I. INTRODUCTION Simultaneous Localization and Mapping (SLAM) is the process of concurrently building a feature based map of the environment and using this map to obtain estimates of the location of the vehicle. The Extended Kalman Filter (EKF) is a usual mechanism for implementing the SLAM algorithm by which the information gathered by the vehicle can be consistently fused to yield bounded estimates of vehicle and feature locations in a recursive fashion. This algorithm first appeared in a seminal paper by Smith, Self and Cheeseman [1-2] and has been a centre point of robotic research, in recent several years [3], [4]. Typically, SLAM has been performed using range-bearing sensors, but there is another implementation for this algorithm called Bearing-Only SLAM which utilizes only bearing measurements using sensors such vision and Infra-Red sensors, which are small, reliable and cheap. Solving the Bearing-Only SLAM problem with vision as the only external sensor is now the goal of much of the effort in the area [5], [6], [7]. Bearing-Only SLAM is hindered by the landmark initialization problem, where the estimated location of a new landmark cannot be determined from a single measurement, and combined information from multiple measurements may be ill-conditioned. The problem with landmark initialization in Bearing-Only SLAM is that a single measurement is not adequate for finding the location of landmarks and at least two measurements are required. Therefore, sequential pairs of
978-1-4799-1506-4 ©2013 IEEE
II. BEARING-ONLY SLAM In this section, we will present a formalization of the bearing-only Extended Kalman Filter SLAM problem. In our work, we have considered the following assumptions: • An autonomous vehicle moves in an unknown environment. The motion of this vehicle is described by its nonlinear equations of motions. • The vehicle is equipped with an Inertial Measurement Unit (IMU) and a pan-tilt rotational camera (a bearingonly sensor). A typical IMU contains accelerometers and gyroscopes that provide information about the motion of a moving body. Using the camera, the vehicle recognizes the environmental context, detects the landmarks and provides bearing measurements from landmarks.
95
2013 IEEE International Conference on Control System, Computing and Engineering, 29 Nov. - 1 Dec. 2013, Penang, Malaysia
x l (k ) = x l (k − 1)
2-D Kinematic Model of the Vehicle An accurate vehicle model is an essential component of most localization schemes. Vehicle models can be of varying degrees of complexity. However, as is the standard for robotic navigation algorithms, we have utilized 2-D kinematic model of the vehicle. So we use a simplified two-dimensional strapdown navigation system for our vehicle. That is presented in Fig. 1.
(3)
where xl(k) is the landmark position vector at time step k. B. Sensor Model A camera is a projective sensor which measures the bearing of image features. The landmark observation process can also be modeled in state-space notation by a non-linear vector function in the discrete form of:
z(k ) = h(x (k )) + w(k )
(4)
where z is the observation made at time step k, h is a model of the observation of system states as a function of time steps and w is a random vector describing measurement corruption noise. Given the current vehicle position (xv, yv) and the position of an observed landmark (xl, yl), the observation of bearing, can be modeled as: ⎛ y (k ) − yv (k ) ⎞ ⎟ − ψ v (k ) z(k ) = arctan ⎜⎜ l ⎟ ⎝ xl (k ) − xv (k ) ⎠
Fig. 1. The vehicle states involving in the SLAM algorithm.
Our vehicle contains two accelerometers and a single axis rate gyroscope, all of which are attached rigidly to the body of the vehicle. The full sets of two dimensional strapdown navigation system equations are [15]:
C. Estimation Process Given the vehicle and observation models described in the previous section, the SLAM process consists of generating the best estimates for the system states given the information available to the system. The estimation process is accomplished using the standard form of EKF and is broken into two steps: 1) Prediction The vehicle position, heading and velocity are predicted forward in time steps using (2) with data supplied by the inertial sensors. Therefore, the state of the system at time k can be represented by the augmented state vector X(k), consisting of the states of the vehicle and the estimated position of the landmark. The state covariance P is propagated forward via:
ψ = ω zb + υ z v xi = ( f xb + υ x ) cosψ − ( f yb + υ y )sin ψ
(
)
v yi = ( f xb + υ x ) sin ψ + f yb + υ y cosψ
(1)
xi = v xi y i = v yi , where the measurements in the body axes of the vehicle in the plane of motion are denoted by the subscript b. It is assumed that navigation is required to take place with respect to a spacefixed reference frame. The measurements in this frame are denoted by the subscript i. ωzb is the measured angular rate and fxb and fyb are the measured specific forces. υx, υy and υz are dynamic noise of sensor measurements. vxi and vyi are the vehicle velocity and xi and yi are the position of vehicle. For implementation and simulation purposes, we describe a discrete time state model for vehicle using Eq. (1). Assuming that the control inputs at time k are u(k) = [ωzb(k) fxb(k) fyb(k)] and the states of the system are x(k) = [ψv(k) vxi(k) vyi(k) xv(k) yv(k)], the discrete equivalent of (1) in general form can be written as :
x(k) = f(x(k-1),u(k))
P − (k ) = ∇ X F(k )P + (k − 1)∇ X FT (k ) + ∇ u F(k )Q(k )∇ u F T (k )
(6)
where ∇ X F(k ) and ∇ u F(k ) are the jacobian of the state transition function with regard to the augment state vector ˆ + (k − 1) and the current input vector u(k) respectively. X 2) Update Assuming that we have already initialized the 2-D position of a feature, the state estimate is updated from further observation via: ˆ + (k ) = X ˆ − (k ) + K (k )ν (k ) X
(2)
(7)
The gain matrix K(k) and innovation υ(k) are calculated as :
B. Landmark Model In the context of SLAM, a landmark is a feature of the environment that can be consistently and reliably observed using the vehicle’s sensor. In the case of vision sensors, common objects such as corners are used as landmarks. For the SLAM algorithm, the feature states are usually assumed to be stationary. Therefore, the dynamic portion of the process model consists only of the vehicle model. This leads to the simple discrete landmark model:
978-1-4799-1506-4 ©2013 IEEE
(5)
(
)
ν (k ) = z (k ) − h Xˆ − (k )
(8)
K (k ) = P − (k )∇ X h T (k )S−1 (k ) S(k ) = ∇ X h (k )P − (k )∇ X h T (k ) + R (k )
(9) (10)
where ∇ x h (k ) is the jacobian of the observation model with regard to the predicted state vector Xˆ + (k − 1) and R(k) is the
96
