关于4轴5轴加工中心坐标旋转方法

admin

关于坐标旋转方法介绍：

关键词：空间旋转、旋转轴

用途：相机位姿估计、无人机位姿估计、3D游戏、3D建模

文章类型：概念、公式总结（本文不带推导过程，若想了解公式是如何推出来的请搜索文献），C++函数展示

@Date:2016-11-04

@Lab: CvLab202@CSU

写在前面的一些概念右手系

关于这个概念，搞3D的人应该都懂，而像我这样做图像处理的可能就对这个知道的比较少了。右手系这个概念其实很简单，看图就懂了。在坐标系中，右手摆成下图的样子，当拇指指向X轴食指指向Y轴时，中指指向了Z轴，满足这个条件的坐标系就是右手系。本文所有概念都在右手系下进行讨论。

右手系

旋转90°到底是怎么转

当我要让一个点，绕Y轴转动了90°，并且用程序计算出了旋转结果，为了验证这个点是否旋转正确，我们需要知道这个90°是怎么转的。在网上搜索了挺多文章，都没有对这个东西进行明确的定义，那么这里给出我的总结。从原点（0,0,0）往Y轴方向看，此时视野中的坐标系降维到二维坐标系XOZ，那么让点绕O点顺时针转90°，即为正确的旋转结果。

问题一：XYZ空间内某点绕X、Y、Z轴旋转一次

这个问题比较简单，网上已经有较多总结：

设旋转前坐标为，旋转后坐标为。

1.绕Z轴旋转γ角

首先给出向量表示：

<span id="MathJax-Element-1-Frame" class="mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="[x′,y′,z′,1]=[x,y,z,1][cos⁡γsin⁡γ00−sin⁡γcos⁡γ0000100001]" role="presentation" style="padding-top: 1px; padding-bottom: 1px; display: inline-block; line-height: 0; font-size: 21.96px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 0px; min-height: 0px; border: 0px; position: relative;">[x′,y′,z′,1]=[x,y,z,1]⎡⎢⎢⎢⎢⎣cosγsinγ00−sinγcosγ0000100001⎤⎥⎥⎥⎥⎦[𝑥′,𝑦′,𝑧′,1]=[𝑥,𝑦,𝑧,1][cos⁡𝛾sin⁡𝛾00−sin⁡𝛾cos⁡𝛾0000100001]

然后是公式表示：

<span id="MathJax-Element-2-Frame" class="mjx-full-width mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="(1)x′=cosγ⋅x−sinγ⋅y(2)y′=sin⁡γ⋅x+coγ⋅y(3)z′=z" role="presentation" style="padding-top: 1px; padding-bottom: 1px; line-height: 0; font-size: 21.96px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 9.758em; min-height: 0px; border: 0px; width: 10000em; position: relative; display: table-cell !important;">x′=cosγ⋅x−sinγ⋅yy′=sinγ⋅x+coγ⋅yz′=z(1)(2)(3)(1)𝑥′=𝑐𝑜𝑠𝛾⋅𝑥−𝑠𝑖𝑛𝛾⋅𝑦(2)𝑦′=sin⁡𝛾⋅𝑥+𝑐𝑜𝛾⋅𝑦(3)𝑧′=𝑧

最后是代码表示

1. <table width="98%" class="t_table"><tbody><tr><td>//将空间点绕Z轴旋转
   
2. //输入参数 x y为空间点原始x y坐标
   
3. //thetaz为空间点绕Z轴旋转多少度，角度制范围在-180到180
   
4. //outx outy为旋转后的结果坐标
   
5. void codeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
   
6. {
   
7.     double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
   
8.     double y1 = y;
   
9.     double rz = thetaz * CV_PI / 180;
   
10.     outx = cos(rz) * x1 - sin(rz) * y1;
    
11.     outy = sin(rz) * x1 + cos(rz) * y1;
    
12. 
    
13. }
    
14. </td></tr></tbody></table>

