一种用于煤仓内壁缺陷检测的悬索并联机器人及控制方法与流程

本发明涉及大空间内壁缺陷检测领域和柔索并联机器人领域，尤其涉及一种用于煤仓、电厂烟囱等大空间内壁的裂缝、锈蚀、凹坑、衬板翘起、螺栓缺失等缺陷视觉检测的悬索并联机器人及控制方法。

背景技术：

煤仓是燃煤电厂在生产过程中存储煤的容器，煤仓内衬板一般采用不锈钢或超高分子量聚乙烯板，由于不锈钢衬板价格便宜，韧性好，安装制作简单且耐冲击，目前在燃煤电厂中应用得比较广泛。但是不锈钢会因焊接、表面划伤等原因接触到燃煤中的氯和硫而腐蚀，而衬板腐蚀过度会有脱落的危险，还会造成煤仓受损。

因此，煤仓内壁需要定期检测。但是目前现有的煤仓内壁检测手段主要是人工检测，要将煤仓静置约半个月，等粉尘沉降，有毒气体排出后，依赖工人带防爆照明灯进入煤仓内部一点一点地查看。煤仓的体积庞大，检测周期长，检测一次会耽误电厂不少的生产工作。且煤仓内部粉尘严重，还含有co等有毒气体，对检测工人的安全构成威胁。

专利[zl201720834837.5]采用无人机的方式进入煤仓内部拍摄图片进行瑕疵检测分析,但此方法有无人机螺旋桨扬尘、内定位难度高、无人机在煤灰粉尘、可燃性气体环境中容易引发爆炸等隐患。且无人机续航时间短，负载能力差，煤仓空间尺寸大，使其较难实际运用于生产实际。

技术实现要素：

本发明实施例所要解决的技术问题在于，提供一种用于煤仓内壁缺陷检测的悬索并联机器人及控制方法。可用于煤仓、电厂烟囱等大空间内壁的裂缝等缺陷视觉检测。

为了解决上述技术问题，本发明实施例提供了一种用于煤仓内壁缺陷检测的悬索并联机器人，包括视觉检测动平台(100)、4个伺服机构(200)、4个滑轮机构(300)、4个柔性绳索(202)，所述4个滑轮机构(300)分别固定于煤仓(400)4个拐角处，所述视觉检测动平台与所述4个柔性绳索连接并分别绕设经所述4个滑轮机构与所述4个伺服机构连接；

所述4个伺服机构(200)均包括绕线滚筒(201)、直线轴承(205)、安装底板(208)、压线机构(210)、编码器(2005)、编码器轮(209)、伺服电机(211)，所述绕线滚筒(201)通过联轴器(212)与伺服电机(211)连接，柔性绳索绕经所述压线机构(210)、编码器轮(209)缠绕在绕线滚筒(201)上，所述编码器(2005)通过编码器轮(209)测出柔性绳索(202)的精确长度变化，通过机器人空间姿态控制方法，确认目标空间位置对应的柔性绳索(202)的长度，实现对视觉检测平台的空间位置控制。

进一步地，所述4个伺服机构(200)还包括同步轮组(203)、丝杆(204)、直线轴承(205)、换向轴承(206)、原点开关(207)；

所述丝杆(204)通过所述同步轮组(203)与所述绕线滚筒(201)同步转动，换向轴承(206)设置于滑动块上，所述滑动块设置于所述丝杆(204)上，且与所述直线轴承(205)固定连接，所述原点开关用于检测所述滑动块的滑动极限位置。

更进一步地，所述视觉检测动平台(100)包括2轴无线光学变焦云台相机(106)、绳索安装板(102)、2轴平衡结构(103，105)、以及配重模块(101)，所述2轴无线光学变焦云台相机安装于所述2轴平衡结构底端，所述配重模块(101)设置于顶端，所述绳索安装板设置于中部。

相应地，本发明实施例还提供了一种用于煤仓内壁缺陷检测的悬索并联机器人的控制方法如下步骤。

步骤1.建立动平台空间姿态和绳长关系模型

该四索牵引并联机器人是6自由度欠约束结构，为了描述动平台的位置姿态，建立全局坐标系o-xyz，在动平台中心建立局部坐标系p-xyz。

建立如下式(1)的动平台空间姿态和绳长模型：

