VFF虚拟力场法

1 #ifndef VFF_HEADER 2 #define VFF_HEADER 3 #include 
     
       4 #include "utils\point.h" 5 #include 
      
        6 #include 
       
         7 #include 
        
          8 // 9 //target全局坐标系下的目标点10 //obstacles围绕激光为中心-180度到180度逆时针激光扫描点11 //theta里程计中的theta角12 //desiredDirection机器人应该运动的方向（全局坐标）13 inline void navigate(const GMapping::Point &target,const std::vector
         
              &obstacles,double theta,14     double maxRobotSpeed,15     double TARGET_ATTRACTIVE_FORCE,double TARGET_SLOW_APPROACHING_DISTANCE,16     double &desiredDirection,17     double &desiredSpeed)18 {19     //MRPT_UNUSED_PARAM(maxRobotSpeed);20     // Forces vector:21     GMapping::Point resultantForce(0,0),instantaneousForce(0,0);22 23     // Obstacles:24     {25         const size_t n = obstacles.size();26         const double inc_ang = 2*M_PI/n;27         double ang = -M_PI + 0.5*inc_ang+theta;//注意此处，从-180度开始逆时针存储数据28         for (size_t i=0;i
         
        
       
      
     

 参考mrpt中的代码，因为其中针对的是全向机器人，所以做了部分修改适用有Heading的机器人。

VFH矢量场直方图

 该方法取机器人周围一定距离范围的窗口，将空间离散为$w_{s}*w_{s}$栅格。

 

扩展阅读

https://github.com/agarie/vector-field-histogram

https://github.com/ecmnet/MAVSlam/tree/c55e63eca4111e01245e0e3389f1e568782096fc/MAVSlam/src/com/comino/slam/vfh/vfh2D

http://www-personal.umich.edu/~johannb/vff&vfh.htm