Based on affine transformation image kinematics and recurrence relations, the sub-region KLT feature extract with was designed and rules were given. Optimization of sum of absolute difference was used to match feature points. Diamond search template was adapted to make matching fast, and adaptive varying template size scheme was proposed to solve the problem the minimum SAD is not exclusive, and detailed procedure were described. In order to ensure that extracted feature points are on background, not on moving robot, adaptive iterative scheme was proposed and algorithm was designed. Through solving over-determined image kinematics equations using the least squares algorithm, got the motion parameters. According to the self-developed jitter compensation model, unintended motion was reduced by Kalman filter. Then use filtered parameters to reconstruct image. Based on two autonomous robots, experiment was completed. Results show method proposed meet requirements of accuracy and real time.