引力机器人cos内容

引力势场主要与机器人和目标点间的距离有关，距离越大，机器人所受的势能值就越大;距离越小，机器人所受的势能值则越小，所以引力势场的函数为:

当存在某个障碍物与目标点距离太近时，机器人到达目标点时，根据势场函数可知，目标点的引力降为零，而障碍物的斥力不为零，此时机器人虽到达目标点，但在斥力场的作用下不能停下来，从而导致目标点不可达的问题，机器人容易出现在目标点附近震荡的现象。

机器人在某个位置时，如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，这将导致机器人受力为0，而停止运动，故无法继续向目标点移动，下图列举了其中的一种情况。

以上缺陷1和2的一种改进思路是，改变斥力场函数，传统的斥力场函数的大小由机器人与障碍物直接的距离决定，方向由障碍物指向机器人，为避免以上缺陷，我们可以把斥力场函数改为由两部分组成，第一部分像传统的斥力场函数一样由障碍物指向机器***小由机器人与障碍物之间距离和机器人与目标点距离共同决定，如下图中的蓝色箭头Freq1所示，第二部分斥力由机器人指向目标点，大小由机器人与障碍物之间距离和机器人与目标点之间距离共同决定，如下图中的绿色箭头Freq2所示，这两部分斥力共同组成了合斥力Freq，如下图中黄色箭头所示，再与红色箭头所示的引力Fatt求合力，得到最终机器人的受力，如紫色的箭头F所示。

进入循环，只要机器人当前位置与目标点之间的距离大于1就循环执行以下程序，将机器人当前状态（位置和速度）（也可以是模拟的当前位置和速度）添加到路径中，然后计算机器人当前位置与障碍物和目标点之间的单位方向向量，然后计算斥力和引力的大小、计算边界带来的斥力，计算合力的大小和方向，更新机器人的状态（位置和速度），以此循环。