Prediction of Future Configurations of a Moving Target in a Time-Varying Environment Using an Autoregressive Model

In this paper, we describe an algorithm for predicting future positions and orientation of a moving object in a time-varying environment using an autoregressive model (ARM). No constraint is placed on the obstacles motion. The model addresses prediction of translational and rotational motions. Rotational motion is represented using quaternions rather than Euler representation to improve the algorithm performance and accuracy of the prediction results. Compared to other similar systems, the proposed algorithm has an adaptive capability, which enables it to predict over multiple time-steps rather than fixed ones as reported in other works. Such algorithm can be used in a variety of applications. An important one is its application in the framework of designing reliable navigational systems for autonomous mobile robots and more particularly in building effective trajectory planners. Simulation results show how significantly this model could reduce computational cost.


Introduction
The importance of designing and producing robots capable of performing tasks in time-varying environments is gaining increasing recognition.Consider, for example, multiple autonomous robots that could replace humans working in unsafe environments-cleaning up hazardous wastes or handling radioactive materials.For true autonomy in such tasks, a capability that would enable each moving robot to react adaptively to its surrounding environment is needed while carrying out a certain task.For instance, each one of the robots needs to navigate between two locations.The situation is similar to that of a person crossing a street.He needs to recognize the presence of obstacles and people around him, identify static and moving ones, and constantly update his knowledge of the environment.Several researchers have described algorithms for robot motion planning systems in static environments.For a recent survey on this subject and a more detailed presentation of the different methodologies on robot motion planning, see [1,2].As for the dynamic environments, there have been fewer studies, for example [3][4][5][6][7][8].All of these assume complete knowledge about the environment and a full control of the motion of obstacles.Conceptually, we may subdivide the problem of robot navigation into three inter-related phases: sensor integration and data fusion, scene interpretation and map building, and trajectory planning.Each of these phases consists of several sub-problems.One of which is the prediction problem that deals with predicting future configurations (positions and orientations) of moving obstacles.This information is needed for trajectory planning of the robot in order to avoid any possible future collisions.In the case of humans, the prediction procedure is usually characterized by a high performance and rarely misses its objective.This may be because of the accurate decisions we make based on the data collected through our biological sensors and what we predict about the environment over a period of time.In this article, we propose an algorithm to predict future configurations of freely moving obstacles based on an autoregressive model (ARM) with conditional maximum likelihood approach and the least squares method to estimate the model parameters.To make our analysis practical and more realistic, we do not assume any control over the trajectories of the moving objects.We assume that pre-i vious and current positions and orientations are available from sensory devices.
In the past, Kehtarnavaz [9] and Elnagar [10] proposed similar algorithms.Kehtarnavaz, proposed a prediction algorithm that is also based on an ARM.Their goal was to establish a collision zone around each moving obstacle and then treat zones as stationary obstacles.Collision zones represent forbidden regions which are defined based on a high likelihood of collision.However, the proposed algorithm has two drawbacks.First, it dealt with translating obstacles only and, second, it is too conservative (transferring the dynamic environment into a static one).Later, Elnagar described another ARM-based algorithm that accounts for the prediction of translating and rotating objects in a truly dynamic sense, [10].
Morkovian models were also used to predict motion as sequential decision problems in a completely known environment.Such a model is called a Markov Decision Process (MDP).A modified version of such a model is also used in local environments where there is uncertainty or lack of information about the environment.This type of a model is called a Partially Observable Markov Decision Process (POMDP).Examples of works that made use of the former model to predict future motion of moving objects in local-based collision avoidance systems may be found in [11][12][13].A similar global-based system is reported in [14].Examples of works that are based on the POMDP model can be found in [15] and [16].Because the POMDP reward function is updated at each time step and hence the POMDP is solved at each time step, the need for a fast and efficient solution method arises.Moreover, all works above rely on fixed time steps too.
Tsai et al. [17] described a model for predicting the positions and the orientations of a moving robot in a 3-D environment based on constant time intervals.The model is based on a potential field where the repulsive force and torque between the robot and the obstacles are used to re-adjust the configurations of the robot so as to keep it far from the obstacles in the environment while passing through bottlenecks in its free space.It should be noted that this model emphasizes on keeping a distance from obstacles rather than precisely predict the configurations.
In this work, we extend the work to take into account variable time-steps rather than fixed ones.This is done based on the feedback received from the prediction phase of previous configurations.The more accurate the predictions are, the longer the time period (between two successive readings of the environment) will be.Our proposed algorithm uses the idea of quaternions opposed to Euler-representation, which was used in all past works, to model rotation.The application of quaternions in ro-botics is not new (for example, see [18,19]).However, the combination of quaternions and variable sample times in the context of robot motion prediction is new.
The next section describes the procedure for predicting positions of a translating object using an ARM.Estimation of the autoregressive parameters along with simulation results for the translational case is presented in Section 2. Predicting a trajectory of a rotating object is discussed in Section 3. Section 4 describes the general algorithm for predicting the motion of a freely moving object.More simulation results are demonstrated in Section 5.

