一种APA型欠驱动机械臂控制方法及采用该方法的巡检装置

本发明涉及的，尤其涉及一种apa型欠驱动机械臂控制方法及采用该方法的巡检装置。

背景技术：

1、带式输送机是实现煤矿运输的关键环节，多处于恶劣的工况下，加之长时间、高负荷运行，导致带式输送机易发生胶带打滑、纵撕、跑偏甚至断带故障，轻则停工停产，影响正常开采工作，重则直接威胁煤矿工人的生命安全。

2、目前常采用轨道式巡检机器人搭载摄像头和传感器对气体、温度、烟雾、声音、图像五类信息进行采集，实现带式输送机故障定位与故障点的预测。然而现阶段所研究的巡检机器人大多位于输送带上方，沿输送带输送的方向运行，其视觉感知无法做到对输送带下表面的检测，存在视野盲区。

3、公开号为cn115892923b的发明专利公开了一种带式输送机智能巡检机器人(如图5所示)，包括检测部、基台部。基台部通过双股轨道和轨轮沿带式输送机输送方向向进行移动时，可带动检测部对带式输送机进行移动巡检，同时在基台部移动时，其通过传动机可带动滚环通过支杆和滑套在基板的下部进行转动，而由于滚环套设在带式输送机的输送皮带外部，且其内环面呈环形阵列状设有多个采集终端，因此基台部沿着带式输送机输送方向进行移动时，滚环可同时在带式输送机的外部进行滚动，从而带动采集终端在带式输送机的外部进行环绕转动，以使采集终端在采集带式输送机的原始图像时，其整体采集轨迹呈一个螺旋型，从而大大提升采集终端采集图像的范围，实现智能巡检机器人巡检时对带式输送机进行全方位巡检。但由于其滚环套设在输送带外部，直径必须足够大；受到带式输送机支架和输送带底部积煤的影响，在实际运行过程中很容易遇到阻碍；固定的螺旋型的检测轨迹，使得视野角度固定，也导致其依然存在视野盲区。

4、因此急需一种辅助检测装置，能够有效解决巡检机器人检测盲区的问题，实现带式输送机局部细化检测。

5、公开号为cn109262612a的发明专利公开了一种基于改进粒子群算法的欠驱动机械臂关节角寻优方法。包括：建立平面三自由度被动-主动-主动型(paa型)欠驱动机械臂动力学模型；然后，针对具有非完整约束特性的欠驱动机械臂动力学模型，利用反演控制思想，将其降阶为两个被动-主动型(pa型)子系统，并明确关节角与机械臂末端位置之间关系；最后，以欠驱动机械臂末端位置与目标位置差值为目标函数，基于粒子群算法的寻优原理，引入模拟退火的metropolis接受准则和人工鱼群算法的拥挤度因子，求解到达目标位置所对应的最优关节角。此发明在欠驱动机械臂位置控制时，可以降低寻优算法陷入局部最优的概率并提高求解精度。但其本质上还是平面二连杆型欠驱动臂的控制，应用在平面三连杆型欠驱动机械臂中需要经过两次降阶控制，控制过程分先后顺序、不连贯、时间比较长。

技术实现思路

1、本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本技术的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

2、鉴于上述现有存在的问题，提出了本发明。

3、因此，本发明解决的技术问题是：现有的巡检机器人存在检测盲区，以及现有三连杆型欠驱动机械臂控制过程分先后顺序、不连贯、时间比较长的问题。

4、为解决现有三连杆型欠驱动机械臂控制过程分先后顺序、不连贯、时间比较长的问题，本发明提供如下技术方案：

5、一种apa型欠驱动机械臂控制方法，包括以下步骤：

6、步骤一：建立平面三自由度主动-被动-主动型(apa型)欠驱动机械臂运动学与动力学模型；

7、步骤二：为两主动杆分别设计正(从起始位置到中间位置)、反(从期望位置到中间位置)两条期望轨迹，并对被动杆正、反轨迹进行拼接，规划一条从起始位置到期望位置的机械臂末端运行轨迹。

8、步骤三：设计模糊滑模变结构控制器，跟踪规划好的轨迹，将滑模变控制与模糊控制相结合，以滑模面函数s的绝对值作为模糊控制的输入，趋近率参数ε,k为模糊控制输出，根据系统当前运动状态与期望运动状态的比较实现参数的自适应调节。

9、作为本发明所述的的一种优选方案，apa型欠驱动机械臂运动学模型如下：

10、

11、作为本发明所述的的一种优选方案，apa型欠驱动机械臂动力学模型如下：

12、

13、其中

14、

15、

16、