复制代码

[/table]


2.绕Y轴旋转β角

首先给出向量表示：

<span id="MathJax-Element-3-Frame" class="mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="[x′,y′,z′,1]=[x,y,z,1][cos⁡β0−sin⁡β00100sin⁡β0cos⁡β00001]" role="presentation" style="padding-top: 1px; padding-bottom: 1px; display: inline-block; line-height: 0; font-size: 21.96px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 0px; min-height: 0px; border: 0px; position: relative;">[x′,y′,z′,1]=[x,y,z,1]⎡⎢⎢⎢⎣cosβ0−sinβ00100sinβ0cosβ00001⎤⎥⎥⎥⎦[𝑥′,𝑦′,𝑧′,1]=[𝑥,𝑦,𝑧,1][cos⁡𝛽0−sin⁡𝛽00100sin⁡𝛽0cos⁡𝛽00001]

然后是公式表示：

<span id="MathJax-Element-4-Frame" class="mjx-full-width mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="(4)x′=cosβ⋅x+sinβ⋅z(5)y′=y(6)z′=−sin⁡β⋅x+cos⁡β⋅z" role="presentation" style="padding-top: 1px; padding-bottom: 1px; line-height: 0; font-size: 21.96px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 10.78em; min-height: 0px; border: 0px; width: 10000em; position: relative; display: table-cell !important;">x′=cosβ⋅x+sinβ⋅zy′=yz′=−sinβ⋅x+cosβ⋅z(4)(5)(6)(4)𝑥′=𝑐𝑜𝑠𝛽⋅𝑥+𝑠𝑖𝑛𝛽⋅𝑧(5)𝑦′=𝑦(6)𝑧′=−sin⁡𝛽⋅𝑥+cos⁡𝛽⋅𝑧

最后是代码表示

1. <table width="98%" class="t_table"><tbody><tr><td>//将空间点绕Y轴旋转
   
2. //输入参数 x z为空间点原始x z坐标
   
3. //thetay为空间点绕Y轴旋转多少度，角度制范围在-180到180
   
4. //outx outz为旋转后的结果坐标
   
5. void codeRotateByY(double x, double z, double thetay, double& outx, double& outz)
   
6. {
   
7.     double x1 = x;
   
8.     double z1 = z;
   
9.     double ry = thetay * CV_PI / 180;
   
10.     outx = cos(ry) * x1 + sin(ry) * z1;
    
11.     outz = cos(ry) * z1 - sin(ry) * x1;
    
12. }</td></tr></tbody></table>

复制代码

[table=98%,none]





3.绕X轴旋转α角

首先给出向量表示：

<span id="MathJax-Element-5-Frame" class="mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="[x′,y′,z′,1]=[x,y,z,1][10000cos⁡αsin⁡α00−sin⁡αcos⁡α00001]" role="presentation" style="padding-top: 1px; padding-bottom: 1px; display: inline-block; line-height: 0; font-size: 21.96px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 0px; min-height: 0px; border: 0px; position: relative;">[x′,y′,z′,1]=[x,y,z,1]⎡⎢⎢⎢⎣10000cosαsinα00−sinαcosα00001⎤⎥⎥⎥⎦[𝑥′,𝑦′,𝑧′,1]=[𝑥,𝑦,𝑧,1][10000cos⁡𝛼sin⁡𝛼00−sin⁡𝛼cos⁡𝛼00001]

然后是公式表示：

<span id="MathJax-Element-6-Frame" class="mjx-full-width mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="(7)x′=x(8)y′=cos⁡α⋅y−sin⁡α⋅z(9)z′=sin⁡α⋅y+sin⁡α⋅z" role="presentation" style="padding-top: 1px; padding-bottom: 1px; line-height: 0; font-size: 21.96px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 9.922em; min-height: 0px; border: 0px; width: 10000em; position: relative; display: table-cell !important;">x′=xy′=cosα⋅y−sinα⋅zz′=sinα⋅y+sinα⋅z(7)(8)(9)(7)𝑥′=𝑥(8)𝑦′=cos⁡𝛼⋅𝑦−sin⁡𝛼⋅𝑧(9)𝑧′=sin⁡𝛼⋅𝑦+sin⁡𝛼⋅𝑧

