关于我随便敲了几行代码就验证了算法的那些事

引言

可能大家觉得这个题目有些奇怪，不过确实奇怪，哈哈哈。随便敲几行代码这个茶里茶气的说法是从我朋友那里得到的，觉得好玩，就起了一个这样的标题。

但是总归不能喧宾夺主，今天博客要讲的，是我之前会议论文中提到的一个关于“机器人姿态规划”方面的想法。

基于欧拉角---角速度的轨迹规划

这个算法的背景是：

当我们已知当前姿态，用R1表示，以及期望姿态，用R2表示。当然这里指的是机器人末端的姿态，那么我们想从R1变到R2，都需要怎么做呢？

按照一般的做法，是需要使用轨迹规划的算法，计算出中间点的姿态，然后控制机械臂完成这些中间点。

但是我就是想反其道而行之，那么我们换一种思路。

首先得到两个旋转之间的旋转变换矩阵，暂且用RP表示；

然后将其转换成欧拉角；

针对每个角度，写出当前时间步的角速度矢量，如[0，0，0.02]，即意味着这是以欧拉角中绕z轴旋转的分量得到的角速度矢量。对了，这个角速度矢量是怎么得到的呢？其实很简单啦，就是随便设置个阻尼系数，用z轴的欧拉角角度值，除以系数就可以；

下一步很重要，就是需要将旋转角速度通过正运动学的旋转部分，转换到机器人基坐标系下！！！因为我们速度控制是需要使用雅可比矩阵的，而雅可比矩阵进行速度控制时，给到的实际上是相对及基坐标系的速度。

最后就是每个周期中分三次控制角速度。

实验验证

我们随便写几行代码，框住的地方是主要核心思路。

第一个矩阵是目标姿态，第二个是初始姿态，第三个是最终调整后的姿态。



对应速度曲线

May the force be with you！