二维平面坐标系转换

二维平面坐标系转换

在运动控制中,常涉及到物料坐标系与载台坐标系之间的转换。
通常用坐标转换矩阵实现同一点在不同坐标系的转换,即指将两个点之间的仿射关系以转换矩阵的形式表现出来,转换矩阵作用于原始点的坐标,得到相对应的通过(平移,缩放,旋转)得到的新的点的坐标。

平移:

点P(x,y)平移得到点P'(x',y'),

矩阵表示:

缩放:

矩阵表示:


旋转:

矩阵表示:


旋转证明:

点P向逆时针旋转了θ度,坐标系如下图,

二维平面坐标系转换_第1张图片

依图可知,

其中的旋转角度θ求法:已知对应坐标系下的两点坐标,求两向量的夹角,两向量的夹角的cos值等于两向量的数量积除以两向量的模的乘积

假设,已知物料坐标系中两点坐标p1(x1,y1),p2(x2,y2),其两点在载台坐标系中的坐标为p3(x3,y3),p4(x4,y4),那么

坐标转换矩阵

如果一个点同时进行了平移,缩放,旋转,用以下坐标变换矩阵:

即就是a,b,c,d为缩放、旋转的系数,e,f为平移量


eg:
物料坐标系与载台坐标系如下:

二维平面坐标系转换_第2张图片
先旋转物料坐标系180°,使其与载台坐标系一致:

二维平面坐标系转换_第3张图片

C#实现坐标转换矩阵:

using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
class CoorTransform
    {
        public CoorTransform() { }

        public double Mark1_cust_position_x { get; set; }
        public double Mark1_cust_position_y { get; set; }
        public double Mark2_cust_position_x { get; set; }
        public double Mark2_cust_position_y { get; set; }
        public double Mark1_stage_position_x { get; set; }
        public double Mark1_stage_position_y { get; set; }
        public double Mark2_stage_position_x { get; set; }
        public double Mark2_stage_position_y { get; set; }
        Matrix matrix_rotation { get; set; }   //转换矩阵

        // 设置转换矩阵矩阵   如不同坐标系应先完成坐标系转换(这里的不同指左手坐标系和右手坐标系)
        public void SetMatrix()
        {
            double s1_x = Mark2_cust_position_x - Mark1_cust_position_x;
            double s1_y = Mark2_cust_position_y - Mark1_cust_position_y;

            double s2_x = Mark2_stage_position_x - Mark1_stage_position_x;
            double s2_y = Mark2_stage_position_y - Mark1_stage_position_y;



            double k = 1;

            var vectorA = new DenseVector(new[] { s1_x, s1_y });

            var vectorB = new DenseVector(new[] { s2_x, s2_y });


            double d = vectorA * vectorB;
            //两向量的夹角的cos值=向量的数量积/向量模的乘积    向量的夹角即为两坐标系间的旋转角度
            var cos_sg = (vectorA * vectorB) / (Math.Sqrt(s1_x * s1_x + s1_y * s1_y) * System.Math.Sqrt(s2_x * s2_x + s2_y * s2_y));

            var sin_sg = Math.Sin(Math.Acos(cos_sg));

            //缩放比例
            k = Math.Sqrt((s2_x * s2_x + s2_y * s2_y) / (s1_x * s1_x + s1_y * s1_y));

            if (((s1_y / s1_x) > (s2_y / s2_x)) & (s1_x > 0))
            {
                sin_sg = -sin_sg;
            }


            double[,] stage2 =
           {

                { k*cos_sg, k*-sin_sg},
                { k*sin_sg,k*cos_sg},
            };

            matrix_rotation = DenseMatrix.OfArray(stage2);

            //两坐标系间的偏移
            double offset_x = Mark1_stage_position_x - Mark1_cust_position_x;
            double offset_y = Mark1_stage_position_y - Mark1_cust_position_y;


            double[,] stage1 =
            {
                { k*cos_sg, k*-sin_sg,offset_x},
                { k*sin_sg,k*cos_sg,offset_y},
                { 0,0,1}
            };

            matrix_rotation = DenseMatrix.OfArray(stage1);
        }

        public void Check(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4,double x5, double y5)
        {
            Mark1_cust_position_x = x1;
            Mark1_cust_position_y = y1;
            Mark2_cust_position_x = x2;
            Mark2_cust_position_y = y2;
            Mark1_stage_position_x = x3;
            Mark1_stage_position_y = y3;
            Mark2_stage_position_x = x4;
            Mark2_stage_position_y = y4;

            SetMatrix();
            //由于物料坐标系做了旋转变换(180°),故此坐标点均乘以-1,即旋转矩阵(-1  0
            //                                                           0  -1)
            var vectorsssss = new DenseVector(new[] { -x5, -y5, 1 });
            var result = matrix_rotation * vectorsssss;
        }
    }

 

 

参考:https://blog.csdn.net/Xu_Claire/article/details/101477540

你可能感兴趣的:(C#,坐标系转换,转换矩阵)