将模型(1)经过移项整理得动平台空间姿态和绳长模型(2)(3):

动平台中心点位姿相对于全局坐标系为

式(1)、式(2)中，p^o为局部坐标系中的原点(即动平台中心)相对于全局坐标系o中的位置矢量，p^o＝[px，py，pz]^t；表示全局坐标系o中绳索i在动平台的安装位置,表示局部坐标系p中绳索i在动平台的安装位置，表示全局坐标系o中绳索i与滑轮最高点接触的位置，表示全局坐标系o中绳索i在动平台的安装位置相对于绳索i与滑轮最高点接触的位置的位置矢量；将滑轮i最高点到动平台绳索i的安装位置这段绳索长度，定义为绳索i的长度，记为li；i取值为[1,2,3,4]；

rp表示全局坐标系o相对于动平台p的空间旋转矩阵，其表达式为：

式(4)、(5)中，α表示动平台绕局部坐标系x轴旋转的角度；β表示动平台绕局部坐标系y轴旋转的角度；γ表示动平台绕局部坐标系z轴旋转的角度；其旋转顺序为先绕局部坐标系z轴旋转γ，再绕局部坐标系y轴旋转β；最后绕局部坐标系x轴旋转α。

步骤2.考虑滑轮半径的影响，修正动平台空间姿态和绳长关系模型

步骤2.1根据式(6)、(7)、(8)求θi

θi＝π-θi，1-θi，2#(8)

式(6)～(8)中，表示滑轮的半径大小；表示全局坐标系o中滑轮i的轴心点坐标位置，表示全局坐标系o中绳索i在动平台的安装位置,θi表示滑轮轴心点到滑轮最高点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角；θi，1表示滑轮轴心点到绳索安装点的线段与全局坐标系z轴所成夹角；θi，2表示滑轮轴心点到绳索安装点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角；

步骤2.2建立修正后的绳长模型

式(9)中l′i表示考虑滑轮半径的影响修正后的绳索i的长度；表示滑轮的半径大小；表示全局坐标系o中滑轮i的轴心点坐标位置，表示全局坐标系o中绳索i在动平台的安装位置,θi表示滑轮轴心点到滑轮最高点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角；θi，2表示滑轮轴心点到绳索安装点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角。

步骤3.考虑绳索在受到拉力作用下的形变，修正动平台空间姿态和绳长关系模型

根据胡克定理

得

式(10)、(11)中，e表示金属丝线材的杨氏模量，单位n/m；f表示外力大小，单位n；l表示金属丝绳索的原长，单位m；s为金属丝绳索的横截面积，单位m2；σ表示金属丝线材的拉伸形变伸长量，单位m。

把步骤2中式(9)算得的绳索修正长度l′i带入式(12)，得到考虑滑轮半径和绳索形变的双重影响，修正后的绳索长度l″i

式(12)中，l″i表示考虑滑轮半径和绳索形变的双重影响下，修正后的绳索i的长度；l0表示绕线轴到相对应的滑轮的长度；l′i表示仅考虑滑轮半径的影响下，修正后的绳索i的长度；e表示本机器人系统中钢索线材的杨氏模量，单位n/m；fi表示绳索i所受拉力，单位n；s为金属丝绳索的横截面积，单位m²。

步骤4.假定绳索模型为理想状态下的直线模型，忽略自身重力作用下的弯曲，将所述绳长模型对时间t进行求导，得到绳长随时间变化的雅克比矩阵。

步骤4.1对求时间t的导数，得到全局坐标系o中动平台绳索i安装点的速度

其中，为全局坐标系o中动平台绳索i安装点的速度；v为全局坐标系o中动平台绳索i安装点的平动速度，w为全局坐标系o中动平台绳索i安装点的角速度为，即

故而

式(15)中，表示动平台中心点位姿变化速度，rp表示全局坐标系o相对于动平台p的空间旋转矩阵；i为3阶单位矩阵。

步骤4.2根据式(16)求绳索i的单位向量

式(16)中，ui表示绳索i的单位向量；表示全局坐标系o中绳索i在动平台的安装位置相对于绳索i与滑轮最高点接触的位置的位置矢量

步骤4.3将所述绳长模型对时间t进行求导，如式(17)所示，得到绳长随时间变化的雅克比矩阵，式(18)。

