Supported by the Major Program of National Natural Science Foundation of China(61690210,61690212);the National Natural Science Foundation of China(61333003,61503100);the Self-Planned Task(SKLRS201716A)of State Key Laboratory of Robotics and System(HIT)