[路径规划] 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