用于打磨机器人的控制方法及系统

摘要

本发明公开了一种用于打磨机器人的控制方法及系统。控制方法包括：步骤S1：对打磨工具及激光测距仪进行安装定位；步骤S2：通过激光测距仪对待打磨件进行外形数据扫描，获得每一扫描点处机器人的位置数据、姿态数据及激光测距仪的距离数据，根据位置数据、姿态数据及距离数据获得扫描曲面的打磨轨迹曲线；步骤S3：根据打磨轨迹曲线进行位置控制模式下的曲面打磨工作，同时结合力控模式对待打磨件进行打磨位置的修正处理。

权利要求

1.一种用于打磨机器人的控制方法，其特征在于，包括：

步骤S1：对打磨工具及激光测距仪进行安装定位，分别得到打磨工具中心点和激光测距仪的位置坐标在末端坐标系下的表示；

步骤S2：通过激光测距仪对待打磨件进行外形数据动态扫描，获得每一扫描点的距离数据及每一扫描点对应的机器人的机械臂的位置数据、姿态数据，根据位置数据、姿态数据及距离数据获得扫描曲面的打磨轨迹曲线；

步骤S3：根据打磨轨迹曲线进行位置控制模式下的曲面打磨工作，同时结合力控模式对待打磨件进行打磨位置的修正处理；

其中，于所述步骤S1中包括：

步骤S11：在机器人的末端依次安装六维力传感器、打磨工具和两个激光测距仪；

步骤S12：建立机器人基座标系、末端坐标系和打磨工具中心点坐标系；

步骤S13：通过测量或标定分别得到打磨工具中心点在末端坐标系下的位置坐标，以及两个激光测距仪在末端坐标系下的位置坐标，根据所建末端坐标系和打磨工具中心点坐标系之间的关系，得到打磨工具在末端坐标系下的姿态数据；

其中，于所述步骤S2中包括：

步骤S21：设定打磨工具的工作范围和待打磨件的打磨区域；

步骤S22：通过控制激光测距仪在随机械臂移动情况下对待打磨件进行外形数据扫描，读取两个激光测距仪的距离数据；

步骤S23：对所采集距离数据进行信号处理；

步骤S24：结合所建末端坐标系及激光测距仪安装位置，根据处理后每一扫描点的位置数据、姿态数据以及与该扫描点所对应的机器人的位置数据和姿态数据，通过计算获得打磨工具中心点所在平面在末端坐标系下表示的期望位置数据和姿态数据；

步骤S25：将打磨工具中心点的期望位置和期望姿态转换到机器人基座标系下，根据机器人基座标系下的期望位置和期望姿态获得打磨轨迹曲线；

其中，于所述步骤S24中包括：

步骤S241：根据不同时刻每一扫描点与相邻扫描点的距离数据确定一个平面；

步骤S242：根据平面和末端坐标系各轴方向构造第一辅助向量f1和第二辅助向量f2，根据第一辅助向量和第二辅助向量获得第一单位向量，所述第一单位向量为平面的法向量，根据末端坐标系设定第二单位向量，所述第二单位向量为取与所述末端坐标系的EX轴平行的单位向量，根据第一单位向量及第二单位向量叉乘得到第三单位向量，同时获得拟合平面在末端坐标系下的位置和姿态；

步骤S243：根据拟合平面在末端坐标系下的位置和姿态，选取所述末端坐标系的EX、EY平面与两个激光测距仪连线交点作为打磨点，获得每一扫描时刻对应的打磨工具中心点在末端坐标系下的期望位置和期望姿态。

2.如权利要求1所述的控制方法，其特征在于，于所述步骤S23中采用数字信号处理方法对位于末端坐标系下的位置数据进行异常采样值剔除、滤波和平滑处理。

3.如权利要求2所述的控制方法，其特征在于，于所述步骤S25中根据以下公式将期望位置和期望姿态转换到机器人基座标系下：；

其中，BTE为末端坐标系在机器人基座标系下的位置和姿态矩阵；

E

为打磨工具中心点在末端坐标系下的位置和姿态矩阵，其中RP为3x3的姿态矩阵，EP为3x1的位置向量。

4.如权利要求1所述的控制方法，其特征在于，于所述步骤S3中根据打磨轨迹曲线，使打磨机器人在基于位置控制模式和力控模式下渐进至打磨起始点，在力位混合控制模式下对打磨的位置进行修正，完成叶片的打磨工作。

5.一种用于打磨机器人的控制系统，其特征在于，打磨机器人的末端装设打磨工具，控制系统包括：两个激光测距仪，装设于打磨工具上；

