一种基于全站仪的空间目标的位置和朝向标定方法

1.一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，包括以下步骤：
S1，构建机械臂所在的坐标系[x1，y1，z1]和全站仪所在的坐标系[x2，y2，z2]，其中z1轴和z2轴保持平行；
S2，确定空间目标首尾两点P、Q在全站仪所在的坐标系中的坐标；
S3，构造全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵；
S4，将点P和点Q在全站仪所在的坐标系中的坐标按平移矩阵和旋转矩阵向机械臂所在的坐标系进行转换，得到点P和点Q在机械臂所在的坐标系中的坐标p(px,py,pz)和q(qx,qy,qz)；
S5，确定目标孔的位置点在机械臂坐标系统下的坐标为q(qx,qy,qz)，其朝向向量为至此空间目标的位置和朝向标定完毕。
2.根据权利要求1所述的一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，所述步骤S2包括以下步骤：
S2-1，由全站仪所在的坐标系原点O2向机械臂所在坐标系的x1-y1平面作垂直投影，得到投影点O2’，在投影点O2’处作平行于全站仪所在的坐标系y2轴的平行线y2’，确定空间目标首尾两点P、Q，由点P和点Q向机械臂所在的坐标系的x1-y1平面上作垂直投影，得到投影点P’和投影点Q’，由投影点P’和投影点Q’分别向平行线y2’作垂线，与平行线y2’相交得到点K和点H；
S2-2，测量全站仪所在的坐标系原点O2到与投影点O2’之间的距离h1，测量点P到投影点P’的距离h2，点Q到投影点Q’的距离h3，投影点P’到平行线y2’的距离d3，投影点Q’到平行线y2’的距离d4，点K到投影点O2’的距离d5，点H到投影点O2’的距离d6；
S2-3，利用测量值表示在全站仪所在的坐标系中空间目标首尾两点P、Q的坐标，其中，点P坐标为[-d3,d5,h2-h1]，点Q坐标为[-d4,d6,h3-h1]。
3.根据权利要求1所述的一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，所述步骤S3包括以下步骤：
S3-1，测量全站仪所在的坐标系原点O2在机械臂所在的坐标系的x1-y1平面上的投影点O2’到x1轴的距离d1和到y1轴的距离d2，测量全站仪所在的坐标系原点O2与其在机械臂所在坐标系的x1-y1平面的投影点O2’之间的距离h1，测量得到两个坐标系绕z轴旋转的相对角度θ；
S3-2，将全站仪所在的坐标系沿着x轴平移d2个单位，变换到机械臂所在的坐标系，全站仪所在的坐标系的点变换到机械臂所在的坐标系以后坐标变为：
将上式写成矩阵形式为：
得到沿着x轴的平移变换矩阵
S3-3，同理，构造沿y轴的平移变换矩阵T(y,-d1)，和沿z轴的平移变换矩阵T(z,-h1)，矩阵如下：
S3-4，绕Z旋转矩阵的构造和推倒过程如下：全站仪所在的坐标系绕z轴旋转θ度，则全站仪所在的坐标系中的点旋转后在机械臂所在的坐标系中的坐标为：
将上式写成矩阵形式为：
得到沿着z轴的旋转矩阵
4.根据权利要求3所述的一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，所述步骤S4中得到的点P在机械臂所在的坐标系中的坐标为：p＝P*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)；点Q在机械臂所在的坐标系中的坐标为：q＝Q*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)。

暂无