旋转中心的计算和使用
1. 为什么要标定旋转中心
2. 如何标定旋转中心
2.1 标定旋转中心的操作步骤
2.2 三点拟合圆心代码
3. Halcon多点拟合圆的方法
4. 如何使用旋转中心计算偏移量
5. 计算一点绕另一点旋转一定角度后新点的坐标
1. 为什么要标定旋转中心
在机器视觉的实际应用中，存在以下典型场景：机械手需要抓取物料，但物料每次的角度不一致，且机械手末端工具中心点（TCP）与其自身的旋转中心不重合。要完成此类抓取任务，通常有两种解决方案：
TCP标定 (Tool Center Point):
主流机械手（如EPSON）通常具备建立新工具坐标系的功能。TCP标定的核心是求解旧工具坐标系与新工具坐标系之间的旋转平移矩阵。
操作流程一般包括：定义新工具编号，放置一个参考点，多次旋转U轴（旋转轴）（通常4次），并在基础坐标系（Base）下移动X、Y轴，使新工具坐标系的末端精确对准参考点。完成对准后执行示教操作即可。
标定旋转中心和旋转半径:
若机器人不具备TCP标定功能（当前市场较为少见），或者操作人员对该功能不熟悉，可能需采用此方法。更常见的情况是使用运动控制卡控制带有旋转轴的运动机构，且该旋转轴上的工具末端中心与旋转轴本身不重合。此时，必须标定旋转中心和旋转半径。
对于带有旋转轴的机构，通常都需要进行旋转中心标定。因为单纯依靠机械定位精度有限，而机器视觉工程师需对最终精度负责。标定旋转中心是实现精确旋转运动的基础。
2. 如何标定旋转中心
2.1 标定旋转中心的操作步骤
在旋转轴的工具上固定一个Mark点。
驱动旋转轴转动不同的角度（至少三个不同角度）。
在每个角度下，获取该Mark点在相机视野中的位置（图像坐标）。
将获取到的图像坐标点（至少三个）转换为世界坐标（需预先完成九点标定）。
使用转换后的世界坐标点拟合一个圆。
该拟合圆的圆心即为旋转中心，圆的半径即为旋转半径。
下图示意了该过程（图片：旋转中心标定示意图）： [图1：旋转中心标定示意图 - media/image1.png]
2.2 三点拟合圆心代码
以下C#代码实现了通过三点坐标计算圆心坐标和半径的功能：
下图示意了三点拟合圆的原理（图片：三点拟合圆原理图）： [图2：三点拟合圆原理图 - media/image2.png]
csharp
using System;
namespace RotateCenterCalculation
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
// 按钮点击事件处理：计算圆心和半径
private void button1_Click(object sender, EventArgs e)
{
try
{
// 解析输入的三个点坐标
double x1 = double.Parse(txtX1.Text);
double y1 = double.Parse(txtY1.Text);
double x2 = double.Parse(txtX2.Text);
double y2 = double.Parse(txtY2.Text);
double x3 = double.Parse(txtX3.Text);
double y3 = double.Parse(txtY3.Text);
// 调用计算函数
double[] xy = RotateCenter(x1, y1, x2, y2, x3, y3);
// 显示结果 (圆心X, 圆心Y, 半径R)
txtxcen.Text = xy[0].ToString();
txtycen.Text = xy[1].ToString();
txtrcen.Text = xy[2].ToString();
}
catch (Exception ex)
{
// 处理输入或计算错误
MessageBox.Show("输入错误或计算失败: " + ex.Message);
}
}
///
/// 根据三点坐标计算圆心和半径
///
/// 点1 X坐标
/// 点1 Y坐标
/// 点2 X坐标
/// 点2 Y坐标
/// 点3 X坐标
/// 点3 Y坐标
///
public double[] RotateCenter(double x1, double y1, double x2, double y2, double x3, double y3)
{
// 根据三点求圆心公式计算中间变量
double a = 2 * (x2 - x1);
double b = 2 * (y2 - y1);
double c = x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1;
double d = 2 * (x3 - x2);
double e = 2 * (y3 - y2);
double f = x3 * x3 + y3 * y3 - x2 * x2 - y2 * y2;
// 计算圆心坐标 (x, y)
double x = (b * f - e * c) / (b * d - e * a);
double y = (d * c - a * f) / (b * d - e * a);
// 计算半径 (圆心到点1的距离) - 修正了原文的笔误 (y1 - x) -> (y1 - y)
double r = Math.Sqrt((x1 - x) * (x1 - x) + (y1 - y) * (y1 - y));
return new double[] { x, y, r };
}
}
}
3. Halcon多点拟合圆的方法
若使用Halcon算法库，可以利用其内置的轮廓拟合功能更便捷地实现多点（超过三点）拟合圆，提高鲁棒性和精度：
下图展示了Halcon多点拟合圆的效果（图片：Halcon拟合圆效果图）： [图3：Halcon多点拟合圆效果 - media/image3.png]
csharp
// 定义点集 (Row, Col 坐标)
HTuple Row = new HTuple(0, 100.5, 100, 3); // Y坐标
HTuple Col = new HTuple(4, 0, 100, 99.4); // X坐标
// 根据点集生成多边形轮廓
HObject Contour;
HOperatorSet.GenContourPolygonXld(out Contour, Row, Col);
// 使用几何鲁棒方法('geotukey')拟合圆轮廓
HTuple Row_C, Column_C, Radius_C, StartPhi_C, EndPhi_C, PointOrder_C;
HOperatorSet.FitCircleContourXld(Contour, "geotukey", -1, 0, 0, 3, 2,
out Row_C, out Column_C, out Radius_C,
out StartPhi_C, out EndPhi_C, out PointOrder_C);
// 生成拟合圆的轮廓用于显示 (蓝色)
HObject ContCircle;
HOperatorSet.GenCircleContourXld(out ContCircle, Row_C, Column_C, Radius_C,
0, 6.28318, "positive", 1); // 6.28318 ≈ 2*π
dev_set_color("blue");
dev_display(ContCircle);
详细方法可参考博客：Halcon多点拟合圆方法
4. 如何使用旋转中心计算偏移量
此步骤是应用旋转中心的核心环节，用于确定末端工具因旋转而产生的平移偏移量。设：
O(X0, Y0): 旋转中心 (已知)
R: 旋转半径 (已知，即旋转中心到工具末端中心点A的距离)
θ: 当前工具末端中心点A相对于初始位置点B的旋转角度 (已知，可由视觉系统获取)
B(Xb, Yb): 工具末端中心点的初始位置（角度为0时的位置）
A(Xa, Ya): 工具末端中心点的当前位置（旋转θ角后的位置）
ΔX, ΔY: 我们需要计算的偏移量（从B点到A点的平移量）
几何关系推导:
初始状态：点B、点O(旋转中心)、点D(B在旋转中心坐标系下的投影点)构成直角三角形△OBD。其中|OB| = R。 [图4：初始状态几何关系 - media/image4.png]
[图5：旋转后状态几何关系 - media/image5.png]
旋转θ角后：点B移动到点A。点O、点A、点C(A在初始坐标系下的投影点)构成直角三角形△OAC。其中|OA| = R。
[图6：旋转偏移量计算原理图 - media/image6.png]
分析偏移量：点A相对于点B的偏移(ΔX, ΔY) 可以分解为旋转中心坐标系下的旋转运动。观察三角形△BAC和△OAD：
△BAC 的边BA 即为总偏移矢量(ΔX, ΔY)。
△OAD 描述了旋转中心坐标系下的旋转关系。
关键相似关系： △BAC 与 △OAD 是相似三角形（对应角相等）。
[图7：相似三角形关系 - media/image7.png]
推导公式： 根据相似三角形比例关系：
[图8：相似三角形比例关系 - media/image8.png] 即：
text
|AD| / R = |AC| / |OA| = sin(θ) => |AD| = R * sin(θ)
|OD| / R = |OC| / |OA| = cos(θ) => |OD| = R * cos(θ) 注意△OAD在旋转中心坐标系下的方向。点A相对于点O的坐标为(OD, AD) = (R * cos(θ), R * sin(θ))。 点B相对于点O的坐标为(R, 0) (初始状态假设沿X轴)。 偏移量(ΔX, ΔY) 即为点A的世界坐标减去点B的世界坐标：
text
ΔX = (X0 + R * cos(θ)) - (X0 + R) = R * (cos(θ) - 1)
ΔY = (Y0 + R * sin(θ)) - (Y0 + 0) = R * sin(θ) [图9：偏移量计算公式 - media/image9.png] 最终公式：
text
ΔX = R * (cos(θ) - 1)
ΔY = R * sin(θ)
5. 计算一点绕另一点旋转一定角度后新点的坐标
第4节的推导过程阐明了旋转中心应用的原理。在实际编程中，更常用的是直接计算一个点(XBefore, YBefore)绕旋转中心(XRotation, YRotation)旋转ARotate角度（弧度制）后的新坐标(XAfter, YAfter)。
坐标旋转公式：
text
XAfter = (XBefore - XRotation) * cos(ARotate) - (YBefore - YRotation) * sin(ARotate) + XRotation
YAfter = (YBefore - YRotation) * cos(ARotate) + (XBefore - XRotation) * sin(ARotate) + YRotation
顺时针旋转角度为负值，逆时针旋转角度为正值。
角度需转换为弧度计算 (弧度 = 角度 * π / 180)。
C# 代码实现：
csharp
using System;
namespace RotatePoint
{
class Program
{
static void Main(string[] args)
{
double newX = 0, newY = 0;
// 示例：计算点(2, 2)绕点(1, 1)顺时针旋转45度(即-45度)后的新坐标
RotatePoint(1, 1, -45, 2, 2, ref newX, ref newY);
Console.WriteLine($"{newX}, {newY}"); // 输出格式修正
Console.ReadKey();
}
///
/// 计算点绕旋转中心旋转后的新坐标
///
/// 旋转中心X坐标
/// 旋转中心Y坐标
/// 旋转角度(度)，顺时针为负
/// 旋转前点的X坐标
/// 旋转前点的Y坐标
/// 旋转后点的X坐标 (ref 输出)
/// 旋转后点的Y坐标 (ref 输出)
///
public static string RotatePoint(double XRotation, double YRotation, double ARotate,
double XBefore, double YBefore,
ref double XAfter, ref double YAfter)
{
try
{
// 将角度转换为弧度
double Rad = ARotate * Math.PI / 180.0;
// 应用旋转公式
XAfter = (XBefore - XRotation) * Math.Cos(Rad)
- (YBefore - YRotation) * Math.Sin(Rad)
+ XRotation;
YAfter = (YBefore - YRotation) * Math.Cos(Rad)
+ (XBefore - XRotation) * Math.Sin(Rad)
+ YRotation;
return "OK";
}
catch (Exception ex)
{
return ex.Message;
}
}
}
}
详细方法可参考博客：点绕点旋转坐标计算
标定旋转中心的核心目的： 正是为了利用上述公式，精确计算出工具末端在旋转任意角度后的新位置坐标 (XAfter, YAfter)。
X, Y和角度纠偏步骤：
角度纠偏： 利用计算出的旋转中心坐标 (X0, Y0) 和旋转半径 R，结合检测到的当前角度偏差 θ，使用第5节的旋转公式计算出将物料转正到目标角度后，其特定点（通常是抓取点或定位点）的理论坐标 (X_corrected, Y_corrected)。
平移纠偏： 比较转正后的理论坐标 (X_corrected, Y_corrected) 与目标位置（模板位置）坐标 (X_target, Y_target)，其差值 (ΔX = X_target - X_corrected, ΔY = Y_target - Y_corrected) 就是需要进行的平移补偿量。这个平移量通常利用预先标定好的相机坐标系到机械手坐标系（或世界坐标系）的仿射变换矩阵进行映射计算。
总结： 本文详细介绍了机器视觉中旋转中心的标定原理（三点拟合法、Halcon拟合法）、核心应用（计算旋转偏移量、计算旋转后坐标）以及其在机械手抓取纠偏中的关键作用（先角度纠偏计算新坐标，再平移补偿）。理解和掌握这些计算方法是实现高精度视觉引导定位的关键。
（欢迎关注获取更多机器视觉技术分享！如果觉得文章有帮助，请点赞支持。）