处理单元，电性连接于两个所述激光测距仪；

控制单元，电性连接于所述处理单元及所述打磨机器人；

其中，所述处理单元对安装好的所述打磨工具及所述激光测距仪进行定位后分别得到打磨工具中心点和激光测距仪在末端坐标系下的表示，通过所述激光测距仪对待打磨件进行外形数据动态扫描，获得每一扫描点的距离数据及对应的机器人的机械臂的位置数据、姿态数据，所述处理单元根据距离数据、位置数据及姿态数据获得打磨轨迹曲线，所述处理单元输出所述打磨轨迹曲线至所述控制单元，所述控制单元根据所述打磨轨迹曲线结合力控模式控制所述打磨机器人对待打磨件进行打磨处理，并实时修正打磨位置和姿态；

其中，所述处理单元包括：

坐标系建立模块，建立机器人基座标系、末端坐标系和打磨工具中心点坐标系；

位置坐标获得模块，通过测量或标定分别得到打磨工具中心点在末端坐标系下的位置坐标，以及两个激光测距仪在末端坐标系下的位置坐标；

其中，所述处理单元包括：

设定模块，设定打磨工作范围和待打磨件的打磨区域；

处理模块，对位置数据、姿态数据及距离数据进行处理；

期望位姿获得模块，根据处理后的位置数据、姿态数据及距离数据获得打磨工具中心点在末端坐标系下的期望位置和期望姿态；

曲线获得模块，将期望位置和期望姿态转换到机器人基座标系下，根据机器人基座标系下的期望位置和期望姿态获得打磨轨迹曲线；

其中，根据不同时刻每一扫描点与相邻扫描点的距离数据确定一个平面；根据平面构造第一辅助向量f1和第二辅助向量f2，根据第一辅助向量和第二辅助向量获得第一单位向量，所述第一单位向量为平面的法向量，根据末端坐标系设定第二单位向量，所述第二单位向量为取与所述末端坐标系的EX轴平行的单位向量，根据第一单位向量及第二单位向量叉乘得到第三单位向量，同时获得拟合平面在末端坐标系下的位置和姿态；根据拟合平面在末端坐标系下的位置和姿态；选取所述末端坐标系的EX、EY平面与两个激光测距仪连线交点作为打磨点，获得每一扫描时刻对应的打磨工具中心点在末端坐标系下的期望位置和期望姿态。

6.如权利要求5所述的控制系统，其特征在于，所述处理模块采用数字信号处理方法对位于末端坐标系下的位置数据、姿态数据及距离数据进行滤波和平滑处理。

7.如权利要求6所述的控制系统，其特征在于，所述期望位姿获得模块根据以下公式将期望位置和期望姿态转换到机器人基座标系下：；

其中 ，  BTE为末端坐标系在机器人基座标系下的位置和姿态矩阵 ；

为打磨工具中心点在末端坐标系下的位置和姿态矩阵，其中ERP为 E

3x3的姿态矩阵，P为3x1的位置向量。

8.如权利要求5所述的控制系统，其特征在于，还包括力传感器，采集所述打磨机器人渐进至打磨起始点过程中的接触力，所述控制单元根据所述接触力结合所述打磨轨迹曲线控制打磨机器人完成打磨工作。

说明书

用于打磨机器人的控制方法及系统

技术领域

[0001] 本发明涉及一种用于打磨机器人的控制方法及系统，特别涉及一种曲面拟合和力位混合修正的打磨机器人的控制方法及系统。

背景技术

[0002] 目前叶片等自由曲面类零件的打磨抛光方法分为人工手持作业和机器人打磨系统作业两种方式，其中，人工手持作业工具靠经验来完成，不仅耗费大量人力资源，难以保证叶片打磨的质量一致性及加工效率，严重阻碍了生产力发展，而且现场的磨削环境较差，打磨产生的有毒气体和粉尘对人员的健康有着较大的潜在威胁。自动化机器人打磨系统可以克服人工手持作业的一些弊端，不仅易于维护，同时结合CAM技术、各种传感器和控制策略可以很好的保证叶片的打磨精度和成品质量，使得产品具有很好的一致性。但是现有的机器人打磨系统中普遍存在系统复杂、因力位传感器自身零点漂移或扫描仪器精度低导致的系统误差以及整体设备成本高昂等问题。

[0003] 现有的机器人打磨系统通常以工业机器人作为载体，采用末端安装柔顺力控打磨设备或力传感器与打磨工具串联安装方式，在控制方法上通常结合CAM技术及相关软件或者扫描建立三维模型获得打磨轨迹，采用工业机器人力控制或者PI/PD控制策略来提高打磨轨迹的跟踪精度。

