Altitude and Motion Estimation for Autonomous Vehicles through Wide-Field-Integration of Optic Flow

Transcription

1 Altitude and Motion Estimation for Autonomous Vehiles through Wide-ield-Integration of Opti low By Ryusuke AKAA, ) aoto KOBAYASHI, ) Mai BADO, ) and Shinji HOKAMOO ) ) Department of Aeronautis and Astronautis, Kyushu University, ukuoka, Japan (Reeived June 2st, 207) his study proposes an estimation method not only veloity dependent motion variables but also altitude information of a vehile. or the estimation of the variables, Wide-ield-Integration (WI) of opti flow algorithm is applied to image data obtained by onboard ameras. In this paper, first opti flow representations for vehile motion is extended so that they are appliable to images obtained by ameras off from the mass enter of a vehile. hen, to make the altitude estimation possible, the extended proedure is applied to opti flow obtained by two ameras: one is plaed at mass enter of a vehile, and another is a different position. he effetiveness of the proposed method is verified by numerial simulations onsidering sensor noises. Key Words: Opti flow, Motion Estimation, Altitude Estimation, WI of opti flow omenlature : sensitivity funtion : referene frame Q : position vetor of a photoreeptor Q : motion parallax vetor p : angular rate around x b -axis q : angular rate around y b -axis r : angular rate around b -axis u : translational veloity along x b -axis v : translational veloity vetor w.r.t. b v : translational veloity along y b -axis w : translational veloity along b -axis x : state variables : altitude : elevation angle : aimuth angle : nearness funtion : numerator of nearness funtion ω : rotational veloity vetor w.r.t. b Supersript : elevational diretion : aimuth diretion Subsripts b : body fixed t : terrain fixed. Introdution or deep spae exploration missions, autonomy is one of the key tehnologies beause the time delay of ommands from Earth is extremely long. hus, in the viinity of target elestial bodies, autonomous motion ontrol of spae probes should be established by using onboard sensors, beause exogenous position sensors (e.g. Global Positioning System: GPS) annot be used. urthermore, suh autonomous ontrol systems are desirable to be light weight, small sie, and low omputation. Wide-ield-Integration (WI) of opti flow is one of the promising tehniques, beause it has the desirable features stated above. WI of opti flow is a motion estimation method inspired by a biologial researh on the visual proessing systems of flying insets ompound eyes. )-6) Opti flow is the vetor field of relative motion to environments, and the integration over a wide field of opti flow (i.e. WI of opti flow) makes the motion estimation robust even in unknown environments. In the standard proedure for WI of opti flow, the translational and rotational veloities of a vehile are estimated by using other sensor outputs for the altitude and attitude information. Usually, the altitude is measured with a laser sensor, while the attitude information is obtained by Inertial avigation System, e.g. aelerometer, gyrosope, magneti sensor, et. his paper disusses a method to estimate not only the translational and rotational veloities but also the vehile s altitude information. 7), 8) he altitude estimation by WI of opti flow are investigated as, ) a bakup system of a laser sensor, 2) an alternative method when a laser sensor is useless due to engine plume in a final touh down phase, and 3) expanded theory from aademi viewpoints. irst, the fundamental relation between vehile s motion and opti flow obtained by a amera is generalied for arbitrary amera positions. hen, by utiliing two sets of opti flow obtained by two image sensors, the altitude estimation method is introdued. inally, the proposed method is verified by numerial simulations onsidering sensor noises.

