旋转向量旋转矩阵求导
由旋转引起的平面角速度
角速度：
弧长：
线速度：
（因为求速度，所以ΔS是极小值，）
由旋转引起的空间角速度
已知角度度向量：
u是单位向量
显然，又有：
显然，又有半径r与P向量的关系：
又根据叉积的定义，有：
因此：
结论：空间点的线速度向量，是角速度向量与该点向量叉积。
旋转矩阵&指数矩阵
一定要有一个直觉， f ' (x) = A f (x) ， 那么一定和e有关。
那么，有没有可能：
R(t) 正是t时刻的旋转矩阵， 将一个向量p(0)转为p(t)
因为：
因此得证
又 ω 是角速度向量，那么：
对其进行积分，区间是[t,t+Δt]，就可以得到：
从而，证明了：
旋转矩阵的导数
已知绕单位向量u旋转θ，与旋转矩阵R，有如下关系：
μx 是单位向量μ的对应的反对称矩阵。
所以，总有一个旋转向量，与R有函数关系
因为R是随着时间变化而变化，令：
由旋转矩阵的性质，可知：
两边同时求导：
令：
那么，上面公式为：
因此：S是反对称矩阵
那么：
结论：旋转矩阵导数 = 反对称矩阵S * 旋转矩阵本身
已知：
对p0求导：
对p0求导，实质就是求p0处的速度：
因此，有：
所以，S就是角速度向量ω的反对称矩阵。
结论：
结论：R的导数 = 角速度向量的反对称矩阵 * R本身
上旋转矩阵的导数2
根据上面的结论，R‘ = ω X R
R = [ x , y ,z ] 三角列向量，那么
R‘ 就是三个单位正交向量的线速度
向量 ω00,2 表示，以0系为基准（左上角），2系相对于0系的角速度向量。
其中：
代入上式：
又，反对称矩阵有如下性质：
因此，上式变为：
代入：
由此，可以得到两个结论：
1. 角速度向量可以叠加，但是要以统一的坐标系作为参考。
2. 进行推广，有如下链式法则，可以看做 ：
p点相对于i - 1 系的角速度 = i系原点相对于 i - 1 系的角速度 + p 点相对于 i系的角速度
以此图为参考，已知：
p0 为P点在0系下的向量
p1 为P点在1系下的向量
o 为1系原点在0系下的向量
那么，他们有如下关系：
假设P点，是与1系固连的，那么P点在0系下的速度 = 1系旋转产生的速度 + 1系移动产生的速度
r 就是 P点在0系下的向量
机械臂的雅可比矩阵
机械臂旋转产生的总角速度 ，根据：
可知，末端关节，由旋转引起的角速度向量为：
（机械臂总是绕Z轴进行旋转）
令：
如果在第n个关机不是转动关机，那么角速度大小为0
总线速度
如果第n个关节，是想对于n-1个关节，沿着zn-1方向移动的速度为d‘n , 那么n关节处，产生的速度为：
由沿着z轴移动产生的总速度：
如果第n个关节，是相对于于n-1个关节，沿着zn-1方向旋转，其角速度为θn，那么n关节处，产生的速度为：
那么，由n - 1关节转动，对n关节产生的速度为：
其中：
由运动正算得到的：
由沿着z轴旋转产生的总速度：
总的线速度：
所以，写成矩阵形式：
特别注意：通常一个运动就是一个关节，同一个关机，d和θ其中一个要为0。
联立起来：
雅可比矩阵就是 6 * 2n那个阵。
运动旋量
已知
（这里假设b和s原点是重合的，为了方便显示，将b拉开）
已知Rsb的各列，ωs 都是s系下的向量，ωs 是旋转向量。
那么，有：
ωb 是旋转向量，以b系作为局部坐标系
使用反对称矩阵运算法则：
（ωs 和 Rsb ， p，都是以s为参考的向量）
ωs 和 Rsb 的等效旋转向量不是同方向的，Rsb 是旋转后的结果，而ωs 应该是这个结果后再次运动。
对Rsb求导，和之前的含义不一样，之前是旋转轴就是P向量。
假设运动不仅仅又旋转引起，那么：
ωb，就是ωs在b系下的向量表示。
p导数，本来是b原点的速度，在s系下的表示，而pb导数，就是b原点的速度，在b系下的表示。
物体运动旋量
描述了动坐标系的角速度向量，以及其线速度向量，在动坐标系下的表达。
再推如下公式：
如果
ps导数，是指s点的速度，在s系下的表达，那么
运动旋量所指的运动，应该如图：
1. s系和b系，作为一个整体，沿着向量 ωs方向运动，还绕ωs 进行旋转，速度为ps向量求导，因此：
s系原点的速度，s系原点的速度，由于不受旋转的影响，为：
2. 而b系，b点总运动速度，在s系下的表达，为：
（注意到，ωb = RsbT ωs 依然成立，因为这只是向量坐标系的转换，ωs 不是必须从b系原点出发）
空间运动旋量
物体运动旋量
（注意到，ωb 并不是已b系原点为旋转点的，从推导过程来看，ωb = Rsb ωs , 旋转点都是s原点，只是在b系下的表达而已）
空间运动旋量
运动旋量，描述了两个东西：
1. 旋转向量在该系下的表达。
2. 系原点的速度，在该系下的表达。
疑问：
Rsb 的推导过程中，s点与b点重合才有 Rsb 的导数（b系基向量的速度在s系下的表达），但是当s和b的原点不重合，那么Rsb的导数，应该如何算？
6维的运动旋量，能不能写成 ：
V(6维) = “6维向量 * 一个数”
研究b的运动，在s系下的运动旋量
为了达到 “6维向量 * 一个数” 这个目的，
s是移动的单位向量
令：
h被称为“节距”
那么，b的运动，在s系下的运动旋量，表示为：
当θ导数不为0
vs为沿旋转轴方向移动的速度。
当θ导数为0时，要写成“6维向量 * 一个数” , 那么：
（此时q是什么已经不重要，只有沿s方向运动）
运动旋量正则化后，剔除了速度，留下了方向
说明：
1. ||ω|| = 1或 || v || = 1
2. ||ω|| = 1 时，θ为角度求导，即角速度；
||v|| = 1时，θ为位移求导，即线速度
*运动旋量，就是要找到一个轴，把 “旋转 + 平移的运动的复合运动”，描述成绕这个轴旋转的一个运动。
*如果只有平移，那么这个旋转半径是无限大的。
为了描述这个运动，需要：
1. 表示这个轴的单位向量 ω
2. 知道了 ω，也要知道半径方向向量r
3. 还有旋转速度θ导数
对于固定坐标系中的两个点p0，p，如果向量 = p - p0，绕p0为原点进行旋转，可以：
将R替换成罗德里格斯公式：
令：
即：由旋转产生的位移向量
令：
（ ω是单位向量，v就是单位角速度下产生的速度）
有一个这样的运动：
1. 只考虑旋转， 不考虑沿旋转轴方向移动。
2. 旋转向量是单位向量 。
3. 那么对于运动旋量S，有：
v是速度的方向，但不是单位速度，ω是单位向量
令：
注意：这个和位姿矩阵T不是同一回事，这里是描述了一个旋转运动的过程
R则是将坐标系原点搬到p0点，然后旋转后的基向量
忘记前面的公式，记住这个图：
以下说那么多，都是非原点旋转的坐标变换矩阵T
罗德里格斯公式：
代入：
现在，有一个转换矩阵T，想要：
令：
将 p' = Tp 展开，R用罗德里格斯公式代替：
假设：
代入试试：
整来整去，其实就是想将：
(思想就是，将p点，变为以p0为原点的局部坐标（或者原点挪到s原点，称为s系下的向量），然后使用旋转矩阵变换（由于使用旋转矩阵R，所以必须是向量），然后再挪回去p0点处)
变为：
v其实就是描述了s随着一起旋转，s的原点的速度方向，要求速度，就
应用
两个位姿，必然有以下关系：
那么，转换矩阵Tscsb 除了转换矩阵的作用，如何用几何解析？
可以假设，p点经过旋转之后，变成p'点。
（p点和p'点，实际就是x轴上的点而已）
套用上面的公式
因此，Tscsb ，实际上就是R和G(θ)v组成的。
而R可以反向，根据罗德里格斯公式，推出旋转向量ω、旋转角度θ
https://blog.csdn.net/Sandy_WYM_/article/details/84309000
有了旋转向量ω、旋转角度θ后：
第4列 = G(θ)v
自然，旋转点q 也可以求出来
至此，可以得出一个结论：
使用q、ω、θ，可以将一个位姿，变化为另一个位姿，实现了：
“旋转 + 平移的运动的复合运动”，描述成绕这个轴旋转的一个运动
要是写得漂亮一点，那就：
这就是运动旋量，
思考：
1. 已知θ是角度，那旋量就可以用于求位姿
2. 如果θ是角速度，是不是可以很容易知道每个点的运动速度?
3. 如果是纯移动的运动，是不是通过计算Tscsb ，能否得到一个无限大的q?
