坐标变换

坐标变换在机器人领域十分常见，推导了几种从易到难的坐标转换：

1.二维平面绕原点旋转

 如图所示，O为原点，旋转角为$ heta$，P0、P1坐标分别为$(x_0,y_0)、(x_1,y_1)$，P0与X轴夹角为$alpha$，设P0和P1到原点距离为$r$，则P0坐标为：

egin{equation}
left{
egin{array}{lr}
x_0 = rcosalpha \
y_0 = rsinalpha 
end{array}
ight.
end{equation}

P1坐标为：

egin{equation}
left{
egin{array}{lr}
x_1 = rcos(alpha+ heta) = rcos hetacosalpha-rsin hetasinalpha \
y_1 = rsin(alpha+ heta) = rsin hetasinalpha+rcos hetasinalpha
end{array}
ight.
end{equation}

 将公式(1)带入到公式2,，消去$r$得

egin{equation}
left{
egin{array}{lr}
x_1 = x_0cos heta-y_0sin heta \
y_1 = x_0sin heta+y_0cos heta
end{array}
ight.
end{equation}

式(3)为由P0逆时针旋转至P1的表达式，也可通过该式求出由P1到P0的变换：

egin{equation}
left{
egin{array}{lr}
x_0 = -x_1sin heta+y_1cos heta \
y_0 = x_1cos heta+y_1sin heta
end{array}
ight.
end{equation}

也可以用矩阵来表示变换,逆时针旋转$ heta$：

egin{equation}
left[
egin{array}{lr}
x_1\
y_1
end{array}
ight ]
=
left[
egin{array}{lr}
cos heta&-sin heta \
sin heta&cos heta
end{array}
ight ]
left[
egin{array}{lr}
x_0\
y_0
end{array}
ight ]
end{equation}

顺时针旋转$ heta$：

egin{equation}
left[
egin{array}{lr}
x_0\
y_0
end{array}
ight ]
=
left[
egin{array}{lr}
-sin heta&cos heta \
cos heta&sin heta
end{array}
ight ]
left[
egin{array}{lr}
x_1\
y_1
end{array}
ight ]
end{equation}

 二者的系数矩阵相互可逆。

2.二维平面绕固定点旋转

 如上图所示，现在改为绕P点旋转，点到P的距离为$r$。推导的方法和1中一致，设P0与x轴正方向夹角为$alpha$。则P0坐标为：

egin{equation}
left{
egin{array}{lr}
x_0 = rcosalpha+x \
y_0 = rsinalpha+y 
end{array}
ight.
end{equation}

P1坐标为：

egin{equation}
left{
egin{array}{lr}
x_1 = rcos(alpha+ heta) +x = rcos hetacosalpha-rsin hetasinalpha+x \
y_1 = rsin(alpha+ heta) +x = rsin hetasinalpha+rcos hetasinalpha+y
end{array}
ight.
end{equation}

将(7)式代入(8)式，消去$r$得：

egin{equation}
left{
egin{array}{lr}
x_1 = (x_0-x)cos heta-(y_0-y)sin heta +x\
y_1 = (x_0-x)sin heta+(y_0-y)cos heta +y
end{array}
ight.
end{equation}

同理也可以由式(9)得到$x_0$、$y_0$的表达式

egin{equation}
left{
egin{array}{lr}
x_0 =-(x_1-x)sin heta+(y_1-y)cos heta+x \
y_0 = (x_1-x)cos heta+(y_1-y)sin heta+y
end{array}
ight.
end{equation}

使用矩阵来表示，逆时针旋转$ heta$：

egin{equation}
left[
egin{array}{lr}
x_1\
y_1
end{array}
ight ]
=
left[
egin{array}{lr}
cos heta&-sin heta \
sin heta&cos heta
end{array}
ight ]
left[
egin{array}{lr}
x_0-x\
y_0-y
end{array}
ight ]
+
left[
egin{array}{lr}
x\
y
end{array}
ight ]
end{equation}

顺时针旋转$ heta$：

egin{equation}
left[
egin{array}{lr}
x_0\
y_0
end{array}
ight ]
=
left[
egin{array}{lr}
-sin heta&cos heta \
cos heta&sin heta
end{array}
ight ]
left[
egin{array}{lr}
x_1-x\
y_1-y
end{array}
ight ]
+
left[
egin{array}{lr}
x\
y
end{array}
ight ]
end{equation}