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

** 以下只针对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();
        }