Translational Motion
In this section, we develop a prediction model to be used by a robot to decide about future positions of translating obstacles.Our intention is to use this model within a trajectory planning algorithm in a time-varying environment.Before the robot starts to interact with its environment, data about visible obstacles are collected, via sensors, for a short period of time.This step enables the robot to learn about any translating obstacles in its visibility field1 at discrete points in time-space.Therefore, modeling using difference equations is appropriate, but since sensory readings are usually noisy, an autoregressive model is more relevant and useful.Formally, an ARM that calculates the position of a translating obstacle, i , at step based on its previous positions is given by: where is the actual random sequence of positions for obstacle along the x-axis.

 
x n e i is a zero-mean white Gaussian noise.Furthermore, the difference equation is said to be of order .Our objective is to estimate the autoregressive coefficients from a number of observations.If the sampling steps, of sensing the environment, are small enough then it is acceptable to assume constant or slowly changing acceleration for a translating obstacle.Hence, it is reasonable to model acceleration of , along the x-axis, with the following first order ARM: Other acceleration components ( y  and ) along the y and z axes are computed similarly.1,1 z  x  is a first order autoregressive parameter and is the prediction error.On the other hand, a new x-position of a translating obstacle can be estimated using Newton's equations where and are the velocity and acceleration of obstacle i along x-axis at step  is the variable time interval between and .The classical positional relationship between acceleration and velocity along x-axis is: Substituting for   x n  and in ( 4) and then equating Equation ( 2) with the result while substituting for yields: where 1) is a 3 rd order ARM (compare with Equation ( 1)).Similarly, we can predict the y and z coordinates of a translating object along y and z-axes, respectively.In general the prediction model for translational motion can be expressed in the following matrix notation: where   , , w x y z  and the autoregressive coefficients are:

