CN114407006A - 用于航空发动机叶盘修复与三维重建的控制方法及其应用 - Google Patents

Abstract

本发明公开了用于航空发动机叶盘修复与三维重建的控制方法及其应用，包括Step1.将运动规划控制器与六自由度工业机器人控制器和旋转工作台伺服驱动器连接；Step2.在旋转工作台上安装航空发动机整体叶盘；Step3.利用D‑H法构建六机器人空间连杆和旋转工作台的坐标系；Step4.通过运动规划控制器控制工业机器人和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划；本方法通过在运动规划控制器内嵌入快速探索随机树算法和前后向逆运动学算法自动计算机器人的行进路径，能够完成航空发动机整体叶盘修复成型与表面强化三维轨迹重建任务，具有控制鲁棒性好、修复和重建精度高的特点。

Description

用于航空发动机叶盘修复与三维重建的控制方法及其应用

技术领域

本发明涉及机器人运动控制技术领域，具体涉及用于航空发动机叶盘修复与三维重建的控制方法及其应用。

背景技术

航空发动机叶盘作为航空发动机的关键组成部分，其自身的可靠性及工况安全对于航空发动机极其重要；然而，由于其长期处于高温、高压、振动和腐蚀且承载量大的工作环境中，这也使航空发动机叶盘很容易出现烧损、疲劳、点蚀、裂纹乃至断裂等缺陷；由于航空发动机叶盘的制造成本，通常采用三维重建航空发动机叶盘的数字化模型，从而准确的定位航空发动机叶盘受损位置，对损伤叶盘进行再制造修复；

由于航空发动机整体叶盘带有多重复杂曲面，且具有旋转不变性，针对其特性设计一种能以较低的计算成本自动计算出航空发动机整体叶盘修复成型与表面强化三维轨迹重建任务中工业机器人运动轨迹，同时使安装航空发动机整体叶盘的旋转工作台配合拍摄任务的规划控制方法及其配套的运动规划控制器具有重大的实际意义。

发明内容

针对上述存在的问题，本发明旨在提供用于航空发动机叶盘修复与三维重建的控制方法及其应用，通过本控制方法能够对三维轨迹重建航空发动机整体叶盘所需的六自由度工业机器人与旋转工作台进行精准、灵活的控制，从而完成航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划，具有控制鲁棒性好、修复和重建精度高的特点。

为了实现上述目的，本发明所采用的技术方案如下：

用于航空发动机叶盘修复与三维重建的控制方法，包括步骤：

Step1.将内置快速探索随机树算法和前后向逆运动学算法的运动规划控制器与六自由度工业机器人控制器和旋转工作台伺服驱动器连接；

Step2.在旋转工作台上安装航空发动机整体叶盘；

Step3.利用D-H法构建六自由度工业机器人空间连杆和旋转工作台的工作坐标系；

Step4.通过运动规划控制器控制六自由度工业机器人和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划。

优选的，步骤Step4所述的运动规划控制器控制六自由度工业机器人控制器和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划的具体过程为利用快速探索随机树算法进行工作空间内无碰撞可行路径的探索，其具体算法过程包括：

Step401.输入障碍物中心位置坐标为P_j，j＝1,2,..,n，障碍物球形包络半径r_i，i＝1,2,..,n,六自由度工业机器人的初始关节角为θ(0)＝θ_a＝(θ₁(0),θ₂(0),..,θ_m(0))，初始关节位置为P_i，i＝1,2,..,n，末端执行器的初始位置为P_Es和目标物体的位置为P_Ed；

Step402.以末端执行器的初始位置P_Es为随机树的根节点，人工引入多个目标点P_goal，设选中目标点P_goal的概率为x_goal，生成一个随机数x,如果x＜x_goal，则P_Ed＝P_rand＝P_goal；如果x＞x_goal，则在惯性空间中随机生成一个随机点P_rand，从所构建的随机树中寻找距离随机点P_rand最近的节点P_near，然后在最近节点P_near和随机点P_rand之间的连线上确定新节P_new点，新节点P_new满足方程|P_near-P_new|＝y，y为随机树的步长；

