本发明属于机器人，特别涉及一种举升机构与蛇形臂协同控制的路径跟随方法。
背景技术：
1、随着核工业技术的发展和核设施退役需求的增大，能够灵活运动于狭小密闭空间的超冗余蛇形臂机器人逐渐进入了相关人员的视野。然而，对于底座固定的蛇形臂机器人，其工作空间受限，仍旧无法较好地适应深度较大的狭小空间。因此，为提高运动空间，带有运动底盘的蛇形臂机器人应运而生，由此带来的底盘与机器人本体的协同规划或控制算法的需求也日益增加。虽然，在研究上，会出现模糊举升机构和蛇形臂控制方法的概念，但是存在路径跟随精度低、工作效率低下等问题，无法满足现代工业生产的需求。因此，如何提高举升机构和蛇形臂的路径跟随精度和工作效率，成为当前亟待解决的问题。基于此，本申请将针对带有一种特殊底盘——举升机构的蛇形臂机器人，提出一种特殊的协同路径规划算法，用以有效指导蛇形臂与该举升机构的系统运动。
技术实现思路
1、本发明提供了一种举升机构与蛇形臂协同控制的路径跟随方法，用于解决现有技术中蛇形臂机器人存在路径跟随精度低、工作效率低的问题。
2、为达上述目的，本发明采用了如下的技术方案：一种举升机构与蛇形臂协同控制的路径跟随方法，该路径跟随方法包括：
3、s201，给定运动路线；
4、s202，直线进给机构开始工作；
5、s203，电动推杆开始工作，被动翻转轴开始旋转；
6、s204，调整蛇形臂关节，使其沿给定路线前进；
7、s205，蛇形臂末端点到达指定位置，轨道变为垂直状态，完成指定任务。
8、可优选地，所述给定运动路线包括进给路线、过渡路线和工作路线；
9、所述进给路线应随所述被动转轴的旋转而发生变化,进给路线包括蛇形臂机构本体的脊线，所述蛇形臂机构本体的脊线是指依次穿过蛇形臂机器人各关节中心点的多个直线段的组合，这些直线段组成一条折线，用于描述蛇形臂机器人本体机构的运动姿态；
10、所述过渡路线应使所述进给路线与所述工作路线实现平稳过渡；
11、所述工作路线应是一条提前规划好的已知的适用于蛇形臂通行的避障路线。
12、可优选地，直线进给机构的驱动方式，包括电机与丝杠螺母机构驱动。
13、可优选地，渡路线的计算方法，步骤如下：
14、s601，计算交点t,方程如下：
15、其中，为进给路线终点处的切线的解析表达式的向量形式，为工作路线起点处的切线的解析表达式的向量形式，该方程为一个向量方程，在二维空间中可以被拆分成两个标量方程，在三维空间中可以被拆分成三个向量方程，本实施例所描述的对象在该步骤中只涉及两个维度的运动，故两个切线必定相交，方程有解t1和t2，根据参数有t1和t2以及所属两个切线的解析表达式，可以计算出交点t的具体坐标；
16、s602，计算方向向量,计算公式如下：
17、
18、式中，向量为交点t指向过渡圆弧中心的方向向量；
19、s603，计算圆心,计算公式如下：
20、
21、
22、其中，为过渡路线分别与进给路线和工作路线的切点之间的圆心角的一半，oo表示过渡路线的圆心点，其值为圆心点的坐标表示；
23、s604，计算切点，计算公式如下：
24、
25、
26、其中，r为过渡路线中圆弧的半径，t1和t2分别为过渡路线的两个端点，同时也是过渡路线与两侧路线的切点；
27、可选的，过渡路线中圆弧的位置和形状可由以下4个参数表示：圆心点坐标，起点坐标，旋转角度和旋转轴，其中，旋转轴的计算公式如下：
28、
29、可优选地，所述步骤s203中电动推杆的作用是为所述直线进给机构提供一个上下翻转的驱动力,在电动推杆的驱动下，直线进给机构和蛇形臂本体机构逐渐由水平状态变为垂直状态。
30、可优选地，步骤s204，涉及到一种关键点与路线之间的匹配算法，其在蛇形臂本体机构处于不同区间位置时存在不同的表现形式，运动路线的三个部分：进给路线、过渡路线和工作路线可采用分段函数的形式表示如下：
31、
32、其中，p1(t)为进给路线的抽象表示，p2(t)为过渡路线的抽象表示，p3(t)为工作路线的抽象表示，t为参数变量，t0表示进给路线的起点参数，t1表示进给路线的终点参数和过渡路线的起点参数，t2表示过渡路线的终点参数和工作路线的起点参数，t3表示工作的终点参数。
33、可优选地，步骤s205中的指定位置应从蛇形臂本体机构的可达空间点中寻找。
34、本发明的有益效果：
35、1、本发明实现了对预定路径的高精度跟随，提高了工作效率和操作安全性。同时，本发明还具有易于实现、成本低廉等优点，具有广泛的应用前景。
36、2、本发明规定了一种从笛卡尔空间到关节空间的映射方法，解决了蛇形臂与底盘协同运动时蛇形臂关节空间中的角度规划问题。
1.一种举升机构与蛇形臂协同控制的路径跟随方法，其特征在于，所述方法包括：
2.根据权利要求1所述的路径跟随方法，其特征在于，所述给定运动路线包括进给路线、过渡路线和工作路线；
3.根据权利要求1所述的路径跟随方法，其特征在于，所述直线进给机构的驱动方式，包括电机与丝杠螺母机构驱动。
4.根据权利要求2所述的路径跟随方法，其特征在于，所述过渡路线的计算方法，步骤如下：
5.根据权利要求1所述的路径跟随方法，其特征在于，所述步骤s203中电动推杆的作用是为所述直线进给机构提供一个上下翻转的驱动力,在电动推杆(2)的驱动下，直线进给机构(4)和蛇形臂本体机构(5)逐渐由水平状态变为垂直状态。
6.根据权利要求1所述的路径跟随方法，其特征在于，所述步骤s204，涉及到一种关键点与路线之间的匹配算法，其在蛇形臂本体机构处于不同区间位置时存在不同的表现形式，运动路线的三个部分：进给路线、过渡路线和工作路线可采用分段函数的形式表示如下：
7.根据权利要求1所述的路径跟随方法，其特征在于，所述步骤s205中的指定位置应从蛇形臂本体机构的可达空间点中寻找。
