机器人跟随算法

REF基于多机器人路径规划与编队控制的优化研究1. 基础模型RRT快速随机树算法模拟树的生长过程起始点为树的根节点在环境中随机采样作为树的叶节点从最近节点到目标点的方向上每隔一定的步长生成一个新的节点若此节点没有与障碍物碰撞则将新节点与最近的节点连接起来不断重复这样的探索过程到目标点则随机树的构造结束以距离目标点最近的叶节点为起点沿树结构逐级回溯父节点即可构建出一条连接起始点与目标点的可行路径。APF人工势场算法模拟物理学中电场的原理目标点看作是吸引源产生吸引力障碍物看作排斥源产生排斥力运动由吸引力和排斥力的合力决定。吸引力与目标点和智能体之间的距离成反比距离越远吸引力越大促使智能体向目标点靠近排斥力则与智能体和障碍物之间的距离有关当智能体靠近障碍物时排斥力迅速增大将智能体推开避免碰撞。领航跟随法领航机器人沿着定义的轨迹移动跟随机器人与领导者保持期望距离和相对方位跟随机器人(follower)依据领航者的位置、速度及姿态等信息动态调节自身运动状态维持与领航机器人的相对运动关系。2. 改进RRT算法实现全局路径规划新生成的节点不单由采样点决定而引入目标点来调整节点的构建使其向目标区域延伸随机树具有明确的方向性由采样点和目标点共同决定。目标导向约束采样将目标导向与随机性相结合的混合采样如果随机数小于目标偏置概率则优先选择在目标点附近的区域进行采样增加路径向目标的收敛性。反之则按照正常的随机采样方式进行节点生成。目标偏置扩展引入启发式估价函数每个随机采样点进行代价评估通过计算每个采样点到起始点和目标点的欧几里得距离来衡量其代价小选择距离最短的采样点添加到随机树中。自适应步长使用最大步长贪心算法实现自适应步长即如果路径上会出现障碍物则将步长减小直到最小步长否则使用最大步长。三次贝塞尔曲线平滑处理引入贝塞尔曲线对路径点进行拟合通过对运动轨迹进行平滑优化处理降低轨迹曲率进而减少任务执行时间和能耗提高机器人的运动性能和作业效率。3. 改进APF算法实现局部路径规划自适应随机扰动优化加入随机扰动打破系统平衡让机器人的路径稍微偏离原本力场平衡的方向从而寻找更好的路径同时对扰动进行约束添加方向限制扰动防止远离目标、接近障碍物或长时间时卡在一个地方则增加扰动幅度增强机器人跳出最优位置的能力。自适应扰动系数以机器人到目标点的距离为基准离目标越远干扰越大保证了机器人稳定加权方向扰动将随机扰动与目标方向结合使得扰动更趋向于目标点保证运动轨迹不偏离目标仍保持一定随机性局部环境感知靠近障碍物时增大扰动以帮助机器人脱离障碍物的影响时间限制限制扰动次数避免扰动无限制增加动态扰动多次扰动仍无效果则增加较大扰动打破停滞状态设置虚拟障碍物优化引入虚拟障碍物防止机器人易陷入局部极小值针对U形障碍物避免陷入“死区”检测局部最小值机器人位置在较长时间内相对稳定可能处于局部最小值状态生成虚拟障碍物在当前位置生成新的虚拟障碍物并设置一定的障碍物影响范围更新障碍物列表虚拟障碍物被添加到障碍物矩阵中作为额外的障碍物参与后续的排斥力计算路径规划机器人会在新产生的斥力作用下调整自身运动轨迹重新规划路径避开障碍物向目标位置靠近优化引力势场函数传统的引力势场是距离的二次函数距离目标位置越近势能就越低鉴于对局部最优问题的考虑将引力函数改进为指数函数形式引导机器人更快地避开障碍物提高机器人行进过程中的稳定性和鲁棒性。4. 改进APF算法实现跟随避障利用人工势场法的实时性提高机器人对外界环境的适应性结合领航者的全局规划能力可以进一步保证编队协同工作时的稳定性。人工势场包括引力场和斥力场在充分考虑协同、避障等因素的基础上使跟随者在运动过程中均能保持一致的方向与队形并结合斥力场和随机扰动的控制策略提高复杂环境中的适应性。引力场由机器人到目标点的间距决定当距离增大时该引力场对目标点的吸引力就越强这样机器人就能更好地接近目标点斥力场也基于两个机器人间距来确定越靠近物体排斥力就会越强防止机器人发生碰撞基于前面对传统APF 算法的改进将其与领导跟随法结合使多机器人协同工作具有良好的灵活性和自适应能力该方法既能有效地改善机器人的编队运动效率还增强了系统在动态环境中的鲁棒性。