Step403.将新节点P_new作为工业机器人末端执行器的位置，然后将工业机器人的关节角θ_new和关节位置P_jiont作为逆运动学问题进行计算；

Step404.判断新节点是否与障碍物有碰撞，若没有碰撞则将新节点添加到随机树上，若碰撞了就剔除该新节点，该新节点作废；

Step405.检测是否为目标位置，或者是否在目标位置附近附近，判断标准为当时，迭代结束，若时，则返回步骤S2，继续迭代；

Step406.从目标节点开始，依次搜索随机树中对应的父节点，直到初始节点；

Step407.然后，获得的节点代表计划的避障路径的路径点，从而获得六自由度工业机器人的避障路径。

优选的，步骤Step403所述的将工业机器人的关节角θ_new和关节位置P_jiont作为逆运动学问题，求解六自由度工业机器人每个关节的位置的过程为：利用前后向逆运动学算法在一条线上定位一个点来找到每个关节位置，其具体过程包括：

第一阶段：从六自由度工业机器人末端执行器开始，向内移动到根节点，沿着链节依次找到每个关节位置；六自由度工业机器人每个关节的位置与关节角度用P_i,i＝1,2…N，θ＝1，2…N表示，P_E′为工业机器人末端执行器的新位置，P₁′为工业机器人根节点的新位置，P_i′为工业机器人第i个关节的新位置，表达式如下：

第二阶段：从根节点开始，向外移动到六自由度工业机器人末端效应器，沿着链节依次找到每个关节位置；该阶段要求根节点处在最初位置，P″_i为每个关节点的新位置，P″_E为末端执行器位置，表达式如下：

第三阶段：一次完整操作后，将工业机器人末端执行器的新位置P″_E作为新节点P_new，如下式所示：

P_new＝P″_E，P_inew＝P″_i

将工业机器人末端执行器下一个到达的位置，设为P_g,利用FABRIK算法重新计算每个关节位置；设置阈值λ，经过多次次完整操作，在基座位置不变的情况下，使末端执行器多次迭代后的位置P_gnew与预设的P_g之间的距离小于λ，|P_gnew-P_g|<λ，当满足上式条件时，停止迭代。

优选的，步骤Step404所述的新节点与障碍物的碰撞判断过程利用空间几何包络原理的碰撞检测方法控制六自由度工业机器人进行，所述碰撞检测方法控制六自由度工业机器人在动作时各环节的包络过程包括：

(1)设工业机器人连杆j一端关节j的位置为：

p_j＝(x_pjw_pj,yw_pj,z_pjw_pj,w_pj) (3)

另一端关节j+1的位置为：

p_j+1＝(x_pj+1w_pj+1,y_pj+1w_pj+1,z_pj+1w_pj+1,w_pj+1) (4)

连杆j包络线圆柱体的直径为2r_Li，圆柱中心线空间直线的参数方程为:

(2)使用最小直径球体作为障碍物的包络，障碍物i的中心位置为：p_oi＝(x_oiw_oi,y_oiw_oi,z_oiw_oi,w_oi) (6)，

障碍物i的包络的球体直径是2r_oj，球体的参数方程为：

(3)当球体中心位置到圆柱体中心线的垂线与圆柱体中心线的交点落在中心线上时，球体中心位置和圆柱体中心线之间的最小距离为：

(4)当球体中心位置到圆柱体中心线的垂线与圆柱体中心线的交点落在中心线的延长线上时，球体中心位置和圆柱体中心线之间的最小距离为：

(9)

当d_Ljoi≤r_Lj+r_oj时，工业机器人连杆j与障碍物i产生碰撞；

当d_Ljoi＞r_Lj+r_di时，工业机器人连杆j与障碍物i不产生碰撞。

优选的，所述的控制方法应用于运动规划控制器。