[0004] 专利文献CN106625153A公开的打磨方法中，其叶片加工轨迹程序是基于CAM软件应用机器人离线编程技术生成的，但是在实际应用过程中由于叶片三维模型结构复杂，CAM技术生成的打磨轨迹不能直接用于叶片打磨；

[0005] 专利文献CN103507070公开了使用三轴力传感器进行力控制的机器人控制装置，通过估算三轴力传感器不能检测的力以及力矩来进行力控制。此装置通过设定力估算点，然后对估算点进行力估算，进而对力估算点进行修正，完成力控制。但是存在如下问题：

[0006] 1)上述专利的力控制方法是通过估算三轴力传感器不能检测的力以及力矩来进行力控制，即对设定的估算点进行力估算，从而进行力估算点进行修正。

[0007] (2)上述专利的力控制方法是估算力控制，因而力控制精度不高，无法保证打磨精度，无法应用与复杂曲面零件的加工。

[0008] 专利文献CN105538095A公开的打磨方法中，需要结合叶片CAD模型、CAM软件和三维扫描测量仪，不仅打磨系统复杂，应用受到CAD模型和CAM软件限制，而且扫描仪的使用大大的提高了设备成本。

[0009] 因此急需开发一种克服上述缺陷的用于打磨机器人的控制方法及系统。

发明内容

[0010] 针对上述问题，本发明一种用于打磨机器人的控制方法，其中，包括：

[0011] 步骤S1：对打磨工具及激光测距仪进行安装定位；

[0012] 步骤S2：通过激光测距仪对待打磨件进行外形数据扫描，获得每一扫描点处机器人的位置数据、姿态数据及激光测距仪的距离数据，根据位置数据、姿态数据及距离数据获得扫描曲面的打磨轨迹曲线；

[0013] 步骤S3：根据打磨轨迹曲线进行位置控制模式下的曲面打磨工作，同时结合力控模式对待打磨件进行打磨位置的修正处理。

[0014] 上述的控制方法，其中，于所述步骤S1中包括：

[0015] 步骤S11：在机器人的末端依次安装六维力传感器、打磨工具和两个激光测距仪；

[0016] 步骤S12：建立机器人基座标系、末端坐标系和打磨工具中心点坐标系；

[0017] 步骤S13：通过测量或标定分别得到打磨工具中心点在末端坐标系下的位置坐标，以及两个激光测距仪在末端坐标系下的位置坐标，根据所建末端坐标系和打磨工具中心点坐标系之间的关系，得到打磨工具在末端坐标系下的姿态数据。

[0018] 上述的控制方法，其中，于所述步骤S2中包括：

[0019] 步骤S21：设定打磨工具的工作范围和待打磨件的打磨区域；

[0020] 步骤S22：通过控制激光测距仪在随机械臂移动情况下对待打磨件进行外形数据扫描，读取两个激光测距仪的距离数据；

[0021] 步骤S23：对所采集距离数据进行信号处理；

[0022] 步骤S24：结合所建末端坐标系及激光测距仪安装位置，根据处理后每一扫描点的位置数据、姿态数据以及与该扫描点所对应的机器人的位置数据和姿态数据，通过计算获得打磨工具中心点所在平面在末端坐标系下表示的期望位置数据和姿态数据；

[0023] 步骤S25：将打磨工具中心点的期望位置和期望姿态转换到基座标系下，根据基座标系下的期望位置和期望姿态获得打磨轨迹曲线。

[0024] 上述的控制方法，其中，于所述步骤S23中采用数字信号处理方法对位于末端坐标系下的位置数据进行异常采样值剔除、滤波和平滑处理。

[0025] 上述的控制方法，其中，于所述步骤S24中包括：

[0026] 步骤S241：根据不同时刻每一扫描点与相邻扫描点的距离数据确定一个平面；

[0027] 步骤S242：根据平面和末端坐标系各轴方向构造第一辅助向量和第二辅助向量，根据第一辅助向量和第二辅助向量获得第一单位向量，根据末端坐标系设定第二单位向量，根据第一单位向量及第二单位向量得到第三单位向量，同时获得拟合平面在末端坐标系下的位置和姿态；

[0028] 步骤S243：根据拟合平面在末端坐标系下的位置和姿态，选取两个激光测距仪安装位置连线中垂面与扫描点连线交点作为打磨点，获得每一扫描时刻对应的打磨工具中心点在末端坐标系下的期望位置和期望姿态。

