Computing C-pace Entropy for View Planning for a Generic Range Senor Model Pengpeng Wang (pwangf@c.fu.ca) Kamal Gupta (kamal@c.fu.ca) Robotic Lab School of Engineering Science Simon Fraer Univerity Burnaby, BC, Canada, V5A 1S6 Abtract We have recently introduced the concept of C-pace entropy a a meaure of knowledge of C-pace for enor-baed path planning and exploration for general robot-enor ytem [1, 2, 3, 5]. The robot plan the next ening action to maximally reduce the expected C-pace entropy, alo called the maximal expected entropy reduction, or MER criterion. The expected C-pace entropy computation, however, made two idealized aumption. The firt wa that the enor field of view (FOV) i a point; and the econd wa that no occluion (or viibility) contraint are taken into account, i.e., a if the obtacle are tranparent. We extend the expected C-pace entropy formulation where thee two aumption are relaxed, and conider a generic range enor with non-zero volume FOV and occluion contraint, thereby modelling a real range enor. Planar imulation how that (i) MER criterion reult in ignificantly more efficient exploration than the naive phyical pace baed criterion (uch a maximize the unknown phyical pace volume), (ii) the new formulation with non-zero volume FOV reult in further improvement over the point FOV baed MER formulation. Real experimental reult with the SFU eye-in-hand ytem, a PUMA 560 equipped with a writ mounted range canner [4] will be reported in the final verion of the paper. 1 Introduction While mot reearch in enor-baed path planning and exploration ha concerned itelf with mobile robot, our recent work ha concentrated on general robot-enor ytem, where the enor i mounted on a robot with non-trivial geometry and kinematic [1, 2, 3, 4, 5]. See alo [6, 7, 8, 9, 10]. Thi cla of robot i broad and include robot ranging from a imple polygonal robot to articulated arm, mobile-manipulator ytem, and humanoid robot [10]. The enor i aumed to be an eye type enor that i capable of providing ditance from a given vantage point (actual implementation may be a laer range canner, paive tereo viion, etc.). Figure 1 how a imple example of uch a robot-enor ytem an eye-in-hand ytem an articulated arm with a writ mounted range enor. The robot mut imultaneouly plan path and ene it environment for obtacle. Unlike for a imple mobile robot (often modelled a a point, ee [11] for next bet view planning for mobile robot), where the robot can move (path planning) and what it hould ene (view planning), ha a much more complex relationhip here [4]. Where to move i bet poed and anwered in configuration pace, the natural pace for path planning. In [1, 2, 5], we howed that the view planning problem i appropriately poed in the configuration pace of the robot the next view hould be planned to give maximum knowledge or information (whether a configuration i free or in colliion with an obtacle) of the C-pace of the robot. What thi implie i that the ening action, which obviouly ene phyical pace, mut be (implicitly or explicitly) tranformed to the configuration pace. Treating the unknown environment tochatically, we introduced the notion of C-pace entropy a a meaure of the robot knowledge of C-pace. The next bet view i then the one that maximize the expected entropy reduction (MER criterion) or, equivalently expected information gain. In contrat, earlier approache had imply ued a naive criterion, uch a maximize unknown phyical pace volume (MPV) in the enor FOV, to chooe the next bet view [6]. In [1, 3] we derived cloed form expreion for expected C-pace entropy reduction, or information gain under a Poion point proce model of the environment [13]. However, two idealized aumption were made regarding the enor in that paper: (i) the enor ha a point field of view (FOV), i.e., it ene a ingle point and (ii) no occluion contraint were taken into account, i.e., a if the enor would ee (get range meaurement) through the obtacle. The
Figure 1: An eye-in-hand ytem a two-link robot with a writ mounted range enor (with triangle FOV) moving in an unknown environment. A key quetion (the view planning problem) i: where hould the robot ene next? next bet view i planned uing thi formulation, i.e., the algorithm compute the point (ay, x max ) which, if ened, would yield maximum expected information gain and place the enor o that the center of the actual FOV (a cone) coincide with x max.in[5], thi formulation wa extended for a beam enor the enor FOV i a ray (beam) of finite length. In other word, aumption (ii) above wa relaxed, but aumption (i), that enor FOV ha finite volume wa till there. zero volume FOV while repecting occluion contraint. In thi paper, we preent the MER computation for a generic range enor that ha a non-zero volume FOV and it ening action i ubject to occluion contraint, thereby modelling real range enor. Thi computation i valid for a Poion point proce model of the environment, admittedly a implification, but the reulting cloed form expreion give u inight and are ueful at leat a approximation. We preent imulation that how clear improvement in the efficiency of exploration with the new formulation. Our initial imulation are planar for eae of viualization. We emphaize that our formulation and reult are valid for 3D environment and are currently being implemented on a real ix-dof SFU eye-in-hand ytem coniting of a PUMA 560 with a writ mounted area-can laer range enor that ha been developed in our lab and wa reported in [4]. We expect to report thee experimental reult in the final verion of the paper. 2 Notation Let A denote the robot and q denote a point in it configuration pace, C. A(q) then denote the region in phyical pace, P, occupied by the robot. Let S denote a enor attached to the robot. We attach a coordinate frame to the enor origin. Let denote the vector of parameter that completely determine the enor frame, i.e., enor configuration. For intance, auming the enor i attached to the endeffector of the robot, for planar cae, =(x, y, θ); for3dcae, =(x, y, z, α, β, γ). The enor configuration pace, denoted by C, i the pace of all poible enor configuration. Let V() P denote the region ened (enor FOV) by the enor at configuration. Subcript free, ob, andunk (or ometime u) denote the known free, known obtacle and unknown region, repectively in phyical and configuration pace. So, for example P ob denote the known obtacle in phyical pace, A unk (q) denote the part of robot lying in unknown phyical pace at configuration q, andc free denote the known free configuration pace. 3 Background: C-pace Entropy and IGF We aume that the obtacle ditribution in the phyical environment i modelled with an underlying tochatic proce (e.g., the Poion model ued later). The kinematic and geometry of the robot, embodied by function A(q) map the probability ditribution in phyical pace to a probability ditribution over the C-pace. Shannon Entropy then provide a meaure of the robot ignorance of the tatu of C-pace. For a generic range enor, which ene a region of non-zero volume, one can compute the expected entropy reduction if a ening action,, i taken, i.e., V() i ened. The information gain (IG) function capture thi notion and i defined a q C IG C () = E{ H(C)} where H(C) denote the current C-pace entropy, E{ H(C)} = E{H(C V()} H(C) denote the expected entropy change after V(), the region to ene at the enor configuration,, i ened. In order to get efficiency in computing, we neglect the mutual entropy term, eentially treating each robot configuration a an independent random variable, i.e., H(C) = H(Q). In thi equation, Q denote the binary random variable (r.v.) correponding to configuration q being free (=0) or in colliion (=1); H(Q) denote the entropy of r.v. Q, i.e., H(Q) =p(q)log(p(q)) + (1 p(q)) log(1 p(q)) (1) where p(q) =Pr[q = free] i the marginal probability that configuration q i colliion-free, alo called the void probability of q. With thi implification one can how that: ĨG C () = E{ H(C)} = ig q() q C where ig q () i given by: ig q() = E{ H(Q)} (2) When V() i ened, the ened information affect the C-pace entropy via each robot configuration q. ig q () i then the partial contribution to information gain via configuration q, if a region V() were to be ened. Furthermore, ig q (x) equal0when
A(q) doe not interect V(). So we need only compute the above ummation over thoe q uch that V() A unk (q) 0. Set of configuration q uch that V() A unk (q) 0i defined a the C-zone of, denoted by χ(). Therefore one can write: IG C () = ig q() q χ() 4 Generic Senor Model We now conider a range enor whoe FOV ha nonzero volume, i.e., V() ianopenetinr 3 and the the actual volume ened i governed by occluion contraint. Mot commercially available range enor that provide range image (uch a the area can laer range enor ued in SFU eye-in-hand ytem [4]) fall into thi category. Fig. 2 how a chematic diagram a the enor ene an unknown region within it FOV. A before, let V u () denote the portion of the FOV that interect P u and i not occluded by known obtacle. Note that V u () might be a multiply-connected et. In the figure, V u ()conit of region A, B, C and D (region E i excluded from V u () ince it i occluded by a known obtacle. After ening, region A, B and C become free; region D remain unknown becaue it i occluded by the ened obtacle (hown in dark). Of coure, the enor alo provide the ditance from the enor origin to the ened obtacle. Figure 2: Illutration of a generic range enor FOV V(). After thi ening action, region A, B and C become free, the black contour i a ened obtacle and region D, occluded by the ened obtacle remain unknown. Region E alo remain unknown, but it i occluded by an already known obtacle. 4.1 Environmental Model Again, we are uing Poion point proce to make a imple model of the robot workpace. Poion point proce i eentially characterized by uniformly ditributed point in pace [13]. In robot motion planning context, thee point, denoted by pt, are obtacle. Given the denity parameter of thi model, λ, the void probability of an arbitrary et B P the probability that there i no point (obtacle) in B denoted by p(b), i given by p(b) =Pr[no pt B]=e λ vol(b) (3) Thi implie that p(q), the void probability of a robot configuration q i given by p(q) =Pr[no pt A(q)] = e λ vol(a unk(q)) (4) Becaue the ening i ubject to occluion contraint, the enor can only detect the very firt point obtacle along each ening ray. Thi very firt otacle i called the hit point and i denoted by hitpt,. The ening action can therefore be eaily viualized a finding a bunch of new hit-point poition in the workpace. 4.2 ig q () Computation For a given q, ig q () i compoed of um of two component, i.e., ig q =(ig q ) 1 +(ig q ) 2. The firt component, (ig q ) 1, correpond to thoe outcome where the enor would ene at leat one hit point inide A(q) V u (), i.e., hitpt A(q) V u (). Let thi et of outcome be denoted event 1. After ening, the robot, were it to be placed at configuration q, A(q), would be in colliion with an obtacle (the ened hit point). So H(Q event 1) = 0 and event 1 H(Q) =H(Q event 1) H(Q) = H(Q). It turn out (not unexpectedly in the light of our earlier beam enor reult reported in [5], however everal technical detail need to be carefully worked out) that the probability of event 1, Pr[ hitpt A(q) V u ()], i the ame a Pr[ pt A(q) V u ()], a if occluion doe not matter! Theorem 2 tate thi reult formally. But firt we how an intermediate reult, that the probability of all the point obtacle inide B to be occluded by point obtacle in front of them i zero! Theorem 1. Pr[all pt Bare occluded] =0where B V u() i any open et and pt are point obtacle whoe ditribution i governed by a Poion point proce. Proof. We firt dicretize V u () intomnumberof nearly-identical cone, V 1, V 2,..., V M a hown in Fig. 3. Conider a cone (with apex at enor origin) which contain V(). Lay a dicrete grid of ize ɛ and connect the boundary of thee cell to the enor origin. V u () functionality can then be captured by thee cone. A M approache infinity, the dicrete model approache the continuou model. Note that B i alo dicretized by thee V i. We label thoe dicretized cone that have common part with B by VB 1, VB 2,..., VB M. We denote the interection of thee VB i with every component of B by B 1, B 2,..., B M repectively and ue front(b) i to denote the ubet of VB i \B,( \ here denote the et minu operation), that i in front of B i along the ening direction. 1 We know that the et of outcome denoted by Event(all pt B are occluded) i a ubet of outcome denoted by Event( occluded pt B), i.e., at leat one point, pt, inide B i occluded. Hence, we have Pr[all pt B are occluded] 1 Note that B could be a multiple-connected et. So the number of B i, M could be greater than the number of dicretized cone, M. And ince the labelling equence i irrelevant to our proof, we can arbitrarily label thee B i
M (1 e λ vol(vbmax) ) vol(b i ) λ =(1 e λ vol(vbmax) ) vol(b V u()) λ Figure 3: Illutration of a poible way of dicretizing V u (). Pr[ occluded pt B]. Furthermore, ince Event(all pt Bare occluded) implie that at leat one point i inide the region B i, and at leat one point i inide front(b) i.sowehave Pr[ occluded pt B] = M (Pr[ pt B i ] Pr[ pt front(b) i ]) M M j i B i B j / the ame cone (Pr[ pt B i ] Pr[ pt front(b) i ] Pr[ pt B j ] Pr[ pt front(b) j ]) +... M Pr[ pt B i ] Pr[ pt front(b) i ] The reaon for inequality ign above i a follow. We regard Event( occluded pt B) a the ummation of the event that occluion happen in any of the B i (the expreion on the right ide of inequality ign) minu the double counting that occur. Thi double counting i eentially the probability of the joint event that occluion happen concurrently in morethanoneb i and hence it i alway greater than or equal to zero, and hence the inequality ign. 2 In the above ummation, for the term Pr[ pt front(b) i ], uing Eq. (3), we have Pr[ pt front(b) i ]=1 e λ vol(front(b)i). Since the volume of front(b) i i le than or equal to the volume of the bigget dicretized cone, denoted by VB max,we will have Pr[ pt front(b) i ] 1 e λ vol(vbmax). Now ubtituting the term Pr[ pt front(b) i ]by 1 e λ vol(vbmax) and taking it out of the ummation, we have, M Pr[ occluded pt B] (1 e λ vol(vbmax) ) Pr[ pt B i ] For the term Pr[ pt B i ], uing Eq. (3) with only firt order expanion, we have, Pr[ pt B i ]=1 e λ vol(b i) = λ vol(b i ) Thi approximation i reaonable ince the volume, vol(b i ), i mall enough when the dicretization reolution i mall enough. Therefore, we have, Pr[all pt Bare occluded] Pr[ occluded pt B] 2 Thiinothing but a generalization of p(a) +p(b) p(a, B), where A and B are two random variable, and p() denotethe probability. A M approache infinity, vol(vb max ) approache zero and therefore (1 e λ vol(vbmax) ) approache zero. So Pr[ occluded pt B] = 0, which implie Pr[all pt Bare occluded] =0. Theorem 2. Pr[ hitpt B]=Pr[ pt B]= 1 e λ vol(b) Proof. The computation of Pr[ hitpt B]ibaed on the complement of thi event, Event(no hitpt B), i.e., Pr[ hitpt B]=1 Pr[no hitpt B]. Event(no hitpt B) can be divided into two event, the event that there are no point obtacle in B and the event that all the point obtacle inide B are occluded, Event(all pt Bare occluded). Uing Eq.(3), the probability of Event(no hitpt B) will be, Pr[no hitpt B]=p(B)+Pr[all pt Bareoccluded] Uing Theorem 1, the econd term i zero, and hence we have, Pr[no hitpt B] = p(b). It then follow that the probability of the complement event, i.e., Pr[ hitpt B]=1 p(b) =1 e λ vol(b). So we have (ig q) 1 = Pr[hitpt A(q) V u()] H(Q) = Pr[pt A(q) V u()] H(Q) = (1 e λ vol(a(q) Vu()) ) H(Q) (5) The econd component, denoted by (ig q ) 2, correpond to a et of outcome in which there doe not exit any hit-point inide A(q) V u (). In thi cae, the tatu of A(q) would either remain unknown, albeit the unknown portion (volume) may have decreaed, or it may become completely free; but it will not be known to be in colliion. Let u denote thi et of outcome by event 2. Uing the dicretized FOV a in Figure 3, let u denote the tate of A(q) V u () after ening by J. Event 2 then correpond to the et of outcome {J : no hitpt A(q) V u ()}. By definition, then we have (ig q) 2 = Pr[J] (H(Q) H(Q J)) (6) where Pr[J] i the probability of A(q) V u () being in tate J after ening. We how that the above expectation turn out to be that of the event (let u call it event 3) that there doe not exit any pt in A(q) V u (), or equivalently that the region A(q) V u () i free! Thi implie that occluion doe not matter in the expectation computation! It i not entirely unexpected in the light of beam enor reult, [5], however everal technical detail need to be worked out. Theorem 3 tate thi reult formally. Theorem 3. Pr[J] H(Q J) =Pr[event 3] H(Q event 3) = e λ vol(a(q) Vu()) H(Q event 3) (7)
Proof. Omitted for lack of pace. Thu, expanding the ummation in Eq. 6, we have, (ig q) 2 = H(Q) Pr[J] Pr[J] H(Q J) (8) The firt term (the firt ummation) above i 1 Pr[hitpt A(q) V u ()]. The expreion for it i given by Theorem 2 if we ubtitute A(q) V u () for B. Furthermore, ubtituting from Theorem 3 for the econd term, we get (ig q) 2 = H(Q) Pr[event 3] Pr[event 3] H(Q event 3) = e λ vol(a(q) Vu()) (H(Q) H(Q event 3)) (9) So umming the two component together, and uing Eq. (5) and (9), we have ig q =(ig q) 1 +(ig q) 2 = H(Q) e λ vol(a(q) Vu()) H(Q event 3) (10) Both H(Q event 3) andh(q) in the above equation are determined uing Eq. (1) and that p(q event 3) = e λ vol(au(q)\vu()). Note that thi reult reduce to the point FOV enor model [1], and to the beam FOV enor model [5], in the limit. The reduction can be hown in a traightforward manner and i omitted for brevity. 5 Algorithm for View Planning Now that we have computed an expreion for IG over enor configuration pace, we can ue the MER criterion to decide the next can, i.e., chooe the enor configuration max uch that max = max{ ig q()}. The algorithm then i a follow: q X u() for every /* according to a certain reolution */ determine V u() ĨG() =0 /* initialize */ for every q if (A u(q) overlap with V u()) compute ig q() ĨG() =ĨG()+ig q() max =max (ĨG()) Determining V u () correpond to determining the interection of the enor FOV with P u while excluding portion of P u occluded by already known obtacle (before ening action), a imple geometric computation. Note that iteration over q (C-pace of the robot) may be prohibitive for robot with many degree of freedom. In thi cae, the ummation can be carried out over a large enough et of random ample. 6 Simulation Reult In order to tet the effectivene of our formulae, we conducted a erie of experiment on the imulated two-link eye-in-hand preliminary ytem hown in Figure 1. The tak for the robot i to explore it environment, trting from it initial configuration. The overall planner ued i SBIC-PRM (enor-baed incremental contruction of probabilitic road map) reported in [3, 4]. Briefly, SBIC-PRM conit of an incrementalized model-baed PRM [14], that operate in the currently known environment; and a view planner that decide a reachable configuration within the currently known environment from which to take the next view, choen according to a criteron. The two ub-planner operate in an interleaved manner. We compare the reult of four different view planning criteria for efficiency of exploration of the phyical and configuration pace. The firt trategy, denoted by RV (random view), i to randomly chooe a viewpoint a the next can. The econd, denoted by MPV (maximum unknown phyical volume) i to chooe the next viewpoint o a to maximize the unknown phyical volume inide the can [6]. The third i to ue point FOV baed MER criterion for viewplanning [1, 2], and place the centre of the actual FOV (the cone) at x max, the ingle point that reult in maximum entropy reduction. The fourth i to ue the generic non-zero volume FOV baed MER criterion derived in thi paper. In all thee cae, the robot tarted off a in Figure 1. A hown in the Figure 4, 5, 6, and 7, the firt two trategie expand the known C-pace much le than the lat two MER criterion baed trategy. Uing RV give u about 8% expanion of known C- pace in 5 can, and the robot reached it goal in 36 can. MPV reult in C-pace expanion by about 54% in 5 can. The point FOV baed MER criterion give much better reult, reulting in about 73% expanion in 5 can. The general FOV baed MER criterion i the bet, better than point FOV baed MER. It made the C-pace expand by about 82 % in 5 can. For reader information, although not relevant here, black dot in thee figure are the node of the probabilitic roadmap ued for planning path. Figure 8 plot C-pace v. number of iteration for the above four view-planning criteria. We can eaily ee that the generic range enor baed MER i the mot efficient one, which expanded C-pace to about 90% in 7 can; point FOV baed MER needed 11 can; MPV needed 19 can; and RV needed 33 can. Figure 4: Known Phyical and C-pace after 5 can: RV criterion
Figure 5: Known Phyical and C-pace after 5 can: MPV criterion Figure 7: Known Phyical and C-pace after 5 can: Generic non-zero volume FOV baed MER criterion. Figure 6: Known Phyical and C-pace after 5 can: Point FOV baed MER criterion 7 Concluion We preented cloed form olution for computing the expected C-pace entropy reduction for a general non-zero volume FOV range enor extending our previou reult that applied to a point FOV enor and take into account the occluion contraint inherent in range enor. Planar imulation how that our new reult lead to more efficient exploration of the robot configuration pace. Our next tep i to implement thee reult for a real ix-dof eye-in-hand ytem, a PUMA 560 with a writ mounted area can laer range finder. The current formulation aume a Poion point proce for obtacle ditribution. It treat obtacle a point. Extending our formulation for a Boolean tochatic model [13] where geometric hape of obtacle i taken into account would be the next tep. Reference [1] Y. Yu and K. Gupta, An Information Theoretic Approach to View Planning with Kinematic and Geometric Contraint, Proc. IEEE ICRA, Seoul, Korea, May 21-26, 2001, pp. 1948-1953. [2] Y. Yu, An Information TheoreticalIncrementalApproach to Senor-baed Motion Planning for Eye-in- Hand Sytem, Ph.D. Thei, Schoolof Engineering Science, Simon Fraer Univerity, 2000. [3] Y. Yu and K. Gupta, View Planning via C-Space Entropy for Efficient Exploration with Eye-in-hand Sytem, Proc. VII Int. Symp. on Experimental Robotic, 2000. Available a lecture note in Control and Information Science, LNCIS271, Springer. pp. 373-384. [4] Y. Yu and K. Gupta, Senor-baed Probabilitic Roadmap: Experiment with an Eye-in-hand Sytem, Advance Robotic, Vol.14, No.6, pp.515-537, 2000. Figure 8: The comparion of C-pace exploration efficiency for the four view-planning algorithm: RV, MPV, Point FOV Baed MER and Generic FOV Baed MER [5] P. Wang and K. Gupta, Computing C-pace Entropy for View Planning Baed on Beam Senor Model, To appear in Proc. Of IROS 2002. [6] E. Krue,R. Gutche and F. Wahl, Effective Iterative Senor Baed 3-D map Building uing Rating Function in Configuration Space, Proc. IEEE ICRA, 1996, pp.1067-1072. [7] P. Renton, M. Greenpan, H. Elmaraghy, and H. Zghal, Plan-n-Scan: A Robotic Sytem for Colliion Free Autonomou Exploration and Workpace Mapping, Jrnl. on Intell. And Robotic Sytem, 24:207-234, 1999. [8] J. Ahuactzin and A. Portilla, A Baic Algorithm and Data Structure for Senor-baed Path Planning in Unknown Environment, Proc. Of IROS 2000. [9] E. Cheung and V. J. Lumelky, Motion Planning for Robot Arm manipulator with Proximity Senor, Proc. IEEE ICRA, Philadelphia, PA, 1988. [10] H. Choet and J. W. Burdick, Senor baed Planning for a Planar Rod Robot, Proc. of the IEEE ICRA, Minneapoli, MN, 1996. [11] H. G. Bano and J. C. Latombe, Robot Navigation for Automatic Contruction Uing Safe Region, Preprint Proc. ISER 2000, pp. 395-404. [12] K. Hirai, M. Hiroe, M. Haikawa and T. Takenaka, The Development of HONDA Humanoid Robot, Proc. Of ICRA 1998, pp. 1321-1326. [13] D. Stoyan and W.S. Kendall, Stochatic Geometry and It Application, J. Wiley, 1995. [14] L. Kavraki, P. Svetka, J. Latombe and M. Overmar, Probabilitic Roadmap for Path Planning in High-dimenionalConfiguration Space, IEEE Tranaction on Robotic and Automation, 12(4): 556-580, Aug. 1996.