用于航空发动机叶盘修复与三维重建的控制方法的应用，所述的运动规划控制器包括内置快速探索随机树算法和前后向逆运动学算法的工业PC机，所述工业PC机通过通讯模块与六自由度工业机器人控制器和装有航空发动机整体叶盘的旋转工作台伺服驱动器连接，通过快速探索随机树算法和前后向逆运动学算法自动计算出六自由度工业机器人运动至最佳三维轨迹重建点的行进路径，同时处理轨迹重建中的静态约束，控制六自由度工业机器人的拖动或手动路径规划，以及自定义控制旋转工作台的频率和旋转角度，同时配置SATA硬盘，用于过程数据和结果数据的储存与管理。

优选的，所述的工业PC机上还设置有内置的上位机软件，且配备有USB、RS485、RS232、网口接口，与六自由度工业机器人控制器以及力传感器、视觉传感器、编码器连接。

优选的，所述的通讯模块支持Ethernet、Socket、OPC、Profibus。

优选的，所述的旋转工作台的伺服驱动器与六自由度工业机器人控制器连接，且工业机器人控制器为从站，运动规划控制器为主站。

优选的，所述的运动规划控制器上设置有用户交互UI界面，且同时配备有集成专用结构体、Public函数、构造及析构函数。

本发明的有益效果是：本发明公开了用于航空发动机叶盘修复与三维重建的控制方法及其应用，与现有技术相比，本发明的改进之处在于：

本发明提出了用于航空发动机叶盘修复与三维重建的控制方法及其应用，本控制方法通过在运动规划控制器内嵌入快速探索随机树(RRT)算法和前后向逆运动学(FABRIK)算法自动计算出工业机器人运动至最佳建模点的行进路径，对由六自由度工业机器人与旋转工作台所搭建的航空发动机整体叶盘三维轨迹重建平台，进行精准、灵活的控制，来很好的处理静态约束，如避免关节限制、运动学奇点或碰撞等问题，同时以较低的计算成本自动计算出三维扫描任务中约束七自由度运动规划，从而完成航空发动机整体叶盘修复成型与表面强化三维轨迹重建任务，具有控制鲁棒性好、修复和重建精度高的优点。

附图说明

图1为本发明用于航空发动机叶盘修复与三维重建的控制方法的流程图。

图2为本发明实施例1航空发动机叶盘修复建模图。

图3为本发明实施例1航空发动机叶盘现场修复图。

具体实施方式

为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。

参照附图1-3所示的一种用于航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划控制方法，包括步骤：

Step1.将内置快速探索随机树算法和前后向逆运动学算法的运动规划控制器与六自由度工业机器人控制器和旋转工作台伺服驱动器连接；

Step2.在旋转工作台上安装航空发动机整体叶盘；

Step3.利用D-H(Denavit-Hartenberg)法构建六自由度工业机器人空间连杆和旋转工作台的工作坐标系；

Step4.通过运动规划控制器控制六自由度工业机器人控制器和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划。

优选的，步骤Step4所述的运动规划控制器控制六自由度工业机器人和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划的具体过程为利用快速探索随机树算法(RRT)进行工作空间内无碰撞可行路径的探索，有效地探索整个工作空间，为末端执行器找到一条无碰撞的可行路径；具体算法步骤如下：

Step401.输入障碍物中心位置坐标为P_j，j＝1,2,..,n，障碍物球形包络半径r_i，i＝1,2,..,n,六自由度工业机器人的初始关节角为θ(0)＝θ_a＝(θ₁(0),θ₂(0),..,θ_m(0))，初始关节位置为P_i，i＝1,2,..,n，末端执行器的初始位置为P_Es和目标物体的位置为P_Ed；

