This paper presents a new feature based scan matching method for solving 6D localization problem in forested environments. The proposed registration process includes two steps. First, the largest group of approximately parallel tree trunk features is utilized to align successive scans along the five dimensions except z direction. Tree correspondences are established by matching point patterns which are abstracted from the position relationships of trees. The optimal 5D transformation is thus determined based on the axes of two key tree pairs which are selected by evaluating their ability of tree alignment. Second, we assign the ground points of two scans into a grid of cells, and minimize z-direction difference of points in shared cells. The experimental results on data collected in real forested environments have demonstrated the effectiveness of this method. © 2012 IEEE.