三轴旋转的最优路径规划

在干扰观测器系列中再插播一条原创算法。

利用罗德里格斯旋转和TD微分跟踪器实现最优旋转规划。

对于一维的给定的规划，在前篇中已经介绍了（如下链接），如果是三维空间中呢？例如常见的三轴旋转，对每一个单轴都进行给定的优化肯定不是最优的，因此对三轴的统一路径规划。

FrancisZhao：PID的TRICK(三)对给定值的处理：给定的滤波、路径规划、安排过渡过程31 赞同 · 4 评论文章

本算法可以应用在无人机的姿态/角度环的给定值规划，卫星的姿态/角度环的给定值规划，云台的姿态/角度环的给定值规划，转台的姿态/角度环的给定值规划。

假设三轴的当前姿态的欧拉角为[0 0 0]，目标给定值为[ 50 40 90]，假设三轴的姿态环直接利用欧拉角，那么肯定是40给定的最快稳定，90给定的最慢稳定，这样在三轴的的旋转路径一定不是最短的，最短的路径是绕定轴，通过一次旋转完成。

前文中阐述利用TD微分跟踪器来规划给定值可以更快的达到稳定并且减少超调，因此在三维旋转中对给定的规划同样能达到相同的效果。但是不可对三个轴分别利用TD微分跟踪器。

因此，利用罗德里格斯旋转和TD微分跟踪器可以达到最优路径和最稳控制的目的。

具体步骤如下：

将目标欧拉角转化为四元数，最好直接利用目标与当前的偏差四元数，提取四元数的失部将其转化为轴角法（旋转向量）的表示方法，得到旋转轴n和旋转角度θ。

假设三轴能提供的最大力矩均为T，则可求出三边均为T的立方体与向量n的交点，交点的模便是TMAX，，若是三轴提供的最大力矩不同，则可以求六面体与向量n的交点的模。

因此TD 的输入量为TMAX和θ，实时的输出规划值为θnew，其值如下图所示。

之后利用向量n和θnew实时做罗德里格斯旋转， 则可得出实时规划的欧拉角或者偏差四元数。

给出本文提出的规划方式和三轴分别用TD规划的方式的对比图如下（红黄蓝三条线是本文的算法）。可知，当机动的角度越大，则节省的时间更多，可以超过30％，更关键的是这种规划是最优规划，节省能量。

本文中的的算法可以实时进行，输入是偏差的角度和三轴最大力矩，输出为规划后的偏差角度，用法与单轴的TD相同，简单方便。

如果看的人多了 ，便提供matlab程序。不过关键的步骤我都列出了，写起来也很简单。