[0029] 上述的控制方法，其中，于所述步骤S25中根据以下公式将期望位置和期望姿态转换到基座标系下：

[0030]

[0031] 其中，BTE为末端坐标系在基座标系下的位置和姿态矩阵；

[0032] 为打磨工具中心点在末端坐标系下的位置和姿态矩阵，其中ERP为3x3的姿态矩阵，EP为3x1的位置向量。

[0033] 上述的控制方法，其中，于所述步骤S3中根据打磨轨迹曲线，使打磨机器人在基于位置控制模式和力控模式下渐进至打磨起始点，在力位混合控制模式下对打磨的位置进行修正，完成叶片的打磨工作。

[0034] 本发明还公开了一种用于打磨机器人的控制系统，其中，打磨机器人的末端装设打磨工具，控制系统包括：

[0035] 两个激光测距仪，装设于打磨工具上；

[0036] 处理单元，电性连接于两个所述激光测距仪；

[0037] 控制单元，电性连接于所述处理单元及所述打磨机器人；

[0038] 其中，所述处理单元对安装好的所述打磨工具及所述激光测距仪进行定位后，通过所述激光测距仪对待打磨件进行外形数据扫描，获得每一扫描点的距离数据及对应的机器人的位置数据、姿态数据，所述处理单元根据距离数据、位置数据及姿态数据获得打磨轨迹曲线，所述处理单元输出所述打磨轨迹曲线至所述控制单元，所述控制单元根据所述打磨轨迹曲线结合力控模式控制所述打磨机器人对待打磨件进行打磨处理，并实时修正打磨位置和姿态。

[0039] 上述的控制系统，其中，所述处理单元包括：

[0040] 坐标系建立模块，建立机器人基座标系、末端坐标系和打磨工具中心点坐标系；

[0041] 位置坐标获得模块，通过测量或标定分别得到打磨工具中心点在末端坐标系下的位置坐标，以及两个激光测距仪在末端坐标系下的位置坐标。

[0042] 上述的控制系统，其中，所述处理单元包括：

[0043] 设定模块，设定打磨工作范围和待打磨件的打磨区域；

[0044] 处理模块，对位置数据、姿态数据及距离数据进行处理；

[0045] 期望位姿获得模块，根据处理后的位置数据、姿态数据及距离数据获得打磨工具中心点在末端坐标系下的期望位置和期望姿态；

[0046] 曲线获得模块，将期望位置和期望姿态转换到基座标系下，根据基座标系下的期望位置和期望姿态获得打磨轨迹曲线。

[0047] 上述的控制系统，其中，所述处理模块采用数字信号处理方法对位于末端坐标系下的位置数据、姿态数据及距离数据进行滤波和平滑处理。

[0048] 上述的控制系统，其中，根据不同时刻每一扫描点与相邻扫描点的距离数据确定一个平面；根据平面构造第一辅助向量和第二辅助向量，根据平面和末端坐标系各轴方向构造第一辅助向量和第二辅助向量，根据第一辅助向量和第二辅助向量获得第一单位向量，根据末端坐标系设定第二单位向量，根据第一单位向量及第二单位向量得到第三单位向量，同时获得拟合平面在末端坐标系下的位置和姿态；根据拟合平面在末端坐标系下的位置和姿态；选取两个激光测距仪安装位置连线中垂面与扫描点连线交点作为打磨点，获得每一扫描时刻对应的打磨工具中心点在末端坐标系下的期望位置和期望姿态。

[0049] 上述的控制系统，其中，所述期望位姿获得模块根据以下公式将期望位置和期望姿态转换到基座标系下：

[0050]

[0051] 其中，BTE为末端坐标系在基座标系下的位置和姿态矩阵； 为打磨工具中心点在末端坐标系下的位置和姿态矩阵，其中ERP为3x3的姿态矩阵，EP为3x1的位置向量。

[0052] 上述的控制系统，其中，还包括力传感器，采集所述打磨机器人渐进至打磨起始点过程中的接触力，所述控制单元根据所述接触力结合所述打磨轨迹曲线控制打磨机器人完成打磨工作。

[0053] 本发明针对于现有技术其有益效果在于：本发明提供的一种曲面拟合和力位混合修正的打磨机器人的控制方法及系统，可以在仅安装两个激光测距仪情况下测得待打磨件的位置和姿态数据，结合力传感器实现对待打磨件的力位混合控制打磨，既节约了成本，又能保证打磨精度，且力传感器的使用也起到了保护作用防止待打磨件被过度打磨，保证了打磨质量。

[0054] 本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所指出的结构来实现和获得。

