一种绳驱蛇形机械臂路径规划首尾运动跟随方法、装置、计算机及存储介质

1.本发明属于机械臂控制技术领域，尤其涉及绳驱蛇形机械臂路径规划运动跟随技术领域。


背景技术：

2.在实际的工程应用中存在大量狭窄复杂的环境需要被探测，如空间站舱室、飞机油箱、建筑物管道等。此类环境的探测一般要求不拆除原有结构下进行。若采用人工方法则检测成本较高，且人员长期在此类特殊环境工作也存在一定安全隐患。因此宜采用机械臂的等现代化机械设备进行探测。传统机械臂结构较为简单、运动自由度有限，难以满足复杂环境的探测需求。


技术实现要素：

3.本发明解决了传统机械臂不能在复杂环境下进行探索的问题。
4.本发明提供一种绳驱蛇形机械臂路径规划首尾运动跟随方法，所述绳驱蛇形机械臂包括n个锥节和n个关节，所述n大于3，所述规划运动跟随方法包括：
5.根据绳驱蛇形机械臂臂体末端点的空间位置末端点下一时刻的空间位置及末端关节的空间位置获取下一时刻臂体末端关节的空间位置和角度
6.根据下一时刻臂体末端关节的空间位置和关节跟随运动规划条件获取第i个关节的下一时刻位置及角度，i＝n-1,n-2,
……
,2，依次类推，直至获得第2关节下一时刻的空间位置和角度
7.根据第2个关节的空间位置及角度获取第1个关节的空间位置及角度
8.根据获取的所有关节的角度完成绳驱蛇形机械臂路径规划运动跟随。
9.进一步，所述获取下一时刻臂体末端关节的空间位置和角度具体为：
10.臂体末端的第n个关节沿上一时刻t该关节的轴向方向运动，所述运动的时间步长为δt，该关节在下一时刻t δt的空间位置到末端距离为第n个锥节的长度ln；则根据求解方程：
[0011][0012]
获得臂体末端的第n个关节的空间位置
[0013]
臂体末端的第n个关节的角度根据求解方程获得：
[0014][0015]
为臂体末端在惯性系下的x、y、z方向的坐标。
[0016]
进一步，所述运动跟随方法还包括判断臂体末端的第n个关节的角度是否在臂体末端的关节的机构运动空间内：若所述关节角度在机构运动空间内，则空间位置为和为下一时刻臂体末端的第n个关节的空间位置和角度；否则，则判定绳驱蛇形机械臂在进行平移运动，并获取当前绳驱蛇形机械臂末端第n个关节的空间位置和角度。
[0017]
进一步，所述获取当前绳驱蛇形机械臂末端第n个关节的空间位置和角度是根据求解方程获得：
[0018][0019]
进一步，所述根据下一时刻臂体末端关节的空间位置和关节跟随运动规划条件获取第i个关节的下一时刻位置及角度，i＝n-1,n-2,
……
,2，依次类推，直至获得第2关节下一时刻的空间位置和角度具体为：
[0020]
获取第i个关节的下一时刻位置根据求解方式：
[0021][0022]
所述i＝n-1,n-2,
……
,2；li为第i个锥节的长度；
[0023]
获取第i个关节的下一时刻角度根据求解方式：
[0024][0025]
其中，为第i个关节在惯性系下的x、y、z方向的坐标；
[0026]
进一步，所述运动跟随方法还包括判断臂体末端的第i个关节的角度是否在臂体末端的关节的机构运动空间内：若所述关节角度在机构运动空间内，则空间位置为和角度为下一时刻臂体的第i个关节的空间位置和角度；否则，则判定绳驱蛇形机械臂在进行平移运动，并获取当前臂体第i个关节的空间位置和角度。
[0027]
进一步，所述获取第1个关节的空间位置及角度根据求解方程：
[0028][0029]
获得第1关节的空间位置其中，l1为第1个锥节的长度；
[0030]
然后再求解方程
[0031][0032]
获得第1个关节下一时刻的角度其中，为第1个关节在下一时刻t δt空间位置上的x、y、z方向的坐标。
[0033]
本发明还提供一种绳驱蛇形机械臂路径规划首尾运动跟随装置，所述绳驱蛇形机械臂包括n个锥节和n个关节，所述n大于3，所述规划运动跟随装置包括：
[0034]
臂体末端空间位置获取单元，用于根据绳驱蛇形机械臂臂体末端点的空间位置末端点下一时刻的空间位置及末端关节的空间位置获取下一时刻臂体末端关节的空间位置和角度
[0035]
关节参数获取单元，用于根据下一时刻臂体末端关节的空间位置和关节跟随运动规划条件获取第i个关节的下一时刻位置及角度，i＝n-1,n-2,
……
,2，依次类推，直至获得第2关节下一时刻的空间位置和角度根据第2个关节的空间位置及角度获取第1个关节的空间位置及角度
[0036]
路径规划运动跟随单元，用于根据获取的所有关节的角度完成绳驱蛇形机械臂路径规划运动跟随。
[0037]
本发明还提供一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，当所述处理器运行所述存储器存储的计算机程序时，所述处理器执行根据上述步骤任一项所述的一种绳驱蛇形机械臂路径规划运动跟随方法。
[0038]
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质用于储存计算机程序，所述计算机程序执行上述步骤任一项所述的一种绳驱蛇形机械臂路径规划运动跟随方法。
[0039]
本发明的优点是：
[0040]
本发明解决了传统机械臂不能在复杂环境下进行探索的问题。
[0041]
(1)本发明采用的绳驱蛇机械臂为多自由度结构，运动具备冗余性，臂体活动灵活适合于复杂狭窄结构的深入探测。
[0042]
(2)本发明提出的运动规划方法能够实现臂体关节对末端轨迹的跟随，实现复杂环境下臂体运动的避障规划。
[0043]
(3)本发明的路径跟随运动规划方法主要通过利用臂体末端的运动轨迹进行跟踪，与人工势场法、栅格法、深度学习智能算法等方法相比，本方案不依靠环境的精确建模，计算过程简单，稳定性高。
[0044]
(4)本发明不依赖关节运动测量信号，减少操作流程，通过末端视觉导航进行末端超冗余臂体运动的避障，进行路径规划；通过设定蛇形臂臂体跟踪其末端运动路径，实现超冗余臂体的运动避障；以关节位置反向递推算法得到臂体关节中心位置，基于臂体关节中心位置求解关节角度，从而确定电机运动量，避免冗余机构逆运动学求解过程中的计算效率低、雅可比矩阵奇异等缺点。
[0045]
本发明适用于狭窄复杂的空间环境的路径规划领域。
附图说明
[0046]
为了更清楚地说明本技术实施例的技术方案，下面将对本技术实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
[0047]
图1为本发明所述的绳驱蛇形机械臂路径规划首尾运动跟随方法的运动跟随规划流程图。
[0048]
图2为本发明所述的方法适用的一种绳驱蛇形机械臂机构的结构示意图；图中包括：驱动基座1，滑轨2，臂体3,视觉相机4。
[0049]
图3为图2中驱动基座1和滑轨2的结构示意图；图中包括：驱动基座1，驱动器5，电机6，丝杠7,拉力传感器8。
[0050]
图4为图2中的臂体3的结构图；图中包括：关节9，锥节10，该图中的放大部分包括万向节11和绳孔12。
[0051]
图5为本发明实施例一所述的臂体运动过程示意图。
[0052]
图6为本发明实施例二所述的锥节位姿示意图。
[0053]
图7为本发明实施例二所述的关节结构及坐标系示意图。
[0054]
图8为本发明实施例三所述的关节运动空间示意图。
[0055]
图9为本发明实施例五所述的关节结构及坐标系示意图。
[0056]
图10为本发明实施例八所述的关节结构及坐标系示意图。
具体实施方式
[0057]
为使本技术实施例的目的、技术方案和优点更加清楚，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本技术一部分实施例，而不是全部的实施例。
[0058]
实施例一、参见图1、图2、图3、图4和图5说明本实施例。本实施例所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法，所述绳驱蛇形机械臂包括n个锥节10和n个关节9，所述n大于3，所述规划运动跟随方法包括：
[0059]
根据绳驱蛇形机械臂臂体末端点的空间位置末端点下一时刻的空间位置及末端关节的空间位置获取下一时刻臂体末端关节的空间位置和角度
[0060]
根据下一时刻臂体末端关节的空间位置和关节跟随运动规划条件获取第i个关节的下一时刻位置及角度，i＝n-1,n-2,
……
,2，依次类推，直至获得第2关节下一时刻的
空间位置和角度
[0061]
根据第2个关节的空间位置及角度获取第1个关节的空间位置及角度
[0062]
根据获取的所有关节的角完成绳驱蛇形机械臂路径规划运动跟随。
[0063]
本实施例中所述臂体末端的关节，序号为n，即：第n个关节，与该关节相邻的锥节为第n个锥节。臂体首端第一个关节的序号为1，即为第1个关节，相邻的锥节为第1个锥节。
[0064]
本实施例所述方法适用的一种绳驱蛇形机械臂，如图2所示，是n为10的一种绳驱蛇形机械臂的结构示意图，其由驱动基座1、滑轨2、臂体3、视觉相机4组成。
[0065]
如图3所示，驱动基座1内部安装有驱动器5、电机6、拉力传感器8等动力装置，同时配置丝杠7机构等传动、绳索固定装置。驱动基座下方安装有水平滑轨，可实现基座沿滑道方向能够跟随臂体移动。
[0066]
所述绳驱蛇形机械臂臂体如图4所示，臂体为绳索驱动的冗余结构，由多个锥节9、关节10连接而成。每个关节为2的自由度万向节11，由3条绳索驱动带动锥节运动。锥节上下端设计有绳孔12结构，可以固定此锥节段驱动绳索和引导绳索至驱动基座。整条臂体共10关节，20自由度。臂体末端安装视觉相机对环境内部进行实时探测，可根据探测结果指导臂体末端的运动轨迹，臂体运动过程如图5所示。
[0067]
通过本发明的路径规划，结合绳驱蛇形机械臂以其多自由度运动高冗余性的特点，可以对油箱内部等复杂狭窄环境进行灵活探测。
[0068]
实施例二、参见图1、图6和图7说明本实施例。本实施是对实施例一所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，
[0069]
所述获取下一时刻臂体末端关节的空间位置和角度具体为：
[0070]
臂体末端的第n个关节沿上一时刻t该关节的轴向方向运动，所述运动的时间步长为δt，该关节在下一时刻t δt的空间位置到末端距离为第n个锥节的长度ln；则根据求解方程：
[0071][0072]
获得臂体末端的第n个关节的空间位置
[0073]
臂体末端的第n个关节的角度根据求解方程获得：
[0074][0075]
为臂体末端在惯性系下的x、y、z方向的坐标。
[0076]
本实施例中，锥节位姿如图5所示。
[0077]
以n为10为例，由第10关节和臂体末端的相对位置可根据d-h参数法计算得到第10
关节的下一时刻关节角度d-h参数法计算关节角度原理为：对于每个锥节基于d-h参数法建立坐标系如图7所示，对应d-h参数参见表1。
[0078]
表1锥节d-h参数
[0079]
坐标系iθi(
°
)di(mm)ai(mm)αi(
°
)0～1θ
10_1
00-90
°
1～2θ
10_2
0l
10
90
°
[0080]
坐标系0到坐标系2的齐次坐标变换矩阵为：
[0081][0082]
其中c
i1
＝cosθ
10_1
；s
i1
＝cosθ
10_1
；c
i2
＝cosθ
10_2
；s
i2
＝cosθ
10_2
。
[0083]q10
为第10关节到第10锥节末端的方向向量：
[0084][0085]
其中，为第10关节在下一时刻t δt空间位置上的x、y、z方向的坐标,展开得到：
[0086][0087]
其中，l
10
为第10个锥节的长度。
[0088]
获得第10关节的关节角度
[0089]
实施例三、参见图1和图8说明本实施例。本实施是对实施例二所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，所述运动跟随方法还包括判断臂体末端的第n个关节的角度是否在臂体末端的关节的机构运动空间内：若所述关节角度在机构运动空间内，则空间位置为和角度为下一时刻臂体末端的第n个关节的空间位置和角度；否则，则判定绳驱蛇形机械臂在进行平移运动，并获取当前绳驱蛇形机械臂末端第n个关节的空间位置和角度。
[0090]
如图8所示，关节的机构运动空间即为关节运动可达到的最大角度范围，由机械臂的自身结构决定。若关节运动角度过大超过极限角度，锥节之间会相互碰撞，造成臂体结构破坏。
[0091]
实施例四、参见图1说明本实施例。本实施是对实施例三所述的一种绳驱蛇形机械
臂路径规划运动首尾跟随方法的进一步限定，所述获取当前绳驱蛇形机械臂末端第n个关节的空间位置和角度是根据求解方程获得：
[0092][0093]
实施例五、参见图1和图9说明本实施例。本实施是对实施例一所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，所述根据下一时刻臂体末端关节的空间位置和关节跟随运动规划条件获取第i个关节的下一时刻位置及角度，i＝n-1,n-2,
……
,2，依次类推，直至获得第2关节下一时刻的空间位置和角度具体为：
[0094]
关节i沿当前时刻锥节i的轴向方向即方向运动，且t δt时刻关节i到关节i 1距离为锥节i长度li，获取第i个关节的下一时刻位置，根据求解方式：
[0095][0096]
所述i＝n-1,n-2,
……
,2；li为第i个锥节的长度；
[0097]
根据d-h参数法，由第i个和第i 1个关节的相对位置可计算得到第i个关节的下一时刻关节角度第i个关节与末端关节均为相同的万向节结构，因此类比末端关节的d-h参数可得到第i个关节的d-h参数及坐标系变换矩阵为：
[0098]
表1锥节d-h参数
[0099]
坐标系iθi(
°
)di(mm)ai(mm)αi(
°
)0～1θ
i_1
00-90
°
1～2θ
i_2
0li90
°
[0100]
图中坐标系0到坐标系2的齐次坐标变换矩阵为：
[0101][0102]
其中，c
i1
＝cosθ
i_1
；s
i1
＝cosθ
i_1
；c
i2
＝cosθ
i_2
；s
i2
＝cosθ
i_2
。qi为关节i到关节i 1的方向向量，与已知量建立等式：
[0103][0104]
其中，为第i个关节在惯性系下x、y、z方向的坐标；
[0105]
获取第i个关节的下一时刻角度根据求解方式：
[0106][0107]
其中，为下一时刻t δt第i个关节的第1个自由度的角度，为下一时刻t δt关节i的第2个自由度的角度，如图4所示，在实际应用中，万向节11包含两个自由度，
[0108]
第2个关节到第n-1个的构型完全相同，关节与锥节连接方式也相同，均为可用图8所述的关节结构图通用化表示。
[0109]
实施例六、参见图1说明本实施例。本实施是对实施例五所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，所述运动跟随方法还包括根据臂体的第i个关节的角度进行第i个关节是否在机构运动空间的判断：若所述关节角度在机构运动空间内，则空间位置为和角度为下一时刻臂体的第i个关节的空间位置和角度；否则，则判定绳驱蛇形机械臂在进行平移运动，并获取当前臂体第i个关节的空间位置和角度。
[0110]
实施例七、参见图1说明本实施例。本实施是对实施例六所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，所述获取当前臂体第i个关节的空间位置和角度是根据求解方程获得：
[0111][0112]
实施例八、参见图1和图10说明本实施例。本实施是对实施例一所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，所述获取第1个关节的空间位置及角度根据求解方程：
[0113][0114]
获得第1关节的空间位置其中，l1为第1个锥节的长度；
[0115]
然后再求解方程
[0116][0117]
获得第1个关节下一时刻的角度其中，为第1个关节在惯
性系下的x、y、z方向的坐标。
[0118]
实施例九、参见图1说明本实施例。本实施是对实施例八所述的一种绳驱蛇形机械臂路径规划首尾运动跟随方法的进一步限定，所述运动跟随方法还包括根据臂体的第1个关节的角度进行第1个关节是否在机构运动空间的判断：当超出运动空间，臂体整体运动无法满足需求，末端轨迹不合理，重新计算末端轨迹。
[0119]
实施例十、本实施例所述的一种绳驱蛇形机械臂路径规划首尾运动跟随装置，所述绳驱蛇形机械臂包括n个锥节和n个关节，所述n大于3，所述规划运动跟随装置包括：
[0120]
臂体末端空间位置获取单元，用于根据绳驱蛇形机械臂臂体末端点的空间位置末端点下一时刻的空间位置及末端关节的空间位置获取下一时刻臂体末端关节的空间位置和角度
[0121]
关节参数获取单元，用于根据下一时刻臂体末端关节的空间位置和关节跟随运动规划条件获取第i个关节的下一时刻位置及角度，i＝n-1,n-2,
……
,2，依次类推，直至获得第2关节下一时刻的空间位置和角度根据第2个关节的空间位置及角度获取第1个关节的空间位置及角度
[0122]
路径规划运动跟随单元，用于根据获取的所有关节的角度完成绳驱蛇形机械臂路径规划运动跟随。
[0123]
实施例十一、本实施例所述的一种计算机设备，其特征在于：包括存储器和处理器，所述存储器中存储有计算机程序，当所述处理器运行所述存储器存储的计算机程序时，所述处理器执行根据实施例一至实施例九任一项所述的一种绳驱蛇形机械臂路径规划运动跟随方法。
[0124]
实施例十二、本实施例所述的一种计算机可读存储介质，所述计算机可读存储介质用于储存计算机程序，所述计算机程序执行实施例一至实施例九任一项所述的一种绳驱蛇形机械臂路径规划运动跟随方法。