2021-11-06关节空间路径规划和算法（采样、搜索）或者末端轨迹优化？_关节空间路径优化

关节空间路径规划

一些概念

一、 摘自 运动规划ompl

1.1. 运动规划 (Motion Planning)

我们这里讲的 运动规划 ，有别于 轨迹规划 (Path Planning)。一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。当然了，这两者没有本质的区别，理论上说MoveIt!和OMPL同样可以用于无人车无人机的规划，但不免有些杀鸡用牛刀的感觉。两者规划的空间维度不同，导致他们的难易程度不同。举例说明，如果不考虑速度加速度，只考虑位置的话，无人车轨迹规划维度是3 (x,y,和角度), 无人机是6 (x,y,z,和另外3个量确定空间的旋转角度)。确定3D空间的一个姿势(pose)需要6个变量，而对于关节数大于6的机械臂结构，它的规划空间维度就大于6，成为冗余系统(redundant system)，从而使规划问题变得更为复杂。所谓冗余系统，就是说，存在多种关节角度配置能够使得终端达到相同的位姿，存在无数的解。这是达到的最终姿势有无数个解，那么如何到达这个最终姿势，整个运动的轨迹，更是存在无数个解。

既然存在无数的解，那么问题来了。很明显，存在两种不同的方向，一种是找到最好的那个解，另一种是快速的找到一个有效的解。前者，大部分算法使用最优规划 (Optimization-based Planning)，后者使用采样规划 (Sampling-based Planning)。具体的区别和算法，不在这里赘述。

1.2. 开源运动规划库 (OMPL).

接上文，而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但 总体来说OMPL还是一个采样规划算法库 。而采样规划算法中，最出名的莫过于 Rapidly-exploring Random Trees ( RRT ) 和 Probabilistic Roadmap ( PRM )了, 当然，这两个是比较老的，还有很多 其他 新算法。

OMPL能做什么？

简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。
为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。

1.3. 逆运动学 (Inverse Kinematics)

什么是逆运动学(IK)？简单说，就是把终端位姿变成关节角度，q=IK§。p是终端位姿(xyz)，q是关节角度。
为什么要用IK？OMPL是采样算法，也就是要在关节空间采样。 这与无人车的规划有一个最明显的区别，无人车的目标就是在采样空间， e.g. 目标是(x,y), 采样空间也是(x,y). 但是对于机械臂，目标是终端空间位置(xyz), 但采样空间却是关节空间(q0,q1,…qN)。有了IK之后，我们就可以把三维空间的目标p转化为关节空间的目标q。那么这样就会让采样算法能算的更快，具体方法不赘述，这样的算法有RRT-Connect，BKPIECE等等双向采样算法。

二、A*

算法实现过程

A*算法作为一种启发式的寻路算法被广泛应用于解决游戏中以及机器人、无人车、无人机的路径规划问题。该算法的步骤为：

1. 把起始节点添加到openList；

2. 重复如下步骤：

a) 寻找openList当中F值最低的节点，将其称为当前节点；

b) 将该节点从openList中删除，并加入closeList当中；

c) 对于当前节点相邻的8个节点

• 如果它不可通过或者已在closeList当中，忽略它，反之如下；

• 如果它不在openList当中，将其添加进去。把当前节点作为它的父节点。同时更新它的F,G和H值。

• 如果它已经在openList当中，通过判断沿当前节点到它的路径的G值是否更小，若更小，则将当前节点作为它的父节点，同时更新它的F和G值。否则，不做任何操作。

d) 当目标节点已添加到closeList时，路径已被找到；或者没有找到目标节点，此时openList已为空。这时候，路径不存在。以上两种情况结束循环。

3. 路径回归。从目标节点开始，沿着每一个节点的父节点移动至起始节点，形成的路径即为所求路径。

基于采样的RRT算法

算法主要包含如下部分：

本篇阐述了基于采样的路径搜索算法——快速搜索随机树（RRT），在地图维度较高或尺寸较大时，基于采样的路径规划算法较基于图搜索的路径规划算法（如A*等）有更高的搜索效率。然而，该算法得到的路径并非全局最优的，甚至每一次计算的得到的结果都有所不同。

路径规划与运动规划？问题

纯几何运动规划路径不适合直接用于控制，至少需要柔顺路线的动力学约束，不能是几个中间点直接连接的多段折线，需要一段曲线。