附图说明

[0055] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

[0056] 图1为本发明控制方法的流程图；

[0057] 图2为图1中步骤S1的分步骤流程图；

[0058] 图3为图1中步骤S2的分步骤流程图；

[0059] 图4为图3中步骤S24的分步骤流程图；

[0060] 图5为激光测距仪、打磨工具的安装示意图；

[0061] 图6为激光测距仪安装位置和坐标系示意图；

[0062] 图7为扫描方向示意图；

[0063] 图8为打磨点位姿示意图；

[0064] 图9为控制过程流程图；

[0065] 图10为位置轨迹曲线示意图；

[0066] 图11为姿态轨迹曲线示意图；

[0067] 图12为本发明控制系统的结构示意图。

[0068] 其中，附图标记为：

[0069] 打磨机器人 1

[0070] 底盘车 2

[0071] 六维力传感器 31

[0072] 两个激光测距仪 LL、LR

[0073] 处理单元 32

[0074] 坐标系建立模块 321

[0075] 位置坐标获得模块 322

[0076] 设定模块 323

[0077] 处理模块 324

[0078] 期望位姿获得模块 325

[0079] 曲线获得模块 326

[0080] 控制单元 33

[0081] 打磨工具 4

[0082] 支架 41

具体实施方式

[0083] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0084] 本发明的示意性实施例及其说明用于解释本发明，但并不作为对本发明的限定。另外，在附图及实施方式中所使用相同或类似标号的元件/构件是用来代表相同或类似部分。

[0085] 关于本文中所使用的方向用语，例如：上、下、左、右、前或后等，仅是参考附图的方向。因此，使用的方向用语是用来说明并非用来限制本创作。

[0086] 关于本文中所使用的“包含”、“包括”、“具有”、“含有”等等，均为开放性的用语，即意指包含但不限于。

[0087] 请参照图1，图1为本发明控制方法的流程图。如图1所示，本发明用于打磨机器人的控制方法包括以下步骤：

[0088] 步骤S1：对打磨工具及激光测距仪进行安装定位。

[0089] 请参照图2，并结合图1，图2为图1中步骤S1的分步骤流程图。如图2所示，于步骤S1中包括：

[0090] 步骤S11：在机器人的末端依次安装六维力传感器、打磨工具和两个激光测距仪。具体地说，请结合图5，图5为激光测距仪、打磨工具的安装示意图。如图5所示，将打磨机器人1安装于可移动的底盘车2上，在打磨机器人1的末端依次安装六维力传感器31、打磨工具
4和两个激光测距仪LL、LR，其中打磨工具4与六维力传感器31串联连接，两个激光测距仪LL、LR安装在打磨工具4的支架41上。

[0091] 需要说明的是，在本实施例中，虽然本发明采用底盘车移动打磨机器人，但本发明并不以此为限，在其他实施例中还可以采用其他能够起到地面移动作用的设备或装置，例如地面移动导轨等。

[0092] 步骤S12：建立机器人基座标系、末端坐标系和打磨工具中心点坐标系。

[0093] 步骤S13：通过测量或标定分别得到打磨工具中心点在末端坐标系下的位置坐标，以及两个激光测距仪在末端坐标系下的位置坐标，根据所建末端坐标系和打磨工具中心点坐标系之间的关系，得到打磨工具在末端坐标系下的姿态数据。

[0094] 具体地说，请结合图6，图6为激光测距仪安装位置和坐标系示意图。如图6所示，建立机器人基座标系(BX，BY，BZ)(图7所示)、末端坐标系(EX，EY，EZ)和打磨工具中心点坐标系(TX，TY，TZ)，通过测量或标定分别得到打磨工具中心点在末端坐标系(EX，EY，EZ)下的位置坐标(0，0，H)，以及激光测距仪LR在末端坐标系下的位置坐标(LRx，LRy，Rh)和激光测距仪LL在末端坐标系下的位置坐标(LLx，LLy，Lh)。

[0095] 步骤S2：通过激光测距仪对待打磨件进行外形数据扫描，获得每一扫描点处机器人的位置数据、姿态数据及激光测距仪的距离数据，根据位置数据、姿态数据及距离数据获得扫描曲面的打磨轨迹曲线。

[0096] 请参照图3，图3为图1中步骤S2的分步骤流程图。如图3所示，于步骤S2中包括：

[0097] 步骤S21：设定打磨工具的工作范围和待打磨件的打磨区域。具体地说，根据打磨机器人的型号和待打磨件的尺寸确定打磨工作范围和待打磨件的打磨区域，将底盘车2移动到打磨区域。

