ros自己写避障算法_基于ROS的码垛机器人运动仿真及轨迹规划中避障运动规划(上)...

3.4.5避障运动规划

要使码垛机器人在ROS系统的仿真环境下，完成在工作空间的避障运动，首先需要在Rviz可视化工具中构建避障的环境，以及选择避障所需要的碰撞检测算法。

避障环境构建的可以通过场景规划完成。规划场景用于表示机器人周围的环境变量，也存储机器人本身的状态。它是由移动组节点内场景规划监测器维护。其节点系统如图3-20所示，场景规划监测器主要检测一下信息:在joint_states话题中的机器人状态信息;采用世界几何监视器传输的传感器信息;世界几何信息，既从用户在planning_scene主题上的输入作为规划的场景差异。场景规划，可以在机器人的运动空间中添加约束对象，使得机器人的运动不与障碍物接触，保证机器人正常作业。通过载入PlanningSceneInterface命令，可以实现在显示的场景中添加与删除物体，通过场景规划接口在RViz中显示约束的障碍物，可以给定障碍物的形状以及在仿真环境中的位置与姿态，其流程图如图3 -21所示。

避障的算法可以通过MoveIt!完成，它强大的功能之一就是实现所控制的机器人或者机械臂绕过障碍物运动。成功的规划的轨迹，不仅保证机器人不与障碍物接触，而且也不允许机器人的末端或任一杆件与本体相触碰。在ROS中一般采用RRT算法实现机器人或者机械臂避障运动。

RRT算法是由S.M.LaValle提出，采用类似枝叶生长的方式对运动的点进行采集，可以完成有障碍物的机器人路径规划问题。RRT算法通过采样点的可行性判断可以实现机器人的碰撞检测，是RRT规划中耗时的主要环节。RRT规划根据一些列与障碍物不接触的点形成一条运动轨迹，具有较强的随机性，不仅使得到的轨迹复杂，而且耗时较长。为此，可以根据码垛机器人某些使用条件下的情况，进行存在障碍物的搬运活动的模拟，通过对运动轨迹优化，从而缩短轨迹运行的复杂性以及缩短用时。

如图3-22所示，模拟码垛机器人在使用环境中，其运动轨迹目标点或路径点过程中具有挡板的情形。对这种情形的机器人运动轨迹进行RRT算法的优化，根据障碍物的尺寸以及坐标范围，机器人的运动轨迹，既采取运动过程中的路径点作为插值点，从而简化采集点的范围，加快轨迹的生成。

假设机器人在左侧挡边左方A点为起始位置，需要经过两个挡板中间的位置B点作为轨迹的一个路径点，最终运动到右侧挡板的右侧C点。如图3 -21所示，为码垛机器人为避障而采取的优化运动轨迹，根据障碍物的尺寸范围，在机器人工作空间允许范围内，可以在障碍物上方或者前方实现运动避障，本文以上方避障为例，对运动轨迹进行优化，减少运动轨迹的随机性，并通过仿真实验验证。