• 如上图所示，实线为经过路径规划算法得到的轨迹，虚线为经过运动规划算法优化后得到的轨迹，红色箭头代表无人机此时的速度方向。

• 1号紫色实线轨迹“非常突兀”，若无人机按照这条轨迹飞行，那么无人机的加速度变化会非常剧烈且不连续，这并不利于无人机的稳定飞行。

• 2号绿色实线为加入动力学约束之后，路径规划算法得到的轨迹，显然，这条曲线较为平滑，且更负责无人机当前的状态。

• 1号紫色虚线与2号绿色虚线为前两者分别进行运动规划算法优化后得到的轨迹。显而易见的是，绿色虚线得到的轨迹更优。

• 由上图可知，无论是否加入动力学约束进行路径规划，最终得到的轨迹均不如路径规划+运动规划算法得到的轨迹平滑。

运动规划问题

运动规划算法根据无人机的运动学与动力学模型，得到其每一时刻的约束条件，如 [公式] 等等，结合环境信息（如障碍物），得到对应的环境约束，使用一定的优化算法，求解符合所有约束条件的最优轨迹。然后，通过控制器对无人机系统进行最优控制。

因此，解运动规划问题在本质上等同于解优化问题。

常用的运动规划算法如MinimumJerk，MinimumSnap，MPC（模型预测控制）等等

个人理解：以上是无人机的路径规划和运动规划，运动规划要考虑动力学约束，输出的控制参数应满足使用。对照机械臂：运动规划->机械臂的轨迹规划，满足约束条件后的输出量可以用于控制器的控制，轨迹优化？？

• 被上一段弄乱了思路，区分一下概念：

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

* 运动规划由路径规划和轨迹规划（时间）组成

路径规划是运动规划的主要研究内容之一。连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。
路径是机器人位姿的一定序列，而不考虑机器人位姿参数随时间变化的因素。路径规划（一般指位置规划）是找到一系列要经过的路径点，路径点是空间中的位置或关节角度，而轨迹规划是赋予路径时间信息。
运动规划（又称运动插补）是在给定的路径端点之间插入用于控制的中间点序列从而实现沿给定的平稳运动。运动控制则是主要解决如何控制目标系统准确跟踪指令轨迹的问题。即对于给定的指令轨迹，选择适合的控制算法和参数，产生输出，控制目标实时，准确地跟踪给定的指令轨迹。
路径规划的目标是使路径与障碍物的距离尽量远同时路径的长度尽量短；轨迹规划的目的主要是机器人关节空间移动中使得机器人的运行时间尽可能短，或者能量尽可能小。轨迹规划在路径规划的基础上加入时间序列信息，对机器人执行任务时的速度与加速度进行规划，以满足光滑性和速度可控性等要求。
————————————————
版权声明：本文为CSDN博主「小白鼠2号」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/wx545644217/article/details/54175035

在关节空间的规划中间点正运动学计算到末端空间

关节空间规划出的路径中间点，映射到笛卡尔空间没问题，但是运动过程会导致上一篇里截图看到的，因为关节空间的直线运动，耦合到一起末端的运动是曲线，较复杂，对于末端夹持负载有约束的情况下不适用，但是对路径中间点之间的运动，回归到运动规划的初始状态了。

• 末端空间路径规划，
  • 可以约束夹持物的位姿，满足其动力学约束
  • 末端运动柔顺
  • 逆解慢
  • 可能无解（限制末端位姿可能无解）
  • 且可能存在突变（相邻路径点逆解左右手突变或者过奇异点）
  • 多解（只限制位置不约束末端姿态因此可能有多解）
• 关节空间
  • 关节空间路径规划的折线路径容易轨迹规划
  • 关节空间路径中间点要停止？或者定义中间状态运动参数（至少需要满足位置约束？）
  • 末端运动复杂，运动参数不可控？

关节空间路径规划没有问题，需要进行的是轨迹规划！

关节空间规划出的路径点计算到末端空间也是路径点，以这几个无碰撞路径点为中间点，下一步进行轨迹规划完成动作即可。
轨迹规划过程中都要考虑再次碰撞检测！关节空间规划若要连贯运动不在中间点速度为零，也会在构型空间内偏离直接插值得到的折线，导致其规划过程中进行的碰撞检测失效！（离散运动规划的碰撞检测？）

网格地图的启发式搜索算法

路径规划算法学习

一些基于搜索的路径规划算法学习笔记