一种机械臂主从控制方法与流程

本发明涉及机械臂控制，更具体地，涉及一种机械臂主从控制方法。

背景技术：

1、主从控制，指的是用户通过操纵杆遥控机械臂末端，使得机械臂末端能够实时保持和操纵杆一样的姿态。虽然开发机械臂控制程序时，开发者主要关注的是机械臂末端的姿态，但由于机械臂的姿态是由各个关节处的电机来控制的，机械臂的上位机只能直接控制电机，因此通常需要根据机器人运动学将机械臂末端姿态和电机组的位置进行相互转换。假设机械臂末端的笛卡尔空间坐标和姿态量为机械臂各个电机位置为则正向机器运动学是为了根据机械臂机械结构得到函数：逆向运动学则相反：

2、由于上位机要直接控制步进电机，需要向步进电机发放脉冲电流，而一般的上位机无法直接生成脉冲电流，因此上位机可以通过电机驱动器内置的控制方式来间接操控步进电机。因此如果要按照上述思路来实现主从控制，就只能在电机位置层面上进行位置追踪。但是通常机械臂末端的在笛卡尔空间坐标系上的各个姿态分量和各个电机的位置并不是线性关系，而电机驱动器通常能直接提供的电机控制命令都是匀速运动，这就使得机械臂在运动过程中，末端会出现很明显的抖动，难以满足精度的要求。

技术实现思路

1、本发明的目的在于克服现有技术对机械臂进行主从控制时，机械臂末端的控制精度低，会出现明显抖动的不足，提供一种机械臂主从控制方法。

2、本发明的目的可采用以下技术方案来达到：

3、一种机械臂主从控制方法，其特征在于，包括如下步骤：

4、s1：根据机械臂末端初始位置和目标位置分别对应的电机组位置和求出机械臂末端从初始置到目标位置时机械臂电机组的最大位移根据精度要求δs，确定路径点数量由于机械臂末端从初始位置移动到第k个路径点时机械臂末端的位置从而得到每个路径点pk对应的电机组位置

5、s2：根据各个路径点之间的距离，确定相邻的两个路径点之间的分段时间长度tk；

6、s3：根据电机组当前的实际位置与下一个需要到达的目标路径点对应的电机组位置之间的位移和该分段时间长度tk，计算电机组的速度命令电机组调整到速度并维持该速度进行运动，直到经过该分段时间tk；

7、s4：重复步骤s3直到走完所有路径点。

8、本发明通过在笛卡尔坐标系上的当前机械臂末端位置和目标位置之间均匀地插入若干个路径点，这些路径点能保证在笛卡尔坐标系上的机械臂末端能够平稳运行。在得到这些路径点后通过机器人运动学计算模块统统转变成电机位置。这样，只要电机组能依次经过这些路径点，就能保证精度。

9、上位机通过电机驱动器驱动电机运动时，通常电机驱动器都会提供定位模式和速度模式，前者需要上位机指定运动目标位置和运动速度，在收到新命令前，驱动器会按照收到的位置和速度控制电机，直到电机到达目标位置后停下。后者则是直接让电机一直按照指定的速度运动，直到收到其他控制命令。而为了保证机械臂末端的运动是一气呵成，在移动的过程中不会停止-启动的停顿，这就要求电机到达路径点时，速度不能降为0，那么驱动器方面使用速度控制模式才能满足该要求。由于电机驱动器可以在极短的时间内将电机调整至目标速度，因此，我们可以假定整个电机组的运动过程是在以途经各个路径点的时刻为分割点的分段匀速运动。在使用速度模式时，必须要保证所有的电机都能在同一时刻到达路径点，因此在前面所述将整个运动过程均匀插入路径点的前提下，相邻的两个路径点之间的分段时间长度tk是固定的，这样由于两个路径点之间的距离确定，使得电机以一定的速度运动时间长度tk便可保证电机能走过一定的距离并且确保机械臂末端是匀速运动。

10、但同时又由于电机的速度变化不可能是瞬间变化的，因此电机需要一定加速或减速时间，电机走过时间tk后的实际位置sk′与当前的路径点位置sk有一定差别。为此，假设电机当前的实际位置sk-1′，电机每次走向下一个的路径点sk时，在分段时间tk固定的情况下，以电机实际的位置sk-1′与下一个需要到达的目标路径点sk之间的距离来重新计算电机的速度vk′，这样，不会产生累计误差，从而保证整个控制过程的准确性。其中，表示在到达第k个路径点时，由若干个电机组成的电机组各个电机速度数值所组成的向量；表示在到达第k-1个路径点时机械臂电机组中的各个电机实际位置数值所组成的向量；表示第k个路径点处机械臂电机组中的各个电机的理想位置数值所组成的向量。

11、进一步的，所述步骤s1之前还包括如下步骤：

12、s0：控制装置读取机械臂的操纵杆的姿态数据，从而确定机械臂末端需要到达的目标位置。