2 2. Estimation Proedure in Standard WI of Opti low his setion first reintrodues the relation between a vehile s motion and opti flow obtained. hen, a standard WI of opti flow proess is explained under the assumption that a amera is plaed at the mass enter of the vehile. 2.. Opti flow Consider a vehile moving a viinity of a elestial surfae, and define two referene frames: an inertial frame t fixed to the surfae, and the vehile fixed frame b whose origin is plaed at the mass enter of a vehile (ig. ). he opti flow is the vetor field of relative veloities between the vehile and the environment. Assume an image sensor is plaed at the mass enter of the vehile, and its image surfae is a sphere of unit radius denoted with S 2. Moreover, there are photoreeptors on the spherial image surfae, and the positions of the photoreeptors are defined with the aimuth and elevation angles; these angles are both measured positive from the x b - and b -axes of the frame b, respetively. hen, the position of a photoreeptor is desribed from two sequential rotations for the aimuth and elevation angles, and, as follows: Q sin os sin sin os. () Consider that the fiduial point on the surfae is stationary to t, and that the referene frame b fixed to the vehile, whih is moving with v = [u v w] and ω = [ p q r ]. hen, the motion parallax vetor Q is defined as the time derivative of Q on the spherial imaging surfae. hen opti flow an be expressed as 8) Q ω Q v v Q Q, (2) where is defined as the inverse of the distane to a point on the elestial surfae. hen, the two omponent of opti flow along the aimuth and elevation diretions an be expressed from Eq. (2) as follows. Q p qs rs us v (3) Q ps q u vs ws (4) where (*) and s(*) indiate os(*) and sin(*), respetively Motion estimation by WI of opti flow he researh on opti flow has started over 50 years ago. 8)-0) However, preise motion estimation by using opti flow is diffiult, beause opti flow is very sensitive to the unertainty of surfae profiles and sensor noises. hus, WI of opti flow has been introdued to utilie a wide range of opti flow for better estimation. )-8) he obtained opti flow is integrated with spae funtions, whih are alled sensitivity funtions. By using the sensitivity funtions, sensor outputs y is obtained as follows: y Q d, (5) 2 S where dsin d d is a solid angle on the spherial image surfae. urthermore, the spae integration in Eq. (5) an be replaed with Riemann sum at the photoreeptor positions without degrading estimation auray, 5) as follow. ig.. Spherial image model for WI of opti flow. k,, y Q k k k k, (6) where k, k and indiates the position and the number of the photoreeptors on the spherial image surfae. or three-dimensional motion estimation, spherial harmonis are frequently used as the sensitivity funtions. urthermore, to distinguish the aimuth and elevational diretional omponents of opti flow, this study uses the following form of sensitivity funtions. 9) j 0 j, (7) 0 j where subsript j indiates the j-th spherial harmonis. hus, by using up to M-th spherial harmonis, the following output vetor is obtained for opti flow omponents measured by photoreeptors. y k k k k Q Q Q Q,, k k k k,, k k M k k,, k k k k,, k k M k k. (8) On the other hand, WI of opti flow virtually supposes a flat, infinite, and horiontal plane instead of unertain surfae profiles. hen, the nearness funtion in Eqs. (3) and (4) an be expressed as follows: s s s s s, (9) where is the altitude from the virtual plane, and are the roll and pith angles of the vehile expressed in the 3-2- Euler angle sequene. hus, the output y in Eq. (8) an 2

3 be also evaluated from the flat surfae assumption by using Eqs. (3), (4) and (9). he following 6-state variables appears linearly in the output equations, x u v w p q r. (0) hus, for the expression y Cx, the standard proedure for WI of opti flow an estimate the 6-motion variables by using the pseudo inverse matrix C as follows: x C y. () 3. Effet of Sensor Position on Opti low In previous setion, image sensors are assumed to be plaed at the mass enter of vehiles, and the opti flow measured by suh ameras is expressed as Eq. (2). Here, the effet of the amera s position off from the mass enter is disussed, and a simple ountermeasure to ope with is introdued. he first and seond terms in the right hand side of Eq. (2) are linear for the angular and translational veloities, respetively. hus, the equation an be rewritten in a simple form as where (, ) Q Aω Bv, (2) A s s s 0, s 0 B s s, and (, ) is the numerator in the right hand side of Eq. (9). he amera s position off from the vehile s mass enter effets in two parts of Eq. (2): altitude and veloity vetor v. he altitude at the amera position is different from that at the mass enter (see ig. 2). hus, in the same way as Eq. (), define the vetor P for the position of the amera w.r.t. b as P P sin os sin sin os, (3) where P indiates the distane from the mass enter, i.e. P P. hen, the altitude at the amera position an be desribed by onsidering the translational matrix from b to t, tb R, as follows. tb R P (4) P ss ss. ig.2. Spherial image plane plaed off from the mass enter of a vehile. ig.3. Camera s translational motion indued by a rotation around the mass enter of a vehile. On the other hand, the amera s veloity vetor v an be expressed from ig. 3 as follows: v vωp. (5) hus, from Eqs. (2) and (5), the opti flow measured by a amera an be expressed by the following relation. (, ) Q Aω Bv PBCω, (6) where is shown in Eq. (4) and C is a 3 3 matrix defined with the amera position. rom Eq. (6), the two omponents of opti flow an be expressed in the same way as Eqs. (3) and (4). herefore, the motion estimation proedure explained in Subsetion 2.2 an be expanded with small modifiations for ameras plaed off from the mass enter of a vehile. 4. Altitude Estimation Added to Motion Estimation or the estimation of the vehile s altitude from the ground, this setion proposes an estimation proedure by using two set of opti flow measured by two ameras. When one amera is assumed to be plaed at the mass enter of a vehile, i.e. P 0, the opti flow is expressed with Eq. (2). hen, define the following state variables instead of Eq. (0). U V W p q r : x V ω, (7) where U u, V v, and W w. hen, sine the vetor x has six omponents, all of the elements an be estimated through a standard proedure for WI of opti flow. urthermore, assume that the vehile has another amera plaed off from the mass enter. hus, the opti flow measured by the seond amera is desribed by Eq. (6). Multiplying in both sides of Eq. (6) leads the following relation: Q Aω Bv PBCω. (8) Here, note that sine the two ameras are boarded on a vehile, the state variables estimated from the seond amera should be same as those from the first one. hus, the V and ω estimated from the first sensor an be substituted into Eq. (8) onsidering v V. hen, the relation is ω P 3