Step402.以末端执行器的初始位置P_Es为随机树的根节点，人工引入多个目标点P_goal，设选中目标点P_goal的概率为x_goal，生成一个随机数x,如果x＜x_goal，则P_Ed＝P_rand＝P_goal；如果x＞x_goal，则在惯性空间中随机生成一个随机点P_rand，从所构建的随机树中寻找距离随机点P_rand最近的节点P_near，然后在最近节点P_near和随机点P_rand之间的连线上确定新节P_new点，新节点P_new满足方程|P_near-P_new|＝y，y为随机树的步长；

Step403.将新节点P_new作为工业机器人末端执行器的位置，然后将工业机器人的关节角θ_new和关节位置P_jiont作为逆运动学问题进行计算；

Step404.判断新节点是否与障碍物有碰撞，若没有碰撞则将新节点添加到随机树上，若碰撞了就剔除该新节点，该新节点作废；

Step405.检测是否为目标位置，或者是否在目标位置附近附近，判断标准为当时，迭代结束，若时，则返回步骤S2，继续迭代；

Step406.从目标节点开始，依次搜索随机树中对应的父节点，直到初始节点；

Step407.然后，获得的节点代表计划的避障路径的路径点，从而获得工业机器人的避障路径，得到六自由度工业机器人建模的最佳工作路径。

优选的，在步骤Step403中，针对六自由度工业机器人的关节角θ_new和关节位置P_jiont作为逆运动学问题，为避免使用旋转角度或矩阵，引入并改进了前后向逆运动学(FABRIK)算法，求解工业机器人每个关节的位置的过程为：利用前后向逆运动学算法在一条线上定位一个点来找到每个关节位置，其具体过程包括：

第一阶段：从六自由度工业机器人末端执行器开始，向内移动到根节点，沿着链节依次找到每个关节位置；工业机器人每个关节的位置与关节角度用P_i,i＝1,2…N，θ＝1，2…N表示，P′_E为工业机器人末端执行器的新位置，P′₁为工业机器人根节点的新位置，P′_i为工业机器人第i个关节的新位置，表达式如下：

第二阶段：从根节点开始，向外移动到六自由度工业机器人末端效应器，沿着链节依次找到每个关节位置；该阶段要求根节点处在最初位置，P″_i为每个关节点的新位置，P″_E为末端执行器位置，表达式如下：

第三阶段：一次完整操作(第一阶段和第二阶段)，将工业机器人末端执行器的新位置P″_E作为新节点P_new，如下式所示：

P_new＝P″_E，P_inew＝P″_i

将工业机器人末端执行器下一个到达的位置，设为P_g,利用FABRIK算法重新计算每个关节位置；设置阈值λ，经过多次次完整操作，在基座位置不变的情况下，使末端执行器多次迭代后的位置P_gnew与预设的P_g之间的距离小于λ，|P_gnew-P_g|<λ，当满足上式条件时，停止迭代。

优选的，在步骤Step404中，为避免与六自由度工业机器人在工作过程中与旋转工作台以及障碍物之间的碰撞，利用空间几何包络原理的碰撞检测方法来进行新节点与障碍物的碰撞判断,使用最小直径圆柱体作为六自由度工业机器人各环节的包络，最小直径球体作为障碍物的包络，其具体过程包括：

(1)设工业机器人连杆j一端关节j的位置为：

p_j＝(x_pjw_pj,yw_pj,z_pjw_pj,w_pj) (3)

另一端关节j+1的位置为：

p_j+1＝(x_pj+1w_pj+1,y_pj+1w_pj+1,z_pj+1w_pj+1,w_pj+1) (4)

连杆j包络线圆柱体的直径为2r_Li，圆柱中心线空间直线的参数方程为:

(2)使用最小直径球体作为障碍物的包络，障碍物i的中心位置为：p_oi＝(x_oiw_oi,y_oiw_oi,z_oiw_oi,w_oi) (6)，

障碍物i的包络的球体直径是2r_oj，球体的参数方程为：

(3)当球体中心位置到圆柱体中心线的垂线与圆柱体中心线的交点落在中心线上时，球体中心位置和圆柱体中心线之间的最小距离为：

