工业机器人末端姿态表示-欧拉角，俯仰滚动角

机器人运动学研究时，通常采用矩阵运算形式，所以包括末端关节的各关节空间姿态是向量形式 ，因此至少需要6个参数表达（n,o,a中的两组），末端关节的姿态在运动学计算时会做为输入参数，表达末端空间姿态的除用向量组形式外还有欧拉角（Euler Angles）、俯仰滚动角（roll-pitch-yaw）（也称横摇角，纵摇角，偏转角）等，这样输入的参数会少些，我知道的广数GSB-RB8就是采用俯仰滚动角输入的，在实际操作中需要一个从向量组到角度的转换；

欧拉角、俯仰滚动角（Tait–Bryan angles）分别参照：

https://en.wikipedia.org/wiki/Euler_angles

https://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles

两者的区别在于旋转轴及次序，例欧拉角为z-x^’-z^’’,俯仰滚动角则为z-x^’-y^’’，欧拉角为x-y^’-x^’’，则俯仰滚动角则为：x-y^’-z^’’，这需要一定的空间想象。 

向量组转为欧拉角： 

α=arg(^ra_hx,^ra_hy) 小于零时α=arg(^ra_hx,^ra_hy)+ π 

β=arg(^ra_hx, ^ra_hxCosα + ^ra_hySinα) 

γ=arg(-^ro_hxSinα+ ^ro_hyCosα, -^rn_hxSinα+ ^rn_hyCosα) 

向量组转为俯仰滚动角： 

ξ = arg(^rn_hx,^rn_hy) 小于零时α=arg(^rn_hx,^rn_hy)+ π 

Ø= arg(^rn_hxCosξ+ ^rn_hySinξ,-^rn_hx,) 

Ψ= arg(-^ro_hxSinξ+ ^ro_hyCosξ, ^ra_hxSinξ - ^ra_hyCosξ) 

arg(x,y)表示复数z=x+iy的幅角，即Atan2(y,x)。

        /// <summary>
        /// 姿态的向量形式转化为角度形式
        /// </summary>
        /// <param name="ev">向量[n,o,a] = {{nx,ox,ax},{ny,oy,ay},{nz,oz,az}}</param>
        /// <param name="euler">true为欧拉角，否则为俯仰滚动角</param>
        /// <returns>角度数组{α，β，ϒ}</returns>
        public static double[] TransToAng(double[,] ev, bool euler)
        {
            double[] RA = new double[3];
            double nx = ev[0, 0];
            double ny = ev[1, 0];
            double nz = ev[2, 0];
            double ox = ev[0, 1];
            double oy = ev[1, 1];
            double oz = ev[2, 1];
            double ax = ev[0, 2];
            double ay = ev[1, 2];
            double az = ev[2, 2];
            double α, β, ϒ;
            if (euler)
            {
                α = Math.Atan(ay / ax);
                α = α < 0 ? α + Math.PI : α;
                β = Math.Atan((ax * Math.Cos(α) + ay * Math.Sin(α)) / az);
                ϒ = Math.Atan((-nx * Math.Sin(α) + ny * Math.Cos(α)) / (-ox * Math.Sin(α) + oy * Math.Cos(α)));
            }
            else
            {
                α = Math.Atan(ny / nx);
                α = α < 0 ? α + Math.PI : α;
                β = Math.Atan((-nz) / (nx * Math.Cos(α) + ny * Math.Sin(α)));
                ϒ = Math.Atan((ax * Math.Sin(α) - ay * Math.Cos(α)) / (-ox * Math.Sin(α) + oy * Math.Cos(α)));
                ϒ = ϒ < 0 ? ϒ + Math.PI : ϒ;
            }
            RA = new double[] { α * 180.0 / Math.PI, β * 180.0 / Math.PI, ϒ * 180.0 / Math.PI };
            return RA;
        }