二维平面坐标系转换
在运动控制中,常涉及到物料坐标系与载台坐标系之间的转换。
通常用坐标转换矩阵实现同一点在不同坐标系的转换,即指将两个点之间的仿射关系以转换矩阵的形式表现出来,转换矩阵作用于原始点的坐标,得到相对应的通过(平移,缩放,旋转)得到的新的点的坐标。
平移:
假设,二维平面的一个点做了如下的平移:
那么,该关系可用如下的矩阵运算表示:
于是,便证明了任意的平移关系可以用坐标变换矩阵表示。
缩放:
假设,二维平面的点做了如下的缩放:
那么,该关系可以用如下的矩阵运算表示:
于是,便证明了任意的缩放关系可以用坐标变换矩阵表示。
旋转
矩阵表示:
旋转证明:
点P向逆时针旋转了θ度,坐标系如下图,
依图可知,
其中的旋转角度θ求法:已知对应坐标系下的两点坐标,求两向量的夹角,两向量的夹角的cos值等于两向量的数量积除以两向量的模的乘积
假设,已知物料坐标系中两点坐标p1(x1,y1),p2(x2,y2),其两点在载台坐标系中的坐标为p3(x3,y3),p4(x4,y4),那么
坐标转换矩阵
如果一个点同时进行了平移,缩放,旋转,用以下坐标变换矩阵:
eg:
物料坐标系与载台坐标系如下:
先旋转物料坐标系180°,使其与载台坐标系一致:
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<double> 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