[路径规划] VFF和VFH

VFF虚拟力场法

 #ifndef VFF_HEADER
 #define VFF_HEADER
 #include <vector>
 #include "utils\point.h"
 #include <stdlib.h>
 #include <math.h>
 #include <algorithm>
 //////////////////////////////////////////////////////////////////////////
 //target全局坐标系下的目标点
 //obstacles围绕激光为中心-180度到180度逆时针激光扫描点
 //theta里程计中的theta角
 //desiredDirection机器人应该运动的方向（全局坐标）
 inline void navigate(const GMapping::Point &target,const std::vector<float>    &obstacles,double theta,
     double maxRobotSpeed,
     double TARGET_ATTRACTIVE_FORCE,double TARGET_SLOW_APPROACHING_DISTANCE,
     double &desiredDirection,
     double &desiredSpeed)
 {
     //MRPT_UNUSED_PARAM(maxRobotSpeed);
     // Forces vector:
     GMapping::Point resultantForce(,),instantaneousForce(,);

     // Obstacles:
     {
         const size_t n = obstacles.size();
         const double inc_ang = *M_PI/n;
         double ang = -M_PI + 0.5*inc_ang+theta;//注意此处，从-180度开始逆时针存储数据
         for (size_t i=;i<n;i++, ang+=inc_ang )
         {
             // Compute force strength:
             //const double mod = exp(- obstacles[i] );
             const double mod = min(1e6, 1.0/ obstacles[i] );

             // Add repulsive force:
             instantaneousForce.x = -cos(ang) * mod;
             instantaneousForce.y = -sin(ang) * mod;
             resultantForce =resultantForce+ instantaneousForce;
         }
     }

     const double obstcl_weight = 20.0/obstacles.size();
     resultantForce =resultantForce* obstcl_weight;
     double resultantForcenorm = sqrt(resultantForce.x    *resultantForce.x+ resultantForce.y+resultantForce.y);
     const double obstacleNearnessFactor = min( 1.0, 6.0/resultantForcenorm);

     // Target:
     const double ang = atan2( target.y, target.x );
     const double mod = TARGET_ATTRACTIVE_FORCE;
     resultantForce =resultantForce+ GMapping::Point(cos(ang) * mod, sin(ang) * mod );

     // Result:
     desiredDirection = (resultantForce.y== && resultantForce.x==) ?
          : atan2( resultantForce.y, resultantForce.x );

     // Speed control: Reduction factors
     // ---------------------------------------------
     double targetnorm=sqrt(target.x    *target.x + target.y*target.y);
     const double targetNearnessFactor = min( 1.0, targetnorm/(TARGET_SLOW_APPROACHING_DISTANCE));
     //desiredSpeed = maxRobotSpeed * std::min(obstacleNearnessFactor, targetNearnessFactor);
     desiredSpeed = min(obstacleNearnessFactor, targetNearnessFactor);
 }
 #endif

参考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