臺灣博碩士論文加值系統

移動式機器人的定位系統以及演算法是機器人應用上的關鍵技術，且隨著硬體條件的進步與改變，整個系統的推導與解決方法也必須跟著推陳出新，至今尚未有一個定位系統可以同時兼顧精確度與系統複雜度並廣泛地被使用。本論文旨在利用最少限度的硬體設備搭配新推導的演算法試著來解這個問題，我們提出一個定位方法，僅需用到單一個地標當做小範圍區域內的原點並量測與機器人之間的距離，加上機器人的移動方向資訊，即可以收斂出相對於一個固定座標系上的座標點與移動路徑。其困難點在於此系統不單存在非線性雜訊的干擾，更因為量測資訊的不足使得我們無法得到一個閉合形式的數學解，因此我們從數學、幾何學、訊號處理等各個角度，深入分析此問題並提出了幾個假設，在最小限度地影響整個系統的使用條件下找到一個可收斂的解析解。
在驗證此演算法的硬體設備選擇方面，地標的部分我們利用二個超音波模組分別裝設在地標和機器人身上，一個當發射台一個當接收端，根據接收端收到的時間差乘上聲速來估測機器人與地標之間的距離。移動指向的部分則採用慣性感測器來估測機器人相對於地磁方位的轉向角度。為了得到更精確的指向資訊，我們推導了一套新的穩健姿態估測演算法，選用方向餘弦矩陣來表示機器人與參考座標系之間關系的旋轉矩陣，其系統動態與量測方程式相較之下具有高線性度，有助於提升估測精確度。為了解決加速規和磁力計會受線性加速度以及環境硬磁和軟磁的干擾問題，我們也提出對應的演算法將加速規、磁力計和陀螺儀的資訊融合、優缺點互補，以得到更加穩健的估測結果。而串連這整個系統的則是用條件狀態限制形態的卡爾曼濾波器。最後，本論文提供模擬與真實環境下的實驗結果來說明所提出方法的可行性。

Mobile robot localization systems and methods are key technologies in robotic applications. The overall system design and algorithms have to be improved following the progress of newly developed hardware. So far there is not a method that is widely used as a general solution due to considerations of the estimation accuracy, cost and system complexity. This dissertation aims to find a new localization method using a limit hardware setup. This algorithm, which is based on the range to a single landmark and heading direction relative to a fixed frame, can converge to a pre-designed path without knowing the initial state. Due to the lack of measured information, this system not only suffers from the nonlinear noise, but also has no closed-form mathematical solution. Therefore, in this dissertation, several specific assumptions by geometrical and mathematical analysis of the problem are made to allow the system find a convergent analytical solution.
A pair of ultrasonic sensors is used to verify the algorithm in experiment. One is installed on the landmark as a receiver and the other on a mobile robot as a transmitter. The time difference scaled by acoustic velocity is calculated as the range measurement between landmark and robot. In addition, the MARG sensors (Magnetic, Angular Rate, and Gravity) are used to estimate the orientation of the robot relative to the Earth’s NED frame (North, East, Down) in a local field. For achieving a robust estimation, a new fusion algorithm is derived. The DCM (Direct Cosine Matrix) is utilized for the rotation matrix relative to a fixed frame because of its linearity in system and measurement equations. Further, the robustness is achieved by following two online methods: compensation of hard iron effect for the magnetometer and separation of the accelerometer signals into gravity projections and linear accelerations. The fusion equations are solved by using a new developed equality and inequality state soft and hard constrained Kalman filter. In conclusion, both simulations and experiments are conducted to validate these proposed algorithms.