17、式中：mi－第i根连杆的质量，li－第i根连杆的杆长，li－第i根连杆质心到前一转动关节的距离，ji－第i根连杆绕自身质心旋转的转动惯量,qi－第i根连杆转角(i＝1、2、3)。

18、作为本发明所述的的一种优选方案，

19、两主动杆正向期望轨迹如下：

20、

21、

22、式中，qi0－第i根连杆的初始角度，qim－第i根连杆的中间角度，k1为时间因子，t∈[0,1/k1]。

23、作为本发明所述的的一种优选方案，

24、两主动杆反向期望轨迹如下：

25、

26、

27、式中，qid－第i根连杆的期望角度，k2为时间因子，t∈[0,1/k2]。

28、作为本发明所述的的一种优选方案，被动杆轨迹根据式(2)利用四阶龙格库塔法进行求解，具体求解公式如下：

29、

30、被动杆的轨迹由规划的主动杆轨迹所决定，主要取决于q1m、q3m、k1、k2值的大小，取值不同时被动杆的轨迹也会随之变化，通过粒子群算法找到合适的q1m、q3m、k1、k2，使得被动杆正反两条轨迹能拼成一条光滑完整的轨迹。

31、作为本发明所述的的一种优选方案，被动杆正反轨迹拼接须遵循的条件及粒子群算法适应度函数如下：

32、

33、

34、在求解出合适的q1m、q3m、k1、k2后，利用时间倒转法将反向轨迹转换为一条从中间位置到期望位置的轨迹，倒转轨迹方程如下：

35、

36、

37、式中：t∈[0,1/k2]

38、将q1m、q3m、k1、k2代入式(3)(4)，与式(10)(11)拼接便得到主动杆的期望轨迹，主动杆通过耦合关系带动被动杆运动，可以实现apa型欠驱动机械臂从起始位置到达期望位置的要求。

39、作为本发明所述的的一种优选方案，步骤三中滑模面和趋近率函数如下：

40、

41、

42、式中，μ是常数，e＝q-qd是系统跟踪位置误差，是系统跟踪速度误差。

43、为解决现有的巡检机器人存在检测盲区的问题，本发明提供如下技术方案：

44、一种带式输送机巡检机器人视觉延伸装置，包括：

45、设置在带式输送机巡检机器人上的支架；

46、设置在支架内的检测单元、供电单元、控制单元；

47、所述检测单元由竖直推杆和平面三连杆组成，竖直推杆负责z方向的移动，平面三连杆在水平面内转动，可达空间为三根杆长之和为半径的半圆，同时平面三杆逆运动学具有多重解，连杆末端搭载感知设备如摄像头进行图像采集，将采集到的图像传输到上位机；

48、所述供电单元为整个带式输送机巡检机器人视觉延伸装置提供独立电力来源；

49、所述控制单元由一个单片机和三个步进电机驱动器组成，分别驱动竖直方向电动推杆电机和平面三连杆第一、第三关节电机，所述控制单元采用如权利要求1-9中任意一种apa型欠驱动机械臂控制方法对平面三连杆进行控制。

50、作为本发明所述的的一种优选方案，所述支架上方布置有两块碳钢板，支架通过磁力吸附在带式输送机巡检机器人上，随巡检机器人一起运动。

51、本发明的有益效果：

52、(1)本发明在轨道巡检机器人沿带式输送机输送方向进行移动巡检时，可通过电动推杆和apa型欠驱动机械臂的相互配合，实现摄像头在带式输送机上下皮带之间进行图像采集。与现有轨道巡检机器人相比，本发明专利使巡检机器人在采集带式输送机的图像信息时，采集的图像范围扩大至上下皮带之间，覆盖原视野盲区，实现轨道巡检机器人对带式输送机进行全方位巡检，提升其巡检效率及效果；与背景所述专利相比，本发明所提巡检机器人视觉延伸装置控制单元、供电单元、检测单元结构紧凑，机械臂尺寸更加合理；机械臂伸缩可避免与支架等的碰撞，运行无阻碍；摄像头拍摄角度可通过机械臂转角变化实现全方位拍摄，视野范围更广，不存在视野盲区。

53、(2)本发明专利公开一种apa(主动-被动-主动)型欠驱动机械臂控制策略，将机械臂的位置控制转化为轨迹规划+轨迹跟踪，通过事先规划一条从初始位置到目标位置的轨迹，进而进行轨迹跟踪实现平面三连杆欠驱动机械臂的直接控制。与背景所述专利相比，本发明提出的控制策略不需要降阶，控制过程不分段，直接对两条主动臂同时进行控制，进而带动整条机械臂按照规划好的轨迹进行运动，控制过程连贯，效率比较高。