4 Sine is shown in Eq. (4), the above relation inludes only one unknown parameter (ote, the seond term in n Eq. (4) an be evaluated as a onstant value when the attitude angles and are measured by other sensors). herefore, the altitude an be estimated from Eq. (9). One is obtained, then the translational veloities are alulated from v V. hus, ombining the estimation of ω ω, the altitude information and the six state variables an be estimated from two sets of opti flow measured by two ameras. ote sine the opti flow has two omponents, the altitude information from Eq. (9) an be evaluated in two ways: from the aimuth and from the elevation omponents. he point to be noted in this altitude estimation is explained in the next setion. 5.. umerial Simulation Q Aω BV PBCω. (9) o verify the effetiveness of the proposed altitude estimation proedure added to the motion estimation, this setion shows some typial numerial simulation results. ote, in the simulations, the followings onditionss are adopted for future experimental verifiationn using a miroo air vehile and real opti flow sensors. irst, the altitude and motion estimations of spae vehile are usually u required at tens or hundreds meters height above planet surfaes. However, the vehile altitude in the simulations is set as [m] for the experiments. ext, the opti flow sensor shown in n ig. 4 will be used in the experiments. he sensorr is Optiallow-Z made by ZMP In., and its proessing algorithm has been improved to generate opti flow vetors for the field of view 45. [deg] 34.6 [deg] with a frame rate of 0 [fps]. hus, the opti flow in the simulations is assumed to be obtained in the same onditions ass the experimental sensor. urthermore, the previous study 8) ) has implied that two opti sensors with 90 [deg]-interval inn the aimuth diretion show reasonable estimation aurayy for ig.4. Opti flow sensor used in future experiments. able. Camera positions used in the simulation. s Aimuth angle [deg] Elevation angle [deg] Distane from.m. R [m] Camera Camera able 2. Vehile s motion parameters used in thee simulations. parameters values Altitude [m] Attitude angles (,, ) [deg] (0, 0, 0) Case : ranslational veloities (u, v, w) [m/s] (0.2, 0.2, 0) Case 2: ranslational veloities (u, v, w) [m/s] (0.2, 0, 0) Angular veloities (p, q, r) [rad/s]] (0, 0, 0) ig.5. Vehile s ranslational veloities in two ases. any diretional translational motion of a vehile. hus, the amera positions in i the simulation are supposed as shown in able. his paper shows the estimation results for the vehile s two ases of motions shown in able 2, whih are same exept the translational veloities. igure 5 shows the geometri relations between the ameras and a translational diretions. ote that in eah ase, uniform random noises are added to both thee aimuth and d the elevationn omponents of opti flow vetors. he maximum magnitude of random noises is speifiedd with 30% of the magnitude of the exat opti flow, whih should be measured in an ideal ondition. igure 6 (a) and a (b) show the estimated result of the vehile s altitude in i Case fromm the aimuth and a the elevation omponents of opti o flow, respetively. hus, for better estimation, usually the two estimated altitude should be averaged at eah instant. igure 7 indiates the estimated motion variables for f the averaged altitude information. rom igs. 6 and 7, it iss seen that both the altitude information and motion variables (translational and rotationall veloities) an be estimated with reasonablee auraies by using two ameras. On the other hand, h ig. 8 shows the estimated altitude information in Case 2. In this ase, the estimation auray from the elevation omponent of opti flow is apparently worse. his reason an be explained from ig. 9. As an be seen, the opti flow vetors measured by thee amera 2 have small values in the elevational diretion. herefore, the estimation auray beomes lower. his implies one realisti proedure for the altitude a estimation as follows. irst, evaluate the magnitudes of two omponents of the obtained opti flow vetors, by sampling or by averaging. When both b omponents are not suffiientlyy small, the altitude information is finalied by averaging thee two estimations from the aimuth and elevation omponents. If one omponent iss small enough, only another omponent should be used for f the altitude estimation. 4

5 ig.6. Vehile ss altitudes estimaation from the aimuth and elevatioon omponents of o opti flow (Casse ). ig.7. Vehile ss motion variablees estimated from m WI of opti floow (Case ). ig.8. Vehile ss altitudes estimaation from the aimuth and elevatioon omponents of o opti flow (Casse 2). ig.9. Opti floow and its enlargeement aptured by y the amera 2 inn Case 2. 5

