机器人运动规划技术介绍

文章目录

运动规划、路径规划和轨迹规划
- 路径规划算法
- - 随机采样的算法
  - 梯度下降法
- 轨迹规划算法
- - 基本思路
  - 开源算法介绍
- 总结
- 参考资料：

运动规划、路径规划和轨迹规划

机器人的运动规划可以看做是包括了路径规划和轨迹规划两个步骤，目的是根据给定的任务，计算出一条可以下发给机器人控制器去执行的轨迹。

路径规划，顾名思义就是在有障碍的空间里找出一条没有碰撞的路径来，一般情况下，这些路径都由一系列的离散点来表示，相邻的点进行线性插值，确保每个离散点和中间的插值点都不会有碰撞。

轨迹规划是根据路径规划生成的离散点，计算一条连续的满足机器人运动学、动力学约束的包含时间信息的轨迹。在轨迹上按照实际的伺服周期采样位置点，下发给伺服控制器，从而使机器人运动到目标位置。

路径规划

生成一些列无碰撞的离散点

轨迹规划

生成连续的轨迹

路径规划算法

路径规划算法一般包括基于随机采样和梯度下降两种。如果某种算法保证在一定时间内可以找到一个解，则具有完备性；如果求出来的解是最优解，则具有最优性。评价路径的优劣一般和优化目标有关系，通常除了不碰撞之外，还会设置路径长度、平滑性的优化目标，希望最终找到一条路径最短、最平滑、无碰撞的路径。比如说，两点之间，有无数中路径可以到达，但是两点直线连接则是路径最短的一条，如果这条直线还没有和周围障碍碰撞，那么就是最优的。

随机采样的算法

开源的OMPL实现了很多中基于随机采样的算法

RRT：基于采样的算法，成功率较高，但是因为随机采样，即便是相同的环境和初始条件，规划出来的路径每次也可能不同
PRM：也是基于采样，不过是先通过离线采样建立一个联通的路图，在根据任务进行最优搜索

梯度下降法

势场法：将障碍物看做一个能够向外产生推力势场的源，利用优化的算法逐次迭代求解，对于简单场景可以保证结果的一致性，但复杂情况下，成功率可能不高，可以参考ROS MoveIT的CHOMP，TrajOpt模块

随机采样的算法对于碰撞检测库的要求是只需要知道是否碰撞即可，而势场法需要知道当前状态下碰撞梯度信息（距离碰撞物的距离，越远则势场越小，越近则势场越大）。

轨迹规划算法

基本思路

路径规划输出的是一系列离散的路径点（waypoint），并不能直接发给机器人执行，需要首先生成一条平滑的连续路径（path）来把这些点串起来；然后再根据机器人各个轴最大速度、加速度等限制，生成带有时间信息的连续轨迹。

轨迹规划第一步是根据离散的路径点生成的路径 $p (s)$ ，需要保证路径的连续，也就是位置、切线、曲率等的连续。

$p(s):通过所有路径点的几何路径，比如样条曲线、贝赛尔曲线q(t):带有时间信息的轨迹，按照插补周期采样下发给控制器s(t):时间参数q(t)=p(s(t))q˙(t)=p′(s)s˙q¨(t)=p′′(s)s˙2+p′(s)s¨轨迹规划需要满足速度、加速度、加加速度的约束：q˙min≤q˙(t)≤q˙maxq¨min≤q¨(t)≤q¨max\begin{aligned}p(s)&: 通过所有路径点的几何路径，比如样条曲线、贝赛尔曲线\\q(t)&: 带有时间信息的轨迹，按照插补周期采样下发给控制器\\s(t)&: 时间参数\\[3ex]q(t) &= p(s(t)) \\\dot q(t) &= p'(s)\dot s\\\ddot q(t) &= p''(s)\dot s^2 + p'(s)\ddot s\\[5ex]轨迹规划&需要满足速度、加速度、加加速度的约束：\\[3ex]\dot q_{min} &\leq \dot q(t) \leq \dot q_{max}\\\ddot q_{min} &\leq \ddot q(t) \leq \ddot q_{max}\\\\\end{aligned}$
轨迹规划的第二步是生成一条时间参数曲线 $s (t)$ ，确保轨迹的速度、加速度等不超过限制。

上面说的两步有时也可以合并为一步，直接计算 $q (t)$ ，具体采用哪种方式和算法有关系。

开源算法介绍

连续性是轨迹规划的基础，在此之上，再有时间最优（Time Optimal）、最小加加速度（Minimum Jerk）等不同的优化目标。