Estimating the Coefficients
To estimate the coefficients  in Equation (6).There are several approaches to do so.We present two of them.The first one is the conditional maximum likelihood approach [20], [21] to estimate both the autoregressive coefficients and 2  , which is the variance of the noise.
The second model is based on the least squares method.
As for the first model, we will work the details of minimizing only.The same procedure is used to minimize the other error terms.
where N is the number of readings.To maximize the logarithmic likelihood function ( c ), we differentiate Equation ( 7) partially with respect to 3,1 3,2 3,3 l , ,  and equating the derivatives to zero.Note that for we can only minimize the summation part of Equation (7).However, looking again at Equation (5) we notice that all 1, 2, 3 j  x j   's are dependent on one coefficient 1,1 x  which can be determined from the following conditional likelihood function [20]: ; l where estimates should fall within the allowable range  Note that (7) is the logarithm of the conditional likelihood function for a third-order autoregressive model whereas (8) represents the function of a first-order model.Maximizing (8) with respect to Using 1,1 x  , we can easily determine the values of  's in Equation ( 5) and consequently predict the future position of a translating obstacle based on its history positions.The model needs at least four points before it starts the prediction process.Compared with the first approach (i.e., computing all x  's), the second one is computationally inexpensive.It should be noted that the estimates for the translational components along the y and z axes can be obtained with the exact procedure described earlier.
The second model which is widely used, [22], for es-timating the coefficient 1,1 x  as it changes with time, is to fit an ARM to the sequence of acceleration in a least squares sense as follows: is a weight factor.For a uniformly changing acceleration, 0     is kept closer to 1.The solution to the above least squares problem is: Similarly, we can obtain coefficients for translation along the y and z axes.Notice the initial values for and are set to 0. We used both models of estimating coefficients in our simulation results.The second model produces better results than the first model when predicting the motion of arbitrary moving objects.However, the first model outperforms the second when predicting for uniformly moving objects.

Simulation Results: Translational Case
We introduce several simulation examples to show how the proposed model works.In Figure 1, we assume a 2D work space in which a point-object is translating.Based on its past positions, a predicted trajectory is generated using the proposed ARM (dotted line).Each prediction process is performed in a variable t  time.The closer the dots are on the graph, the smaller the sampling time is.If the prediction is accurate, the time interval of the next reading is enlarged.The main difference between the two graphs in Figures 1 and 2 is how the coefficient  is estimated?Figure 1 shows a predicted trajectory of a translating point-object over a long period of time (120 sampling steps) with varying acceleration.Although the point does not follow a structured or well-defined2 trajectory, as in Figure 2, the predicted result (dotted line) is quite close to the actual trajectory (solid line).The mean square errors are 1.12 and 0.82 distance-units in upper and lower graphs, respectively.A simulation involving a well-defined trajectory (sin(x) curve) is demonstrated in Figure 2. In this case, almost a perfect match (mean square errors are 0.0016 and 0.0017 distance-units in upper and lower graphs, respectively) is obtained between the actual and predicted future posi-  tions.While the upper graph of each figure shows the result of using the conditional likelihood method [10], the lower one depicts the result of using the least squares technique [9].The lower graph shows slightly better results in the case of varying acceleration.Comparing our results to these two models ( [19,10]) where prediction is carried out based on a fixed time step, we did achieve as good results based on the adaptive model but with less time steps.For example, we obtained the results of Fig- ures 1 and 2 in 88 and 99 steps, respectively, compared to 120 and 360 steps when either of the systems of [10] or [9] is used.This is a significant improvement.

Rotational Motion
For a moving point-object or a sphere, the analysis introduced so far is sufficient to predict its next position.However, it is not enough to predict the motion of a more general object.This is because the most general type of motion an object can undergo is a combination of translation and rotation.We have already dealt with the translational component.Now, we describe a similar model for the rotational component.Without loss of generality, we represent a given moving object with its center of mass and some other reference (feature) points that belong to the object.
For example, a line segment in space is defined by its center of mass and end-points.We only predict the trajectory of the center of mass and then use it for the other reference points.Reference points are used to show the orientation of a given object in a 3D-space.Specifically, the problem in general can be stated as follows: given M key-frames

represents the number of previous orientations, what is the expected orientation of key-frame
frames with respect to a global frame of reference3 , W?
, , x n  are the rotation angles about X, Y and Z axes, respectively.
The mathematical analysis for developing the rotational-prediction model is exactly analogous to the one of the translational case.If we assume that the object undergoes a constant angular acceleration, which is a fair assumption when the sampling steps are small for the type of application we consider, then the angular acceleration can be modeled with the following ARM of order 1: A new orientation of a rotating obstacle O i can be predicted by the following classical relation: where and are the angular velocity and acceleration of obstacle O i at step (n − 1), respectively.Since the change in rotational angles is expected to be small because is small, Equation ( 12) can be rewritten in a third order ARM as Equation ( 5) to produce similar results.Similar to the translational case, future orientation of a rotating obstacle based on its previous orientations can be predicted.
However, instead of representing the rotation as a combination of 3 rotations as mentioned earlier, we choose a different approach.That is rotation about an arbitrary axis in space.This is frequently found when modeling robot manipulators.For example, a grip of a robot usually rotates around an axis that is inclined with the principle axis of its arm.This type of rotation is known as rotation using quaternions.A good introduction to the mathematics of quaternions and their relationship to rotation can be found in [23].A unit-length quaternion ( q a b c d     i j k , where ) corresponds to the point in 4-dimensions.There is a natural map that takes a quaternion and produces a rotation.For example, quaternion corresponds to a rotation One should notice that the mapping of quaternions to rotations is 2-to-1.There are several advantages for using quaternions to describe rotations instead of using Euler angles.First, the space of a quaternion has the same space as the set of rotations whereas this is not the case for Euler angles except for 3 × 3 orthogonal matrices with determinant 1.Further, the number of parameters used to describe a quaternion is 4 compared to 9 in Euler representation.The disadvantage of using quaternions is the 2-to-1 mapping, which requires the choice between two values for the appropriate one.
Rotation using quaternions can be viewed as a sequence of translations and simple rotations that will make the arbitrary axis coincide with one of the principle coordinate axes, then rotation is performed, and finally the axis is returned to its original position.In general, we perform the following sequence of steps: translate one of the endpoints of quaternion to the origin, perform rotation around x and y axes to align the quaternion with the z-axis, now rotate around the z-axis with the desired rotation value, reverse rotation around the x and y axes, and finally apply reverse translation.

Complete Algorithm
The following algorithm summarizes the steps needed to predict the (n + 1) th future position and orientation of a free moving object in space based on it's first n positions and orientations: Input: previous positions and orientations.

Simulation Results: Rotational Case
In the following simulations, we predict the orientations of an open cubic object that is specified by its end-points.
In the first simulation, the object rotates around an arbitrary axis AB, where A = (0.5,0.5,1) and B = (0.5,0.5,0), which is pointed at in the figure.This axis is parallel to the z-axis.We rotate the object around this axis under varying angular acceleration.Figure 3 shows a top view of the 3D environment of both the actual and the predicted configurations of the cube.We only show a limited number of frames because otherwise the graph will be cluttered and difficult to follow.Another view of the environment, but in 3D, is shown in Figure 4. Figure 5 depicts the predicted angle values compared to the actual readings.The mean square errors are 0.0168 and 0.00142 distance-units in upper and lower graphs, respectively.In all simulations, solid lines represent the actual set of  readings whereas the dotted lines characterize the predicted ones.In this simulation, we have 120 readings but the algorithm performed only 59 readings and produced these good prediction results.In the second simulation, we change the rotation axis AB to be defined by A = (0,0,0) and B = (1,1,1) under the same angular acceleration of the first simulation.While Figure 6(a) shows the several frames of the actual and predicted configuration of the cube in 3D. Figure 7 depicts the prediction of a rotating triangle after 12 and all readings, respectively.Out of 200 readings, the algorithm performed only 69 readings and produced these good prediction results (the mean square errors are 0.0685 and 0.0545 distance-units by both  models, respectively).Figure 8 shows the actual versus predicted results; notice the sampling rate along the curves.
Figure 9 shows another simulation of a simple representation of a 3-link manipulator rotating around two rotational axes (link 1 and link 2 in the figure).Figure 10 shows another simulation of a simple representation of a 3-link manipulator rotating around an arbitrary axis.

Conclusions
We have described an adaptive algorithm for predicting future positions and orientations of moving obstacles in a 3D time-varying environment using an ARM.Orientations are represented as quaternions.Configurations of moving obstacles are not known priori, but we assume  knowledge of previous positions and orientations are available to the ARM from sensory devices.Four readings are required by the algorithm before the prediction process starts.Simulation results show how close the predicted trajectory to the original one, which compares favorably with other existing models.
However, the proposed algorithm outperforms similar existing ones in terms of computational cost because of its adaptive capability while maintaining accurate prediction results.In addition, the use of quaternions simplified the analysis and improved accuracy of the prediction process when compared to other models that use Euler angles representation to model rotation.The proposed algorithm can be used in a variety of applications.An important one is its application in the framework of designing reliable navigational systems for autonomous mobile robots and more particularly in building effective trajectory planners.
are autoregressive parameters, and

For i from 1
to No_of_obstacles do For each feature point in obstacle i do 

Figure 3 .
Figure 3.A 2D view of the prediction of a rotating cubic object.

Figure 4 .
Figure 4.A 3D view of the rotating cubic object.

Figure 5 .
Figure 5. Prediction vs actual readings over the time interval for rotational angles based on (9) (upper graph) and (10) (lower graph).

Figure 6 .Figure 7 .
Figure 6.Prediction of a rotating cubical object around a rotational axis.

Figure 8 .
Figure 8.The prediction of the rotating cube but around a different rotation axis.

Figure 9 .
Figure 9.A simulation of a 3-link manipulator and its representation.

Figure 10 .
Figure 10.A manipulator rotating as a rigid body.