(4)当球体中心位置到圆柱体中心线的垂线与圆柱体中心线的交点落在中心线的延长线上时，球体中心位置和圆柱体中心线之间的最小距离为：

(9)

当d_Ljoi≤r_Lj+r_oj时，工业机器人连杆j与障碍物i产生碰撞；

当d_Ljoi＞r_Lj+r_di时，工业机器人连杆j与障碍物i不产生碰撞。

一种应用上述控制方法实现的用于航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划控制运动规划控制器，所述控制器包括内置快速探索随机树算法和前后向逆运动学算法的工业PC机，所述工业PC机通过通讯模块与六自由度工业机器人控制器和装有航空发动机整体叶盘的旋转工作台伺服驱动器连接，所述的规划控制方法由运动规划控制器实现，通过快速探索随机树算法(RRT)和前后向逆运动学算法(FABRIK)自动计算出六自由度工业机器人运动至最佳三维轨迹重建点的行进路径，同时处理轨迹重建中的静态约束，如避免关节限制、运动学奇点或碰撞等，控制六自由度工业机器人的拖动或手动路径规划，以及自定义控制旋转工作台的频率和旋转角度，同时配置SATA硬盘，用于过程数据和结果数据的储存与管理。

优选的，所述的工业PC机上还设置有内置的上位机软件，且配备USB、RS485、RS232、网口(支持Ethernet协议)等多路多种接口，可满足三维重建所需的六自由度工业机器人控制器以及力传感器、视觉传感器、编码器的输入输出需求。

优选的，为便于进行数据传输，保证数据传输速度和精确度，所述的通讯模块主要支持Ethernet、Socket、OPC、Profibus等，根据三维重建工作中工业机器人控制器以及旋转工作台伺服驱动器所支持的通讯协议，在运动规划控制器中选择适配的通讯协议，旋转工作台的伺服驱动器与六自由度工业机器人控制器连接，并将该工业机器人控制器设置为从站，运动规划控制器设置为主站，即可完成通讯。

优选的，为便于进行数据输入和人为控制，所述的运动规划控制器提供用户交互UI界面，实现用户对工业机器人的拖动或手动路径规划，以及自定义控制旋转工作台的频率、旋转角度，同时配备有集成专用结构体、Public函数、构造及析构函数等，便于高级用户通过指令集实现深度定制功能。

优选的，所述的嵌在工业PC机内的上位机软件基于C语言的代码编写与算法开发，以VC++为主体的代码，Visual Studio编译环境；可根据用户指定任务，自动计算工业机器人的运动轨迹，开放Public函数，便于用户通过指令集实现定制功能；系统程序内置相关功能模块，具有关节极限回避，奇异点规避，碰撞干涉预测等功能。

实施例1：利用本发明所述的运动规划控制器实施机器人轨迹规划方法进行叶盘修复成形的具体过程包括

步骤S1：将面阵结构光相机安装在工业机械臂臂端，同时将待测叶片装夹在旋转工作台上；

步骤S2：定义工业机器人执行器末端坐标系{O₀}，基座坐标系{O₁},旋转工作台坐标系{O₂}；

步骤S3：通过运动规划控制器内设的D-H(Denavit-Hartenberg)法建立工业机器人与旋转工作台的运动学方程；

步骤S4：根据待测扇叶复杂程度设置旋转工作台的单次旋转角度，以及停留时间；

步骤S501：指定2个测量点为工业机器人末端执行器的目标位置；

步骤S502：使用最小直径圆柱体作为六自由度工业机器人各环节的包络，最小直径球体作为旋转工作台以及待测扇叶和结构光相机包络；

步骤S503：在RRT算法中引入人工引导生成随机点发以及前后向逆运动学(FABRIK)算法，探索工作空间，搜寻工业机器人末端执行器从初始位置到目标位置的无碰撞可行路径；