式(17)中，表示全局坐标系o中，绳索i长度变化速度，为全局坐标系o中动平台绳索i安装点的速度；jinv为绳长随时间变化的雅克比矩阵，又称逆速度雅克比矩阵。其中，

式(19)中，ui表示绳索i的单位向量；rp表示全局坐标系o相对于动平台p的空间旋转矩阵；表示局部坐标系p中绳索i在动平台的安装位置，

步骤5.建立运动学正解方程，并求最优解。

步骤5.1由动平台空间姿态和绳长关系模型(2)，得第i根柔索长度的运动学正解方程(21)

步骤5.2求运动学正解方程的最优解。

步骤5.2.1

由于本机器人系统欠约束，有无穷多个解。根据最小势能原理，力学系统处于稳定平衡时，势能最小，即pz最小。使用牛顿迭代法，多次迭代，得到最优解(x1，y1，z1，α1，β1，γ1)。

步骤5.2.2

具体计算时，求反解时只要求得到目标的三维空间位置(x0，y0，z0)，由于云台的复合铰链结构，安装板角度对云台相机的角度不产生影响，第一次计算时默认目标位置(x0，y0，z0，0，0，0)，通过计算，可以求出线长l1～l4；再求正解，根据势能最小原理，多次迭代得(x1，y1，z1，α1，β1，γ1)，此时(x0，y0，z0)已经与(x1，y1，z1)发生偏移，比较δx＝x1-x0；δy＝y1-y0；δz＝z1-z0是否达到精度要求。如果达不到要求，设置目标位姿为(x0，y0，z0，α1，β1，γ1)，再次计算l1～l4；……多次迭代，再次计算比较第j次迭代xj-x0；yj-y0；zj-z0是否达到精度要求。

实施本发明实施例，具有如下有益效果：本发明实现了视觉检测云台精确的空间定位以及实时的视觉图像处理，取代了传统人工检测的方式，极大的提高了工作效率，降低了设备检修的维护成本。本发明公布的悬索并联机器人通过视觉检测动平台的结构，使运动过程中即便绳索安装板不平，相机依然保持水平作业的特点，为悬索并联机器人运动时末端执行器不平稳提供了新的解决方案。

附图说明

图1为本发明煤仓内壁缺陷检测悬索并联机器人总效果图。各部件分别为：

100——视觉检测动平台、300——滑轮机构、400——煤仓

图2为本发明煤仓检测系统中视觉检测动平台示意图。各部件分别为：

106——2轴无线光学变焦云台相机、103，105——2轴平衡结构、1004——电池、101——配重模块3、102——绳索安装板。

图3-a为本发明煤仓检测系统中伺服机构装配体的主视图。

201——绕线滚筒、202——柔性绳索、203——同步轮组、204——丝杆、205——直线轴承、206——换向轴承、207——原点开关、208——安装底板、209——编码器轮(209)、210——压线机构、211——伺服电机、212——联轴器。

图3-b为本发明煤仓检测系统中伺服机构装配体的后视图。

213——编码器。

图4为本发明煤仓检测系统中滑轮机构示意图。

301——轴承座、302——滑轮支座、303——墙壁安装支架

图5为本发明煤仓内壁缺陷检测悬索并联机器人运动学模型坐标系示意图。

图6为本发明煤仓内壁缺陷检测悬索并联机器人运动学模型矢量计算示意图。

图7为本发明煤仓内壁缺陷检测悬索并联机器人滑轮局部示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述。

本发明实施例的一种用于煤仓内壁缺陷检测的悬索并联机器人，如图1所示安装。

4滑轮机构(300)固定于煤仓(400)4个拐角处，如图1。绕线滚筒(201)通过联轴器(212)与伺服电机(211)连接。

所述4个伺服机构(200)均包括绕线滚筒(201)、直线轴承(205)、安装底板(208)、压线机构(210)、编码器(2013)、编码器轮(209)、伺服电机(211)，所述绕线滚筒(201)通过联轴器(212)与伺服电机(211)连接，柔性绳索绕经所述压线机构(210)、编码器轮(209)缠绕在绕线滚筒(201)上，所述编码器(213)通过编码器轮(209)测出柔性绳索(202)的精确长度变化，通过机器人空间姿态控制方法，确认目标空间位置对应的柔性绳索(202)的长度，实现对视觉检测平台的空间位置控制。

