Pose and Position Estimation of Dozer Blade in 3- dimensional by Integration of IMU with Two RTK GPSs

Transcription

1 Pose and Poston Estmaton of Dozer Blade n 3- dmensonal by Integraton of IMU wth Two RTK GPSs D.I. Sun a and S.H. Km b, Y.S. Lee b, S.K. Lee a, C.S. Han c a Mechatroncs Engneerng of Hanyang Unversty of Republc of Korea b Graduate School of Hanyang Unversty, Republc of Korea c Robot Engneerng of Hanyang Unversty of Republc of Korea E-mal: jenussd@hanyang.ac.kr, cshan@hanyang.ac.kr Abstract Automaton system n constructon machne such as excavator, loader and other thngs has been developed to mprove the performance and reduce the cost, tme and manpower. Dozer s no excepton. Dozer s a crawler equpped wth a substantal metal plate known as blade used to push large quanttes of sol, sand, or other materals durng levelng, dggng or converson work. Dozer can be found on a wde range of stes, whch are mnes, quarres and road. Therefore, the cost and tme consumng n dozer workng are hghly sgnfcant matter. However, n order to mplement the automaton system n constructon equpment, gudance system for constructon workng has to be done before. Because the automaton system can perform the operaton and work better wth nformaton such as velocty, pose and posture etc. thorough the sensor. These system s called machne gudance system because gudance system s appled to constructon machne. Shortly, f gudance system s done very well, the achevement of automaton system could be better. Dozer s also no excepton. Ths paper wll descrbe the method for estmatng the pose and poston of dozer blade, namely gudance of dozer, by sensor fuson or ntegraton of nertal measurement unt devce wth two RTK GPSs. And then the proposed method s verfed by expermental result. Keywords Machne Gudance; Dozer; Sensor Fuson 1 Introducton Dozer s a crawler equpped wth a substantal metal plate known as blade used to push large quanttes of sol, sand, or other such materal durng constructon such as gradng, levelng, dggng or converson work. [1] Dozer can be found on a wde range of stes whch are mnes, quarres, road and express constructon etc. Therefore, the cost and tme consumng n dozer workng are hghly sgnfcant matters. The automaton system of dozer s assstance system for mprovng the performance such as accuracy and effcency. A drver drves and commands the constructon works through the system nterface. After that, system do work accordng to order by usng the nformaton from sensor systems nstalled n dozer and feld. Operator just pay attenton to only drvng. Blade motons such as lftng, tltng, anglng known as ptch, roll and yaw are operated automatcally durng drvng. As mentoned above, consequently, automated dozer brngs the enhancement of productvty and effcency rather than manual. Fgure 1 ncludes the these contents. Komatsu, representatve Japanese company wrote a techncal paper about dozer automaton system whch s ncludng the effcency. [2] Fgure 1. The table s presented n Komatsu, Japanese development company: Advanced performance n both cost and accuracy part The automaton system n cvl engneerng s called machne control. And the system whch ncorporates the poston sensors, moton measurng unts and other devces s to provde on-board wth nformaton about the movement of machne such as dozer and excavator. And ths system s called machne gudance. In ths