13、实现主从控制即在控制过程中，控制装置不断读取操纵杆的姿态数据，让机械臂以最快速度追上该姿态数据所确定的目标，从而实现机械臂跟随操纵杆运动。而控制装置为控制电机的运动的装置，如控制装置可以包括上位机和电机驱动器。

14、进一步的，所述步骤s4还包括如下步骤：

15、若走完所有路径点之前机械臂末端的目标位置发生改变，则仅改变机械臂当前的目标路径点之后的路径点，并利用目标路径点作为下一次路径点规划时机械臂末端的初始位置，重新进行步骤s1；若机械臂末端的目标位置没有发生改变，则走完所有路径点之后，电机速度降为0。

16、由于使用操作杆控制机械臂时是不断调整使得主从控制的运动目标时刻发生变化，如果机械臂需要将已经规划的路径完全走完之后再走新的路径，那么机械臂末端跟随操纵杆的效果就很差，因此当运动目标发生变化时，需要在当前路径未走完就必须切换到新路径，而切换方式也会影响机械臂运动的精准度。本发明中，当机械臂末端的目标位置发生改变时，只要电机以某个路径点为目标路径点进行运动时，任何路径变化都不会改变当前的目标路径点，电机仍以当前目标路径点位为目标进行运动，同时，以该目标路径点对应的机械臂末端位置作为机械臂末端的初始位置进行新的路径点规划。这样，在主从控制的运动目标发生变化时，机械臂的控制精度更高，运动连贯性更好。

17、进一步的，所述步骤s2中，由于电机组在相邻的两个路径点之间的位置差以及要到后一个路径点时的速度之间的关系为根据用户对机械臂末端所需要的运动速度来决定决定分段时间长度tk。

18、在完成路径点的规划后，每相邻的两个路径点中前一个路径点对应的电机组位置为后一个路径点对应的电机组位置为而达到后一个路径点时需要电机组达到速度为因此，根据这样通过控制分段时间tk即可达到需要的速度整个电机组运动过程也是机械臂末端从起始位置运动到目标位置的过程，在分段确定的情况下，分段时间长度越小，整体运动时间越短，从而末端运动速度越快。

19、此处的速度为没有误差的理想速度，但由于电机的速度变化不可能是瞬间变化的，因此电机需要一定加速或减速时间，为了避免此过程中产生的误差不断累计，因此本发明在所述步骤s3中，以各个电机实际的位置sk-1′与下一个需要到达的目标路径点sk之间的距离来重新计算电机组的速度但由电机加速时间较短，最终电机组的实际运动速度与规划的理想速度相差不大，不影响机械臂的控制效果。

20、进一步的，利用一个串口与一台电机匹配。

21、延迟主要是通信延迟造成的，一些延迟高的电机驱动器通信通常是用串口，为了降低延迟，就需要一个串口配一个电机。

22、进一步的，一个电机对应一个控制线程，且电机控制线程之间通过多线程同步栅栏发送控制命令。

23、这样，可以尽可能确保控制命令同时发出，从而保证电机组各个电机运动的同步性。

24、进一步的，所述电机为步进电机。

25、步进电机能确保较为精确地移动，减少误差。

26、进一步的，当所述电机组以最后一个路径点为目标路径点时，计算电机组各个电机实际位置与目标路径点的位置差，并计算各个电机以当前速度运动直至到达目标路径点位置所需要的补差时间t′，然后根据各电机计算得到的最小补差时间t′min，使得各电机以当前的速度继续运动度t′min。

27、为了尽可能减小机械臂停止运动后机械臂末端的实际位置与目标位置之间的误差尽可能的小，电机在走完规划的路径点之后仍继续运动一段微小的时间以减小电机的速度变化时间产生的误差。

28、进一步的，所述当电机组以最后一个路径点为目标位置时，目标速度为vn，加速时间为δt，最终电机组的实际位置为

29、设变速前的电机组速度为当前第n段路径的目标速度为加速时间为δt，分段时间长度为tn，当前路径点的目标位置为则最终的电机实际位置

30、进一步的，所述电机组走完最后一个路径点时实际位置与目标路径点的位置差为

31、根据上述的速度与位移的关系式得到将该等式带入电机的实际位置得到：

32、与现有技术相比，本发明的有益效果是：

33、(1)本发明通在当前机械臂末端位置和目标位置之间均匀地插入若干个路径点，使得电机组能依次经过这些路径点，就能保证精度，同时，通过固定两个路径点之间的分段运动时间，从而以电机实际的位置与下一个需要到达的目标路径点之间的距离来计算电机的速度，这样，不会产生累计误差，从而保证整个控制过程的准确性。

34、(2)本发明在当机械臂末端的目标位置发生改变时，电机仍以当前目标路径点位为目标进行运动，同时，以该目标路径点对应的机械臂末端位置作为机械臂末端的初始位置进行新的路径点规划。这样，在主从控制的运动目标发生变化时，机械臂的控制精度更高，运动连贯性更好。