压线机构(210)为压线轮，使得柔性绳索能准确带动编码器轮(209)转动。

4个伺服机构(200)还包括同步轮组(203)、丝杆(204)、直线轴承(205)、换向轴承(206)、原点开关(207)、安装底板(208)；

所述丝杆(204)通过所述同步轮组(203)与所述绕线滚筒(201)同步转动，换向轴承(206)设置于滑动块上，所述滑动块设置于所述丝杆(204)上，且与所述直线轴承(205)固定连接，所述原点开关用于检测所述滑动块的滑动极限位置。

柔性绳索(202)缠绕在绕线滚筒(201)上，柔性绳索(202)通过换向轴承(206)绕过编码器轮(209)、压线机构(210)，再经过滑轮机构(300)连接与视觉检测动平台(100)的绳索安装板(102)上的4个安装位置连接。绕线滚筒(201)通过同步轮组(203)和丝杆(204)传动，使设备在运行过程中柔性绳索(202)能够以螺旋线在绕线滚筒上收放卷。

所述视觉检测动平台(100)包括2轴无线光学变焦云台相机(106)、绳索安装板(102)、2轴平衡结构(103，105)、以及配重模块(101)，所述2轴无线光学变焦云台相机安装于所述2轴平衡结构底端，所述配重模块(101)设置于顶端，所述绳索安装板设置于中部。

换向轴承(206)设置于滑动块上，滑动块设置于所述丝杆(204)上，且与所述直线轴承(205)固定连接，原点开关(207)包括设置于安装底板(208)上的光电眼，滑动块上设置有一感应板，当感应板随滑动块滑动并置于光电眼时，发出触发信号。

硬件安装完毕之后，通电打开软件，设置煤仓的形状、尺寸、相机等参数

本发明实施例还提供了应用于上述用于煤仓内壁缺陷检测的悬索并联机器人的控制方法，包括如下步骤：

步骤1.建立动平台空间姿态和绳长关系模型

该四索牵引并联机器人是6自由度欠约束结构，为了描述动平台的位置姿态，建立全局坐标系o-xyz，在动平台中心建立局部坐标系p-xyz。

建立如下式(1)的动平台空间姿态和绳长模型：

将模型(1)经过移项整理得动平台空间姿态和绳长模型(2)(3):

动平台中心点位姿相对于全局坐标系为

式(1)、式(2)中，p^o为局部坐标系中的原点(即动平台中心)相对于全局坐标系o中的位置矢量，p^o＝[px，py，pz]^t；表示全局坐标系o中绳索i在动平台的安装位置,表示局部坐标系p中绳索i在动平台的安装位置，表示全局坐标系o中绳索i与滑轮最高点接触的位置，表示全局坐标系o中绳索i在动平台的安装位置相对于绳索i与滑轮最高点接触的位置的位置矢量；将滑轮i最高点到动平台绳索i的安装位置这段绳索长度，定义为绳索i的长度，记为li；i取值为[1,2,3,4]；

rp表示全局坐标系o相对于动平台p的空间旋转矩阵，其表达式为：

式(4)、(5)中，α表示动平台绕局部坐标系x轴旋转的角度；β表示动平台绕局部坐标系y轴旋转的角度；γ表示动平台绕局部坐标系z轴旋转的角度；其旋转顺序为先绕局部坐标系z轴旋转γ，再绕局部坐标系y轴旋转β；最后绕局部坐标系x轴旋转α。

步骤2.考虑滑轮半径的影响，修正动平台空间姿态和绳长关系模型

步骤2.1根据式(6)、(7)、(8)求θi

θi＝π-θi，1-θi，2#(8)

式(6)～(8)中，表示滑轮的半径大小；表示全局坐标系o中滑轮i的轴心点坐标位置，表示全局坐标系o中绳索i在动平台的安装位置,θi表示滑轮轴心点到滑轮最高点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角；θi，1表示滑轮轴心点到绳索安装点的线段与全局坐标系z轴所成夹角；θi，2表示滑轮轴心点到绳索安装点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角；

步骤2.2建立修正后的绳长模型