Ros 的 MoveIT的模块Time Parameterization包含了三种轨迹规划算法：

Iterative Parabolic Time Parameterization，IPTP
Iterative Spline Parameterization，ISP
Time-optimal Trajectory Generation，TOTP

这三种方法都可以实现在关节空间规划加速度约束的时间最优轨迹，以为加加速度不约束，所以规划出来的结果，加速度是要么最大、要么最小、要么零，即Bang-Bang Trajectory。

和TOTP类似的一种算法是Toppra，它的实现比Totp更简单一些，且对路径不做要求，而Totp要求路径必须是采取直线-圆弧过度的方式。

对于加速度连续轨迹，Toppra的作者提出了Totp3算法，但是过于复杂，至今没有比较好的实现。可以参考的论文：

Smooth Time Optimal Trajectory Planning for Industrial Manipulators, PhD thesis by Daniela Constantinescu
Optimal trajectory planning for industrial robots by A. Gasparetto *, V. Zanotto

以上介绍的都是离线轨迹规划，轨迹都是通过一系列点的，计算时间一般在几十到几百ms，甚至更长。

如果轨迹需要实时调整，比如改变速度、目标位置，那么就需要实时计算轨迹，也就是在线轨迹规划，一般要求计算时间要控制在几ms。

对于***点到点（Point-to-Point）***的轨迹规划，因为只要两个点，时间最优轨迹就是双S速度曲线轨迹，一般点到点计算速度快，大多可以用于在线规划。

RML库实现了这个算法，但是其只开源了速度连续的部分，加速度连续部分并没有开源，因为计算速度很快，也适用于在线计算。
OptControl，这也是ros的一个模块，基本实现了和RML相同的功能，但是使用上稍显复杂。
OJET，也是计算Double S轨迹的，可以在网页上试用。

总结

目前在工业上还是很少有用到路径规划的，随机采样算法不能保证一致性，难以满足工业严格的节拍要求；势场法可能会陷入局部最优，导致最终优化失败（这也是所有基于优化的算法的一个难题）。对于可靠性要求极高的工业项目，这是难以接受的。工业现场的大多数场景还是离线编程与现场示教。随着3D视觉的持续发展，也出现很多把实时路径规划技术应用到抓取项目中的案例，国内做3D视觉的公司，几乎都在同时做抓取、播种等机器人相关方案，比如武汉的库伯特，上海的梅卡曼德等，国外做Bin Picking的更多了，传统的机器人厂商之外，还有很多只做视觉产品、软件的公司，如日本MUJIN，美国Energid。

机器人视觉 Pick-it 3D 智能分拣机器人 - [YouTube]

轨迹规划机器人公司是必须要做的，一般机器人示教器都提供了MoveL/MoveJ/MoveC等运动指令，属于点到点的轨迹，采取梯形或者双S轨迹，如果是多个点的话，会有一些过渡、融合的（blending）算法，从而让机器人在中间无需停止。但是很少有机器人公司直接提供样条曲线、时间最优轨迹等模块给用户使用。

参考资料：

Toppra： Pham, Hung & Pham, Quang Cuong. (2018). A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis. IEEE Transactions on Robotics. 34. 645 - 659. 10.1109/TRO.2018.2819195.
Totp: Kunz, Tobias & Stilman, Mike. (2012). Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. 10.15607/RSS.2012.VIII.027.
RML：
1. -http://www.reflexxes.ws/
2. https://github.com/Reflexxes/RMLTypeII
3. Kröger, Torsten. (2010). On-Line Trajectory Generation in Robotic Systems - Basic Concepts for Instantaneous Reactions to Unforeseen (Sensor) Events. 10.1007/978-3-642-05175-3.
OptimalConntrol:
1. Marius Beul and Sven Behnke: Fast Full State Trajectory Generation for Multirotors
  In Proceedings of International Conference on Unmanned Aircraft Systems (ICUAS), Miami, FL, USA, June 2017
2. Marius Beul and Sven Behnke: Analytical Time-optimal Trajectory Generation and Control for Multirotors
  In Proceedings of International Conference on Unmanned Aircraft Systems (ICUAS), Arlington, VA, USA, June 2016
3. https://github.com/AIS-Bonn/opt_control
比较好的教材：
1. Luigi Biagiotti · Claudio Melchiorri， Trajectory Planning for Automatic Machines and Robots：一本比较全面的介绍轨迹规划的教材
2. Modern Robotics: Mechanics, Planning, and Control , https://github.com/NxRLab/ModernRobotics