[0098] 步骤S22：通过控制激光测距仪在随机械臂移动情况下对待打磨件进行外形数据扫描，读取两个激光测距仪的距离数据。具体地说，根据步骤S21确定的打磨工作范围，控制打磨机器人1带动激光测距仪LL、LR移动到待打磨件上方，请参照图7，图7为扫描方向示意图，其中箭头方向代表扫描方向，此时打磨工具中心点为B，控制打磨机器人1在竖直平面内沿任意直线由打磨工具中心点为B运动到C点，移动过程中激光测距仪LL、LR实时扫描待打磨件的待打磨表面，实时获得末端坐标系(EX，EY，EZ)的位置数据P和姿态数据Q，以及此时两侧激光测距仪LL、LR距待打磨表面的距离数据LRD、LLD，完成数据采集。

[0099] 步骤S23：对所采集距离数据进行信号处理。具体地说，整合位置数据P和姿态数据Q，以及将此时两侧激光测距仪LL、LR距待打磨表面的距离数据LRD、LLD去除异常值，并对数据进行滤波和平滑处理，在本实施例中，采用数字信号处理方法对位于末端坐标系下的位置数据进行异常采样值剔除、滤波和平滑处理，在本实施例中，采用最小二乘法和滑动平均法对数据进行滤波和平滑处理，得到可用的数据。

[0100] 步骤S24：结合所建末端坐标系及激光测距仪安装位置，根据处理后每一扫描点的位置数据、姿态数据以及与该扫描点所对应的机器人的位置数据和姿态数据，通过计算获得打磨工具中心点所在平面在末端坐标系下表示的期望位置数据和姿态数据。

[0101] 步骤S25：将打磨工具中心点的期望位置和期望姿态转换到基座标系下，根据基座标系下的期望位置和期望姿态获得打磨轨迹曲线。

[0102] 请参照图4，图4为图3中步骤S24的分步骤流程图。如图4所示，步骤S24中包括：

[0103] 步骤S241：根据不同时刻每一扫描点与相邻扫描点的距离数据确定一个平面；

[0104] 步骤S242：根据平面和末端坐标系各轴方向构造第一辅助向量和第二辅助向量，根据第一辅助向量f1和第二辅助向量f2获得第一单位向量a，根据末端坐标系设定第二单位向量n，根据第一单位向量及第二单位向量得到第三单位向量，同时获得拟合平面在末端坐标系下的位置和姿态；

[0105] 步骤S243：根据拟合平面在末端坐标系下的位置和姿态，选取两个激光测距仪安装位置连线中垂面与扫描点连线交点作为打磨点，获得每一扫描时刻对应的打磨工具中心点在末端坐标系下的期望位置和期望姿态。

[0106] 具体地说，请参照图8，图8为打磨点位姿示意图。如图8所示，上述采集到的数据LRD和LLD为从1～i的数组，分别为[LRD1,LRD2…LRDi，LRDi+1…LRDn]和[LLD1,LLD2…LLDi，LLDi+1…LLDn]，在i时刻，两个激光测距仪采集到的数据分别为LRDi和LLDi，由平面几何定理知，三个点可以确定一个平面，因此，不失一般性，从数据数组中引入右侧激光测距仪下一时刻数据LRDi+1，得到一个平面，当由i到i+1之间的时间间隔较短时，得到的平面可认为是待打磨表面为曲面，利用上述三点构造辅助向量f1和f2，取与EX轴平行的单位向量作为平面的n向量，同时利用辅助向量f1与f2的叉积运算，单位化后可求得平面的法向量a。

[0107] a＝norm(f1×f2)

[0108] 利用单位向量n和a，可以得到单位向量o。

[0109] o＝a×n

[0110] 从而求得拟合平面在末端坐标系(EX，EY，EZ)EX，EY，EZ下的位置和姿态。

[0111]

[0112] 其中，n、o、a分别为打磨点所在平面的三个坐标轴，EX、EY、EZ分别为末端坐标系的三个坐标轴。

[0113] 选取EX、EY平面与两个激光测距仪连线交点p作为打磨点，则打磨工具中心点末端坐标系(EX，EY，EZ)下的期望位置为EP＝(0，(LLy+Lry)/2，0)。至此得到每一个扫描时间点E E时，打磨工具中心点在末端坐标系(EX，EY，EZ)下的期望位置P和期望姿态RP。