摘要 i
Abstract ii
誌謝 iv
Contents vi
List of Figures viii
List of Tables ix
List of Notations x
Chapter 1 1
Introduction 1
1.1 Overview of Landmark Based Localization 2
1.1.1 Current Location Sensing Technologies 3
1.1.2 Range Estimation 3
1.2 Overview of Orientation Estimation 4
1.2.1 Reference Frame 5
1.2.2 Rotation Matrix 7
1.2.3 Difficulties of MARG Sensors 9
1.3 Overview of State Constrained Kalman Filter 10
1.4 Outline of Proposed Methods 14
1.4.1 Localization Using a Single Landmark and Heading Information 14
1.4.2 DCM based Orientation Estimation Using MARG sensors 14
1.4.3 A Soft and Hard State Constrained Kalman Filter 15
1.5 Contributions of this Dissertation 16
1.6 Dissertation Organization 17
Chapter 2 18
Localization Using a Single Landmark and Heading Information 18
2.1 Introduction 18
2.2 Problem Statement 20
2.3 Bounded and Converged Condition 22
2.4 Implementation Scenarios 28
2.4 Estimation Algorithm 29
2.5 Summary 31
Chapter 3 32
DCM based Orientation Estimation Using MARG sensors 32
3.1 Introduction 32
3.2 DCM-based Orientation Estimator 36
3.3 Gravity Projection 38
3.4 On-line Hard Iron Compensation 40
3.5 The Architectures of the Observer 42
3.6 Self-Rotation and Calibration Device in 2D Application 43
3.7 Summary. 45
Chapter 4 46
A Soft and Hard State Constrained Kalman Filter 46
4.1 Introduction 46
4.2 Weighted Least Square with Constraints 47
4.3 Linear Soft and Hard State Constrained Kalman Filter 48
4.4 Nonlinear State Constrained Kalman Filter 53
4.5 Inequality State Constrained Kalman Filter 55
4.6 Summary 56
Chapter 5 57
Simulation and Experimental Results 57
5.1 A Soft and Hard State Constrained Kalman Filter 57
5.1.1 Simulation Results 57
5.2 DCM based Orientation Estimation Using MARG sensors 59
5.2.1 Simulation Results 59
5.2.2 Experimental Results 62
5.3 Localization Using a Single Landmark and Heading Information 64
5.3.1 Simulation Results 64
5.3.2 Hardware of Experiments 76
5.3.3 Experimental Results 80
5.4 Summary 83
Chapter 6 84
Conclusions and Future Research Topics 84
6.1 Conclusions 84
6.2 Future Research Topics 85
References 86
Appendix I 93

[1] J. Hightower and G. Borriello, "Location systems for ubiquitous computing", IEEE Computer, vol. 34, pp.57 -66 2001
[2] Y. Zhang, W. Wu, and Y. Chen, “A range-based localization algorithm for wireless ksensor networks,” Journal of Communications and Networks, vol. 7, no. 4, pp. 429–437, 2005.
[3] D. H. Titterton and J. L. Weston, “Strapdown Inertial Navigation Technology”, 1997 :Peregrinus
[4] J. B. Kuipers, “Quaternions and Rotation Sequences”, Princeton Univ. Press, 1999.
[5] E. Kraft, "A quaternion-based unscented Kalman filter for orientation tracking," Proc. IEEE 6th Int. Conf. Inf. Fusion, pp.47–54, 2003.
[6] J. L. Marins, X. Yun, E. R. Backmann, R. B. McGhee, and M. Zyda, "An extended Kalman filter for quaternion-based orientation estimation using MARG sensors," IEEE/RSJ Int. Conf. Intelligent Robots Systems, pp.2003–2011, 2001.
[7] J. K. Lee and E. J. Park, "Minimum-order Kalman filter with vector selector for accurate estimation of human body orientation," IEEE Trans. Robotics, vol. 25, no. 5, pp.1196–1201, 2005.
[8] F. L. Markley, "Fast quaternion attitude estimation from two vector measurements, " J. Guid. Control Dyn., vol. 25, no. 2, pp.411–414, 2002.
[9] D. Simon and T. Chia, "Kalman filtering with state equality constraints," IEEE Trans. Aerosp. Electron. Syst., vol. 39, no. 1, pp.128–136, 2002.
[10] D. Simon, "Kalman filtering with state constraints: a survey of linear and nonlinear algorithms," Control Theory &; Applications, IET, vol. 4, pp.1303–1318, 2010.
[11] Wen W., Durrant-Whyte H.: “Model-based multi-sensor data fusion”. IEEE Int. Conf. on Robotics Automation, Nice,France, 1992, pp. 1720–1726
[12] Porrill J.: “Optimal combination and constraints for geometrical sensor data”, Int. J. Robot. Res., 1988, 7, (6), pp. 66–77
[13] Alouani A., Blair W.: “Use of a kinematic constraint in tracking constant speed, maneuvering targets”, IEEE Trans. Autom. Control, 1993, 38, (7), pp. 1107–1111
[14] Wang L., Chiang Y., Chang F.: “Filtering method for nonlinear systems with constraints”, IEE Proc. Control Theory Appl., 2002, 149, (6), pp. 525–531
[15] Chia T.: “Parameter identification and state estimation of constrained systems”. PhD thesis, Case Western Reserve University, 1985
[16] Simon D.: “Optimal state estimation” (John Wiley &; Sons, 2006)
[17] Shimada N., Shirai Y., Kuno Y., Miura J.: “Hand gesture estimation and model refinement using monocular camera – ambiguity limitation by inequality constraints". IEEE Int. Conf. on Automatic Face Gesture Recognition Nara, Japan, 1998, pp. 268–273
[18] Simon D., Simon D.L.: “Constrained Kalman filtering via density function truncation for turbofan engine health estimation”, Int. J. Syst. Sci., 2010, 41, (2), pp. 159–171
[19] Ko S., Bitmead R.: “State estimation for linear systems with state equality constraints”, Automatica, 2007, 43, (8), pp. 1363–1368
[20] J.-S. Hu, C.-Y. Chan, C.-K. Wang, and C.-C. Wang, “Simultaneous localization of mobile robot and multiple sound sources using microphone array,” in Proc. 2009 IEEE intl. conf. on Robotics and Automation, 2009, pp. 4004–4009.
[21] C.-C. Wang, C. Thorpe, S. Thrun, M. Hebert, and H. Durrant-Whyte, “Simultaneous localization, mapping and moving object tracking,” The International Journal of Robotics Research, vol. 26, no. 9, pp. 889–916, September 2007.
[22] J. J. Guerrero , A. C. Murillo and C. Sagues "Localization and matching using the planar trifocal tensor with bearing-only data", IEEE Trans. Robot., vol. 24, no. 2, pp.494 -501 2008

