一种仿生机器蛇多关节自动校准复位方法、系统、计算机设备和存储介质与流程

本发明涉及机器人运动控制与多关节执行器校准领域，尤其涉及一种仿生机器蛇多关节自动校准复位方法、系统、计算机设备和存储介质。

背景技术：

1、仿生机器蛇通常由多个关节沿蛇体纵向依次连接，各关节由总线舵机驱动。装配偏差、传动间隙、长期磨损及冲击载荷会使各舵机的实际零位偏离控制器记录的中心位置，造成蛇体姿态偏斜、复位后仍残留角度偏差，因此需要对各关节进行零位校准，使每个舵机的实际零位与控制器记录的中心位置重新一致。

2、现有校准作业多采用人工方式，操作人员逐个读取舵机位置、判断零位并写入偏移量。随着关节数量增多，舵机通信编号与沿蛇体的实际关节顺序往往因出厂编号、更换维护等原因而不一致，人工建立二者对应关系耗时且易错，难以保证每个关节的零位偏差均被准确识别和补偿，补偿的完整性无法保证。

3、若为提高效率而让多个舵机同时自动向目标零位运动，由于机器蛇的相邻关节沿蛇体连续布置，相邻舵机在同一时段运动会带动相邻蛇身节段发生接触、挤压或卡滞，使读取到的实际位置因机械干涉而失真，导致后续补偿所依据的位置偏差不可信，补偿结果的可靠性难以保证。

4、此外，常用总线舵机的硬件偏移寄存器具有有限的可写范围，仅靠硬件偏移只能补偿位于该允许范围内的偏差，超出该范围的部分无法由硬件单独补偿，补偿同样不完整；校准参数写入非易失性存储器后，还可能因通信丢帧、写入失败或校验异常而未正确保存，且缺乏逐关节的定量验证手段，难以确认每个舵机的补偿参数是否真正生效。

5、因此，如何对仿生机器蛇多个关节的零位偏差进行完整补偿，并保证补偿结果可靠有效，是本领域需要解决的技术问题。

技术实现思路

1、本发明提供一种仿生机器蛇多关节自动校准复位方法、系统、计算机设备和存储介质，能够在自动建立舵机与关节顺序对应关系的基础上，避免相邻关节机械干涉、对超出硬件偏移寄存器允许范围的位置偏差进行完整补偿，并对校准参数实现可靠固化与逐关节验证。

2、为实现上述目的，本发明提供以下的技术方案：

3、一种仿生机器蛇多关节自动校准复位方法，所述仿生机器蛇包括沿蛇体依次连接的多个总线舵机以及与所述多个总线舵机通信连接的控制器，所述方法包括：

4、由所述控制器对所述多个总线舵机进行在线检测，以获得在线舵机序列，并根据所述在线舵机序列建立所述多个总线舵机对应的关节顺序；

5、根据所述多个总线舵机沿蛇体的连接关系确定相邻舵机，并将所述多个总线舵机划分为至少两个校准组，使具有相邻连接关系的舵机被分配至不同的校准轮次；按照所述校准轮次依次驱动各校准组中的舵机向目标零位运动，并在舵机到达所述目标零位后读取对应舵机的实际位置；

6、根据所述实际位置与所述目标零位之间的位置偏差，结合对应舵机的硬件偏移寄存器的允许偏移范围确定硬件偏移量，并将所述位置偏差中未由所述硬件偏移量补偿的剩余偏差转换为对应舵机的软件中心位置修正量；

7、将所述硬件偏移量写入对应舵机的硬件偏移寄存器，并将所述软件中心位置修正量对应的软件中心位置保存至非易失性存储器；

8、分别回读所述硬件偏移寄存器中的硬件偏移量和所述非易失性存储器中的软件中心位置，并将回读结果与对应的目标值进行比较；当任一舵机的回读结果与对应目标值不一致时，仅针对该舵机重新执行校准参数写入和回读验证，直至验证一致或达到预设重试条件。

