Hostname: page-component-586b7cd67f-g8jcs Total loading time: 0 Render date: 2024-11-21T22:34:02.842Z Has data issue: false hasContentIssue false
  • English
  • Français

Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map

Published online by Cambridge University Press:  01 July 2008

Wooram Park ,
Yan Liu ,
Yu Zhou ,
Matthew Moses  and
Gregory S. Chirikjian
Wooram Park
Affiliation:
Department of Mechanical Engineering, Johns Hopkins University, Baltimore, MD 21218, USA. E-mail: [email protected]
Yan Liu
Affiliation:
Department of Mechanical Engineering, Johns Hopkins University, Baltimore, MD 21218, USA. E-mail: [email protected]
Yu Zhou
Affiliation:
Department of Mechanical Engineering, Stony Brook University, Stony Brook, New York 11794-2300
Matthew Moses
Affiliation:
Department of Mechanical Engineering, Johns Hopkins University, Baltimore, MD 21218, USA. E-mail: [email protected]
Gregory S. Chirikjian*
Affiliation:
Department of Mechanical Engineering, Johns Hopkins University, Baltimore, MD 21218, USA. E-mail: [email protected]
*
*Corresponding author. E-mail: [email protected]
Get access Rights & Permissions [Opens in a new window]

Summary

A nonholonomic system subjected to external noise from the environment, or internal noise in its own actuators, will evolve in a stochastic manner described by an ensemble of trajectories. This ensemble of trajectories is equivalent to the solution of a Fokker–Planck equation that typically evolves on a Lie group. If the most likely state of such a system is to be estimated, and plans for subsequent motions from the current state are to be made so as to move the system to a desired state with high probability, then modeling how the probability density of the system evolves is critical. Methods for solving Fokker-Planck equations that evolve on Lie groups then become important. Such equations can be solved using the operational properties of group Fourier transforms in which irreducible unitary representation (IUR) matrices play a critical role. Therefore, we develop a simple approach for the numerical approximation of all the IUR matrices for two of the groups of most interest in robotics: the rotation group in three-dimensional space, SO(3), and the Euclidean motion group of the plane, SE(2). This approach uses the exponential mapping from the Lie algebras of these groups, and takes advantage of the sparse nature of the Lie algebra representation matrices. Other techniques for density estimation on groups are also explored. The computed densities are applied in the context of probabilistic path planning for kinematic cart in the plane and flexible needle steering in three-dimensional space. In these examples the injection of artificial noise into the computational models (rather than noise in the actual physical systems) serves as a tool to search the configuration spaces and plan paths. Finally, we illustrate how density estimation problems arise in the characterization of physical noise in orientational sensors such as gyroscopes.

Keywords

State estimationLie groupsnonholonomic motion planningexponential map
Type
Article
Information
Robotica , Volume 26 , Special Issue 4: Geometry in Robotics: Sensing , July 2008 , pp. 419 - 434
Copyright
Copyright © Cambridge University Press 2008

Access options

Get access to the full version of this content by using one of the access options below. (Log in options will check for institutional or personal access. Content may require purchase if you do not have access.)

References

