矩阵连乘 动态规划_技术分享 | 实时运动规划

1942年，阿西莫夫在短篇小说《Runaround（环舞）》中提出了机器人三定律：

1. 机器人不得伤害人类
2. 除非违背第一定律，机器人必须遵从人类的命令
3. 除非违背第一和第二定律，机器人必须保护自己

之所以有这样的定律存在，其本质就是希望机器人在工作时，需要满足一些不同优先级的约束（不伤害人类、按照要求工作、不损坏自身）。

其实不仅仅在科幻小说里是这样，在真实场景中也存在类似的情形：机器人在工作的过程中，为了保证机器人与周围环境、人员的安全，需要同时处理多种任务，满足多种约束。

下面的动图展示了基于实时轨迹规划技术与如本规划控制器的动态轨迹调整功能实现的机器人多任务实时规划控制功能。

机器人对目标位姿（用外部装置控制）追踪的同时，能够「优雅地」避开障碍物，并且避开机器人关节极限和奇异点

基于实时规划技术能够轻松实现医疗机器人中广为人知的远心点约束（Remote Center of Motion）

实时运动规划需要处理动态变化的现实环境，这就需要机器人能对当下的状态实时地更新动作。因此这样的运动规划算法，也可以看做是一个运动策略（motion policy），即根据当前的状态（state），返回一个满足约束的动作（action）。

但是，在处理多种任务约束的时候，自然就会遇到一个问题：哪个任务约束的优先级更高？换句话说，当两个任务冲突时，机器人应该优先处理哪个任务？

比如今天你想让你的机器人帮你拿走放在桌上的饮料，而你人又正好挡在桌子前，机器人可能一伸手就揍到你了，那这时候怎么办，拿还是不拿呢？拿饮料是主人的命令，拿了又伤害了主人，苦恼的机器人...

这时候第一、二定律就告诉它，别拿，因为不伤害人类才是第一原则！

那接下来，robotics researcher 们头疼的事情就来了，怎么model（建模）这样机器人的决策（规划）行为呢？

这是一个多任务问题。以饮料的例子，这个被称为「拿饮料」任务的目标就有

1. Target Reaching (接近目标物，移动过去伸手拿饮料)

2. Joint Limit Avoidance (避开机器人自身的关节极限，保护自己)

3. Collision Avoidance (避开所有的障碍物，保护自己，保护环境)

4. External Force Compliance (顺应来自人的外力，不伤害人)

也就是说虽然我们认为的拿饮料是一个任务，实际上对于处于机器人来说则需要同时(并行)地考虑这四个目标。

在机器人学中，我们可以将每一个目标建模成是一个加速度，作用在机器人的不同流形空间上，例如关节空间。然后为了使这个目标收敛到一个理想值，我们可以利用控制理论制造一个满足李雅普诺夫稳定性判据的二阶系统。

比如说，Target Reaching，我们可以制造一个末端和目标之间的二阶系统, 如：

其中 x_d 和 x 分别是目标位姿和机器人当前末端位置，K, B 是某个正定矩阵。这就是著名的 Operational Space Control (O. Khatib)中提到的方法。我们可以设计 K 和 B，并证明这个系统是否稳定且衡量出此系统的收敛速度等等特性。

可是这样简单的二阶系统并不能满足我们对「优先级」的需求，即我们希望每一个目标的优先级（或者说重要性）会随着机器人的状态改变而改变。

对此，我们可以定义在不同流形上的几何动态系统（Geometric Dynamic System， GDS）：

GDS 通过引入曲率项（curvature terms），则提供了一个非常量的「惯量矩阵」(non-constant inertia matrix）且理论可证稳定的系统。

不同的动态系统产生的避障轨迹对比：左图是使用一般的二阶系统，右边是使用设计的GDS产生的

有了这些定义在不同流形的加速度，最后就需要用一种优雅的方式将他们结合起来，以输出最终机器人的关节加速度指令。可以将最优的关节加速度写成：

由不同的几何动态系统在不同流形上定义的加速度通过相对应的Jacobian雅可比矩阵结合在一起，而每个动态系统的「惯量矩阵」则巧妙地表达了相对应的目标的「优先级」（或重要性），有了这些，机器人就能灵活优雅地对每个时间点做出最适当的规划。

以上就是对如本科技所开发的实时运动规划算法的一个简单介绍，欢迎有兴趣的朋友一起讨论。