机器人轨迹规划（熊友伦）

机器人轨迹规划（熊友伦）

http://blog.csdn.net/jyc1228/article/details/3991881

http://blog.csdn.net/wx545644217/article/details/54175035

运动规划、路径规划、轨迹规划的联系与区别？

·      motion planning

·      path planning

·      trajectory planning

运动规划由路径规划和轨迹规划（时间）组成，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。
路径是机器人位姿的一定序列，而不考虑机器人位姿参数随时间变化的因素。路径规划（一般指位置规划）是找到一系列要经过的路径点，路径点是空间中的位置或关节角度，而轨迹规划是赋予路径时间信息。
运动规划（又称运动插补）是在给定的路径端点之间插入用于控制的中间点序列从而实现沿给定的平稳运动。运动控制则是主要解决如何控制目标系统准确跟踪指令轨迹的问题。即对于给定的指令轨迹，选择适合的控制算法和参数，产生输出，控制目标实时，准确地跟踪给定的指令轨迹。

路径规划的目标是使路径与障碍物的距离尽量远同时路径的长度尽量短；轨迹规划的目的主要是机器人关节空间移动中使得机器人的运行时间尽可能短，或者能量尽可能小。轨迹规划在路径规划的基础上加入时间序列信息，对机器人执行任务时的速度与加速度进行规划，以满足光滑性和速度可控性等要求。

 

局部路径规划主要解决机器人定位和路径跟踪问题，方法有人工势场法、模糊逻辑法等。

 

全局路径规划将全局目标分解为局部目标，再由局部规划实现局部目标，方法有可视图法、环境分割法（自由空间法、栅格法）等。

 

开源运动规划库（open motion planninglibrary）是一个运动规划C++库，其中包含很多运动规划领域的前沿算法。总体上OMPL是一个基于采样规划算法库。
为什么选择OMPL？OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一，很多新算法都在OMPL上开发。很多其他软件（ROS/MoveIt）都使用OMPL做运动规划。

 

做机械臂导航时遇到的问题8：逆运动学求解：在笛卡尔空间规划

http://blog.csdn.net/weixin_39579805/article/details/78968702  详细介绍整个从生成urdf文件到运动成功的过程。

以上部分参考于 ROS 运动规划 (MotionPlanning): MoveIt! 与OMPL

 http://blog.csdn.net/improve100/article/details/50619925

 

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-exploringRandom 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)。p是终端位姿(xyz)，q是关节角度 position。

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

 

而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!还结合了其他一些功能，总之MoveIt!就是个大接口。。 

·      MoveIt!能做什么？一句话，MoveIt!就是一个模块化的接口，让你在最短时间内，不用自己写太多代码，就能配置出一个ROSPackage来为你的机械臂做运动规划。

 

 

 

 

 

如何创建 URDFpackage

 

准备URDF package 
首先我们要准备一个机械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手头没有现成的URDF，可以从此处下载一个库卡LWR简化模型URDF，这是一个固定底座7自由度的机械臂。 
从该连接处依次进入examples/sovlers/ik_solver_demo/resources，下载里面的lwr_simplified.urdf。 

复制代码

1 2 3 4

cd path_to_catkin_ws/src catkin_create_pkg lwr_description cd lwr_description mkdir urdf

将下载好的lwr_simplified.urdf放入urdf文件夹中，这样一个urdf package便创建好了。 

 

MoveIt 配置包详解 
打开刚刚创建好的lwr_moveit_config文件夹，我们发现有config和launch两个文件夹。

 

3.1 MoveIt! 配置文件先看config，里面有

·      fake_controllers.yaml：这是虚拟控制器配置文件，方便我们在没有实体机器人，甚至没有任何模拟器（如gazebo）开启的情况下也能运行MoveIt。

·      joint_limits.yaml：这里记录了机器人各个关节的位置速度加速度的极限，这些都会被用于以后的规划中。

·      kinematics.yaml：这里就是上一章2.2.4里面设置的东西，用于初始化运动学求解库

·      lwr.srdf：这个是一个重要的MoveIt配置文件，我们将在下一节详解。