9、可选的，所述在线检测包括：

10、所述控制器在预设舵机编号范围内依次向各候选编号发送位置读取指令，并将返回有效位置数据的候选编号确定为在线舵机编号，以形成所述在线舵机序列；

11、当所述在线舵机序列中的舵机编号不连续时，所述控制器按照在线舵机的原编号由大到小的顺序依次执行编号写入，以将所述在线舵机重新编号为连续编号。

12、可选的，在重新编号过程中，所述控制器针对按照原编号由大到小排列的各在线舵机依次写入对应的新编号，使每个在线舵机待写入的新编号不大于该在线舵机的原编号，以降低编号重写过程中不同舵机占用相同通信地址的可能性。

13、可选的，所述将所述多个总线舵机划分为至少两个校准组，包括：

14、根据所述多个总线舵机的关节顺序，将具有相邻编号或相邻关节位置的舵机划分至不同校准组；

15、在第一校准轮次中驱动第一校准组中的舵机向所述目标零位运动，并保持第二校准组中的舵机停止；

16、在所述第一校准轮次完成后，在第二校准轮次中驱动所述第二校准组中的舵机向所述目标零位运动，并保持所述第一校准组中的舵机停止，

17、使任意具有相邻连接关系的两个舵机在校准过程中不同时处于运动状态；

18、所述多个总线舵机按照关节顺序依次编号时，将编号为奇数的舵机划分为第一校准组，将编号为偶数的舵机划分为第二校准组，并依次执行所述第一校准组和所述第二校准组的校准。

19、可选的，所述结合对应舵机的硬件偏移寄存器的允许偏移范围确定硬件偏移量，并将所述位置偏差中未由所述硬件偏移量补偿的剩余偏差转换为对应舵机的软件中心位置修正量包括：

20、将所述位置偏差与所述硬件偏移寄存器的允许偏移范围进行比较；

21、当所述位置偏差位于所述允许偏移范围内时，将所述位置偏差确定为所述硬件偏移量，并将所述软件中心位置保持为预设中心位置；

22、当所述位置偏差超出所述允许偏移范围时，将与所述位置偏差同方向且不超过所述允许偏移范围边界的偏移量确定为所述硬件偏移量，并根据所述位置偏差与所述硬件偏移量之间的差值确定所述软件中心位置修正量；

23、其中，所述硬件偏移量和所述软件中心位置修正量基于同一位置坐标关系确定；

24、所述硬件偏移量通过对所述位置偏差进行限幅得到，所述软件中心位置根据所述位置偏差与限幅得到的硬件偏移量之间的剩余偏差进行修正，使所述硬件偏移量与所述软件中心位置共同补偿所述位置偏差。

25、可选的，所述控制器内置有运动控制模块，所述运动控制模块用于生成各总线舵机的步态控制脉冲；

26、所述非易失性存储器设有与所述多个总线舵机对应的校准参数存储区域，所述校准参数存储区域至少存储对应舵机的舵机编号、硬件偏移量和软件中心位置，并进一步存储用于识别校准参数版本的数据版本标识和用于校验所存储校准参数的数据校验值；

27、所述控制器在将校准参数写入所述非易失性存储器后，对所写入的校准参数进行校验，并在校验通过后利用所述软件中心位置更新所述运动控制模块中的中心位置数据；

28、当所述非易失性存储器中的校准参数校验不通过时，所述控制器保持更新前的所述运动控制模块中的中心位置数据，并重新执行对应校准参数的写入和校验；

29、当所述校准参数校验通过时，所述控制器将校验通过的校准参数对应的软件中心位置更新至所述运动控制模块；

30、所述运动控制模块利用更新后的中心位置数据生成对应总线舵机的步态控制脉冲，以所述软件中心位置作为对应舵机的步态控制中心项，并根据舵机的关节顺序、时间参数以及步态参数生成多个舵机对应的步态控制脉冲；

