二维旋转公式

ros的tf工具包可以很方便的实现任意坐标系之间的坐标转换。但是，如果只是想简单的测试想法，而又不想编写过于庞杂的代码，考虑自己写二维旋转的函数。而与二维旋转问题对偶的另一个问题便是二维坐标系旋转变换。这两个问题的形式基本一样，只是旋转的角度相差一个负号。就是这个容易搞混，所以做个笔记，以备查用。

而（此文只针对二维）旋转则是表示某一坐标点 ( x 1 , y 1 ) (x_1, y_1) (x1​,y1​)某一坐标系下绕原点逆时针（正向）旋转角度 θ \theta θ后得到新的坐标点 ( x 2 , y 2 ) (x_2, y_2) (x2​,y2​)。

推导： 假定 v = ( x , y ) v=(x, y) v=(x,y), v ′ = ( x ′ , y ′ ) v'=(x',y') v′=(x′,y′),如上图有 x = r c o s ( ϕ ) , y = r s i n ( ϕ ) , x ′ = r c o s ( ϕ + θ ) , y ′ = r s i n ( ϕ + θ ) x=rcos(\phi),y=rsin(\phi),x'=rcos(\phi+\theta),y'=rsin(\phi+\theta) x=rcos(ϕ),y=rsin(ϕ),x′=rcos(ϕ+θ),y′=rsin(ϕ+θ)(注意，上图有几处错误，坐标轴边上的 c o s / s i n ( θ ) cos/sin(\theta) cos/sin(θ)应改为 c o s / s i n ( ϕ + θ cos/sin(\phi+\theta cos/sin(ϕ+θ)。展开 x ′ , y ′ x',y' x′,y′可得： x ′ = r c o s ( ϕ ) c o s ( θ ) − r s i n ( ϕ ) s i n ( θ ) = x c o s ( θ ) − y s i n ( θ ) x'=rcos(\phi)cos(\theta)-rsin(\phi)sin(\theta)=xcos(\theta)-ysin(\theta) x′=rcos(ϕ)cos(θ)−rsin(ϕ)sin(θ)=xcos(θ)−ysin(θ) y ′ = r s i n ( ϕ ) c o s ( θ ) + r c o s ( ϕ ) s i n ( θ ) = x s i n ( θ ) + y c o s ( θ ) y'=rsin(\phi)cos(\theta)+rcos(\phi)sin(\theta)=xsin(\theta)+ycos(\theta) y′=rsin(ϕ)cos(θ)+rcos(ϕ)sin(θ)=xsin(θ)+ycos(θ)

矩阵形式为: [ x ′ y ′ ] = [ c o s ( θ ) − s i n ( θ ) s i n ( θ ) c o s ( θ ) ] [ x y ] \left[\begin{matrix}x' \\ y'\end{matrix}\right]=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right]\left[\begin{matrix}x \\ y\end{matrix}\right] [x′y′​]=[cos(θ)sin(θ)​−sin(θ)cos(θ)​][xy​]

则二维旋转矩阵为： A = [ c o s ( θ ) − s i n ( θ ) s i n ( θ ) c o s ( θ ) ] (1) A=\left[\begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix}\right] \tag{1} A=[cos(θ)sin(θ)​−sin(θ)cos(θ)​](1)

假设有一坐标系 X O Y XOY XOY，经过逆时针(正向)旋转角度 θ \theta θ后，得到新的坐标系 X ′ O ‘ Y ′ X'O‘Y' X′O‘Y′。得到原来坐标系中的坐标 ( x , y ) (x,y) (x,y)在新坐标系下的坐标值被称为坐标系转换。

x ′ = x c o s ( θ ) + y s i n ( θ ) = x c o s ( − θ ) − y s i n ( − θ ) x'=xcos(\theta)+ysin(\theta)=xcos(-\theta)-ysin(-\theta) x′=xcos(θ)+ysin(θ)=xcos(−θ)−ysin(−θ) y ′ = − x s i n ( θ ) + y c o s ( θ ) = x s i n ( − θ ) + y c o s ( − θ ) y'=-xsin(\theta)+ycos(\theta)=xsin(-\theta)+ycos(-\theta) y′=−xsin(θ)+ycos(θ)=xsin(−θ)+ycos(−θ)

所以二维坐标旋转变换矩阵为: B = [ c o s ( θ ) s i n ( θ ) − s i n ( θ ) c o s ( θ ) ] = [ c o s ( − θ ) − s i n ( − θ ) s i n ( − θ ) c o s ( − θ ) ] (2) B=\left[\begin{matrix} cos(\theta) & sin(\theta) \\ -sin(\theta) & cos(\theta)\end{matrix}\right]=\left[\begin{matrix} cos(-\theta) & -sin(-\theta) \\ sin(-\theta) & cos(-\theta)\end{matrix}\right] \tag{2} B=[cos(θ)−sin(θ)​sin(θ)cos(θ)​]=[cos(−θ)sin(−θ)​−sin(−θ)cos(−θ)​](2)

对比公式（1）与（2），可发现，二维旋转与二维坐标旋转形式一致，只是当旋转都为正向（逆时针）时，角度相差一个负号。也即，在同一坐标轴下将某一点 ( x , y ) (x,y) (x,y)沿原点正向（逆时针）旋转角度 θ \theta θ后得到的新坐标点 ( x ′ , y ′ ) (x',y') (x′,y′)，等价于将点 ( x , y ) (x,y) (x,y)所在的坐标系 X O Y XOY XOY逆向(顺时针)旋转角度 θ \theta θ后，在新的坐标系 X ′ O ′ Y ′ X'O'Y' X′O′Y′下， ( x , y ) (x,y) (x,y)对应的新坐标点 ( x ′ , y ′ ) (x',y') (x′,y′)。拿起纸笔，多摆弄上面两张解释图，就清楚了。