6 6. Conlusion his paper proposed a proedure to estimate both the vehile s altitude and its translational and rotational veloities by using two ameras. or this purpose, the relation between opti flow and the vehile s motion has been expanded so that they an deal with ameras plaed off from the mass enter of a vehile. hen the estimation proedure has been introdued. he effetiveness of the proposed method has been verified by numerial simulations onsidering sensor noises. Aknowledgments his researh is supported by Japan Soiety for the Pro-motion of Siene (JSPS) KAKEHI Grant-in-Aid for Challenging Exploratory Researh o. 5K4254, and by Sekisui Chemial Grant Program for Researh on Manufaturing Based on Innovations Inspired by ature. Referenes ) Humbert, J. S.,: Bio-Inspired Visuomotor Convergene in avigation and light Control Systems, Ph.D. Dissertation, Mehanial Engineering, Department, California Inst. of ehnology, Pasadena, CA, ) Humbert J. S., Murray R. M. and Dikinson M. H.: A Control-Oriented Analysis of Bio-inspired Visuomotor Convergene, Proeedings of the IEEE Conferene on Deision and Control, pp , ) Humbert J. S. and rye M. A.: Extrating Behaviorally Relevant Retinal Image Motion Cues via Wide-ield Integration, Proeedings of the Amerian Control Conferene, pp , ) Epstein M., Waydo S., et al.: Biologially Inspired eedbak Design for Drosophila light, Proeedings of the Amerian Control Conferene, pp , ) Conroy, J., Gremillion, G., Ranganathan, B. and Humbert, J. S.: Implementation of Wide-ield Integration of Opti low for Autonomous Quadrotor avigation, Autonomous Robots. 27, o. 3, pp , ) Kobayashi., Oishi M., Kinjo Y., Kubo D., and Hokamoto S: Motion estimation of autonomous spaeraft near asteroid using wide-field-integration of opti flow, Proeedings of the 25th International Symposium on Spae light Dynamis, USB, pp.-0, ) akata R., Kobayashi., Bando M., and Hokamoto S.: Altitude and motion estimation for small UAVs by using opti flow, Proeedings of the 8th KAIS-Kyushu University Symposium on Aerospae Engineering, ) Gibson, J., he Pereption of the Visual World, Houghton Mifflin, Oxford, England, U.K., pp. 7 44, ) Koenderink J. J. and van Doorn A. J.: ats on Opti low, Journal of Biologial Cybernetis, 56, pp , ) Egelhaaf, M., Kern, R., Krapp, H. G., Kretberg, J., Kurt, R. and Wareha, A.: eural enoding of behaviourally relevant visual-motion information in the fly, rends in eurosienes, 25, pp , ) Hyslop, A. M., and Humbert, J. S.,: Autonomous avigation in hree-dimensional Urban Environments Using Wide-ield Integration of Opti low, Journal of Guidane, Control, and Dynamis, Vol. 33, o., pp , ) Hyslop A. M., Krapp H. G. and Humbert J. S.: Control theoreti interpretation of diretional motion preferenes in opti flow proessing interneurons, Journal of Biologial Cybernetis, Vol. 03, pp , ) Shoemaker M. A. and Hokamoto S.: Appliation of Wide-ield Integration of Opti low to Proximity Operations and Landing for Spae Exploration Missions, Advanes in the Astronautial Sienes, Vol. 42, pp.23-36, 20. 4) Io, D., Weiss,. and Seidl,.: Constant-Opti-low Lunar Landing: Optimality and Guidane, Journal of Guidane, Control, and Dynamis, 34, o. 5, pp , 20. 5) Shoemaker, M. A. and Hokamoto, S.: Comparison of Integrated and on-integrated Wide-ield Opti low for Vehile avigation, Journal of Guidane, Control, and Dynamis, Vol. 36, o.3, pp ) Sakamoto H., and Hokamoto S.: Attitude Angle Estimation of Spae Probe through Wide-ield Integration of Opti low, ransations of the Japan Soiety for Aeronautial and Spae Sienes, Aerospae ehnology Japan, Vol. 2, o. ists29 pp.pd_4-pd_46, 204 7) Kobayashi., akata R., Bando M., and Hokamoto S.: A ovel Attitude Estimation Method Based on Wide-ield-Integration of Opti low for Autonomous avigation, Proeedings of the 206 Asia-Paifi International Symposium on Aerospae ehnology (APISA), ) Kobayashi., Oishi M., Kinjo Y., and Hokamoto S.: Experimental verifiation of wide-field-integration of opti flow for state estimation, ransations of the Japan Soiety for Aeronautial and Spae Sienes, Aerospae ehnology Japan, Vol.4, o. ists30, Pd_63-Pd_68, ) Kobayashi., Bando M., Hokamoto S.: Improvement of Wide-ield-Integration of Opti low Considering Pratial Sensor Restritions, Journal of Mehanis Engineering and Automation, (aepted) 6