31、所述步态控制脉冲由所述软件中心位置与周期性步态控制量叠加得到，并根据所述多个总线舵机对应的软件中心位置确定所述周期性步态控制量的允许幅值，使所生成的步态控制脉冲处于对应舵机的有效控制范围内。

32、可选的，所述回读验证包括：

33、从对应舵机的硬件偏移寄存器回读硬件偏移量，并从所述非易失性存储器回读软件中心位置；

34、分别将回读的硬件偏移量和软件中心位置与对应的目标硬件偏移量和目标软件中心位置进行比较；

35、当任一比较结果超出对应的预设容差范围时，仅对产生所述比较结果的舵机重新执行校准参数写入及回读验证；

36、当重新执行次数达到预设重试次数且仍未通过验证时，记录对应舵机的故障状态并输出校准失败信息；

37、在所述校准参数回读验证通过后，所述控制器再次驱动所述多个总线舵机向所述目标零位运动，并读取各总线舵机的验证位置；当各总线舵机的验证位置均处于预设位置误差范围内时，判定所述多关节自动校准复位完成。

38、另一方面，本发明还提供了一种仿生机器蛇多关节自动校准复位系统，包括：

39、在线检测模块，用于对沿蛇体依次连接的多个总线舵机进行在线检测，以获得在线舵机序列，并根据所述在线舵机序列建立所述多个总线舵机对应的关节顺序；

40、分组校准模块，用于根据所述多个总线舵机沿蛇体的连接关系确定相邻舵机，并将所述多个总线舵机划分为至少两个校准组，使具有相邻连接关系的舵机被分配至不同的校准轮次，并按照所述校准轮次依次驱动各校准组中的舵机向目标零位运动；

41、偏差补偿模块，用于根据各总线舵机的实际位置与所述目标零位之间的位置偏差，结合对应舵机的硬件偏移寄存器的允许偏移范围确定硬件偏移量，并根据未由所述硬件偏移量补偿的剩余偏差确定软件中心位置；

42、参数存储模块，用于将所述硬件偏移量和软件中心位置保存至非易失性存储器；

43、验证模块，用于回读所述硬件偏移量和软件中心位置，并将回读结果与对应目标值进行比较，在任一舵机的回读结果不一致时，仅针对该舵机重新执行校准参数写入和回读验证。

44、另一方面，本发明还提供了一种计算机设备，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述计算机程序由所述处理器执行时，使所述计算机设备执行如上任一项所述的仿生机器蛇多关节自动校准复位方法。

45、另一方面，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，使所述处理器执行如上任一项所述的仿生机器蛇多关节自动校准复位方法。

46、与现有技术相比，本发明具有以下有益效果：

47、（1）通过由控制器对多个总线舵机进行在线检测并获得在线舵机序列，进而建立各总线舵机对应的关节顺序，使每个关节均被自动识别并纳入校准，避免了人工建立编号与关节顺序对应关系的耗时与差错；同时，通过根据实际位置与目标零位之间的位置偏差，结合硬件偏移寄存器的允许偏移范围确定硬件偏移量，并将未由硬件偏移量补偿的剩余偏差转换为软件中心位置修正量，使超出硬件偏移寄存器允许范围的偏差也能被完整补偿。

48、（2）通过根据各总线舵机沿蛇体的连接关系确定相邻舵机，并将具有相邻连接关系的舵机分配至不同的校准轮次，使相邻舵机不会同时向目标零位运动，避免机械干涉导致读取的实际位置失真，保证补偿依据可靠；通过将硬件偏移量写入硬件偏移寄存器、将软件中心位置保存至非易失性存储器，并分别回读二者与目标值比较、仅对不一致的舵机重新执行写入和回读验证，使补偿参数得以持久保存且能够被逐关节定量确认，保证补偿结果真正生效。

49、本发明具有其它的特性和优点，这些特性和优点从并入本文中的附图和随后的具体实施方式中将是显而易见的，或者将在并入本文中的附图和随后的具体实施方式中进行详细陈述，这些附图和具体实施方式共同用于解释本发明的特定原理。