[发明专利]一种基于全站仪的空间目标的位置和朝向标定方法

权利要求书

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]。