步骤S504：根据工业机器人运动起始节点所对应的时间信息，规划旋转工作台的旋转角度与频率，工业机器人末端执行器到达测量点后，旋转工作台开始转动，离开测量点前，旋转工作台停止转动；

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (10)

1.用于航空发动机叶盘修复与三维重建的控制方法，其特征在于：包括步骤：

Step1.将内置快速探索随机树算法和前后向逆运动学算法的运动规划控制器与六自由度工业机器人控制器和旋转工作台伺服驱动器连接；

Step2.在旋转工作台上安装航空发动机整体叶盘；

Step3.利用D-H法构建六自由度工业机器人空间连杆和旋转工作台的工作坐标系；

Step4.通过运动规划控制器控制六自由度工业机器人和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划。

2.根据权利要求1所述的用于航空发动机叶盘修复与三维重建的控制方法，其特征在于：步骤Step4所述的运动规划控制器控制六自由度工业机器人控制器和旋转工作台工作，进行航空发动机整体叶盘修复成型与表面强化三维轨迹重建规划的具体过程为利用快速探索随机树算法进行工作空间内无碰撞可行路径的探索，其具体算法过程包括：

Step401.输入障碍物中心位置坐标为P_j，j＝1,2,..,n，障碍物球形包络半径r_i，i＝1,2,..,n,六自由度工业机器人的初始关节角为θ(0)＝θ_a＝(θ₁(0),θ₂(0),..,θ_m(0))，初始关节位置为P_i，i＝1,2,..,n，末端执行器的初始位置为P_Es和目标物体的位置为P_Ed；

Step402.以末端执行器的初始位置P_Es为随机树的根节点，人工引入多个目标点P_goal，设选中目标点P_goal的概率为x_goal，生成一个随机数x,如果x＜x_goal，则P_Ed＝P_rand＝P_goal；如果x＞x_goal，则在惯性空间中随机生成一个随机点P_rand，从所构建的随机树中寻找距离随机点P_rand最近的节点P_near，然后在最近节点P_near和随机点P_rand之间的连线上确定新节P_new点，新节点P_new满足方程|P_near-P_new|＝y，y为随机树的步长；

Step403.将新节点P_new作为工业机器人末端执行器的位置，然后将工业机器人的关节角θ_new和关节位置P_jiont作为逆运动学问题进行计算；

Step404.判断新节点是否与障碍物有碰撞，若没有碰撞则将新节点添加到随机树上，若碰撞了就剔除该新节点，该新节点作废；

Step405.检测是否为目标位置，或者是否在目标位置附近附近，判断标准为当时，迭代结束，若时，则返回步骤S2，继续迭代；

Step406.从目标节点开始，依次搜索随机树中对应的父节点，直到初始节点；

Step407.然后，获得的节点代表计划的避障路径的路径点，从而获得六自由度工业机器人的避障路径。

3.根据权利要求2所述的用于航空发动机叶盘修复与三维重建的控制方法，其特征在于：步骤Step403所述的将工业机器人的关节角θ_new和关节位置P_jiont作为逆运动学问题，求解六自由度工业机器人每个关节的位置的过程为：利用前后向逆运动学算法在一条线上定位一个点来找到每个关节位置，其具体过程包括：

第一阶段：从六自由度工业机器人末端执行器开始，向内移动到根节点，沿着链节依次找到每个关节位置；六自由度工业机器人每个关节的位置与关节角度用P_i,i＝1,2…N，θ＝1，2...N表示，P′_E为工业机器人末端执行器的新位置，P₁′为工业机器人根节点的新位置，P_i′为工业机器人第i个关节的新位置，表达式如下：

第二阶段：从根节点开始，向外移动到六自由度工业机器人末端效应器，沿着链节依次找到每个关节位置；该阶段要求根节点处在最初位置，P_i″为每个关节点的新位置，P″_E为末端执行器位置，表达式如下：

第三阶段：一次完整操作后，将工业机器人末端执行器的新位置P″_E作为新节点P_new，如下式所示：

P_new＝P″_E，P_inew＝P″_i

