[路径规划] VFF和VFH

VFF虚拟力场法

#ifndef VFF_HEADER
#define VFF_HEADER
#include <vector>
#include "utilspoint.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(0,0),instantaneousForce(0,0);

    // Obstacles:
    {
        const size_t n = obstacles.size();
        const double inc_ang = 2*M_PI/n;
        double ang = -M_PI + 0.5*inc_ang+theta;//注意此处，从-180度开始逆时针存储数据
        for (size_t i=0;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==0 && resultantForce.x==0) ?
        0 : 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