[路径规划] VFF和VFH

VFF虚拟力场法

 1 #ifndef VFF_HEADER
 2 #define VFF_HEADER
 3 #include <vector>
 4 #include "utils\point.h"
 5 #include <stdlib.h>
 6 #include <math.h>
 7 #include <algorithm>
 8 //////////////////////////////////////////////////////////////////////////
 9 //target全局坐标系下的目标点
10 //obstacles围绕激光为中心-180度到180度逆时针激光扫描点
11 //theta里程计中的theta角
12 //desiredDirection机器人应该运动的方向（全局坐标）
13 inline void navigate(const GMapping::Point &target,const std::vector<float>    &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<n;i++, ang+=inc_ang )
29         {
30             // Compute force strength:
31             //const double mod = exp(- obstacles[i] );
32             const double mod = min(1e6, 1.0/ obstacles[i] );
33 
34             // Add repulsive force:
35             instantaneousForce.x = -cos(ang) * mod;
36             instantaneousForce.y = -sin(ang) * mod;
37             resultantForce =resultantForce+ instantaneousForce;
38         }
39     }
40 
41     const double obstcl_weight = 20.0/obstacles.size();
42     resultantForce =resultantForce* obstcl_weight;
43     double resultantForcenorm = sqrt(resultantForce.x    *resultantForce.x+ resultantForce.y+resultantForce.y);
44     const double obstacleNearnessFactor = min( 1.0, 6.0/resultantForcenorm);
45 
46     // Target:
47     const double ang = atan2( target.y, target.x );
48     const double mod = TARGET_ATTRACTIVE_FORCE;
49     resultantForce =resultantForce+ GMapping::Point(cos(ang) * mod, sin(ang) * mod );
50 
51     // Result:
52     desiredDirection = (resultantForce.y==0 && resultantForce.x==0) ?
53         0 : atan2( resultantForce.y, resultantForce.x );
54 
55     // Speed control: Reduction factors
56     // ---------------------------------------------
57     double targetnorm=sqrt(target.x    *target.x + target.y*target.y);
58     const double targetNearnessFactor = min( 1.0, targetnorm/(TARGET_SLOW_APPROACHING_DISTANCE));
59     //desiredSpeed = maxRobotSpeed * std::min(obstacleNearnessFactor, targetNearnessFactor);
60     desiredSpeed = min(obstacleNearnessFactor, targetNearnessFactor);
61 }
62 #endif