A RADAR-BASED SYSTEM AND METHOD FOR REAL-TIME SIMULTANEOUS LOCALIZATION AND MAPPING

摘要

A method for performing Simultaneous Localization And Mapping (SLAM) of the surroundings of an autonomously controlled moving platform (such as UAV or a vehicle), using radar signals, comprising the following steps: receiving samples of the received IF radar signals, from the DSP; receiving previous map from memory; receiving data regarding motions parameters of the moving platform from an Inertial Navigation System (INS) module, containing MEMS sensors data; grouping points to bodies using a clustering process; merging bodies that are marked by the clustering process as separate bodies, using prior knowledge; for each body, creating a local grid map around the body with a mass function per entry of the grid map; matching between bodies from previous map and the new bodies; calculating the assumed new location of the moving platform for each on the mL particles using previous frame results and new INS data; for each calculated new location of the mL particles with normal distribution, sampling N assumed locations; for each body and each body particle from the previous map and for each location particle, calculating the velocity vector and orientation of the body between previous and current map, with respect to the environment using the body velocity calculated in previous step and sampling N2 particles of the body, or using image registration to create one particle where each particle contains the location and orientation of the body in the new map; for each particle from the collection of particles of the body: propagating the body grid according the chosen particle; calculating the Conflict of the new observed body and propagated body grid, using a fusion function on the parts of the grid that are matched; calculating Extra Conflict as a sum of the mass of occupied new and old grid cells that do not have a match between the grids; calculating particle weight as an inverse weight of the combination between Conflict and Extra conflict; for each N1 location particle, calculating the weight as the sum of the best weight per body for that particle; resampling mL particles for locations according to the location weight; for each body, choosing mB particles from the particles with location in one of the mL particles for the chosen location, and according to the particles weights; for each body and each one of the mB particles calculate the body velocity using motion model; and creating map for next step, with all the chosen particles and the mass function of the grid around each body, for each body particle according to the fusion function.