传统人工势场法程序

1、1 / 7 传统人工势场法程序 主程序： 起点位置 计算引力需要的增益系数 计算斥力的增益系数，都是自己设定的。 障碍影响距离，当障碍和车的距离大于这个距离时，斥力为， 即不受该障碍的影响。也是自 己设定。 障碍个数 步长 循环迭代次数 如果不能实现预期目标，可能也与初始的增益系数，设置的不合适有关。 给出障碍和目标信息 这个向量是()*维，其中是目标位置，剩下的都是障碍的位置。 循环初始，将车的起始坐标赋给 * 始化纟结束 开始主体循环 * 循环开始 ()()是保存车走过的每个点的坐标。刚开始先将起点放进该向量。 ()(); 调用计算角度模块 ()是计算出来的车和障碍，和目标之间的与轴之间的

2、夹角，统一规定角度为逆时针方向，用 这个模块可以计算出来。 调用计算引力模块 ()()是车和目标之间的角度，目标对车是引力。 ()为了后续计算斥力在引力方向的分量赋值给 ()计算出目标对车的引力在方向的两个分量值。 ()()计算斥力用的角度，是个向量，因为有个障碍，就有个角度。 调用计算斥力模块 ()计算出斥力在方向的分量数组。 计算合力和方向，这有问题，应该是数，每个循环的时候合力的大小应该是一个唯一的数, 不是数组。应该把斥力的所有分量相加，引力所有分量相加。 方向的合力 方向的合力 ()()合力与轴方向的夹角向量 计算车的下一步位置 )()()是应该完全相等的时候算作到达， 还是只是接近

3、就可以? 时候编程。 记录迭代到多少次，到达目标。 现在按完全相等的 ()()把路径向量的最后一个点赋值为目标 ()(); * * 画出障碍，起点 目标，路径点 画出路径 (); (); 路径向量是二维数组分别是数组的元素的集合，是两个一维数组。 障碍的坐标 ; 计算角度分程序： ()是引力，斥力与轴的角度向量是起点坐标，是目标和障碍的坐标向量 是障碍数目 ()() ()() (AA) () () 表示是目标 ，是()*矩阵 3 / 7 ()保存每个角度在向量里面，第一个元素是与目标的角度，后面都是与障碍的角度 计算引力分程序： ()输入参数为当前坐标，目标坐标，增益常数 ，分量和力的角度 把

4、路径上的临时点作为每个时刻的 ()()A()()A路径点和目标的距离平方 ()路径点和目标的距离 *(); *(); 计算斥力分程序: 斥力计算 ()输入参数为当前坐标，是目标和障碍的坐标向量，增益常数 ，障碍，目标方向的角度 ()()()A()()A路径点和障碍的距离平方 ()()路径点和障碍的距离保存在数组中 ()如果每个障碍和路径的距离大于障碍影响距离，斥力令为 () () ()*()A*()A)分解的向量 ()()*()()() ()()*() 判断距离是否在障碍影响范围内 ()叠加斥力的分量 () 改进势场法程序： 主程序： J 4 / 7 障碍和目标，起始位置都已知的路径规划 ，意

5、图实现从起点可以规划出一条避开障碍到达目 标的路径。 初始化车的参数 起点位置 计算引力需要的增益系数 初始化 计算斥力的增益系数，都是自己设定的。 障碍影响距离，当障碍和车的距离大于这个距离时，斥力为， 即不受该障碍的影响。也是自 己设定。 障碍个数 J 步长 循环迭代次数 如果不能实现预期目标，可能也与初始的增益系数，设置的不合适有关。 给出障碍和目标信息 这个向量是()*维，其中是目标位置，剩下的都是障碍的位置。 循环初始，将车的起始坐标赋给 * 始化纟结束 开始主体循环 * 循环开始 ()()是保存车走过的每个点的坐标。刚开始先将起点放进该向量。 ()(); 调用计算角度模块 ()是计

6、算出来的车和障碍，和目标之间的与轴之间的夹角，统一规定角度为逆时针方向， 用这个模块可以计算出来。 调用计算引力模块 ()()是车和目标之间的角度，目标对车是引力。 ()为了后续计算斥力在引力方向的分量赋值给 ()计算出目标对车的引力在方向的两个分量值。 ()()计算斥力用的角度，是个向量，因为有个障碍，就有个角度。 调用计算斥力模块 ()计算出斥力在方向的分量数组。 计算合力和方向，这有问题，应该是数，每个循环的时候合力的大小应该是一个唯一的数, 不是数组。应该把斥力的所有分量相加，引力所有分量相加。 方向的合力 方向的合力 ()()合力与轴方向的夹角向量 计算车的下一步位置 ()()*()

7、; ()()*(); 保存车的每一个位置在向量中 判断 5 / 7 ()()()()是应该完全相等的时候算作到达，还是只是接近就可以？现在按完全相 等的时候编程。 记录迭代到多少次，到达目标。 J 记录此时的值 如果不符合的条件，重新返回循环，继续执行。 大循环结束 ()()把路径向量的最后一个点赋值为目标 ()(); * 画出障碍，起点，目标，路径点 * 画出路径 (); (); 路径向量是二维数组分别是数组的元素的集合，是两个一维数组。 障碍的坐标 ; / II II II | 计算角度分程序： ()是引力，斥力与轴的角度向量是起点坐标，是目标和障碍的坐标向量 ，是()*矩阵 是障碍数目

8、()()(); ()()(); ()()A()A); () ()(); ()(); 表示是目标 ()保存每个角度在向量里面，第一个元素是与目标的角度，后面都是与障碍的角度 计算引力分程序: ()输入参数为当前坐标，目标坐标，增益常数 ，分量和力的角度 把路径上的临时点作为每个时刻的 ()()A()()A路径点和目标的距离平方 6 / 7 ()路径点和目标的距离 *()() *(); 计算斥力分程序： 斥力计算 ()输入参数为当前坐标，是目标和障碍的坐标向量，增益常数 ，障碍，目标方向的角 度 ()()A()()A路径点和目标的距离平方 ()路径点和目标的距离 ()()()A()()A路径点和障碍的距离平方 ()()路径点和障碍的距离保存在数组中 ()()A()()A； ()； ()如果每个障碍和路径的距离大于障碍影响距离，斥力令为 ()； ()； ()； ()； () ()*()*()*(A)分解的向量 ()*()A)*(A)分解的向量 ()()*()()() ()*()*(