[0114] 步骤S25：将期望位置和期望姿态转换到基座标系下，根据基座标系下的期望位置和期望姿态获得打磨轨迹曲线。具体地说，由于各个扫描时间点下机器人末端坐标系的位置数据P和姿态数据Q已在扫描过程中实时获得，因此，利用如下公式可将打磨工具点在末端坐标系(EX，EY，EZ)下的期望位置和姿态转换到基座标系(BX，BY，BZ)下：

[0115]

[0116] 其中，BTE为末端坐标系在基座标系下的位置和姿态矩阵； 为打磨工具中心点在末端坐标系下的位置和姿态矩阵，其中ERP为3x3的姿态矩阵，EP为3x1的位置向量。

[0117] 利用上述公式获得打磨轨迹曲线。其中，打磨轨迹曲线包括：位置轨迹曲线和姿态轨迹曲线，利用打磨机器人位置和姿态控制解耦这一特点完成对打磨机器人位置和姿态进行实时计算控制，计算得到的结果可直接用于打磨机器人末端打磨工具位姿的实时控制。

[0118] 需要说明的是，在本文实施例中所有位姿变换都是基于矩阵表示的，但本发明并不以此为限，在其他实施例中也可以通过指数积等其他方法表示。

[0119] 步骤S3：根据打磨轨迹曲线进行位置控制模式下的曲面打磨工作，同时结合力控模式对待打磨件进行打磨位置的修正处理。具体地说，于步骤S3中根据打磨轨迹曲线，使打磨机器人在基于位置控制模式和力控模式下渐进至打磨起始点，在力位混合控制模式下对打磨的位置进行修正，完成叶片的打磨工作。

[0120] 具体地说，请参照图9，图9为控制过程流程图。如图9所示，结合上述拟合的轨迹曲线，使打磨机器人在位置控制和基于力控模式下渐进至打磨起始点，在力位混合模式下完成拟合轨迹的打磨工作。控制打磨机器人在位置模式下渐进至打磨起始点，渐进过程中机器人在力控模式下始终测量、判定此时的接触力大小，直至接触力满足力控模式作用条件，此时力控优先级高于位置控制优先级，不仅保证了打磨工具与待打磨件之间始终存在一定的打磨力，同时又能结合激光传感器采集拟合到的表面数据保证末端工具可以完美贴合待打磨件表面形状。然后调整打磨机器人位姿，改变激光测距仪扫描区域，完成该工段打磨工作，移动底盘车2至其他工段，完成待打磨件的整体打磨工作。

[0121] 其中，接触力是通过装设于打磨机器人上的六维力传感器采集获得的，控制处理单元判定接触力的大小，直至接触力满足力控模式作用条件，开启力控模式，并控制打磨机器人沿打磨轨迹曲线进行打磨工作。

[0122] 以下，结合具体实施例，以待打磨件为叶片对本发明的用于打磨机器人的控制方法进行说明。

[0123] 1.依次安装六维力传感器、打磨工具和激光测距仪，对六维力传感器和激光测距仪进行调式、标定和校准，得到激光测距仪在末端坐标系下的安装位置；

[0124] 2.在打磨机器人工作范围，控制打磨机器人和激光测距仪移动到待打磨工件上方，沿空间直线运动，进行叶片外形数据扫描，得到各个扫描点的位置和姿态以及此时两激光测距仪的数值，部分数值如下表所示：

[0125] 表1各扫描点序号、机器人位姿及传感器数据表

[0126]14055 1 12381 69 23285 23050 388 1369 2439 2554 1
14011 1 12378 67 23285 23050 386 1366 2430 2554 2
13954 1 12380 68 23285 23050 387 1371 2422 2526 3
… … … … … … … … … … …
7990 1 12379 68 23285 23050 386 6645 2127 2251 116
7935 1 12379 68 23285 23050 386 6641 2133 2263 117

[0127] 3.利用本文所提算法，将上述表格中的数值进行分析、处理和拟合，得到叶片在基座标系下的位置和姿态数据，也即打磨时机械臂末端打磨工具中心点的位置以及打磨工具的姿态。根据以上数据拟合出打磨轨迹曲线，如图10所示，图10为位置轨迹曲线示意图，其中Q1代表打磨工具中心的x方向的位置变化曲线，Q2代表打磨工具中心的Y方向的位置变化曲线，Q3代表打磨工具中心Z方向的位置变化曲线；如图11所示，图11为姿态轨迹曲线示意图，其中Q4代表代表X方向的转角变化曲线，Q5代表Y方向的转角变化曲线；

[0128] 4.结合扫描的叶片的表面形状，可初步确定所求角度满足实际形状，控制处理单元根据位置和姿态数据，控制打磨机器人在力位混合控制方法下根据拟合后的打磨轨迹曲线进行跟踪打磨。

