IROS2019国际学术会议论文集 0869

Multi Vehicle Cooperative Local Mapping Using Split Covariance Intersection Filter Hao Li and Ming Yang Abstract Local 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 people s 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 P 1 1 P 1 2 1 X P P 1 1 X1 P 1 2 X2 1 Pi P P 1 1 P1iP 1 1 P 1 2 P2iP 1 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 w 0 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 t 1 inv G 0 ut XO t 1 More specifi cally for each mapped object state XOj t F ut XOj t 1 inv G 0 ut XOj t 1 3 PiOj t FXOjPiOj t 1FT XOj Fu uF T u Pi PdOj t FXOjPdOj t 1FT 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 t 1and 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 K OKT 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 RXR RR 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 indexed 1 to 4 move on the same road from left to right There are a group of environment objects e g 42 objects indexed 1 to 42 on both sides of the road for example we may imagine them as pedestrians trees poles It is worthy noting that the vehicles map environment objects without using any a priori scenario information The simulation scenario is designed like this only for demonstration convenience and has no essential infl uence on the performance of above methods Fig 1 Simulation scenario for cooperative local mapping We set simulation conditions according to the assumed functions specifi ed in section III Each vehicle can only cooperate with its immediate neighbouring vehicles before and behind In