式(9)中l′i表示考虑滑轮半径的影响修正后的绳索i的长度；表示滑轮的半径大小；表示全局坐标系o中滑轮i的轴心点坐标位置，表示全局坐标系o中绳索i在动平台的安装位置,θi表示滑轮轴心点到滑轮最高点的线段与滑轮轴心点到绳索i在滑轮上的切点的线段所成夹角；θi，2表示滑轮轴心点到绳索安装点的线段与滑轮轴心到绳索i在滑轮上的切点的线段所成夹角。

步骤3.考虑绳索在受到拉力作用下的形变，修正动平台空间姿态和绳长关系模型

根据胡克定理

得

式(10)、(11)中，e表示金属丝线材的杨氏模量，单位n/m；f表示外力大小，单位n；l表示金属丝绳索的原长，单位m；s为金属丝绳索的横截面积，单位m2；σ表示金属丝线材的拉伸形变伸长量，单位m。

把步骤2中式(9)算得的绳索修正长度l′i带入式(12)，得到考虑滑轮半径和绳索形变的双重影响，修正后的绳索长度l″i

式(12)中，l″i表示考虑滑轮半径和绳索形变的双重影响下，修正后的绳索i的长度；l0表示绕线轴到相对应的滑轮的长度；l′i表示仅考虑滑轮半径的影响下，修正后的绳索i的长度；e表示本机器人系统中钢索线材的杨氏模量，单位n/m；fi表示绳索i所受拉力，单位n；s为金属丝绳索的横截面积，单位m²。

步骤4.假定绳索模型为理想状态下的直线模型，忽略自身重力作用下的弯曲，将所述绳长模型对时间t进行求导，得到绳长随时间变化的雅克比矩阵。

步骤4.1对求时间t的导数，得到全局坐标系o中动平台绳索i安装点的速度

其中，为全局坐标系o中动平台绳索i安装点的速度；v为全局坐标系o中动平台绳索i安装点的平动速度，w为全局坐标系o中动平台绳索i安装点的角速度为，即

故而

式(15)中，表示动平台中心点位姿变化速度，rp表示全局坐标系o相对于动平台p的空间旋转矩阵；i为3阶单位矩阵。

步骤4.2根据式(16)求绳索i的单位向量

式(16)中，ui表示绳索i的单位向量；表示全局坐标系o中绳索i在动平台的安装位置相对于绳索i与滑轮最高点接触的位置的位置矢量

步骤4.3将所述绳长模型对时间t进行求导，如式(17)所示，得到绳长随时间变化的雅克比矩阵，式(18)。

式(17)中，表示全局坐标系o中，绳索i长度变化速度，为全局坐标系o中动平台绳索i安装点的速度；jinv为绳长随时间变化的雅克比矩阵，又称逆速度雅克比矩阵。其中，

式(19)中，ui表示绳索i的单位向量；rp表示全局坐标系o相对于动平台p的空间旋转矩阵；表示局部坐标系p中绳索i在动平台的安装位置，

步骤5.建立运动学正解方程，并求最优解。

步骤5.1由动平台空间姿态和绳长关系模型(2)，得第i根柔索长度的运动学正解方程(21)

步骤5.2求运动学正解方程的最优解。

步骤5.2.1

由于本机器人系统欠约束，有无穷多个解。根据最小势能原理，力学系统处于稳定平衡时，势能最小，即pz最小。使用牛顿迭代法，多次迭代，得到最优解(x1，y1，z1，α1，β1，γ1)。

步骤5.2.2

具体计算时，求反解时只要求得到目标的三维空间位置(x0，y0，z0)，由于云台的复合铰链结构，安装板角度对云台相机的角度不产生影响，第一次计算时默认目标位置(x0，y0，z0，0，0，0)，通过计算，可以求出线长l1～l4；再求正解，根据势能最小原理，多次迭代得(x1，y1，z1，α1，β1，γ1)，此时(x0，y0，z0)已经与(x1，y1，z1)发生偏移，比较δx＝x1-x0；δy＝y1-y0；δz＝z1-z0是否达到精度要求。如果达不到要求，设置目标位姿为(x0，y0，z0，α1，β1，γ1)，再次计算l1～l4；……多次迭代，再次计算比较第j次迭代xj-x0；yj-y0；zj-z0是否达到精度要求。

以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。