·      ompl_planning.yaml：这里是配置OMPL各种算法的各种参数。

 

3.3.1 demo.launch 
demo是运行的总结点，打开我们可以看到他include了其他的launch文件。

其中第14行说，如果有需要，发布静态的tf。比如说，你的机器人基座不在世界坐标的原点，你可以发布一个静态tf来描述机器人在世界坐标中的位置。

第17-21行，就是我们发布虚拟机器人状态的地方了，当然，如果你有实体机器人或者有gazebo之类的模拟器，你需要去掉这一部分，有其他相应的节点来发布机器人状态。

26-32行运行了另一个moveit重要的节点，move group。

 

3.3.2 move_group.launch 
顾名思义，move group的功能是让一个规划组群动起来。怎么动，那就要做运动规划了，

在move_group.launch第24-26行定义了运动规划库的使用，我们可以看到，默认的是使用ompl运动规划库。

 

3.3.3 planning_context.launch
这里我们可以看到，定义了所使用的urdf和srdf文件，以及运动学求解库。不建议手动更改这些，但是如果你需要使用不同的urdf，srdf，可以在这里更改。

 

 

 

对于自己生产的moveit launch 配置包，可以做一些自己个性化的修改

 

3.3.4 setup_assistant.launch
如果你需要更改一些配置，那么可以直接运行

复制代码

1

roslaunch lwr_moveit_config setup_assistant.launch

这样就可以基于当前设置做更改，而不是重新设置。

报错出现

Fail: ABORTED: No motion plan found. Noexecution attempted" and at terminal of rviz it give

ros  官方的解答

 https://answers.ros.org/question/240723/fail-aborted-no-motion-plan-found-no-execution-attempted/

 

出现报错可能的三种原因

1、关节位置达不到

2、算法库有问题，换成ikfast  的算法库

3、更换plannerid

 

pose1 might simply be unreachable by the arm -> use a pose of whichyou know that it is reachable

·     pose1 is reachable but the inverse kinematics plugin failed to find ajoint configuration that results in the end-effector being near that pose ->use a different IK or different parameters (trac_ik or a specialized ikfastperform good)

·     The planner failed to find apath to the goal -> is there a path by which you can move from the currentconfiguration to the goal configuration (within the specified software limits)

·     if there are paths the plannershould be able to find, you might want to try a different planner -> e.g. group.setPlannerId("RRTConnectkConfigDefault")

·     the planner might find the pathwith more time available -> group.setPlanningTime()

 

 

 

 

机械臂轨迹规划——三次样条对比研究

http://www.guyuehome.com/752

 

在pipeline的前级，运动规划器根据环境计算得出一条空间轨迹，但是这条轨迹只包含位置关系，并没有告诉机器人应该以怎样的速度、加速度运动，于是，

就要经过AddTimeParameterization这个模块，为这条空间轨迹进行速度、加速度约束，并且计算运动到每个路点的时间。

可以去看MoveIt中AddTimeParameterization模块的的代码实现（moveit_core\trajectory_processing\src\iterative_time_parameterization.cpp），大概了解模块的运行过程，但是像我这样的算法渣渣，实在看不懂它这么实现的原理，于是就去google这个算法，终于找到了这个算法的作者。

 

这个算法的全称是Time-Optimal Path Parameterization Algorithm，简称TOPP，是不是听上去感觉就很NB。

关于这个算法的原理，

作者是南阳理工的Quang-CuongPham

作者发布了一篇论文 «Ageneral, fast, and robust implementation of the time-optimal pathparameterization algorithm»，

算法的原始实现在github上：https://github.com/quangounet/TOPP，但是本人数学渣实在看不明白那么多的公式横飞，

 

这个算法十分酷炫，只需要输入一条路径所有路点的位置信息，就可以根据时间最优的原则，输出所有点的速度、加速度、时间信息。

这个算法也不是无敌的，其中有一个关键问题就是加速度存在抖动的问题，可以参见github上的讨论：https://github.com/ros-planning/moveit/issues/160。

 

样条插值是一种工业设计中常用的、得到平滑曲线的一种插值方法，三次样条又是其中用的较为广泛的一种。三次样条的数学定义如下：

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