2 paper, these systems are appled to dozer, so we can name t as a dozer machne control and gudance. As mentoned above, dozer machne control s a factor that can brng a lot of benefts. In order to mplement the dozer machne control, however, machne gudance must be done before. Because, the control can be performed by recevng the movement nformaton lke poston, orentaton, pose and velocty etc. by usng the sensor systems. To put t brefly, f machne gudance s performed greatly, the achevement of dozer machne control system could be better. For ths reason, machne gudance s also sgnfcant part, and the sensor system should be nstalled well too. Ths paper wll descrbe the method about pose and poston estmaton of blade by ntegraton of IMU wth two RTK GPSs. And then proposed algorthm s verfed by expermental results. 2 Well Used Sensor Fuson Method Examples about Pose Estmaton Many researcher have worked about sensor fuson method for estmatng the pose and poston of vehcle. The UAV(Unmanned Aeral Vehcle), shp and flght has to nstall the navgaton system for drvng as well. Keong, [3], proposed the method about determnng headng and ptch by usng a sngle dfference GPS. Kumar, [4], used the extended Kalman flter for dong ntegraton of nertal navgaton system and global postonng system. Godha, [5], suggested the ntegrated GPS and INS system for pedestran navgaton n a sngle degrade envronment. Ths paper has a novelty value n approach to non-vehcle system. In domestc, Ko, [6], performed the ndoor localzaton of a moble robot by usng external sensor. Yeom, [7], used the GPS and accelerometer for estmatng the poston of movng object. To mplement the dozer machne control, Xue, [8], analyzed the knematcs of dozer by smulaton, and Sun, [9], me, studed on sensor fuson algorthm for estmatng the atttude of dozer blade. Fgure 2 llustrates a system confguraton n detal. Fgure 2. System confguraton: IMU and GPS To generate the Cartesan coordnates system, nstead of GPS whch has longtude, lattude and heght, UTM (Unversal Transverse Mercator) coordnates s used. Based on ths, the frame of dozer, sensor and reference frame can be formed. Reference frame s naturally set to be the orgn of UTM coordnate system. And then frame wth respect to cabn of dozer and blade are set by conventon,.e., roll s defned by rotaton about axs that pontng forward and yaw s rotaton that done by upward axs. Ptch s done by rght hand rule. We nstalled the GPS and IMU for recevng the pose and poston of blade and cabn. Especally, pose of blade s estmated by calculaton of GPS data for compensatng the unstable n IMU devce, whch s expressed wth respect to orgn of frame n UTM coordnates system. Ths s why frame of sensor system s necessary. 3.2 Pose Estmaton The process of lnear Kalman flter s used for estmatng the pose of blade relably. Fgure 3 s algorthm about lnear Kalman flter that s used n. 3 Estmatng the Poston and Pose of Dozer Blade 3.1 System confguraton and coordnates In machne gudance of dozer, sensor system may have to be nstalled to dozer. Frstly, two IMU are used to get the pose, namely, roll, ptch and yaw of blade and cabn at perod of 100Hz. Secondly, GPS antennas are used to get the poston. The one s base antenna that wll be nstalled on the outsde of dozer, the others are attached on the blade. The rover wll be n the cabn. Fgure 3. Modfed Kalman flter algorthm for estmatng the pose

3 3.2.1 Predcton and Measurement Modelng There are two process modellngs n the lnear Kalman flter. One s predcton and another s measurement. Predcton modellng s defned as follows, equaton (1). x k1 where Ax, k 1 1 Roll IMU Roll IMU x, A( N) Ptch IMU PtchIMU 0 43 yawimu YawIMU (1) N s the step parameter for GPS data, and state of x wll be compared wth pose derved from measurement modellng. Measurement modellng s defned as follows, equaton (2), (3), (4) and (5). RollGPS ( N) zn PtchGPS ( N) YawGPS ( N) (2) Roll GPS=arcsn((z 1-z 2)/Baselne) where z 1 and z 2 s heght data of GPS Yaw GPS=arctan((y 1-y 2)/(x 1-x 2))+π/2 Where, x 1>x 2, π/2 arctan((y 1-y 2)/(x 1-x 2)) π/2 Yaw GPS=arctan((y 1-y 2)/(x 1-x 2))-π/2 Where, x 1<x 2, π/2 arctan((y 1-y 2)/(x 1-x 2)) π/2 (3) (4) (5) Each functon s used n lnear Kalman flter so that the atttude of blade can be estmated more precsely. 3.3 Estmaton of Edge Poston Most mportant thngs n dozer machne gudance n constructon workng s to know the bottom poston of blade, namely edge. Because, landform have a consderable effect on workng process. However, edge poston cannot be estmated drectly because of sensor nstallaton on the top of the blade. Therefore, n order to estmate the edge poston by usng the sensor system, knematcs has to be used as alternatve soluton. Fgure 4 s explanng overall frame of body, lnk and jont. Each Table 1 and 2 s Denavt-Hartenberg parameters table about fgure 4. Fgure 4. The assgnment of lnk frames n dozer blade havng 3 degrees of freedom Table 1. D-H parameters table concerned wth antenna a (deg) 1 a 1 ( m) d ( m ) (deg) 5 0 a a Baselne/ a