将工业机器人末端执行器下一个到达的位置，设为P_g,利用FABRIK算法重新计算每个关节位置；设置阈值λ，经过多次次完整操作，在基座位置不变的情况下，使末端执行器多次迭代后的位置P_gnew与预设的P_g之间的距离小于λ，|P_gnew-P_g|<λ，当满足上式条件时，停止迭代。

4.根据权利要求2所述的用于航空发动机叶盘修复与三维重建的控制方法，其特征在于：步骤Step404所述的新节点与障碍物的碰撞判断过程利用空间几何包络原理的碰撞检测方法控制六自由度工业机器人进行，所述碰撞检测方法控制六自由度工业机器人在动作时各环节的包络过程包括：

(1)设工业机器人连杆j一端关节j的位置为：

p_j＝(x_pjw_pj,yw_pj,z_pjw_pj,w_pj) (3)

另一端关节j+1的位置为：

p_j+1＝(x_pj+1w_pj+1,y_pj+1w_pj+1,z_pj+1w_pj+1,w_pj+1) (4)

连杆j包络线圆柱体的直径为2r_Li，圆柱中心线空间直线的参数方程为:

(2)使用最小直径球体作为障碍物的包络，障碍物i的中心位置为：p_oi＝(x_oiw_oi,y_oiw_oi,z_oiw_oi,w_oi) (6)，

障碍物i的包络的球体直径是2r_oj，球体的参数方程为：

(3)当球体中心位置到圆柱体中心线的垂线与圆柱体中心线的交点落在中心线上时，球体中心位置和圆柱体中心线之间的最小距离为：

(4)当球体中心位置到圆柱体中心线的垂线与圆柱体中心线的交点落在中心线的延长线上时，球体中心位置和圆柱体中心线之间的最小距离为：

(9)

当d_Ljoi≤r_Lj+r_oj时，工业机器人连杆j与障碍物i产生碰撞；

当d_Ljoi＞r_Lj+r_di时，工业机器人连杆j与障碍物i不产生碰撞。

5.根据权利要求1所述的用于航空发动机叶盘修复与三维重建的控制方法，其特征在于：所述的控制方法应用于运动规划控制器。

6.根据权利要求5所述的用于航空发动机叶盘修复与三维重建的控制方法的应用，其特征在于：所述的运动规划控制器包括内置快速探索随机树算法和前后向逆运动学算法的工业PC机，所述工业PC机通过通讯模块与六自由度工业机器人控制器和装有航空发动机整体叶盘的旋转工作台伺服驱动器连接，通过快速探索随机树算法和前后向逆运动学算法自动计算出六自由度工业机器人运动至最佳三维轨迹重建点的行进路径，同时处理轨迹重建中的静态约束，控制六自由度工业机器人的拖动或手动路径规划，以及自定义控制旋转工作台的频率和旋转角度，同时配置SATA硬盘，用于过程数据和结果数据的储存与管理。

7.根据权利要求6所述的用于航空发动机叶盘修复与三维重建的控制方法的应用，其特征在于：所述的工业PC机上还设置有内置的上位机软件，且配备有USB、RS485、RS232、网口接口，与六自由度工业机器人控制器以及力传感器、视觉传感器、编码器连接。

8.根据权利要求6所述的用于航空发动机叶盘修复与三维重建的控制方法的应用，其特征在于：所述的通讯模块支持Ethernet、Socket、OPC、Profibus。

9.根据权利要求6所述的用于航空发动机叶盘修复与三维重建的控制方法的应用，其特征在于：所述的旋转工作台的伺服驱动器与六自由度工业机器人控制器连接，且工业机器人控制器为从站，运动规划控制器为主站。

10.根据权利要求6所述的用于航空发动机叶盘修复与三维重建的控制方法的应用，其特征在于：所述运动规划控制器上设置有用户交互UI界面，且同时配备有集成专用结构体、Public函数、构造及析构函数。

Images