本发明属于机械臂运动规划，具体涉及一种串联机械臂的自平行运动规划方法。
背景技术：
1、隧道掘进专用设备多采用重型-液压驱动的串联机械臂，依靠装载其上的末端执行器完成作业任务，如凿岩台车及凿岩机、锚杆台车及凿岩机、湿喷台车及喷枪等。串联机械臂末端执行器的位置和姿态由机械臂的关节位置来决定，根据作业任务不同，对机械臂末端执行器的运动模式有不同要求。当要求执行器在操作手柄控制下移动且朝向不变时，即为自平行运动模式，又分为无移动约束的自平行运动、相对自身参考面平行移动的自平行运动、相对作业参考面平行移动的自平行运动。
2、通过正、逆运动学模型可建立末端执行器位姿和机械臂关节位置之间的关系，当规划执行器自平行移动时，一般方法为采用速度级的运动学模型，即基于机械臂的jacobian矩阵伪逆，根据末端自平行运动期望速度规划关节速度；但是该方法需要精确控制机械臂各关节的运动速度来实现执行器的自平行运动模式，不适用于难以精确控制关节速度的电磁比例阀控制的重型机械臂。
3、因此，需要一种适用于电磁比例阀控制的重型机械臂的自平行运动规划方法。
技术实现思路
1、有鉴于此，为解决上述现有技术的不足，本发明的目的在于提供一种位置级的自平行运动规划方法，通过规划关节位置，在较容易实施的高精度关节位置控制下实现执行器的自平行运动模式，适用于电磁比例阀控制的重型机械臂。
2、为实现上述目的，本发明所采用的技术方案是：
3、一种串联机械臂位置级的自平行运动规划方法，包括以下步骤：
4、s1、建立台车坐标系{0}，根据具体作业任务建立作业坐标系{f}，校准作业坐标系与台车坐标系的位姿矩阵；
5、s2、求解串联机械臂末端相对于台车坐标系{0}的位姿矩阵，确认末端初始朝向和末端初始位置；
6、s3、根据作业目标确认自平行运动模式，所述运动模式为无移动约束的自平行运动、相对自身参考面平行移动的自平行运动、相对作业参考面平行移动的自平行运动其中之一；
7、s4、根据自平行运动模式，规划串联机械臂转动关节和伸缩关节的关节位置，用于控制串联机械臂末端在移动过程中朝向和位置。
8、可选择地，所述机械臂由摆动关节θ1、俯仰关节θ2、臂伸缩关节q3、滚动关节θ4、俯仰关节θ5、摆动关节θ6和梁伸缩关节q7共7个关节和连杆串联而成。
9、进一步地，所述s1中，包括以下子步骤：
10、s11、定义自身参考面为台车坐标系{0}的yoz平面，位置为执行器位置；
11、s12、定义作业参考面为作业坐标系{f}的xoz平面；
12、s13、校准出作业坐标系与台车坐标系的位姿矩阵
13、进一步地，所述s2中，求解机械臂末端相对于台车坐标系{0}的位姿矩阵并根据位姿矩阵求解初始位置下此时的机械臂末端朝向a和末端位置p。
14、更进一步地，所述s3中，运动模式具有以下约束：
15、无移动约束的自平行运动模式，约束仅为末端朝向恒定不变；
16、相对自身参考面的自平行运动模式，约束为在台车坐标系{0}下末端朝向恒定不变和末端位置的x方向运动速度为0；
17、相对作业参考面的自平行运动模式，约束为在作业坐标系{f}下末端朝向恒定和末端位置的yf方向运动速度为0。
18、更进一步地，所述s4中，如果规划的关节位置使得a的齐次向量为恒定值，则末端在移动过程中朝向不变；如果规划的关节位置使得p的某方向分量为恒定值，则末端将相对以该方向为法向量的平面平行移动。
19、具体地，所述s4中，无移动约束的自平行运动模式下，规划串联机械臂各关节位置时，需要保证运动过程中末端朝向a＝[ax ay az]t的齐次向量为恒定值。
20、具体地，所述s4中，相对自身参考面的自平行运动模式下，规划串联机械臂各关节位置时，需要保证运动过程中末端朝向a＝[ax ay az]t的齐次向量为恒定值且末端位置的x方向运动速度为0。
21、具体地，所述s4中，相对作业参考面的自平行运动模式下，规划串联机械臂各关节位置时，已知作业坐标系与台车坐标系的位姿矩阵需要保证作业坐标系{f}下末端朝向和y方向速度为0。
22、本发明的有益效果是：
23、本发明提供了一种自平行运动规划方法，基于位置级规划机械臂关节位置，不需要考虑jacobian矩阵伪逆和关节速度，适用于电磁比例阀控制的重型机械臂；
24、根据运动模式的不同，设计不同的约束条件，从而实现对机械臂位置不同的移动规划，满足无移动约束的自平行运动、相对自身参考面平行移动的自平行运动、相对作业参考面平行移动的自平行运动不同模式的要求。
1.一种串联机械臂位置级的自平行运动规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述机械臂由摆动关节θ1、俯仰关节θ2、臂伸缩关节q3、滚动关节θ4、俯仰关节θ5、摆动关节θ6和梁伸缩关节q7共7个关节和连杆串联而成。
3.根据权利要求1所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s1中，包括以下子步骤：
4.根据权利要求2所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s2中，求解机械臂末端相对于台车坐标系{0}的位姿矩阵并求解位姿矩阵获得机械臂末端朝向a和末端位置p：
5.根据权利要求4所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s3中，运动模式具有以下约束：
6.根据权利要求4所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s4中，如果规划的关节位置使得a的齐次向量为恒定值，则末端在移动过程中朝向不变；如果规划的关节位置使得p的某方向分量为恒定值，则末端将相对以该方向为法向量的平面平行移动。
7.根据权利要求5所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s4中，无移动约束的自平行运动模式下，规划串联机械臂各关节位置时，需要保证运动过程中末端朝向的方向余弦为恒定值，运动约束如下：
8.根据权利要求5所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s4中，相对自身参考面的自平行运动模式下，规划串联机械臂各关节位置时，需要保证运动过程中末端朝向的方向余弦为恒定值且末端位置的x方向运动速度为0，运动约束如下：
9.根据权利要求5所述的一种串联机械臂位置级的自平行运动规划方法，其特征在于，所述s4中，相对作业参考面的自平行运动模式下，规划串联机械臂各关节位置时，已知作业坐标系与台车坐标系的位姿矩阵需要保证作业坐标系{f}下末端朝向和yf方向速度为0，运动约束如下：
