国家高新技术研究发展计划项目2012AA041508;河北省自然科学基金项目E2014202154;机器人技术与系统国家重点实验室开放研究课题SKLRS-2013-ZD-04资助.Supported by National High-Tech R&D Program2012AA041508;Natural Science Foundation of Hebei ProvinceE2014202154;External-Planned Task of State Key Laboratory of Robotics and SystemSKLRS-2013-ZD-04