Abstract
Abstract : This paper addresses movement and observation planning for mobile robots under position uncertainty. A sequence of actions that minimizes the total time needed for reaching a destination while guaranteeing that the collision probability is less than a prescribed threshold is planned. The problem is formulated as a path planning problem on a roadmap with additional constraints on covariance matrices expressing position uncertainty, for which a novel branchand-bound based solution is presented. Moreover, a heuristic technique for creating a roadmap based on a new criterion that encapsulates both collision safety and localization ability is proposed. Simulation study is performed to evaluate computational complexity and relations between some characteristic parameters and obtained solutions.
Introduction
One of the most basic tasks required to a mobile robot is to move to a destination point swiftly and safely without human intervention. To accomplish this objective, the robot plans a path to the destination using a map and moves through the environment along the planned path. As the robot travels a longer distance, however, uncertainty of its position increases due to disturbance and the error of odometry. Moreover, in urban navigation, GPS information is unreliable since its resolution (typically in the order of meters) is not high enough and it becomes unavailable when satellites are occluded by buildings. In such situations, the robot needs to observe surrounding objects using its sensors and match sensory information with the map in order to accurately localize itself in the environment,
Introduction
In the mobile robot literature, path planning and selflocalization are often treated as independent topics. In selflocalization studies, it is often the case that the robot is either human-operated or simply moves along a given series of viapoints. Most path planning studies, on the other hand, assume that a robot always has access to accurate enough self-position. However, path planning and self-localization are highly interdependent. For example, one cannot expect to reduce position uncertainty by performing observation in an area where no distinct landmark is around. Moreover, rough terrains, compared to flat roads, create larger disturbance and thus have larger risk of deteriorating position accuracy.
Introduction
One way to consider the effect of self-localization in path planning is to incorporate some sensing criteria into a cost function. The sensory uncertainty field (SUF) [1] quantifies the accuracy of localization that is based on range sensing and line fitting. The value of SUF is used in the cost function as a weighting factor for the distance between adjacent points along a path. The coastal navigation technique [2] quantifies infor-mation content as the mutual information of self-position probability distribution before and after sensing. The cost function in this case is defined as the weighted sum of path length and information content.
Introduction
Using the theory of Bayesian estimation, one can evaluate the change of position uncertainty and the risk of collision in a probabilistic and model-based manner. When one assumes (local) linearity for movement and observation models, position uncertainty is expressed in terms of Gaussian distribution and change in its mean and covariance by movement and observation can by calculated by simple algebraic operations using Extended Kalman Filter (EKF). In [3] , [4] , the volume of uncertainty ellipsoid determined by the covariance matrix is used as a penalty factor in the cost function. In [5] , the expected value of path length with respect to position probability distribution is used as a cost function. In more recent works, collision avoidance is treated as explicit constraints. This includes chance constraints [6] , non-intersecting constraints between uncertainty ellipsoids and obstacles [7] , and matrix inequality constraints on convariance matrices [8] .
Introduction
Most path planning studies considering position uncertainty intends to minimize the cost of movement, sometimes augmented with some uncertainty measure, and very few of them consider the cost of observation. In fact, in most case, observation is considered to take place every time the robot makes a move. In [8] , observation is executed periodically in a fixed time interval. However, some types of mobile robots needs to stop or at least to slow down at observation points, and therefore the cost of observation is significant. For example, small-sized mobile robots, which are sensitive to the unevenness of urban roads, should decrease its moving speed at observation points in order to reduce sensing error caused by vibration. In addition, when a robot performs omni-directional observation using a sensor with limited field of view, the robot should stop and rotate its sensor to obtain an omni-directional view at the observation point. In such situations, not only sensing itself and associated signal processing but also time required for stopping or slowing down adds to observation cost. When this point is taken into account, it becomes rather beneficial to skip observa- tions if the robot is far from obstacles and the risk of collision is low, or if there are few visible landmarks around and thus expected reduction in position uncertainty is small. In [9] , planning of both path and observation points using a cost function expressing risk of collision and observation cost is performed on a roadmap embedded in a workspace. This study employs a simple heuristic model of position uncertainty in which the uncertainty increases monotonically while the robot moves and it is reset to a fixed level after every observation. Moreover, the roadmap is constructed based solely on the topology of free space.
Introduction
Based on the above background, this research proposes a planning method that minimizes the time required for both movement and observation while ensuring that the collision probability is below a threshold. In the following sections, we first introduce the movement and observation models of a mobile robot. Next, the planning problem is formulated as a path planning problem on a roadmap embedded in a workspace. Following a similar line to some of the previous works, we rely on the theory of Extended Kalman Filter for estimating the evolution of position uncertainty. The main contribution of this work is twofold. First, a heuristic roadmap construction technique based on a simple measure that encapsulates both risk of collision and localization ability is proposed. Second, a planning formulation in which movement and observation are treated as independent actions with associated costs is presented together with an efficient solution algorithm based on branchand-bound. The characteristics of the proposed method is evaluated from different aspects in numerical simulations.
Introduction
The following notation is used in this paper. For two integers i 1 and i 2 , [i 1 : i 2 ] denotes a set of integers {i 1 
Problem Setting

Workspace and Robot's Motion and Observation Models
In the following, the model of a workspace and that of a mobile robot equipped with a range sensor are explained. The workspace is a bounded rectangular region and several polygonal obstacles are placed inside the workspace. A set of points in the workspace that are not included in the obstacles is called the free space. The workspace is denoted by X ⊂ R 2 and the free space is denoted by X f ⊂ X. A mobile robot is considered to be omni-directional; it can move in any direction without making a turn. The position of the robot in the workspace is defined as the state of the robot and it is denoted by x ∈ R 2 . When a robot moves to a new position x , it moves along a straight line connecting x and x in a constant speed v. Hence the state transition can be written as follows:
Workspace and Robot's Motion and Observation Models
Here, u ∈ R 2 denotes the control input, which is a displacement in the robot's position in a single move. The amount of time spent by a move is given by u /v. Moreover, ν ∈ R 2 is a random variable expressing the error of movement whose probability is given by the normal distribution N(0, Q). Here, the covariance Q is considered to be proportional to the travel distance:
Workspace and Robot's Motion and Observation Models
Here, Q r denotes the movement error rate, the growth rate of the covariance with respect to the travel distance.
Workspace and Robot's Motion and Observation Models
The robot is equipped with a sensor that can measure the distance to nearby obstacles in all directions in 2D plane. In a single observation, the robot measures the distance to obstacles in the interval of 2π divided by M, where M denotes the directional resolution of the sensor. The measurement variable is defined as a M-dimensional vector
Workspace and Robot's Motion and Observation Models
where
Workspace and Robot's Motion and Observation Models
is a function that returns the minimum distance from the point p to an obstacle in the direction given by θ. The M-dimensional vector denotes the sensing error and its probability density function is given by N(0, R), where R denotes the covariance matrix.
Task Description and Planning on a Roadmap
A task the robot must achieve is to move from a starting point to a destination point in minimal time while avoiding the obstacles. As the robot travels longer distance, the error of its position grows larger. Therefore, in order to safely navigate through the obstacles, the robot should observe the surrounding scene and reduce its position uncertainty at some intermediate points along the path to the destination. As already mentioned, observation costs some amount of time because of temporary slowing down, signal processing and localization computation. This means the number of observation should be as small as possible in order to minimize the time to reach the destination.
Task Description and Planning on a Roadmap
Planning both movement and observation in a continuous 2D plane is quite difficult, however. Here, we address the planning problem on a discrete roadmap structure that consists of via-points distributed over the free space and connectivities between them. A roadmap is defined as an undirected graph G = N, L , where N denotes the set of nodes (via-points) and L ⊂ N×N denotes the set of links. We must have a node placed at the starting point and one at the destination point. These nodes are denoted by n s ∈ N and n g ∈ N, respectively. The position of the via-point corresponding to the node n is denoted by p n ∈ X f . When planning on a roadmap, the robot is limited to move between via-points whose nodes are connected by a link, and the locations at which it makes observations is also limited to via-points. Symbolically, a move is expressed as a transition between different nodes and an observation can be expressed seen as a transition from a node to itself (a self-loop). In this setup, a movement and observation planning problem reduces to a path planning problem on a graph, which is informally stated as follows:
Task Description and Planning on a Roadmap
Time-optimal path planning under position uncertainty: Find a path from n s to n g such that the probability of collision based on the position uncertainty is smaller than a threshold, and the transit time is minimized. Figure 1 illustrates the relation between obstacles, a roadmap, and a path. A path is depicted by a series of nodes connected by arrows. The change of position uncertainty along the path is depicted by ellipsoids. In the figure, the 4th and the 6th nodes of the path are selected as observation points. A path is considered safe if all ellipsoids do not intersect with obstacles. A more formal definition of the above problem is given by (15) in Section 4. For preparation, a quantitative analysis of the change of self-position uncertainty during motion and observation is briefly reviewed in the next Section.
Analysis of Self-Position Uncertainty Based on Extended Kalman Filter
In EKF-based localization, the estimated position is expressed as a normally distributed random variable. Let us denote this by x ∼ N(μ, Σ). Here, the tolerance region in which the robot's position is included with the probability p is given as an ellipsoid:
Analysis of Self-Position Uncertainty Based on Extended Kalman Filter
Here, the right hand side is the quantile function of the χ-square distribution with n degrees of freedom. Substituting the state dimension 2 to n and setting p as 99.9% gives the right-hand-side value of approximately 13.82. Let us call the ellipsoid given by (4) the tolerance ellipsoid and denote it by X tol (μ, Σ).
Movement update
Let (μ, Σ) and (μ , Σ ) be a pair of the expected value and the covariance of the estimated position before and after a move, respectively. Then we have
Movement update
where Q is given by (2) . Observation update Now, let (μ, Σ) and (μ , Σ ) be a pair of the expected value and the covariance of the estimated position before and after an observation, respectively. Moreover, let y be the value of a measurement. Then we have the following:
Movement update
where I is the Fisher information matrix of the sensor model given by
Movement update
From (6a), we can see that the amount of reduction in position uncertainty is determined by the matrix I. Roughly speaking, as the elements in I take larger values, the elements in postobservation covariance Σ take smaller values. Moreover, since I is given by (7), the elements in I take large values when the error of measurement (determined by R) is small and the partial derivatives (sensitivity) of the measurement variable with respect to position is large. Therefore, the matrix I contains quantitative information of how much and in which direction the position uncertainty can be reduced by making an observation in a specified position. For illustration, Fig. 2 shows tolerance ellipsoids after making an observation at different points in an example workspace with the vertical size of 25 m. Here, tolerance ellipsoid before observation is set as a circle with the radius 0.1 m. For visualization purpose, the ellipsoids in the figure are drawn larger than its actual size. The sensing range is set as 1.5 m. When an obstacle boundary is visible within the sensing range, the tolerance ellipsoid contracts in the perpendicular direction to the boundary after observation. Moreover, the magnitude of contraction becomes larger when the observation point is closer to a boundary or more than one boundary is visible. This is because, from (7), the elements of I become greater as the number of reflected rays of the range sensor increases.
Time-Optimal Movement and Observation Planning Considering Position Uncertainty

Planning Problem Formulation
First, we formulate a movement and observation planning problem as a path planning problem on a roadmap. The actions the robot takes (each of them is either movement or observation) are counted by steps. Let n k be the node the robot will be at when it makes the k-th action. Moreover, let μ k and Σ k denote the expected value and the covariance of the robot position at the k-th step. The initial condition is given by
Planning Problem Formulation
Here, Σ s denotes the initial uncertainty. Moreover, the terminal condition is n N = n g (9) where N denotes the number of steps in the action sequence. One may possibly impose an explicit constraint on the terminal covariance, but this time we accept arbitrary terminal covariance as long as it satisfies the collision avoidance constraint explained later.
Planning Problem Formulation
At the k-th step (k < N), the robot makes a move if n k n k+1 and it makes an observation if n k = n k+1 . When the robot moves to another node, a control input that drives the robot from the expected value of the current estimated position μ k to the next via-point p n k+1 is applied; thus,
Planning Problem Formulation
Therefore, we have
Planning Problem Formulation
where Q k is given by Q r u k . Next, we consider when the robot makes an observation. For updating the estimated position using (6b), we need a sample of measurement variable, y k . But since planning is done before the robot makes an actual observation, a sample value is obviously unavailable. To cope with this, we replace y k with the expected measurement value, which is given by h(μ k ). Thus we obtain
Planning Problem Formulation
In the above update equation, the expected position is unchanged by observation. Note that the covariance update equation is unaffected by this approximation. From (11b) and (12b), at any step k ∈ [0 : N], the expected position is located exactly at a via-point and the position uncertainty is expressed by Σ k . The collision avoidance constraint is handled in the following way. The estimated position at the k-th step is expressed by the normal distribution N(μ k , Σ k ). If the following condition is satisfied, the probability of collision at the node n k is less than 1 − p:
Planning Problem Formulation
Finally, the cost function is defined as follows.
Planning Problem Formulation
This cost function measures the total amount of time needed for executing a given sequence of actions. The movement time is given by the distance between two via-points divided by the moving speed. The observation time is given by a constant α. Summarizing the above, we obtain the following optimization problem.
Planning Problem Formulation
As discussed earlier, the "planned" expected position μ k matches with the via-point position p n k and therefore it does not have to be included in decision variables. Note that the number of steps N is not a constant but also subject to optimization. The feasibility of the above problem depends on the configuration of obstacles and a roadmap. The following set of conditions gives a sufficient condition for the feasibility of (15).
Planning Problem Formulation
• The roadmap has at least one connected path from n s to n g .
Planning Problem Formulation
• For each node n ∈ N, I n is non-singular.
Planning Problem Formulation
• For each pair of nodes
Planning Problem Formulation
The first condition is obviously required. The second condition indicates that the position covariance Σ can be made arbitrarily small by repeating observation updates (6a) sufficiently many times at every node. This fact combined with the third condition ensures that (13) can be fulfilled after every movement. In Section 4.3, we discuss a heuristic method for constructing a roadmap that satisfies the above conditions.
Remark 1
In the above problem, we impose collision avoidance constraints at via-points only; we do not exclude the possibility of collision while moving between via-points. One way to handle this would be, instead of performing collision test of ellipsoids, to perform collision test for a convex-hull of two ellipsoids; one at a via-point the robot moves from, and another at the via-point the robot moves to. For ease of implementation, however, we circumvent this situation simply by not connecting a pair of nodes with a link when there is a fear of moving very close to an obstacle.
Solution Based on Branch and Bound
The solution space of the problem (15) is an infinite set since the step length N is indefinite. Even if N is fixed, the number of paths grows exponentially with respect to N. Moreover, the covariance matrix Σ k is dependent on the whole history of transitions from the starting position. This makes the application of recursive algorithms such as dynamic programming difficult. Here, we employ the branch and bound technique to obtain a globally optimal solution while dramatically reducing the number of paths to evaluate. For preparation, let us define the following relaxed problem obtained simply by removing the collision avoidance constraints from (15).
Solution Based on Branch and Bound
In this relaxed problem, observation becomes a useless action just to pay extra cost. This means it is equivalent to the shortest path problem, which can be computed efficiently by dynamic programming. Let f relax (n) be a function that returns the minimum cost of the relaxed problem taking n as the starting node. This provides a good lower bound that can be used to prune unnecessary branches in the search tree.
Solution Based on Branch and Bound
The algorithm is shown in Algorithm 1. The algorithm has a global variable D(n), which stores a tuple (T , Σ, J) for each n ∈ N. Here, T stores the current-best path to n, Σ stores the terminal covariance at n and J stores the cost of T . The procedure SEARCH evaluates a partial path T and expands the search tree if necessary. First, at line 8, the tail element of T is obtained. Next, the path goes through three tests. The first test is the collision test (line 10). If a collision is detected in COLLISION, this path is truncated and no further search is performed. The second test is what we call the forward pruning (line 11). In the function FORWARD, the heuristic cost of T , which is given by the sum of "cost-so-far" J and "cost-to-go" f relax (n), is compared with the current best cost to n g . If the former is greater than or equal to the latter, this path is truncated.
Algorithm 1 Branch and Bound Algorithm for Action Planning
return D(n g ) 6: end procedure
Algorithm 1 Branch and Bound Algorithm for Action Planning
if collision(p n , Σ) = false, return 11:
Algorithm 1 Branch and Bound Algorithm for Action Planning
if forward(n, Σ, J) = false, return 12:
Algorithm 1 Branch and Bound Algorithm for Action Planning
if backward(n, Σ, J) = false, return 13:
Algorithm 1 Branch and Bound Algorithm for Action Planning
for n ∈ N adj (n) in ascending order of f relax (n ) do 14:
Algorithm 1 Branch and Bound Algorithm for Action Planning
if n n g and J + f relax (n) ≥J, return false 25:
Algorithm 1 Branch and Bound Algorithm for Action Planning
if n = n g and J <J,
Algorithm 1 Branch and Bound Algorithm for Action Planning
return true 27: end function
Algorithm 1 Branch and Bound Algorithm for Action Planning
if J ≥J and Σ ≥Σ, return false 31:
Algorithm 1 Branch and Bound Algorithm for Action Planning
return true 33: end function If T gives a feasible path to the destination with a smaller cost, then the current best solution is updated (line 25). The third test is the backward pruning (line 12). Here, the terminal covariance and the cost-so-far of the current path T are compared with those stored in D(n). If both are greater or equal to the current best, this implies a better path to n has already been found and therefore T can be truncated. Here, the covariance matrices are compared in terms of matrix inequality. In the contrary case where both are smaller, the current best is updated (line 31). If the path passes all these tests, finally the path is expanded (line 13 to 16). Here, the order of node expansion has a strong influence on the performance of the branch-and-bound method. In this algorithm, the movement transitions are expanded first in the ascending order of the cost-to-go and the observation transition is expanded next.
Remark 2
The same technique as the backward pruning in Algorithm 1 has already appeared in [8] . The forward pruning, to the authors' knowledge, has not appeared in the literature.
Roadmap Design Based on Information-Weighted Safety Measure
This section describes a heuristic roadmap construction method suitable for planning under position uncertainty. A primary requirement for a roadmap is to fulfill a set of feasibility conditions introduced in Section 4.1. In addition, the number of nodes in a roadmap should be as small as possible, since the computational cost grows exponentially with respect to the size of the roadmap even if we employ the algorithm presented in the previous section.
Roadmap Design Based on Information-Weighted Safety Measure
The well known visibility graph technique [10] generates a roadmap by connecting obstacle corners visible to each other. Although this technique is suitable for obtaining a short path to a destination, by nature, it provides paths that pass very close to the obstacles, and therefore it is not suitable for planning under uncertainty. Another widely used technique is roadmap construction based on the Generalized Voronoi Diagram (GVD) [11] . A GVD is a set of points whose distance to the nearest two obstacles are equal. Therefore, by distributing nodes over a GVD, one obtains a node layout that maximizes the margin between the nodes and the obstacle boundaries. When the sensing range is limited, however, placing nodes too far away from the obstacles becomes rather disadvantageous for reducing position uncertainty and it could result in increased risk of collision. In [12] , probabilistic roadmap is generated by importance sampling using a probability distribution based on Sensory Uncertainty Field. In this manner, nodes are likely to be placed where at least one obstacle is visible, but at the same time, nodes could be placed very close to the obstacle boundaries, because probability is based solely on localization ability. Although such useless nodes will be rejected at the path planning phase, they will cause increased computation cost.
Roadmap Design Based on Information-Weighted Safety Measure
Qualitatively speaking, desirable node location is where the distance to nearby obstacles is large enough but not greater than the sensing range. Now, we propose a quantitative measure that reflects this requirement.
Roadmap Design Based on Information-Weighted Safety Measure
x n = arg min
Roadmap Design Based on Information-Weighted Safety Measure
Here, x ∈ X f is a point in the free space, I(x) is the information matrix at x and x n is the nearest obstacle point from x in terms of the Euclidean metric. Therefore, d(x) gives the Mahalanobis distance from a point to the nearest (in the Euclidean sense) obstacle point using I(x) as a weighting matrix.
Roadmap Design Based on Information-Weighted Safety Measure
In Figs. 3(a)(b) , for the same example workspace as Fig. 2 , each position of the free space is drawn in grayscale with color intensity proportional to d(x). In Fig. 3(a) and Fig. 3(b) , the sensing range is set as 1.5 m and 3.0 m, respectively. As an effect of the weighting matrix I(x), the distance to obstacles is evaluated larger in a direction in which position uncertainty can be reduced substantially, and vice versa. An extreme case is when there is no obstacle within the sensing range; in this case, the information matrix I(x) is the zero matrix and thus d(x) is 0. In the figures, such regions are filled in black.
Roadmap Design Based on Information-Weighted Safety Measure
One natural way to determine a node layout base on the func-tion defined above would be to place nodes in locations with high d(x) value. In this paper, this procedure is done manually. An example will be shown in Section 5. Automating the roadmap generation is beyond the scope of this paper and considered as future work. One possible way would be generating nodes by importance sampling using d(x) as a probability density function.
Numerical Experiments
This section evaluates the proposed planning method in simulations. The specification of the robot and its sensor are listed in Tables 1 and 2 . The error of the sensor is given by the standard deviation multiplied by three (3σ). Moreover, the move- Table 1 Sensor specification. ment error rate of the robot is given by 3σ per unit movement distance. A workspace used in simulation and a roadmap created based the criterion described in the previous section are shown in Fig. 4 . In each figure, gray polygonal shapes depict obstacles. In Fig. 4(a) , the free space is drawn in grayscale where the color intensity at each position is proportional to the minimum Euclidean distance to an obstacle. We can observe that the ridges of the intensity potential draw GVD. In Fig. 4(b) , the grayscale intensity is determined by (17). A roadmap is created manually by placing nodes in locations with high intensity. The obtained roadmap is shown in Fig. 4(c) .
Numerical Experiments
First, planning is done in three cases where the observation cost takes different values. The results are shown in Fig. 5 . The values of observation cost are, from the top figure to the bottom, 1.0, 10.0 and 20.0. In each figure, the planned path is depicted by solid lines and uncertainty at each node of the path is depicted by the tolerance ellipsoid of the covariance matrix. Each plan navigates the robot to the goal without collision; that is, all tolerance ellipsoids do not intersect with the obstacles. When the cost of observation is small, a shorter path is selected and relatively frequent observations are made. When the cost of observation is large, on the other hand, a longer path which has less obstacles nearby is selected and fewer observations are made. In fact, the path shown in Fig. 5(c) is an admissible path with the fewest number of observations. The reader might notice that each tolerance ellipsoid depicted in the figures is close to a circle. One reason for this is that we consider an omni-directional robot; an ellipsoid expands equally in all directions at movement update. Another reason is that at least two obstacle edges with different orientations are visible from each node. This means that the information matrix I n is non-singular, and as a result, an ellipsoid contracts in all directions at observation update. For these reasons, ellipsoids are unlikely to get flattened significantly in one direction. Figure 6 shows the comparison of the conventional and the proposed planning strategies in terms of total cost. Here, the conventional strategy refers to selecting the shortest path and making an observation at every via-point. Obviously, in the conventional strategy, the total cost linearly increases with respect to the observation cost. In contrast, in the proposed strategy, the growth of the total cost is much slower since an appropriate route as well as a minimum required number of observation points are planned according to the observation cost.
Numerical Experiments
Next, we change the value of the movement error rate and observe its influence on some quantities including computation time, optimal movement cost and optimal number of observations. The reason we compare them in terms of movement error rate is that this parameter has a strong influence on the number of admissible paths. At this time, the observation cost is set to 0. The results are shown in Fig. 7 . The upper figure shows the change of computation time, the number of searched nodes (the number of times SEARCH is called in Algorithm 1) and the number of collision tests plotted in logarithmic scale. The lower figure shows the change of the movement cost and the number of observations of the optimal solution. The movement cost exhibits step-wise increase, and moreover, while the movement cost is constant, the number of observations increases monotonically. In particular, when the movement error rate is 0.0, obviously the shortest path is selected and the number of observation is 0. In contrast, when the movement error rate is 2.0, the problem has no admissible solution. The computation time does not show monotonic increase with respect to the movement error rate, but it show rises and falls repeatedly synchronizing with the change of optimal path. As the movement error rate increases, a path whose movement cost is larger than the shortest path tends to be selected. Therefore, the computation time of the proposed algorithm, which searches in the best-first manner, tends to become larger. However, in the intervals of movement error rate where the same optimal path is obtained, the computation time shows steady decrease. This is because the number of admissible paths (thus the size of the search tree) decreases as the movement error rate becomes larger. Finally, the motion of a robot making a sequence of actions based on a planned path is evaluated in simulation. In simulation, the values of the movement error ν and the observation error at each time instant are sampled at random from normal distributions N(0, Q) and N(0, R), respectively. For illustration, an example of simulated robot movement is shown in Fig. 8 . Here, the plan is the one shown in Fig. 5(a) . Solid lines depict the "real" position of the robot and dashed lines depict the position estimated by EKF. We can observe that the robot makes observations at planned observation points and the error between the real and the estimated positions is reduced at these points.
Numerical Experiments
Next, for each of the three planned paths shown in Figs. 5(a)-(c), some statistics are obtained based on 1000 simulation trials. In Table 3 , J * denotes the cost of the planned path and J (ave., min. and max.) denotes the average, the minimum and the maximum of the costs of simulation trials. Here, for each trial, 
Numerical Experiments
The difference from (14) is that the movement cost is calculated based on the actual movement distance rather than the distance between the adjacent via-points. Recall that u k is given by (10) . Moreover, "exceed" is the ratio of the number of steps the real position got out of the tolerance ellipsoid to the total number of steps. Theoretically, this must be around 0.1%, since the tolerance ellipsoid is given by (4) with the probability p = 99.9%, but the results show slightly larger values. This is mainly due to the linear approximation of the observation model in EKF; the reduction of position uncertainty after a single observation would not be as expected at positions where higher order derivatives of the observation model is not negligible. Note, however, that getting out of the tolerance ellipsoid does not immediately imply collision with an obstacle. Indeed, "collision", the ratio of the number of collisions to the total number of steps, resulted to be 0% for all cases.
Conclusion
In this research, an action planning problem to minimize the total time required for movement and observation for reaching a destination considering collision safety under position uncertainty is studied. One future direction would be to extend the framework to mobile robots with different sensor models such as camera-based localization. Another direction would be to bring this problem to a multi-robot setting, in which robots can communicate with other robots to reduce their position uncertainty.

{'paper_id': '58967128', 'title': 'Path Planning of Mobile Robots Considering Position Uncertainty and Cost of Observation', 's2_url': 'https://api.semanticscholar.org/CorpusID:58967128', 'source': 'S2ORC'}