** 以下只针对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();
}
结果：
结尾发个广告，基于YOLO V8写的软件发布B站了，看到帮忙点个赞，过些天我会发源码给好朋友们！
链接：基于PySide6开发的YOLO V8视觉检测系统
拜了个拜。。。