[23] L. Jaulin, "Range-only slam with occupancy maps: A set-membership approach," Robotics, IEEE Trans. on, vol. 27, no. 5, pp. 1-6, 2011.
[24] A. Costa, G. Kantor, and H. Choset, “Bearing-only landmark initialization with unknown data association,” in Proc. IEEE Int. Conf. Robot. Autom.,2004, pp. 1164–1770.
[25] N. M. Kwok, G. Dissanayake, and Q. P. Ha, "An efficient multiple hypothesis filter for bearing-only slam," in Proc. 2004 IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems, Sendai, Japan, Sept. 2004.
[26] F. A. Belo, P. Salaris, and A. Bicchi, “3 known landmarks are enough for solving planar bearing slam and fully reconstruct unknown inputs,” in Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on. IEEE, 2010, pp. 2539–2545.
[27] R. M. Vaghefi, M. R. Gholami, and E. G. Str ¨om, “Bearing-only target localization with uncertainties in observer position,” in Proc. IEEE PIMRC workshops, Sept. 2010, pp. 238–242.
[28] H. Sert, A. Kokosy, and W. Perruquetti, "A single landmark based localization algorithm for non-holonomic mobile robots," in IEEE International Conference on Robotics and Automation, 2011, pp. 293-298.
[29] A. Bais, R. Sablatnig, and J. Gu, "Single landmark based self-localization of mobile robots," in Proceedings of the 3rd Canadian Conference on Computer and Robot Vision (CRV'06) - Volume 00: IEEE Computer Society, 2006, pp. 67.
[30] A. Bais, R. Sablatnig, J. Gu, and Y. M. Khawaja, “Location tracker for a mobile robot,” in Proceedings of the IEEE International Conference on Industrial Informatics, July 2007.
[31] A. Bais, R. Sablatnig, J. Gu, and S. Mahlknecht, “Active single landmark based global localization of autonomous mobile robots,” in Proceedings of the International Conference on Visual Computing, ser. Lecture Notes in Computer Science, G. B. et al., Ed., no. 4291. Lake Tahoe, Nevada, USA: Springer-Verlag Berlin Heidelberg, November 2006, pp. 202 –211.
[32] S. J. Julier and J. J. LaViola, “On Kalman filtering with nonlinear equality constraints,” IEEE Trans. Signal Process., vol. 55, no. 6, pp. 2774–2784, June 2007.
[33] C. Yang and E. Blasch, “Kalman Filtering with Nonlinear State Constraints”, IEEE Transactions on Aerospace and Electronic Systems, vol. 45, no. 1, Jan. 2009, pp. 70-84.
[34] R. E. Kalman, “A new approach to linear filtering and prediction problems,” Trans. ASME-J. Basic Eng., vol. 82, pp. 35–45, 1960.
[35] G. Dissanayake, S. Sukkarieh, E. Nebot, and H. Durant-Whyte, “The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications,” IEEE Trans. Robot. Automat., vol. 17, pp. 731–747, Oct. 2001.
[36] S. Saripalli, J.M. Roberts, P.I. Corke, G. Buskey, and G.S. Sukhatme. “A tale of two helicopters,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, volume 1, pages 805-810, Las Vegas, pp. 27-31 Oct. 2003.
[37] A. M. Sabatini, C. Martelloni, S. Scapellato, and F. Cavallo, “Assessment of walking features from foot inertial sensing,” IEEE Trans. Biomed. Eng.,vol. 52, no. 3, pp. 486–494, Mar. 2005.
[38] G. Welch and E. Foxlin, “Motion tracking: No silver bullet, but a respectable arsenal,” IEEE Comput. Graph. Appl., vol. 22, no. 6, pp. 24–38, Nov./Dec. 2002.
[39] E. Bachmann, R. McGhee, X. Yun, and M. Zyda, “Inertial and magnetic posture tracking for inserting humans into networked virtual environments,” in Proc. ACM Symp. Virtual Reality Softw. Technol, Banff, Canada, pp. 9–16, Nov. 2001.
[40] Liu, W. and Zhou, Y, “Recovering the position and orientation of a mobile robot from a single image of identified landmarks,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, pp. 1065–1070, 2007
[41] S. A. H. Tabatabaei, A. Gluhak, and R. Tafazolli, "A Fast Calibration Method for Triaxial Magnetometers," IEEE Trans. Instrum. Meas., vol. 62, pp.2929–2937, 2013.

[42] J. L. Marins, X. Yun, E. R. Backmann, R. B. McGhee, and M. Zyda, "An extended Kalman filter for quaternion-based orientation estimation using MARG sensors," IEEE/RSJ Int. Conf. Intelligent Robots Systems, pp.2003–2011, 2001.
[43] Z. Ercan, V. Sezer, H. Heceoglu, C. Dikilitas, M. Gokasan, A. Mugan, S. Bogosyan “Multi-sensor data fusion of DCM based orientation estimation for land vehicles”, IEEE Int. Conf. on Mechatronics, pp. 672–677, 2011
[44] Nguyen Ho Quoc Phuong, “Study On Orientation Estimation With Three Different Representations”. In Proc. Int. Symposium on Electrical &; Electronics Engineering, Vietnam, pp. 80-88, 2007
[45] Anh-Tung Dang, Vinh-Hao Nguyen, "DCM-based orientation estimation using cascade of two adaptive extended Kalman filters", IEEE Int. Conf. on Control, Automation and Information Sciences, Vietnam, pp.152-157, 2013.
[46] D. Jurman, M. Jankovec, R. Kamnik, and M. Topic, "Calibration and data fusion solution for the miniature attitude and heading reference system," Sensors and Actuators A: Physical, vol. 138, pp.411–420, Aug. 2007.
[47] R. Zhou, D. Sun, Z. Zhou, and D. Wang, "A linear fusion algorithm for attitude determination using low cost MEMS-based sensors," Measurement, vol. 40, no. 3, pp.322–328, 2007.
[48] E. Edwan, J. Zhang, J. Zhou and O. Loffeld, "Reduced DCM based attitude estimation using low-cost IMU and magnetometer triad," in Proc. of the 2011 Workshop on Positioning Navigation and Communication, pp. 1-6, 2011.
[49] J. E. Bortz, “A new mathematical formulation for strap-down inertial navigation,” IEEE Trans. Aerosp. Electron. Syst., vol. 7, no. 1, pp. 61–66, Jan. 1971.
[50] C. Verplaeste, “Inertial proprioceptive devices: Self-motion-sensing toys and tools,” IBM Syst. J., vol. 35, no. 3-4, pp. 639–650, 1996.

[51] Eric R. Bachmann, X. Yun, and A. Brumfield, “Limitations of attitude estimation algorithms for inertial/magnetic sensor modules,” IEEE Robotics and Automation Magazine, vol. 14, no. 3, pp. 76–87, Sep. 2007.
[52] J. C. Fang, H. W. Sun, J. J. Cao, X. Zhang, and Y. Tao, "A novel calibration method of magnetic compass based on ellipsoid fitting," IEEE Trans. Instrum. Meas., vol. 60, no. 6, pp.2053–2061, 2011.
[53] V. Petrucha and P. Kaspar, "Calibration of a triaxial fluxgate magnetometer and accelerometer with an automated non-magnetic calibration system," Sensors, IEEE, vol., no., pp.1510–1513, 25-28 Oct. 2009.
[54] Zhuohua Lin, M. Zecca, S. Sessa, L. Bartolomeo, H. Ishii, and A. Takanishi, "Performance evaluation of the wireless inertial measurement unit WB-4 with magnetic field calibration," in IEEE Int. Conf. on Robotics and Biomimetics (ROBIO), Guangzhou, China, pp.2219–2224, 2012
[55] W. Denne, “Magnetic Compass Deviation and Correction,” 3rd Ed. Glasgow: Brown and Ferguson, 1979.
[56] X. Yun , C. Aparicio , E. R. Bachmann and R. B. McGhee "Implementation and experimental results of a quaternion-based Kalman filter for human body motion tracking", Proc. IEEE Int. Conf. Robot. Autom., pp.317 2005
[57] Bachmann, E. R., Duman, I., Usta, U. Y., McGhee, R.B., Yun, X. P., Zyda, M. J.,“Orientation Tracking for Humans and Robots Using Inertial Sensors," Proc. of Symposium on Computational Intelligence in Robotics &; Automation, Monterey, CA, November 1999.
[58] Z.-Q. Zhang , X.-L. Meng and J.-K. Wu, "Quaternion-based Kalman filter with vector selection for accurate orientation tracking", IEEE Trans. Instrum. Meas., vol. 61, pp.2817 -2824 2012

[59] J.-S. Hu, and K.-C. Sun. “A Robust Orientation Estimation Algorithm Using MARG Sensors”. IEEE Transactions on Instrumentation and Measurement. 2014.
[60] J.-S. Hu, K.-C. Sun, and C.-Y. Cheng. “Kinematic Human Walking Model for Normal-Gait-Speed Estimation Using Tri-axial Acceleration Signals at Waist Location”. IEEE Transactions on Biomedical Engineering. 2013
[61] J.-S. Hu, C.-Y. Tseng, K.-C. Sun and M.-Y. Chen. “IMU-Assisted Monocular Visual Odometry Including the Human Walking Model for Wearable Applications,” IEEE International Conference on Robotics and Automation(ICRA), Karlsruhe, Germany, pp. 2894-2899, 2013.
[62] J.-S. Hu, K.-C. Sun, and C.-Y. Cheng. “GNSS/INS sensor fusion using Kalman filter with covariance adaptation”. 2013 International Automatic Control Conference (CACS), Nantou, Taiwan, pp. 52-57, 2013
[63] J.-S. Hu, K.-C. Sun, and C.-Y. Cheng. “A model-based human walking speed estimation using body acceleration data”. IEEE International Conference on Robotics and Biomimetics (ROBIO), Guangzhou, China, pp. 1985-1990, 2012
[64] J.-S. Hu, and K.-C. Sun. “Human gait estimation using a reduced number of accelerometers,” in Proc. SICE Annual Conference 2010, Taipei, Taiwan, pp. 1905-1909, 2010
[65] J.-S. Hu, K.-C. Sun, and J.-J. Wang, "Self-balancing control and manipulation of a glove puppet robot on a two-wheel mobile platform", IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), St. Louis, MO, USA, pp. 424-425, 2009
[66] J.-S. Hu, K.-C. Sun, and J.-J. Wang, "The glove puppet robot: X-puppet", IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), Nice, France, pp. 4145-4146, 2008