[发明专利]一种基于Aruco码的平面旋转角度视觉测量方法及装置

权利要求书

1.一种基于Aruco码的平面旋转角度视觉测量方法，其特征在于：步骤如下：

步骤1：建立坐标系

利用Aruco码建立世界坐标系与旋转物体坐标系，然后通过相机对Aruco码的识别，获取旋转物体上的一个转动点在世界坐标系下的坐标；

步骤1.1：将两个不同ID的Aruco码分别置于固定平面与旋转物体上，确保两者都在相机视野范围内；

步骤1.2：以Aruco码的一个角点为原点，相邻的两条边为坐标轴，建立右手坐标系，以固定平面上Aruco码所确定的坐标系为世界坐标系，旋转物体上Aruco码所确定坐标系为旋转物体坐标系，以旋转物体坐标系原点为转动点，获取该转动点在世界坐标系下的坐标；

获取转动点在世界坐标系下的坐标，具体包括以下步骤：

步骤1.2.1：利用棋盘格标定出相机的内参数矩阵M；

步骤1.2.2：获取旋转物体坐标系相对于相机坐标系的旋转矩阵与平移向量，具体方法为：获取Aruco码四个角点的像素坐标，然后获取四个角点在旋转物体坐标系的三维坐标，通过PnP迭代算法估计相机的位姿，获取旋转矩阵与平移向量；设世界坐标系相对相机坐标系的旋转矩阵与平移向量为R₁、T₁，旋转物体坐标系为R₂、T₂；

步骤1.2.3：设旋转物体坐标系原点在世界坐标系下的坐标为(X₁,Y₁,Z₁)，在像素坐标系下的坐标为(u₁,v₁)，根据世界坐标系、相机坐标系、图像坐标系与像素坐标系之间的转换关系，有以下坐标转换关系：

其中，Z₂＝T₂[0 0 1]^T，为转旋转物体坐标系原点到相机坐标系原点的距离；为获取(u₁,v₁)到(X₁,Y₁,Z₁)的变换关系，对上式做如下变形：

其中，

则有

进而得到了(u₁,v₁)到(X₁,Y₁,Z₁)的变换关系，在获取转动点像素坐标(u₁,v₁)之后，得到其在世界坐标系下的坐标；

步骤2：确定旋转角

在世界坐标系下确定旋转物体的旋转平面与旋转中心，在旋转平面内转动点绕旋转中心转动的角即为旋转角；

步骤3：计算旋转角度

在旋转平面内，转动点的起始位置与终止位置分别与旋转中心组成两个空间向量，计算两个空间向量的夹角，得到旋转角度的大小。

2.根据权利要求1所述的一种基于Aruco码的平面旋转角度视觉测量方法，其特征在于：在步骤2中，所述旋转角的确定方法具体包括以下步骤：

步骤2.1：转动旋转物体，实时采集并保存转动点在世界坐标系下的坐标值，得到一个三维点云；

步骤2.2：利用最小二乘法拟合三维点云的分布平面，得到物体的旋转平面；

步骤2.3：世界坐标系有三个平面分别为x-o-y平面、x-o-z平面及y-o-z平面，将三维点云投影到上述任意一坐标平面内，得到呈椭圆分布的二维点云，通过最小二乘法获取二维点云的拟合椭圆方程，并由此获取椭圆的中心坐标；

步骤2.4：将椭圆中心投影到旋转平面内，得到的三维空间点即为物体的旋转中心；

步骤2.5：在旋转平面内，转动点绕旋转中心转动的角即为旋转角。