[发明专利]一种工具头非接触姿态测量与控制方法

权利要求书

1.一种工具头非接触姿态测量方法，其特征是包括：

具有激光指示点，设置于工具头上用于指示工具头作业点位置；

具有不少于一个面传感器，设置于工具头上用于测量固有视角以及工具头工作或测量时的高度，所述面传感器以工具头作用点为圆心，并且沿工作头呈圆心或圆周分布；

通过面传感器获取被测物体的深度信息，每一个面传感器测量获取一个曲面，根据深度信息及面传感器当前视角计算视角内实际物体曲面模型；

根据传感器视角覆盖范围及传感器分布位置使用最小二乘法或曲面积分重建的方法测量物体作业点位置手法范围的曲面模型。

2.根据权利要求1所述的一种工具头非接触姿态测量方法，其特征是：所述曲面测量方法具体包括有根据面传感器的位置及在工具头上的分布，分别计算深度数据集，根据数据集覆盖范围(X,Y)坐标点，将获得的曲面进行拼接，交叉范围处的数据采用最小二乘法拟合点位深度数据处理，数据处理后形成测量圆或测量圆环，或数据集平面范围内拼接的并集。

3.根据权利要求1所述的一种工具头非接触姿态测量方法，其特征是：所述激光指示点设置为一个，用于辅助操作人员定位测量点或者工作目标点。

4.根据权利要求2所述的一种工具头非接触姿态测量方法，其特征是：所述面传感器的视角范围XY平面集合记为A：

{(X_a，Y_a)∣X_a∈A，Y_a∈A}；

被测量物体作业点范围集合记为W,在工作坐标系下因测量距离不同所产生测量集合A,B,C....N与作业点集合W的关系：

W∈(A∪B∪C…∪N)。

5.根据权利要求2所述的一种工具头非接触姿态测量方法，其特征是：基于传感器安装位置信息及传感器深度数据，建立工具坐标系下的测量范围内的数据集合，进而得到曲面方程F_i：

F_i(X,Y,Z)＝0。

6.根据权利要求2所述的一种工具头非接触姿态测量方法，其特征是：当使用一个或多个传感器，通过最小二乘法计算覆盖区域重叠部分重建得到曲面函数F_n(X,Y,Z)＝0的步骤为：

将曲面方程转换为因变量为Z的表达式即Z＝D(X，Y)，重叠区域的数据集的数据代入建立的目标函数：

将数据集代入后对目标函数中曲面未知参数求偏导数，在极值点处取得数据集目标函数偏差最小值；

根据求得参数代入曲面方程，带入轨迹点处数据集求得轨迹点处曲面沿X和Y方向的切线及轨迹点邻域的曲面的法向量。

7.一种工具头非接触姿态控制方法，其特征是包括：根据指示点计算工具头工作位置及康养手法配置信息在基于工作位置生成的工作坐标系下XY平面上固定手法轨迹；

机器人工作坐标系下生成的轨迹曲线投影至测量物体所在(X,Y)位置的曲面上，计算得到物体表面投影轨迹曲线上或曲线周围各邻域内的曲面信息；

通过获取的曲面信息构建曲面方程，计算曲面上轨迹点位处的沿机器人定义坐标系下X轴与Y轴方向曲面切向量；

根据曲面切向量求得轨迹点位处的曲面法向量

根据曲面上轨迹点处的法向量和已知工具头坐标系下Z轴矢量的相对关系，使用矢量旋转罗德里格公式计算工具头Z轴矢量的与曲面法向量的约束关系。

8.根据权利要求7所述的一种工具头非接触姿态控制方法，其特征是：根据测量得到的曲面信息计算曲面轨迹点处的目标约束姿态，并且根据工具头当前姿态，通过罗德里格公式或轴角坐标系将当前工具Z轴向量变换至与曲面向量约束位置；

变换方法步骤为：计算工具头Z轴单位矢量与曲面单位法向量的矢量积与数量积，得到单位矢量和夹角θ，并计算出工具头姿态变换矩阵ΔR：

其中I₃是3x3单位矩阵。

9.根据权利要求7所述的一种工具头非接触姿态控制方法，其特征是：通过在工作坐标系XY平面上生成固定或自定义轨迹；

在Z轴方向或曲面法向量的约束方向实现相关约束距离控制，所述步骤为：通过计算工具头作用点P_a(X_a，Y_a，Z_a)与轨迹点P_i(X_i，Y_i，Z_i)的距离关系

沿轨迹点处曲面法向量向上的投影计算控制距离，得到轨迹插补控制中的位置控制距离ΔP: