机器人跟随算法

REF：基于多机器人路径规划与编队控制的优化研究

1. 基础模型

• RRT（快速随机树）算法：模拟树的生长过程，起始点为树的根节点，在环境中随机采样作为树的叶节点，从最近节点到目标点的方向上，每隔一定的步长生成一个新的节点，若此节点没有与障碍物碰撞，则将新节点与最近的节点连接起来，不断重复这样的探索过程到目标点，则随机树的构造结束，以距离目标点最近的叶节点为起点，沿树结构逐级回溯父节点，即可构建出一条连接起始点与目标点的可行路径。

31fd30b99337cf77fd93e6a805bbab71.png

• APF（人工势场）算法：模拟物理学中电场的原理，目标点看作是吸引源产生吸引力，障碍物看作排斥源产生排斥力，运动由吸引力和排斥力的合力决定。吸引力与目标点和智能体之间的距离成反比，距离越远吸引力越大，促使智能体向目标点靠近；排斥力则与智能体和障碍物之间的距离有关，当智能体靠近障碍物时，排斥力迅速增大，将智能体推开，避免碰撞。

e6268ddd693bac8aeeb703f0fbdc8dbd.png

• 领航跟随法：领航机器人沿着定义的轨迹移动，跟随机器人与领导者保持期望距离和相对方位，跟随机器人(follower)依据领航者的位置、速度及姿态等信息，动态调节自身运动状态，维持与领航机器人的相对运动关系。

95ac817d9486c468f240b9ba101076de.png

2. 改进RRT算法实现全局路径规划

新生成的节点不单由采样点决定，而引入目标点来调整节点的构建，使其向目标区域延伸，随机树具有明确的方向性，由采样点和目标点共同决定。

• 目标导向约束采样：将目标导向与随机性相结合的混合采样，如果随机数小于目标偏置概率，则优先选择在目标点附近的区域进行采样，增加路径向目标的收敛性。反之，则按照正常的随机采样方式进行节点生成。

cbed324797b77390d3d598647b8ee4ea.png

• 目标偏置扩展：引入启发式估价函数，每个随机采样点进行代价评估，通过计算每个采样点到起始点和目标点的欧几里得距离来衡量其代价，小选择距离最短的采样点添加到随机树中。

6155508fb222b3140b115ff16c3dbd8f.png

• 自适应步长：使用最大步长贪心算法实现自适应步长，即如果路径上会出现障碍物，则将步长减小直到最小步长，否则使用最大步长。

72c71eb0eb23dcb2eb6b13a32820780f.png

• 三次贝塞尔曲线平滑处理：引入贝塞尔曲线对路径点进行拟合，通过对运动轨迹进行平滑优化处理，降低轨迹曲率，进而减少任务执行时间和能耗，提高机器人的运动性能和作业效率。

95aa7241966aeb6038db9585299150fd.png

3. 改进APF算法实现局部路径规划

49b3f4972c02e6ac409abc028157b3c2.png

• 自适应随机扰动优化：加入随机扰动打破系统平衡，让机器人的路径稍微偏离原本力场平衡的方向，从而寻找更好的路径，同时对扰动进行约束（添加方向限制扰动防止远离目标、接近障碍物或长时间时卡在一个地方则增加扰动幅度）增强机器人跳出最优位置的能力。
  • 自适应扰动系数：以机器人到目标点的距离为基准，离目标越远，干扰越大，保证了机器人稳定
  • 加权方向扰动：将随机扰动与目标方向结合，使得扰动更趋向于目标点，保证运动轨迹不偏离目标仍保持一定随机性
  • 局部环境感知：靠近障碍物时增大扰动，以帮助机器人脱离障碍物的影响
  • 时间限制：限制扰动次数避免扰动无限制增加
  • 动态扰动：多次扰动仍无效果，则增加较大扰动打破停滞状态
• 设置虚拟障碍物优化：引入虚拟障碍物防止机器人易陷入局部极小值（针对U形障碍物），避免陷入“死区”：
  1. 检测局部最小值：机器人位置在较长时间内相对稳定，可能处于局部最小值状态
  2. 生成虚拟障碍物：在当前位置生成新的虚拟障碍物，并设置一定的障碍物影响范围
  3. 更新障碍物列表：虚拟障碍物被添加到障碍物矩阵中，作为额外的障碍物参与后续的排斥力计算
  4. 路径规划：机器人会在新产生的斥力作用下，调整自身运动轨迹，重新规划路径避开障碍物向目标位置靠近
• 优化引力势场函数：传统的引力势场是距离的二次函数，距离目标位置越近，势能就越低，鉴于对局部最优问题的考虑，将引力函数改进为指数函数形式，引导机器人更快地避开障碍物，提高机器人行进过程中的稳定性和鲁棒性。

f702d58b9cd72e8f4d5135514ee5b380.png

4. 改进APF算法实现跟随避障

利用人工势场法的实时性，提高机器人对外界环境的适应性，结合领航者的全局规划能力，可以进一步保证编队协同工作时的稳定性。

人工势场包括引力场和斥力场，在充分考虑协同、避障等因素的基础上，使跟随者在运动过程中均能保持一致的方向与队形，并结合斥力场和随机扰动的控制策略，提高复杂环境中的适应性。

• 引力场：由机器人到目标点的间距决定，当距离增大时，该引力场对目标点的吸引力就越强，这样机器人就能更好地接近目标点
• 斥力场：也基于两个机器人间距来确定，越靠近物体，排斥力就会越强，防止机器人发生碰撞

基于前面对传统APF 算法的改进，将其与领导跟随法结合，使多机器人协同工作具有良好的灵活性和自适应能力，该方法既能有效地改善机器人的编队运动效率，还增强了系统在动态环境中的鲁棒性。