三次样条方程组的推导和求解公式这里就不详述了，可以参考《数学分析》（李庆扬）的2.6节和5.3节。重要的是来看一下三次样条插补的效果如何，我们取机器人一轴的路径数据作为实验样本，来看一下三次样条的效果如何。

 

      Matlab中提供了spline box，其中包含了三次样条的实现，我们先在Matlab里边看一下效果如何。Matlab实现的代码如下：

 

%三次样条测试

1.%原始数据点

2.X = [0, 0.422955130,0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131,1.274332912, 1.409208218, 1.585026197, 2];

3.Y = [0, 0.14881055128822188,0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358,0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498,1.4880380234595283, 1.6368410759230068];

4.s =csapi(X,Y);   %三次样条

5.fnplt(s, 'r');         %绘制样条曲线

6.hold on

7.plot(X,Y,'o')       %绘制原始数据点

8.v=fnder(s,1);     %样条曲线一阶导数得到速度曲线

9.fnplt(v, 'g');        %绘制速度曲线

10.hold on

11.a=fnder(v,1);     %样条曲线二阶导数得到加速度曲线

12.fnplt(a, 'b');        %绘制加速度曲线

13.     legend('位置样条曲线','原始数据点','速度曲线','加速度曲线')

接下来就是要自己实现了，基于的原理就是《数值分析》上边讲到的

 

参考了网上众多实现的代码。二话不说，先上代码：

      头文件cubicSpline.h：

 

#ifndef_CUBIC_SPLINE_H_

1.  #define _CUBIC_SPLINE_H_

2.   

3.  class cubicSpline

4.  {

5.  public:

6.      typedef enum_BoundType

7.      {

8.          BoundType_First_Derivative,

9.          BoundType_Second_Derivative

10.         }BoundType;

11.      

12.     public:

13.         cubicSpline();

14.         ~cubicSpline();

15.      

16.         void initParam();

17.         void releaseMem();

18.      

19.         bool loadData(double *x_data, double *y_data, int count, doublebound1, double bound2, BoundType type);

20.         bool getYbyX(double &x_in, double &y_out);

 

21.     protected:

22.         bool spline(BoundType type);

23.      

24.     protected:

25.         double *x_sample_, *y_sample_;

26.         double *M_;

27.         int sample_count_;

28.         double bound1_, bound2_;

29.     };

30.      

31.     #endif /* _CUBIC_SPLINE_H_ */

 

源文件pcubicSpline.cpp：  http://www.guyuehome.com/752

 

 

 

 

 

 

 

空间三点如何做三次样条的插值

http://bbs.csdn.net/topics/390769094

 

(1)
U = At^3+Bt^2+Ct+D
这是一根参数曲线，U可以理解成d分量的向量，t是参数, 取值范围取[0, 1]
你要空间，那d=3，U = (x,y,z),  有ABCD也就是4个3分量的向量，i.e. 12个未知量
约束条件是两个定点P1, P2（2*3），两个导数向量D1, D2（2*3），也是12个 所以首先可以说明你的问题可以求出定解

(2)
U = At^3+Bt^2+Ct+D
dy/dt = 3At*2 + 2Bt + C
对于1到d的每一个维度i，可以把你的定点和导数对应的维度分量带入: 
U(0) = P1, U(1) = P2, U'(0) = k*D1, U'(1) = k*D2
实际求得时候，就是x(0) = P1.x, x'(0) = D1.x, x(1) = P2.x, x'(1) = D2.x 联立求解得到A.x, B.x, C.x, D.x
然后再作y, z
得到曲线之后，对t在01区间采样，用opengl话(U.x, U.y, U.z)的折线段

(3)
因为你只有4个约束，三次样条退化成了一个空间三次多项式曲线；如果你有更多的约束，那次数不提高的情况下，函数曲线需要在中间分段（但保证2阶连续），这才是三次样条。上面有个k，那个和你提取导数向量的大小，你随便取取吧

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

ROS源码阅读---局部路径规划之DWAPlannerROS分析

内含详细的源代码解读但无实际的例子

https://www.cnblogs.com/sakabatou/p/8297479.html