【解决视觉引导多个位置需要标定多个位置的问题】

** 以下只针对2D定位，就是只有X、Y、Rz三个自由度的情况。**

假设一种情况，当视觉给机器人做引导任务时，零件有多个，分布在料框里，视觉需要走多个位置去拍，那么只需要对第一个位置确定拍照位，确定试抓位，其余的拍照位置通过只平移XY，零件特征偏移归0的方式确定，这种方式当两垛零件方向不一样的时候就不得不单独走9点。
为了解决这个问题，提高调试效率，现在分享一种通过计算来偷懒的方法。
通过这种方式，只需要9点标定一次，然后通过告知机器人当前拍照位坐标，计算零件在Base下的坐标。

1. 数学原理：

坐标系ABC分别代表机器人基座、Tool、相机。
已知的有B_A：示教器上能看到。
已知的有C_B: 物体在相机坐标系位置。
求：C_A的位置= C_A = B_A * C_B
简单来说就是只要知道C_A和C_B相乘就能算出来C_A，矩阵公式我就不画了，相信大家都见过。。。

在这里插入图片描述

2. 直接本主题，上代码：

namespace Math_Matrix
{/// <summary>/// 已知B_A坐标系关系，C_B坐标关系，求C_A/// </summary>public static class Position{/// <summary>/// 机器人工具中心在Base的平移X坐标/// </summary>public static double ToolCenterInBaseX { get; set; }/// <summary>/// 机器人工具中心在Base的平移Y坐标/// </summary>public static double ToolCenterInBaseY { get; set; }/// <summary>/// 机器人工具中心在Base的旋转Z坐标/// </summary>public static double ToolCenterInBaseRz { get; set; }/// <summary>/// 物体在工具坐标系下的平移X坐标/// </summary>public static double ObjectInToolCenterX { get; set; }/// <summary>/// 物体在工具坐标系下的平移Y坐标/// </summary>public static double ObjectInToolCenterY { get; set; }/// <summary>/// 物体在工具坐标系下的旋转Z坐标/// </summary>public static double ObjectInToolCenterRz { get; set; }/// <summary>/// 计算物体在机器人底座的位置/// </summary>/// <returns>坐标XYR</returns>public static XYR CalculaterObjectInBaseXYR(){Matrix<double> OB = CalculateTransformationMatrix();double r00 = OB.At(0, 0);double r10 = OB.At(1, 0);double Tx = OB.At(0, 3);double Ty = OB.At(1, 3);double Rz = Math.Atan2(r10, r00) * 180 / Math.PI; return new XYR(Tx, Ty, Rz);}/// <summary>/// 矩阵相乘/// </summary>/// <returns>结果矩阵</returns>private static Matrix<double> CalculateTransformationMatrix(){Matrix<double> TW = DenseMatrix.OfArray(new double[,] {{ Math.Cos(ToolCenterInBaseRz*Math.PI/180), -Math.Sin(ToolCenterInBaseRz*Math.PI/180),0, ToolCenterInBaseX},{ Math.Sin(ToolCenterInBaseRz*Math.PI/180), Math.Cos(ToolCenterInBaseRz*Math.PI/180),0 ,ToolCenterInBaseY},{ 0, 0, 1 ,0},{ 0, 0, 0,1 }});Matrix<double> OT = DenseMatrix.OfArray(new double[,] {{ Math.Cos(ObjectInToolCenterRz*Math.PI/180), -Math.Sin(ObjectInToolCenterRz*Math.PI/180), 0,ObjectInToolCenterX},{ Math.Sin(ObjectInToolCenterRz*Math.PI/180), Math.Cos(ObjectInToolCenterRz*Math.PI/180),0, ObjectInToolCenterY},{ 0, 0, 1,0 },{ 0, 0, 0,1 }});return TW * OT;}}/// <summary>/// 记录坐标结果/// </summary>public struct XYR{/// <summary>/// 平移X坐标/// </summary>public double X { get; }/// <summary>/// 平移Y坐标/// </summary>public double Y { get; }/// <summary>/// Z轴旋转角度/// </summary>public double Rotation { get; }/// <summary>/// 构造的时候传入坐标值/// </summary>/// <param name="x">X</param>/// <param name="y">Y</param>/// <param name="rotation">Rz</param>public XYR(double x, double y, double rotation){X = x;Y = y;Rotation = rotation;}}
}

运行验证图上红色ABC坐标系：

  static void Main(string[] args){Position.ToolCenterInBaseX = 6;Position.ToolCenterInBaseY = 8;Position.ToolCenterInBaseRz = -40;Position.ObjectInToolCenterX = 8.8;Position.ObjectInToolCenterY = 9.8;Position.ObjectInToolCenterRz = 150;XYR pos = Position.CalculaterObjectInBaseXYR();Console.WriteLine(pos.X);Console.WriteLine(pos.Y);Console.WriteLine(pos.Rotation);Console.ReadKey();}

结果：
在这里插入图片描述

运行验证图上红色A B’ C坐标系：

static void Main(string[] args){Position.ToolCenterInBaseX = 13;Position.ToolCenterInBaseY = 6;Position.ToolCenterInBaseRz = 0;Position.ObjectInToolCenterX = 6;Position.ObjectInToolCenterY = 4;Position.ObjectInToolCenterRz = 110;XYR pos = Position.CalculaterObjectInBaseXYR();Console.WriteLine(pos.X);Console.WriteLine(pos.Y);Console.WriteLine(pos.Rotation);Console.ReadKey();}