最后是代码表示

//将空间点绕X轴旋转 //输入参数 y z为空间点原始y z坐标 //thetax为空间点绕X轴旋转多少度，角度制范围在-180到180 //outy outz为旋转后的结果坐标 void codeRotateByX(double y, double z, double thetax, double& outy, double& outz) {     double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确     double z1 = z;     double rx = thetax * CV_PI / 180;     outy = cos(rx) * y1 - sin(rx) * z1;     outz = cos(rx) * z1 + sin(rx) * y1; } 复制代码

问题二：空间点绕任意轴旋转

首先，需要定义"任意轴"的单位向量，例如X轴可以用向量来表示。

那么假设旋转轴的单位向量为，旋转前坐标为，旋转后坐标为，旋转角为，于是有：

<span id="MathJax-Element-7-Frame" class="mjx-full-width mjx-chtml MathJax_CHTML" tabindex="0" data-mathml="(10)x′=(vx⋅vx⋅(1−cosθ)+cosθ)⋅x+(vx⋅vy⋅(1−cosθ)−vz⋅sinθ)⋅y+(vx⋅vz⋅(1−cosθ)+vy⋅sinθ)⋅z(11)y′=(vx⋅vy⋅(1−cosθ)+vz⋅sinθ)⋅x+(vy⋅vy⋅(1−cosθ)+cosθ)⋅y+(vy⋅vz⋅(1−cosθ)−vx⋅sinθ)⋅z(12)z′=(vx⋅vz⋅(1−cos⁡θ)−vy⋅sin⁡θ)⋅x+(vy⋅vz⋅(1−cos⁡θ)+vx⋅sin⁡θ)⋅y+(vz⋅vz⋅(1−cos⁡θ)+cos⁡θ)⋅z" role="presentation" style="padding-top: 1px; padding-bottom: 1px; line-height: 0; font-size: 13.4667px; overflow-wrap: normal; word-spacing: normal; float: none; direction: ltr; max-width: none; max-height: none; min-width: 47.023em; min-height: 0px; border: 0px; width: 10000em; position: relative; display: table-cell !important;">x′=(vx⋅vx⋅(1−cosθ)+cosθ)⋅x+(vx⋅vy⋅(1−cosθ)−vz⋅sinθ)⋅y+(vx⋅vz⋅(1−cosθ)+vy⋅sinθ)⋅zy′=(vx⋅vy⋅(1−cosθ)+vz⋅sinθ)⋅x+(vy⋅vy⋅(1−cosθ)+cosθ)⋅y+(vy⋅vz⋅(1−cosθ)−vx⋅sinθ)⋅zz′=(vx⋅vz⋅(1−cosθ)−vy⋅sinθ)⋅x+(vy⋅vz⋅(1−cosθ)+vx⋅sinθ)⋅y+(vz⋅vz⋅(1−cosθ)+cosθ)⋅z(10)(11)(12)(10)𝑥′=(𝑣𝑥⋅𝑣𝑥⋅(1−𝑐𝑜𝑠𝜃)+𝑐𝑜𝑠𝜃)⋅𝑥+(𝑣𝑥⋅𝑣𝑦⋅(1−𝑐𝑜𝑠𝜃)−𝑣𝑧⋅𝑠𝑖𝑛𝜃)⋅𝑦+(𝑣𝑥⋅𝑣𝑧⋅(1−𝑐𝑜𝑠𝜃)+𝑣𝑦⋅𝑠𝑖𝑛𝜃)⋅𝑧(11)𝑦′=(𝑣𝑥⋅𝑣𝑦⋅(1−𝑐𝑜𝑠𝜃)+𝑣𝑧⋅𝑠𝑖𝑛𝜃)⋅𝑥+(𝑣𝑦⋅𝑣𝑦⋅(1−𝑐𝑜𝑠𝜃)+𝑐𝑜𝑠𝜃)⋅𝑦+(𝑣𝑦⋅𝑣𝑧⋅(1−𝑐𝑜𝑠𝜃)−𝑣𝑥⋅𝑠𝑖𝑛𝜃)⋅𝑧(12)𝑧′=(𝑣𝑥⋅𝑣𝑧⋅(1−cos⁡𝜃)−𝑣𝑦⋅sin⁡𝜃)⋅𝑥+(𝑣𝑦⋅𝑣𝑧⋅(1−cos⁡𝜃)+𝑣𝑥⋅sin⁡𝜃)⋅𝑦+(𝑣𝑧⋅𝑣𝑧⋅(1−cos⁡𝜃)+cos⁡𝜃)⋅𝑧