4 Table 2. D-H parameters table concerned wth antenna 2 (deg) 1 a 1 ( m) d ( m ) (deg) 4-90 a a 4 6 Baselne/ a 6 The reference frame s denoted by {U}. {J} or {0} s frame of ptch jont and orgn of ths s on the mddle of two ptch jont. Frame 7 s end effector and 6 s used for estmatng the orgn poston of {0}. Theta 1, 2 and 3 s angle of ptch, yaw and roll. The transformaton matrx of body expressed wth respect to UTM coordnates system can be derved by IMU data whch s nstalled n cabn. By dong the forward knematcs overall, edge poston of blade wll be estmated. 4 Experment and Evaluaton 4.1 Expermental Setup Fgure 5 s showng the dozer and sensor system and where experment was done. gven by equaton (6) Q, R H 1 0, P(0) Result and Evaluaton (6) Experment was done n roll, ptch and yaw moton of blade wth non-drvng. In ths paper, expermental result wll be mentoned about roll moton wth nondrvng representatvely. The results of estmatng the pose n roll moton at perod of 100Hz through the sensor ntegraton are n Fgure 6. The results of graph whch represent the estmated edge poston wth regard to x, y and z axs n roll moton can be founded n Fgure 7, 8, and 9. In Fgure 6, pose, n other words, rotaton data from IMU and estmatons derved from sensor ntegraton are matched well. If more ant-vbraton system to sensor s appled well, the result wll be better. Addtonally, Fgure 7, 8 and 9 are modfed lttle for analysng the proposed method. In the fgure, UTM zone poston value s removed. Consequently, edge poston of blade s estmated as expected. Fgure 5. Expermental setup: Dozer, sensors and locaton that s Hanyang Unv. at Ansan The samplng rate of GPS and IMU are set to be dfferent. GPS s 20Hz and IMU s 100Hz. Lnk parameters and jont parameters of D-H parameter were measured. Baselne s 2.78m, a 1 s 1.69m, a 3 s 0.51m, a 4 s 0.23m, a 6 s 0.85m and d 6 s Baselne/2. Each parameter Q, R, H and P 0 of sensor ntegraton part are 5 Concluson In ths paper, estmaton method about pose and poston of dozer blade was proposed for machne control. To do that, IMU and RTK GPS were nstalled n dozer system. The estmaton results about edge poston were done as expected. However, sensor error such as black out, satellte factor and naccurate measurement of lnk parameter were exsted. Ths s a problem awatng to be solved wth software and hardware. The estmated poston was not compared wth real edge poston. Therefore, more relable evaluaton s needed and should be done later. Pose and poston of dozer, not blade, was not handled. But for dong machne control, gudance system about cabn and blade of dozer are necessary. So, ths wll have to be done later also. As mentoned n ntroducton, machne gudance s necessary part n mplementaton of machne control. Fnal goal of ths research s to do the machne control. When machne gudance s completed, machne control

5 of dozer wll be progressed based on ths research. Fgure 6. The graph of test results about estmated pose of blade n roll moton: IMU data s compensated by GPS Fgure 7. The result graph of test about estmatng the edge poston wth regard to x axs n roll moton Fgure 8. The result graph of test about estmatng the edge poston wth regard to y axs n roll moton

6 Fgure 9. The result graph of test about estmatng the edge poston wth regard to z axs n roll moton 6 Acknowledgements Ths research was supported by the Industral Strategc technology development program (No ) funded by the Mnstry of Trade, Industry and Energy (MOTIE), KOREA and was supported by a grant(14scip-b ) from Smart Cvl Infrastructure Research Program funded by Mnstry of Land, Infrastructure and Transport(MOLIT) of Korea government and Korea Agency for Infrastructure Technology Advancement(KAIA) References [1] Wkpeda, [2] Kazuhko Hayash, et al. Development of D61EX_PX-23 Bulldozer wth automatc control system of work equpment. Komatsu techncal report, (2013): [3] Keong, Jun Han., Determnng headng and ptch usng a sngle dfference GPS/GLONASS approach. Natonal Lbrary of Canada= Bbloth èque natonale du Canada, [4] Kumar, Vkas. Integraton of nertal navgaton system and global postonng system usng Kalman flterng. Dss. INDIAN INSTITUTE OF TECHNOLOGY, BOMBAY MUMBAI, [5] Godha, Saurabh, G é rard Lachapelle, and M. Elzabeth Cannon. "Integrated GPS/INS system for pedestran navgaton n a sgnal degraded envronment." ION GNSS. Vol [6] Ko, Nak-Yong, and Tae-Gyun Km. "Indoor Localzaton of a Moble Robot Usng External Sensor." Journal of Insttute of Control, Robotcs and Systems 16.5 (2010): [7] Yeom, Jeong-Nam, et al. "Poston Estmaton System of Movng Object usng GPS and Accelerometer." Journal of Korea Multmeda Socety 12.4 (2009): [8] Gang Xue, C. S. Han, Y. S. Lee, C.U.J, M. S. Kang. "A study on the knematc analyss and dynamc smulaton for automatc 3D control of the dozer blade." Conference paper of KSPE, (2015.5): [9] D. I. Sun, C. S. Han, Y. S. Lee, S. H. Km, S. H. Lee. "A Study on Sensor Fuson Algorthm for Dozer s Blade Poston and Atttude Estmaton." Conference paper of KSPE, (2016.5):