机器人点位姿绕指定轴旋转计算

位姿绕任一指定轴旋转指定角度后的位姿计算

问题：位姿p(pz，py，px)（坐标值为p0）绕指定轴q（坐标q0加方向（单位向量qn））旋转角度A如何计算?

1.首先已q建立任一坐标系，不妨设局部坐标系O1，原点为q0，Oz = qn，Ox为垂直与Oz的任一单位向量，Oz叉乘Ox得到Oy。局部坐标系旋转矩阵为Mo = (Ox，Oy，Oz，q0)，矩阵求逆得到矩阵Mo-1。

2. 计算局部坐标系O1下，p的位姿p1(Pz,Py,Px)。计算方法如下：

（1）P0 = Mo-1*(p0)。

（2）设z0 =(0,0,0)，设Z0 = Mo-1*(z0),则：

Px = Mo-1*(px)-Z0

Py = Mo-1*(py)-Z0

Px = Mo-1*(pz)-Z0

3. 那么这个问题转化为在局部坐标系下，p1绕(0,0,1)旋转角度A的问题。

绕z轴旋转角度A的旋转矩阵：

Mz =

则：

P0 = Mz*P0

Px = Mz*Px

Py = Mz*Py

Pz = Mz*Pz

4. 转换到世界坐标系下，设z0 =(0,0,0)，则：

P0 = Mo * P0 

Px = Mo * (Px - z0)

Py = Mo * (Py - z0)

Pz = Mo * (Pz - z0)