计算时照着公式代入即可。

最后给出代码实现：

//定义返回结构体 struct Point3f {     Point3f(double _x, double _y, double _z)     {         x = _x;         y = _y;         z = _z;     }     double x;     double y;     double z; }; //点绕任意向量旋转，右手系 //输入参数old_x，old_y，old_z为旋转前空间点的坐标 //vx，vy，vz为旋转轴向量 //theta为旋转角度角度制，范围在-180到180 //返回值为旋转后坐标点 Point3f RotateByVector(double old_x, double old_y, double old_z, double vx, double vy, double vz, double theta) {     double r = theta * CV_PI / 180;     double c = cos(r);     double s = sin(r);     double new_x = (vx*vx*(1 - c) + c) * old_x + (vx*vy*(1 - c) - vz*s) * old_y + (vx*vz*(1 - c) + vy*s) * old_z;     double new_y = (vy*vx*(1 - c) + vz*s) * old_x + (vy*vy*(1 - c) + c) * old_y + (vy*vz*(1 - c) - vx*s) * old_z;     double new_z = (vx*vz*(1 - c) - vy*s) * old_x + (vy*vz*(1 - c) + vx*s) * old_y + (vz*vz*(1 - c) + c) * old_z;     return Point3f(new_x, new_y, new_z); } 复制代码

问题三：空间点绕xyz轴连续旋转

前面的问题比较基础，到这个问题就需要一点空间想象力了。

首先我假设一个点绕x、y、z轴旋转90°，最终它会落在哪里？这个答案不是唯一的，因为旋转的顺序将会影响到最终的结果。

以点(1,2,3)为例

A 我让它首先绕x轴转90°，再绕y轴转90°，再绕z轴转90°。

1. <table width="98%" class="t_table"><tbody><tr><td>double x = 1, y = 2, z = 3;
   
2. codeRotateByX(y, z, 90, y, z);
   
3. codeRotateByY(x, z, -90, x, z);
   
4. codeRotateByZ(x, y, -90, x, y);
   
5. cout << endl << "   (1,2,3) -> (" << x << ',' << y << ',' << z << ")" << endl << endl;</td></tr></tbody></table>

复制代码

旋转结果是：

B 这一次我让它首先绕z轴转90°，再绕y轴转90°，最后绕z轴转90°。

1. <table width="98%" class="t_table"><tbody><tr><td>double x = 1, y = 2, z = 3;
   
2. codeRotateByZ(x, y, -90, x, y);
   
3. codeRotateByY(x, z, -90, x, z);
   
4. codeRotateByX(y, z, 90, y, z);
   
5. cout << endl << "   (1,2,3) -> (" << x << ',' << y << ',' << z << ")" << endl << endl;</td></tr></tbody></table>

复制代码

这次的结果是：

显然，不同的旋转顺序导致了结果的不同，因此在处理空间内绕轴旋转的问题时，我们需要严格定义每次旋转的顺序，否则会导致错误的答案。