[0129] 请参照图12，图12为本发明控制系统的结构示意图。如图12所示，本发明控制系统包括：六维力传感器31、两个激光测距仪LL、LR、处理单元32及控制单元33；六维力传感器31与打磨工具4串联连接，六维力传感器31采集打磨机器人1渐进至打磨起始点过程中的接触力；两个激光测距仪LL、LR安装在打磨工具4的支架41上；处理单元，电性连接于两个激光测距仪LL、LR；控制单元33电性连接于打磨机器人1及六维力传感器31及处理单元32；处理单元32对安装好的打磨工具4及激光测距仪LL、LR进行定位后，激光测距仪LL、LR对待打磨件进行外形数据扫描，获得每一扫描点的位置数据、姿态数据及距离数据，处理单元32根据位置数据、姿态数据及距离数据获得打磨轨迹曲线，处理单元32输出打磨轨迹曲线至控制单元33，控制单元33根据所述打磨轨迹曲线结合力控模式控制打磨机器人1对待打磨件进行打磨处理。

[0130] 具体地说，控制单元33根据打磨轨迹曲线，控制打磨机器人在位置控制和基于阻抗控制的力控模式下渐进至打磨起始点，在力位混合模式下完成打磨工作。

[0131] 其中，处理单元32包括：

[0132] 坐标系建立模块321，建立机器人基座标系、末端坐标系和打磨工具中心点坐标系；

[0133] 位置坐标获得模块322，通过测量或标定分别得到打磨工具中心点在末端坐标系下的位置坐标，以及两个激光测距仪LL、LR在末端坐标系下的位置坐标；

[0134] 设定模块323，设定打磨工作范围和待打磨件的打磨区域；

[0135] 处理模块324，对位置数据、姿态数据及距离数据进行处理，其中处理模块采用最小二乘法和滑动平均法对位于末端坐标系下的位置数据、姿态数据及距离数据进行滤波和平滑处理；

[0136] 期望位姿获得模块325，根据处理后的位置数据、姿态数据及距离数据获得打磨工具中心点在末端坐标系下的期望位置和期望姿态，其中，期望位姿获得模块325根据第i时刻及第i+1时刻的位置数据、姿态数据及距离数据确定一平面；根据平面构造第一辅助向量和第二辅助向量，根据第一辅助向量和第二辅助向量获得第一单位向量，设定第二单位向量，根据第一单位向量及第二单位向量获得拟合平面在末端坐标系下的位置和姿态；根据拟合平面在末端坐标系下的位置和姿态；选取两个激光测距仪安装位置连线中垂面与扫描点连线交点作为打磨点，获得每一扫描点的打磨工具中心点在末端坐标系下的期望位置和期望姿态；

[0137] 曲线获得模块326，将期望位置和期望姿态转换到基座标系下，根据基座标系下的期望位置和期望姿态获得打磨轨迹曲线。

[0138] 其中，期望位姿获得模块325根据以下公式将期望位置和期望姿态转换到基座标系下：

[0139]

[0140] 其中，BTE为末端坐标系在基座标系下的位置和姿态矩阵；

[0141] 为打磨工具中心点在末端坐标系下的位置和姿态矩阵，其中ERP为3x3的姿态矩阵，EP为3x1的位置向量。

[0142] 值得注意的是，在本发明的另一实施例中，控制单元33可为独立设置或可集成于打磨机器人的控制系统中。

[0143] 综上所述，本发明提出的用于打磨机器人的控制方法及系统具有以下优点：

[0144] 1、本发明提出了利用两个激光测距仪完成打磨平面姿态拟合这一思路，并给出了姿态拟合方法。

[0145] 2、本发明将曲面拟合和力位混合修正结合起来用于打磨装置的控制，既通过曲面拟合和位置控制保证了打磨时给定轨迹的精度和打磨装置的刚度，又通过力控保证了打磨过程中的柔性和贴合程度。

[0146] 3、本发明中任意相邻扫描时间点处的位置和姿态采用线性插值进行过渡，并公开了插值方法，保证了打磨过程中姿态变化的连续性。

[0147] 基于此，使得本发明在用于打磨机器人后，在价格相对较低的力传感器和激光测距仪等设备下，既能保证曲面拟合的精度以及打磨过程中接触力和打磨位置的双重控制，实时在线修正打磨轨迹，保证打磨精度，又能避免扫描仪等昂贵外部设备以及摆脱对打磨工件CAD模型和CAM软件的依赖，降低了生产成本。

[0148] 尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。