Aiming at the high computational complexity of the traditional Rao-Blackwellized Particle Filtering (RBPF) method for simultaneous localization and Mapping (SLAM), an optimization method of RBPF-SLAM system is proposed, which is based on lidar and least square line segment feature extraction as well as raster, reliability mapping continuity. Validation test results show that less storage in constructing a map with this method is occupied, and the computational complexity is significantly reduced. The effect of noise data on feature data extraction results is effectively avoided. It also solves the problem of error accumulation caused by noninteger grid size movement of unmanned vehicle in time update stage based on Markov positioning scheme. The improved RBPF-SLAM method can enable the unmanned vehicle to construct raster map in real time, and the efficiency and accuracy of map construction are significantly improved.