1.Titterton, D. H. and Weston, J. L., Strapdown Inertial Navigation Technology (Peter Peregrinus Ltd., London, UK, 1997).Google Scholar
2.Lawrence, A., Modern Inertial Technology (Springer-Verlag, New York, USA, 1993).CrossRefGoogle Scholar
3.Jekeli, C., Inertial Navigation Systems with Geodetic Applications (Walter de Gruyter, Berlin; New York, 2001).CrossRefGoogle Scholar
4.Makadia, A. and Daniilidis, K., “Rotation estimation from spherical images,” IEEE Trans. Pattern Anal. Mach. Intell. 28, 1170–1175 (2006).CrossRefGoogle ScholarPubMed
5.Bayro-Corrochano, E. and Zang, Y., “The motor extended Kalman filter a geometric approach for rigid motion estimation,” J. Math. Imaging and Vis. 13, 205–228 (2000).CrossRefGoogle Scholar
6.Chirikjian, G. S. and Kyatkin, A. B., Engineering Applications of Noncommutative Harmonic Analysis (CRC Press, Boca Raton, FL, 2001).Google Scholar
7.Gelfand, I. M., Minlos, R. A. and Shapiro, Z. Ya., Representations of the Rotation and Lorentz Groups and their Applications (Macmillan, New York, 1963).Google Scholar
8.Gurarie, D., Symmetry and Laplacians: Introduction to Harmonic Analysis, Group Representations and Applications (Elsevier Science Publisher, The Netherlands, 1992).Google Scholar
9.Mackey, G. W., The Theory of Unitary Group Representations (The University of Chicago Press, Chicago, 1976).Google Scholar
10.Maslen, D. K., Fast Transforms and Sampling for Compact Groups (Ph.D. Dissertation, Department of Mathematics, Harvard University, May 1993).Google Scholar
11.Maslen, D. K. and Rockmore, D. N., “Generalized FFTs–-a survey of some recent results,” DIMACS Series Discrete Math. Theor. Comput. Sci. 28, 183–237 (1997).CrossRefGoogle Scholar
12.Miller, W., Lie Theory and Special Functions (Academic Press, New York, 1968).Google Scholar
13.Miller, W. Jr., “Some applications of the representation theory of the Euclidean group in three-space,” Commun. Pure App. Math. 17, 527–540 (1964).CrossRefGoogle Scholar
14.Naimark, M. A., Linear Representations of the Lorentz Group (Macmillan, New York, 1964).Google Scholar
15.Peter, F. and Weyl, H., “Die Vollständigkeit der primitiven Darstellungen einer geschlossenen kontinuierlichen Gruppe,” Math. Ann. 97, 735–755 (1927).CrossRefGoogle Scholar
16.Sugiura, M., Unitary Representations and Harmonic Analysis, 2nd ed. (North-Holland, Amsterdam, 1990).Google Scholar
17.Taylor, M. E., Noncommutative Harmonic Analysis (Mathematical Surveys and Monographs, American Mathematical Society, Providence, RI, 1986).CrossRefGoogle Scholar
18.Varshalovich, D. A., Moskalev, A. N. and Khersonskii, V. K., Quantum Theory of Angular Momentum (World Scientific, Singapore, 1988).CrossRefGoogle Scholar
19.Vilenkin, N. J. and Klimyk, A. U., Representation of Lie Groups and Special Functions, vols. 1–3 (Kluwer Academic Publishers, Dordrecht, Holland 1991).CrossRefGoogle Scholar
20.Wigner, E., “On Unitary Representations of the Inhomogeneous Lorentz Group,” Ann. Math. 40 (1), 149–204 (1939).CrossRefGoogle Scholar
21.Zelobenko, D. P., Compact Lie Groups and their Representations (Translations of Mathematical Monographs, American Mathematical Society, Providence, RI, 1973).Google Scholar
22.Kim, P. T., “Deconvolution density estimation on SO(N),” Ann. Stat. 26 (3), 1083–1102 (1998).CrossRefGoogle Scholar
23.Silverman, B. W., “Kernel density estimation using the fast Fourier transform,” Appl. Stat. 31, 93–99 (1982).CrossRefGoogle Scholar
24.Hendriks, H., “Nonparametric estimation of a probability density on a Riemannian manifold using Fourier expansions,” Ann. Stat. 18 (2), 832–849 (1990).CrossRefGoogle Scholar
25.Silverman, B. W., Density Estimation for Statistics and Data Analysis (Chapman and Hall, London, 1985).Google Scholar
26.Aramanovitch, L. I., “Quaternion nonlinear filter for estimation of rotating body attitude,” Math. Methods Appl. Sci. 18 (15), 1239–1255 (1995).CrossRefGoogle Scholar
27.Bar-Itzhack, I. Y. and Oshman, Y., “Attitude determination from vector observations–-quaternion estimation,” IEEE Trans. Aerosp. Electron. Syst. 21 (1), 128–136 (1985).CrossRefGoogle Scholar
28.Bar-Itzhack, I. Y. and Idan, M., “Recursive attitude determination from vector observations–-Euler angle estimation,” J. Guid. Control Dyn. 10 (2), 152–157 (1987).CrossRefGoogle Scholar
29.Burns, W. K., ed., Optical Fiber Rotation Sensing (Academic Press, Boston, 1994).Google Scholar
30.Carta, D. G. and Lackowski, D. H., “Estimation of orthogonal transformations in strapdown inertial systems,” IEEE Trans. Autom. Control AC-17, 97–100 (1972).CrossRefGoogle Scholar
31.Chaudhuri, S. and Karandikar, S.S., “Recursive methods for the estimation of rotation quaternions,” IEEE Trans. Aerosp. Electron. Syst. 32 (2), 845–854 (1996).CrossRefGoogle Scholar
32.Comisel, H., Forster, M., Georgescu, E., Ciobanu, M., Truhlik, V. and Vojta, J., “Attitude estimation of a near-earth satellite using magnetometer data,” Acta Astronautica 40 (11), 781–788 (1997).CrossRefGoogle Scholar
33.Crassidis, J. L., Lightsey, E. G. and Markley, F. L., “Efficient and optimal attitude determination using recursive global positioning system signal operations,” J. Guid. Control Dyn. 22 (2), 193–201 (1999).CrossRefGoogle Scholar
34.Crassidis, J. L. and Markley, F. L., “Predictive filtering for attitude estimation without rate sensors,” J. Guid. Control Dyn. 20 (3), 522–527 (1997).CrossRefGoogle Scholar
35.Lefferts, E. J., Markley, F. L. and Shuster, M. D., “Kalman filtering for spacecraft attitude estimation,” J. Guid. Control Dyn. 5 (5), 417–429 (1982).CrossRefGoogle Scholar
36.Duncan, T. E., “An Estimation problem in compact Lie groups,” Syst. Control Lett. 10, 257–263 (1998).CrossRefGoogle Scholar
37.Lo, J. T. -H. and Eshleman, L. R., “Exponential Fourier densities on SO(3) and optimal estimation and detection for rotational processes,” SIAM J. Appl. Math. 36 (1), 73–82 (1979).CrossRefGoogle Scholar
38.Lo, J. T.-H., “Optimal estimation for the satellite attitude using star tracker measurements,” Automatica 22 (4), 477–482 (1986).CrossRefGoogle Scholar
39.Oshman, Y. and Markley, F. L., “Minimal-parameter attitude matrix estimation from vector observations,” J. Guid. Control Dyn. 21 (4), 595–602 (1998).CrossRefGoogle Scholar
40.Oshman, Y. and Markley, F. L., “Sequential attitude and attitude-rate estimation using integrated-rate parameters,” J. Guid. Control Dyn. 22 (3), 385–394 (1999).CrossRefGoogle Scholar
41.Park, F. C., Kim, J. and Kee, C., “Geometric descent algorithms for attitude determination using the global positioning system,” J. Guid. Control Dyn. 23 (1), 26–33 (2000).CrossRefGoogle Scholar
42.Savage, P. G., “Strapdown inertial navigation integration algorithm design part 1: Attitude algorithms,” J. Guid. Control Dyn. 21 (1), 19–28 (1998).CrossRefGoogle Scholar
43.Savage, P. G., “Strapdown inertial navigation integration algorithm design part 2: Velocity and position algorithms,” J. Guid. Control Dynamics 21 (2), 208–221 (1998).CrossRefGoogle Scholar
44.Shuster, M. D. and Oh, S. D., “Three-axis attitude determination form vector observations,” J. Guid. Control Dyn. 4 (1), 70–77 (1981).CrossRefGoogle Scholar
45.Smith, R. B., ed., Fiber Optic Gyroscopes, SPIE Milestone Series, vol. MS 8 (SPIE Optical Engineering Press, Washington, DC, 1989).Google Scholar
46.Willsky, A. S., “Some Estimation Problems on Lie Groups” Geometric Methods in System Theory (Mayne, D. Q. and Brockett, R. W., eds.) (Reidel Publishing Company, Dordrecht-Holland, 1973) pp. 305–313.CrossRefGoogle Scholar
47.Willsky, A. S., Dynamical Systems Defined on Groups: Structural Properties and Estimation (PhD Dissertation, Department of Aeronautics and Astronautics, MIT, 1973).Google Scholar
48.Moses, H. E., “Irreducible representations of the rotation group in terms of the axis and angle of rotation,” Ann. Phys. 42, 343–346 (1967).CrossRefGoogle Scholar
49.Moses, H. E., “Irreducible representations of the rotation group in terms of the axis and angle of rotation,” Ann. Phys. 37, 224–226 (1966).CrossRefGoogle Scholar
50.Moses, H. E., “Irreducible representations of the rotation group in terms of Euler's theorem,” Il Nuovo Cimento 40 (4), 1120–1138 (1965).CrossRefGoogle Scholar
51.Applebaum, D. and Kunita, H., “Lévy flows on manifolds and Lévy processes on Lie groups,” J. Math. Kyoto. Univ. 33/34, 1103–1123 (1993).Google Scholar
52.Brockett, R. W., “Notes on Stochastic Processes on Manifolds,” In Systems and Control in the Twenty-First Century (Byrnes, C. I. et al. eds.) (Birkh . . . a user, Boston, 1997) pp. 75–101.CrossRefGoogle Scholar
53.Fokker, A. D., “Die Mittlere Energie rotierender elektrischer Dipole in Strahlungs Feld,” Ann. Physik. 43, 810–820 (1914).CrossRefGoogle Scholar
54.Gardiner, C. W., Handbook of Stochastic Methods, 2nd ed. (Springer-Verlag, Berlin, 1985).Google Scholar
55.Itô, K., “Brownian Motions in a Lie Group,” Proceedings of the Japan Academy, vol. 26, 4–10 (1950).Google Scholar
56.Itô, K., “Stochastic differential equations in a differentiable manifold,” Nagoya Math. J. 1, 35–47 (1950).CrossRefGoogle Scholar
57.Itô, K., “Stochastic differential equations in a differentiable manifold (2),” Sci. Univ. Kyoto Math., Series A 28 (1), 81–85 (1953).Google Scholar
58.Kunita, H., Stochastic Flows and Stochastic Differential Equations (Cambridge University Press, Cambridge, UK, 1997).Google Scholar
59.Mc Connell, J., Rotational Brownian Motion and Dielectric Theory (Academic Press, New York, 1980).Google Scholar
60.Mc Kean, H. P. Jr., “Brownian motions on the 3-dimensional rotation group,” Mem. Coll. Sci. Univ. Kyoto, Series A 33 (1), 2538 (1960).Google Scholar
61.Planck, M., “Uber einen Satz der statistischen Dynamik und seine Erweiterung in der Quantentheorie,” Sitz. ber. Berlin A Akad. Wiss. 1917, 324–341 (1917).Google Scholar
62.Risken, H., The Fokker–Planck Equation, Methods of Solution and Applications, 2nd ed. (Springer-Verlag, Berlin, 1989.Google Scholar
63.Roberts, P. H. and Ursell, H. D., “Random walk on a sphere and on a Riemannian manifold,” Phil. Trans. R Soc. London A252, 317–356 (1960).Google Scholar
64.Kampen, N. G. van, Stochastic Processes in Physics and Chemistry (North Holland, Amsterdam, 1981).Google Scholar
65.Yosida, K., “Integration of Fokker–Planck's equation in a compact Riemannian space,” Arkiv für Matematik 1 (9), 71–75 (1949).CrossRefGoogle Scholar
66.Higham, D. J., “An algorithmic introduction to numerical simulation of stochastic differential equations,” SIAM Rev. 43 (3), 525–546 (2001).CrossRefGoogle Scholar
67.Murray, R. M., Li, Z. and Sastry, S. S., A Mathematical Introduction to Robotic Manipulation (CRC Press, Ann Arbor MI, 1994).Google Scholar
68.Smith, P., Drummond, T. and Roussopoulos, K., “Computing MAP Trajectories by Representing, Propagating and Combining PDFs over Groups,” Proceedings of the 9th IEEE International Conference on Computer Vision, vol. 2, Nice, France (2003) pp. 1275–1282.Google Scholar
69.Su, S. and Lee, C. S. G., “Manipulation and propagation of uncertainty and verification of applicability of actions in assembly tasks,” IEEE Trans. Syst. Man Cybern. 22 (6), 1376–1389 (1992).Google Scholar
70.Zhou, Y., Chirikjian, G. S., “Probabilistic Models of Dead-Reckoning Error in Nonholonomic Mobile Robots,” Proceedings of ICRA'03, Taipei, Taiwan (Sep. 14–19, 2003) pp. 1594–1599.Google Scholar
71.Wang, Y. and Chirikjian, G. S., “Error propagation on the Euclidean group with applications to manipulator kinematics,” IEEE Trans. Robot. 22 (4), 591–602 (2006).CrossRefGoogle Scholar
72.Wang, Y. and Chirikjian, G. S., “Worksapce generation of hyper-redundant manipulators as a diffusion process on SE(N),” IEEE Trans. Robot. Autom. 20 (3), 339–408 (2004).CrossRefGoogle Scholar
73.Wang, Y. and Chirikjian, G. S., “A New Potential Field Method for Robot Path Planning,” Proceedings of ICRA'00, San Francisco, CA (Apr. 2000) pp. 977–982.Google Scholar
74.Choset, H., Burgard, W., Hutchinson, S., Kantor, G., Kavraki, L. E., Lynch, K. and Thrun, S., Principles of Robot Motion: Theory, Algorithms, and Implementation (MIT Press, Cambridge, MA, USA, 2005).Google Scholar
75.Chirikjian, G. S. and Ebert-Uphoff, I., “Numerical convolution on the Euclidean group with applications to workspace generation,” IEEE Trans. Robot. Autom. 14 (1), 123–136 (1998).CrossRefGoogle Scholar
76.Ebert-Uphoff, I. and Chirikjian, G. S., “Inverse Kinematics of Discretely Actuated Hyper-Redundant Manipulators Using Workspace Densities,” Proceedings of ICRA'96, Minneaoplis, MN, pp. 139–145. Minneapolis, MN, USA.Google Scholar
77.Mason, R. and Burdick, J. W., “Trajectory Planning Using Reachable-State Density Functions,” 2002 IEEE International Conference on Robotics and Automation, Washington, DC (May 2002) pp. 273–280.Google Scholar
78.Kyatkin, A. B. and Chirikjian, G. S., “Computation of robot configuration and workspaces via the Fourier transform on the discrete motion group,” Int. J. Robot. Res. 18 (6), 601–615 (1999).CrossRefGoogle Scholar
79.Li, Z. and Canny, J. F., eds., Nonholonomic Motion Planning (Kluwer Academic Publishers, Dordrecht, The Netherlands, 1993).CrossRefGoogle Scholar
80.Laumond, J. -P., ed., Robot Motion Planning and Control (Springer, New York, USA, 1998).CrossRefGoogle Scholar
81.Chirikjian, G. S. and Burdick, J. W., “A modal approach to hyper-redundant manipulator kinematics,” IEEE Trans. Robot. Autom. 10, 343–54 (1994).CrossRefGoogle Scholar
82.Webster, R. J. III, Kim, J. S., Cowan, N. J., Chirikjian, G. S. and Okamura, A. M., “Nonholonomic modeling of needle steering,” Int. J. Robot. Res. 25, 509–525 (2006).CrossRefGoogle Scholar
83.Park, W., Kim, J. S., Zhou, Y., Cowan, N. J., Okamura, A. M. and Chirikjian, G. S., “Diffusion-Based Motion Planning for a Nonholonomic Flexible Needle Model,” IEEE International Conference on Robotics and Automation (2005) pp. 4611–4616. Barcelona, Spain.Google Scholar
84.Alterovitz, R., Simeon, T. and Goldberg, K., “The stochastic motion roadmap: A sampling framework for planning with Markov motion uncertainty,” Proc. Robotics: Science and Systems III, June 2007, Atlanta, CA.CrossRefGoogle Scholar