IROS2019国际学术会议论文集0869

Multi-Vehicle Cooperative Local Mapping Using Split Covariance Intersection Filter Hao Li and Ming Yang AbstractLocal mapping plays an important role in outdoor intelligent vehicle applications and multi-vehicle cooperative lo- cal mapping which takes advantage of vehicular communication can bring considerable benefi ts to this important task. In this paper, a multi-vehicle cooperative local mapping architecture using split covariance intersection fi lter (Split CIF) is proposed. In the proposed method, a vehicle can fl exibly perform co- operative local mapping with other vehicles in decentralized way, without complicated monitoring and controlling of data fl ow among vehicles; fused maps can be shared freely among vehicles. An effi cient and accurate implementation of the Split CIF is also introduced. A simulation-based comparative study demonstrates the potential and advantage of the proposed multi-vehicle cooperative local mapping architecture using Split CIF. I. INTRODUCTION Mapping, which is usually juxtaposed with localization as simultaneous localization and mapping (SLAM), has long since been an important research topic in mobile robotics 1 2. Single-robot SLAM in indoor environments has been intensively researched and is considered by many researchers as a well solved problem; one may refer to a representative textbook 3 for a comprehensive knowledge of typical techniques on single-robot SLAM. Besides single-robot SLAM, researches on multi-robot SLAM in indoor environments can also be dated back to many years ago; typical examples include Kalman fi lter based multi-robot SLAM 4 5 and particle fi lter based multi-robot SLAM 6 7. In recent years, researches on intelligent vehicles have largely extended peoples knowledge of mobile robotics to outdoor traffi c environments. Traditional indoor SLAM techniques have been adapted for outdoor applications 8 9 10. Although outdoor mobile robotics is closely related to traditional indoor mobile robotics, there exists some noticeable difference between them. For outdoor intelligent vehicle applications, local mapping tends to be more im- portant than global mapping. Besides, local mapping has a loose relationship with localization, mainly for two reasons: fi rst, the availability of qualifi ed on-vehicle motion sensors and GPS (Global Positioning System) make outdoor local SLAM simply reduce to a mapping process. Second, human- driven vehicles still make up an absolutely major part of all vehicles nowadays; for human-driven vehicles, accurate vehicle localization which is indispensable for full-automated This research work is supported by the SJTU (Shanghai Jiao Tong Univ.) Young Talent Funding (WF220426002). H. Li, Assoc. Prof. and M. Yang, Prof. are with the Department of Au- tomation, Shanghai Jiao Tong University (SJTU), Shanghai, 200240, China. Email:haoli; mingyang intelligent vehicles is not so needed, whereas local mapping of environment objects would still be valuable for advanced driving assistance purpose. Therefore, in this paper, we focus on vehicle local map- ping in outdoor traffi c environments. Just like a group of cooperative robots outperform a single robot by fi nishing exploration and mapping tasks faster and more accurately in traditional indoor applications, multi-vehicle cooperative local mapping of environment objects can overcome in- herent mapping limitations of a single vehicle in outdoor environments, as demonstrated in some existing cooperative solutions 11 12 13 14 15. Besides, state-of-the-art vehicular communication technologies 16 17 can fairly support realization of cooperative local mapping. Unlike indoor multi-robot SLAM applications where a group of robots usually have a common goal of exploring and mapping certain environment, vehicles in outdoor traffi c scenarios do not have such kind of common goal even when they cooperate with each other. Outdoor multi-vehicle co- operative local mapping is intended for benefi ting individual vehicles from their own perspective, and hence is of inherent decentralized nature. Besides, realizing multi-vehicle coop- erative local mapping in a decentralized way also has the merit of being fl exible in handling highly dynamic vehicle relationships in outdoor traffi c environments. For a decentralized cooperation architecture, an essential issue is how to handle and fuse estimates with potential correlation. Cyclic update or circular reasoning 18 can occur due to careless handling of inter-estimate correlation and can further lead to the over-convergence problem i.e. a harmful situation where estimates converge quickly to erroneous values or even severely diverged values with extremely large confi dence given to these misled values. Monitoring and controlling the data fl ow among coopera- tive vehicles tend to be a natural idea to handle inter-estimate correlation and are followed by many existing research works. For examples, heuristic rules may be designed for data monitoring and controlling, such as the dependency tree method which allows data to fl ow only from ancestor distri- butions to descendant distributions and keeps the relationship among distributions updated dynamically 18 19. Although these heuristic methods can be implemented conveniently, they still suffer from the risk of circular reasoning due to their incomplete monitoring and controlling of the data fl ow. As presented in 20 21, more sophisticated data transfer schemes enable decentralized mobile robots to obtain centralized-equivalent estimates with delays, yet this sort of methods cannot guarantee the availability of fused estimates 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE2153 in time; besides, their communication requirement as well as computational requirement is demanding due to large pedigrees of data to be relayed and processed. Another kind of methods is to let each vehicle only share its independent information with other vehicles but forbid it to distribute any potentially correlated data 11 22. Such methods can eliminate the risk of circular reasoning; however, their drawback is that they deprive a vehicle of the chance to benefi t from or benefi t vehicles beyond its direct cooperation range. Many existing research works follow above idea of mon- itoring and controlling data fl ow among vehicles, because the data fusion methods (such as the Kalman fi lter) that they adopt cannot guarantee yielding consistent estimates when fusing data with correlation (especially unknown correla- tion); as a consequence, they do need certain mechanism of data fl ow monitoring and controlling to avoid the occasion of fusing correlated data. A new idea of realizing decentralized cooperative localiza- tion of multiple vehicles without monitoring and controlling of their data fl ow is reported in 23. The idea is to take ad- vantage of the split covariance intersection fi lter (Split CIF) 24 25, a special data fusion method which guarantees the fusion consistency even when fusing data of unknown correlation and can maintain known independent information in estimates as well. As a contribution of this paper, we extend the methodology presented in 23 to realization of cooperative local mapping using Split CIF. Compared with cooperative localization, cooperative local mapping involves estimation of much more environment entity states and the implementation effi ciency of the Split CIF is not a negligible issue. As another contribution of this paper, we point out a theoretically proved fact i.e. the convexity of the w-optimization problem involved in the Split CIF, whereupon an effi cient and accurate implementation of the Split CIF can be fairly designed to enable the proposed cooperative local mapping method to be computationally feasible. II. SPLITCOVARIANCEINTERSECTIONFILTER Readers can refer to 25 for details of the split covariance intersection fi lter (Split CIF) which can be formalized as (1). P1= P1d/w + P1i P2= P2d/(1 w) + P2i P = (P1 1 + P1 2 )1 X = P(P1 1 X1+ P1 2 X2)(1) Pi= P(P1 1 P1iP1 1 + P1 2 P2iP1 2 )P Pd= P Pi where X1,P1d+ P1i and X2,P2d+ P2i denote two source estimates in split form, and X,Pd+ Pi denotes the fused estimate. For a generic estimate in split form X,Pd+ Pi, the covariance component Pdrepresents the maximum degree to which the estimate is potentially correlated with others, and the covariance component Pi represents the degree of its independence. In (1), w 0,1 and w is determined by minimizing the determinant of the new covariance, namely by solving the following w-optimization problem (2): w = arg min w0,1 det(P(w)(2) The implementation effi ciency and accuracy of the Split CIF relies on the w-optimization whose objective function is complicated. We may venture a variety of optimization techniques heuristically for (2); however, without any knowl- edge of properties of (2), the only way to guarantee certain solution accuracy is an exhaustive search at certain resolution interval: we set the resolution interval to 0.001, for example, and compute det(P(w) of all w 0,1 at such resolution interval to determine the optimal (or semi-optimal) w. This exhaustive search is adopted in previous works 23. We have recently found a theoretical proof for the convexity of the w-optimization problem (2); proof details are omitted here due to limited paper space. This desirable property of the w-optimization problem can help us design a search algorithm much more effi cient than above exhaustive search method. The search algorithm is designed according to the golden section and Newton search spirit 26, as given in the following pseudo code: GOLDEN SECTION-NEWTON ALGORITHM wl= 0, fl= P(0) = det(P2d+ P2i); wr= 1, fr= P(1) = det(P1d+ P1i); wsl= 0.382, fsl= detP(wsl); wsr= 0.618, fsr= detP(wsr); IF (fl fsl) RETURN 0 IF (fr fsr) RETURN 1 WHILE (wr wl T1) / Golden section search IF (fsl fsr) wr= wsr;fr= fsr; wsr= wsl;fsr= fsl; wsl= wl+ 0.618 (wsl wl); fsl= detP(wsl); ELSE wl= wsl;fl= fsl; wsl= wsr;fsl= fsr; wsr= wr 0.618 (wr wsr); fsr= detP(wsr); ENDIF ENDWHILE w = (wl+ wr)/2 DO/ Newton search fl= detP(w w); fw= detP(w); fr= detP(w + w); d1= (fr fl)/(2 w); d2= (fr+ fl 2 fw)/w2; w = w d1/d2; WHILE (|d1/d2| T2) RETURN w Above golden section-Newton algorithm consists of two consecutive processes, namely golden section search and 2154 Newton search. The golden section search is fi rst used to shrink the solution range to a small enough interval according to a tolerance T1, then the Newton search is used to converge the solution quickly to an accurate value with an error tolerance tuned by T2. The convexity of the w-optimization problem (2) guarantees that above algorithm converges to a global optimal solution. III. COOPERATIVELOCALMAPPING A. Basic Functions for Outdoor Vehicles Given multiple vehicles, suppose each vehicle only interactswith(perceivingandcommunicating)its neighbouring vehicles and perform cooperative mapping in a decentralized manner. Abstracted from feasible fi eld practice in reality, the following functions are assumed available for them. Mapping of environment objects: Each vehicle can map surrounding objects within a local range and output a lo- cal position measurement for each object. Stereo-vision sensors and range sensors can realize this function. Relative positioning: Each vehicle can reliably estimate the relative poses of its immediate neighbouring ve- hicles. In practice, this function can be realized via perceptive sensors such as range sensors. Motion monitoring and object association: Each vehicle possesses of motion data (from odometers, accelerome- ters, gyroscopes etc.) that can be used to predict relative movements of environment objects. Each vehicle is also able to correctly associate environment objects which are modelled as a stationary objects with their potential movements treated as random process noise. Communication: Vehicular communication is available for sharing data among neighbouring vehicles. Time-stamping: Each vehicle can timestamp its data according to an absolute time reference. In practice, this function can be provided by the GPS device which is a standard on-vehicle device nowadays. We have to admit that above assumptions are somewhat ideal. Data association is a non-trivial issue and sometimes it is diffi cult to have deterministic data association results. Be- sides, the point and stationary object model is not generally applicable, though it can be applicable to pedestrians, trees, poles etc. On the other hand, presenting a complete solution with all implementation considerations is out of the focus of this paper. We focus rather on presenting a cooperative local mapping architecture using Split CIF and demonstrating its potential and advantage in decentralized data fusion. B. Evolution of Local Map The decentralized formalism for each vehicle in the pro- posed cooperative local mapping architecture is the same and is described from the perspective of one single vehicle i.e. an ego-vehicle. The kinematic bicycle model (denoted compactly as func- tion G) 23 is used to describe vehicle motion. For the ego-vehicle, denote its mapped object states as XO= XO1,XO2,.,XOm whose evolution is formalized as XO,t= F(ut,XO,t1) , inv(G(0,ut) XO,t1 More specifi cally, for each mapped object state XOj,t= F(ut,XOj,t1) , inv(G(0,ut) XOj,t1(3) PiOj,t= FXOjPiOj,t1FT XOj+ FuuF T u+ Pi PdOj,t= FXOjPdOj,t1FT XOj Where and inv() are compounding notations (see Ap- pendix); utdenotes the motion data of current control period t and is assumed to follow the Gaussian distribution N(ut,u); FXOjand Fudenote respectively the Jacobian matrices of the function F with respect to XOj,t1and ut; Pidenotes the evolution model error. Both the independent and correlated covariance parts PiOjand PdOjare evolved. C. Local Map Update with Ego-Vehicle Measurements When the ego-vehicle has new perception measurements of environment objects, it uses them to update the exist- ing map. Given a new object measurement ZOfollowing the Gaussian distribution N(ZO,O), if it is not associ- ated with any existing object state from the map XO= XO1,XO2,.,XOm, then simply add it to XOas a newly initiated object state. Otherwise, suppose ZOis associated with XOj, then it is used to update XOjas follows: P1= PdOj,t+ PiOj,t P2= O K = P1(P1+ P2)1 XOj,t= XOj,t+ K(ZO XOj,t)(4) POj,t= (I K)P1 PiOj,t= (I K)PiOj,t(I K)T+ KOKT PdOj,t= POj,t PiOj,t Above formalism (4) is derived by setting w = 1 in the Split CIF formalism (1) note that ZOis completely independent with zero correlated covariance part. D. Local Map Update with Local Maps from Other Vehicles When the ego-vehicle receives a local map formed by its neighbouring vehicle, it uses this extra local map to update and augment its own map. Suppose the pose of the neigh- bouring vehicle relative to the ego-vehicle is estimated as XRN. Given an object state estimate ON= XON,PdON+ PiON in the shared local map, transform ONinto the local coordinates system of the ego-vehicle XOE= R(XRN,XON) , XRN XON PiOE= RXOPiONRT XO+ RXRRR T XR PdOE= RXOPdONRT XO where Rdenotes the error covariance of XRN; RXO and RXRdenote respectively the Jacobian matrices of the function R with respect to XONand XRN. 2155 If the estimate XOEis not associated with any object state from the map XO= XO1,XO2,.,XOm formed by the ego-vehicle, then simply add it to the map as a newly initiated object state. Otherwise, suppose XOEis associated with the estimate XOjof the local map XO , then fi rst take PdOj,t,PiOj,t and PdOE,PiOE as input and use the golden section- Newton algorithm presented in section II to output a solution wopt, and second use XOEto update XOjas in (5) which is an equivalent but numerically better variant of the Split CIF formalism (1). P1= PdOj,t/wopt+ PiOj,t P2= PdOE,t/(1 wopt) + PiOE,t K = P1(P1+ P2)1 XOj,t= XOj,t+ K(XOE XOj,t)(5) POj,t= (I K)P1 PiOj,t= (I K)PiOj,t(I K)T+ KPiOE,tKT PdOj,t= POj,t PiOj,t IV. SIMULATIONS A. Comparative Study Simulation tests are performed to evaluate and compare the proposed cooperative local mapping method using Split CIF and two baseline methods. We have to admit that the gap between simulation and fi eld practice always exists, yet simulation which eliminates the infl uences of ad hoc implementation factors can fairly demonstrate the reasonableness and potential of a method and is especially suitable for a comparative study. The methods under tests are as follows: Single Vehicle Mapping 3 (SM): Each ego-vehicle establishes a dynamic local map of environment object states only using its own sensor data Compared with a global map established in traditional indoor SLAM way, a dynamic local map is more valuable to outdoor intelligent vehicles More specifi cally, the ego vehicle evolves its object state estimates via (3) and update them with new perceptive measurements via the Kalman fi lter (essentially equivalent to (4) but without maintaining the split estimate form). Independent State Exchange Based Cooperative Map- ping 11 22 (ISECM): Each ego-vehicle maintains independently a local map of environment objects as in single vehicle mapping and can share this map to other vehicles. Each ego-vehicle can generate an augmented map by fusing its own local map and those shared by other vehicles; however, the ego-vehicle uses the augmented map only as the fi nal output at current period, but neither uses it to evolve object states nor shares it with other vehicles. Cooperative Mapping Using Split CIF (SCIFCM): Details are described in section III. As illustrated in Fig.1, a main simulation scenario for comparative study is abstracted from real traffi c scenarios. A chain of vehicles (e.g. four vehicles, in