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

说明书

本发明公开了一种工具头非接触姿态控制方法，通过大于等于1个面传感器，非接触测量被测物体局部表面并返回深度信息，根据传感器测量数据集计算物体表面深度信息，构建被测量物体表面曲面模型，通过1个激光光源照射明确指示作业点，计算作业点附近规划的轨迹与构建曲面模型表面姿态关系，并选用矢量旋转公式得到工具头的插补位置姿态插补控制算法，约束工具头在运动中相对曲面模型上的轨迹点处的姿态为设计姿态。本发明采用的面传感器成本低，检测视角范围宽、实时性好。方案曲面模型求解算法稳定，测量盲区小。可有效避免点的距离测量因盲区大而造成工具头与被测物体在运动中接触的情况发生。

技术领域

本发明涉及艾灸机器人或机器人康养护理领域，具体地说，是一种工具头非接触姿态测量与控制方法。

背景技术

艾灸是用艾草制绒作出艾条或艾炷，点燃以温灼穴位的皮肤表面，达到温通经脉、调和气血的目的的一种治疗方法。随着艾灸的推广，及科学技术的发展，艾灸设备也日渐增多。常见的艾灸设备是通过治具将艾条燃烧点在人体穴位或特定部位以特定手法和姿态进行熏烤。由于艾灸需要在特定穴位或一系列穴位目标上运行传统的艾灸手法，因此对于艾灸设备而言，应要求其工具头与人体特定的穴位静止或运动中保持一定的姿态，从而满足传统艾灸保健的手法要求。

由于人体表面是非平面的状态，因此需要在静止或运动过程准确中测量人体表面的轮廓变化，使机器人作业点与人体之间保持约束位置和姿态并施加作业手法，而实际使用中，如何测量人体表面曲面轮廓并减少或消除测量盲区，实现工具头有效的调节避免与皮肤接触，控制位置姿态约束实现作业手法成为需要解决的问题。

发明内容

针对现有技术存在的不足，本发明的主要目的在于提供一种成本低、测量盲区小、非接触的曲面模型测量及工具头姿态控制方案。

为实现上述目的，本发明提供了如下技术方案：一种工具头非接触姿态测量方法，包括：

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

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

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

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

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

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

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

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

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