sympy 符号计算库在机器人旋转矩阵方面的应用（代码）
sympy 符号计算库在机器人旋转矩阵方面的应用（代码）
机器人旋转变换
一、变换矩阵
若计算变换矩阵是基于旋转坐标系（上一个坐标系）来计算的，则计算过程用右乘；若计算变换矩阵是基于固定坐标系（初始坐标系）来计算的，则计算过程用左乘，同时，若已知变换矩阵计算坐标系中某点坐标，也使用左乘。
以下代码是在Jupyter中运行的，同时也是基于旋转坐标系的公式推导，先Z再Y最后Z。
from sympy import sin, cos, pprint
x,y,z=sym.symbols('x,y,z')
Q_z=sym.Matrix([[cos(x),-sin(x),0],[sin(x),cos(x),0],[0,0,1]])
Q_y_1=sym.Matrix([[cos(y),0,sin(y)],[0,1,0],[-sin(y),0,cos(y)]])
Q_z_2=sym.Matrix([[cos(z),-sin(z),0],[sin(z),cos(z),0],[0,0,1]])
Q=Q_z*Q_y_1*Q_z_2
pprint(Q)
二、证明
1、det(Q)=1
from sympy import pi,sin,cos,pprint,Symbol,simplify
x=Symbol('x')
Q=sym.Matrix([[cos(x),-sin(x),0],[sin(x),cos(x),0],[0,0,1]])
Q_det=sym.det(Q)
Q_det=sym.simplify(Q_det)
print('---------------------')
print('Q=')
pprint(Q)
print('---------------------')
print('|Q|=')
print(Q_det)
2、^-1= ^T
from sympy import pi,sin,cos,pprint,Symbol,simplify
x=Symbol('x')
Q=sym.Matrix([[cos(x),-sin(x),0],[sin(x),cos(x),0],[0,0,1]])
Q_Inverse=sym.simplify(Q**-1)
Q_Tr=Q.T
print('---------------------')
print('Q=')
pprint(Q)
print('---------------------')
print('Q^-1=')
pprint(Q_Inverse)
print('---------------------')
print('Q^T=')
pprint(Q_Tr)
3、′ = ^
from sympy import sin,cos,pprint,simplify,Symbol,eye
x=Symbol('x')
Q=sym.Matrix([[cos(x),-sin(x),0],[sin(x),cos(x),0],[0,0,1]])
Q_Tr=Q.T
I=eye(3)
P=W*(Q**-1)
P=simplify(P)
print('---------------------')
print('Q=')
pprint(Q)
print('---------------------')
print('I=')
pprint(I)
print('---------------------')
print('Q*P=I')
print('P=')
pprint(P)
print('---------------------')
print('Q^T=')
pprint(Q_Tr)
4、^-1= ^T（普遍存在）
对于绕着任意轴（X轴、Y轴、Z轴）旋转α角度的初等变换均满足以上等式。
from sympy import sin,cos,pprint,simplify,Symbol,eye
u,v,w=sym.symbols('u,v,w')
Q_x=sym.Matrix([[1,0,0],[0,cos(u),-sin(u)],[0,sin(u),cos(u)]])
Q_y=sym.Matrix([[cos(v),0,sin(v)],[0,1,0],[-sin(v),0,cos(v)]])
Q_z=sym.Matrix([[cos(w),-sin(w),0],[sin(w),cos(w),0],[0,0,1]])
Q_x_Inverse=sym.simplify(Q_x**-1)
Q_y_Inverse=sym.simplify(Q_y**-1)
Q_z_Inverse=sym.simplify(Q_z**-1)
Q_x_Tr=Q_x.T
Q_y_Tr=Q_y.T
Q_z_Tr=Q_z.T
print('---------------------')
print('Q_x=')
pprint(Q_x)
print('Q_x^-1=')
pprint(Q_x_Inverse)
print('Q_x^T=')
pprint(Q_x_Tr)
print('---------------------')
print('Q_y=')
pprint(Q_y)
print('Q_y^-1=')
pprint(Q_y_Inverse)
print('Q_y^T=')
pprint(Q_y_Tr)
print('---------------------')
print('Q_z=')
pprint(Q_z)
print('Q_z^-1=')
pprint(Q_z_Inverse)
print('Q_z^T=')
pprint(Q_z_Tr)
5、 ≠
一般情况下，矩阵的乘法是不满足交换律的，若多次旋转均基于上一坐标系的同一轴（比如：5次旋转，但是都是基于上一坐标系的Z轴转动），则结果满足 = 。
若多次旋转中只要有一次转动的轴发生变换，则左乘和右乘的结果是不一样的，即 ≠ 。
from sympy import sin,cos,pprint,simplify,Symbol,eye
u,v=sym.symbols('u,v')
Q_x=sym.Matrix([[1,0,0],[0,cos(u),-sin(u)],[0,sin(u),cos(u)]])
Q_y=sym.Matrix([[cos(v),0,sin(v)],[0,1,0],[-sin(v),0,cos(v)]])
Q_1=Q_x*Q_y
Q_2=Q_y*Q_x
print('Q_x*Q_y=')
pprint(Q_1)
print('Q_y*Q_x=')
pprint(Q_2)