一种人形机器人一体化关节抗扰动控制方法及控制系统与流程

本发明涉及人形机器人一体化关节控制领域，具体讲的是一种人形机器人一体化关节抗扰动控制方法及控制系统。

背景技术：

1、人形机器人在军事、医疗、救援以及现代制造业中将发挥重要作用，能够满足任务的多样性及环境的复杂性要求。而人形机器人一体化关节的运动精度和抗扰动能力对于其工作效果和稳定性至关重要，同时关节驱动系统是人形机器人运动的核心组成部分。随着人们对人形机器人末端的快速定位、精确抓取和动作稳定等需求日益增加，这对一体化关节的抗扰动控制技术提出了更高的要求。因此如何提高人形机器人一体化关节运动稳定性及其抗扰动控制方法成为研究的重点。

技术实现思路

1、本申请要解决的一个技术问题是，克服以上相关技术的缺陷，提供一种运算速度快、计算精度高、系统抗扰动和可靠性高、硬件成本低的人形机器人一体化关节抗扰动控制方法及控制系统。

2、本发明解决技术问题所采用的一个技术方案为：一种人形机器人一体化关节抗扰动控制方法，通过电流环矢量控制算法来控制人形机器人的永磁同步电机，具体包括：对于电流环矢量控制算法的各步骤归一化处理；对归一化处理后的各步骤计算公式中的三角函数通过查表辅助的数值拟合方法求解。

3、作为优选，所述表中共含有900个数组，每个数组保存了角度值、相应正弦值、相应余弦值，角度值范围为0°-89.9°，步长为0.1°；查表辅助的数值拟合方法求解过程为：将待求解的角度值θ分解为小数部分第一位之前的部分θa和小数部分除第一位之后的其他部分θb，即θ＝θa+θb；然后在表中分别查出θa的正、余弦值sinθa、cosθa；最后通过拟合公式计算角度值θ的正、余弦值，拟合公式为：

4、拟合公式中的拟合参数k1和k2会根据角度值θ距离表中前一个角度值θa之间的角度差θb自行调整，能够获得更好的拟合准确度。

5、本申请的人形机器人一体化关节抗扰动控制方法与相关技术相比，具有以下优点：将占用资源较大的三角函数常规运算处理改进为通过查表辅助的数值拟合方法，可以最大程度上提高求解速度和求解精度，同时降低查表深度和查表广度，从而在保证快速高精度求解三角函数的同时尽量减少对硬件资源的占用，实现电流环矢量控制算法高速运行来满足人形机器人一体化关节高动态抗扰动的性能要求。

6、作为优选，归一化处理后的电流环矢量控制算法的各步骤的计算公式为：

7、(1)clarke变换：

8、c1＝1；c2＝0；x1＝ia；x2＝0；

9、

10、isa＝y1；isβ＝y2；

11、(2)park变换：

12、c1＝cos(θe)；c2＝sin(θe)；x1＝isa；x2＝isβ；

13、c3＝-sin(θe)；c4＝cos(θe)；x3＝isa；x4＝isβ；

14、isd＝θe1；isq＝θe2；

15、(3)pi控制器：

16、c1＝kd new；c2＝kd old；x1＝eid(k)；x2＝eid(k-1)；

17、c3＝kq_new；c4＝kq_old；x3＝eiq(k)；x4＝eiq(k-1)；

18、vsd(k)＝vsd(k-1)+θe1；

19、vsq(k)＝vsq(k-1)+θe2；

20、(4)park逆变换：

21、c1＝cos(θe)；c2＝-sin(θe)；x1＝vsd；x2＝vsq；

22、c3＝sin(θe)；c4＝cos(θe)；x3＝vsd；x4＝vsq；

23、vsa＝θe1；vsβ＝θe2；

24、(5)空间矢量脉宽调制所在空间扇区：

25、

26、va＝vsβ；vb＝θe1；vc＝θe2；

27、(6)计算svpwm各相理论开关时间：

28、

29、x＝θe1+θe2；y＝θe1；z＝θe2；

30、(7)计算svpwm各相实际脉宽调制时间：

31、

32、t1sat＝θe1；t2sat＝θe2；

33、其中，由独立的除法器运算，θe1为经过第一位置与速度感应模块解析获得的电机端的电角度值，θe2为经过第二位置与速度感应模块解析获得的负载端的电角度值。

34、本发明解决技术问题所采用的另一个技术方案为：一种人形机器人一体化关节抗扰动控制系统，包括一软核处理器，用于完成控制节拍较低的速度环和位置环组成的外环实时控制任务，负责和上位机的各通讯模块、以及整个控制系统的实时监控；空间矢量控制从系统，用于完成上述人形机器人一体化关节抗扰动控制方法中的电流环矢量控制算法，所述空间矢量控制从系统通过总线挂载到软核处理器上；

35、所述空间矢量控制从系统包括位置环模块、速度环模块，基于fpga的电流环模块，智能功率模块，永磁同步电机，电机端编码器，第一位置与速度感应模块，精密谐波减速机，精密谐波减速机机端负载，负载端多圈绝对值编码器，第二位置与速度感应模块和功能模块，所述功能模块接收接收第一位置与速度感应模块反馈的电机端转角和转速及第二位置与速度感应模块反馈的负载端转角和转速，通过比较和运算，将转角反馈值反馈给位置环模块，将速度反馈值反馈给速度环模块。

36、本申请的人形机器人一体化关节抗扰动控制系统与相关技术相比，具有以下优点：结合自适应滑模的电流环抑制扰动的控制方法，不仅能够自适应地调节控制参数，即使在控制对象发生改变或工作环境变化的情况下，也能保持较好的控制效果。通过应用fpga并行运算和准确匹配算法特点的时分复用技术，达到了在1.6us内完成电流环矢量控制算法的优异性能。通过自适应调节控制参数来抵消控制系统受到的外部干扰和噪声，增进系统的稳定性和可靠性，最终实现真正的一体化关节抗扰动控制，既能提高控制性能，又能有效节省硬件成本。各种外设可通过总线挂载到软核处理器上，具备良好的扩展和性能适配性能，可以兼容各种数字式编码器，支持多种通讯协议。

37、作为优选，基于fpga的电流环矢量控制模块包括运算核心单元、状态机、参数选择单元、中间变量存储器和内部连接模块；所述运算核心单元为以四个独立的乘法器和两个并行加法器组成的乘加计算单元，用于完成电流环矢量控制算法各步骤共享的两个基本运算公式：y1＝c1×x1+c2×x2。通过核心运算单元

38、y2＝c3×x3+c4×x4

39、高速并行实现和时分复用，不仅有效提高了整个电流环矢量控制运算速度，还明显减少了对fpga宝贵的乘法器资源需求。

技术特征：

1.一种人形机器人一体化关节抗扰动控制方法，通过电流环矢量控制算法控制人形机器人的永磁同步电机，具体包括：对电流环矢量控制算法的各步骤归一化处理；其特征在于，对归一化处理后的各步骤计算公式中的三角函数通过查表辅助的数值拟合方法求解。

2.根据权利要求1所述的一种人形机器人一体化关节抗扰动控制方法，其特征在于，所述表中共含有900个数组，每个数组保存了角度值、相应正弦值和相应余弦值，角度值范围为0°-89.9°，步长为0.1°；查表辅助的数值拟合方法求解过程为：将待求解的角度值θ分解为小数部分第一位之前的部分θa和小数部分除第一位之后的其他部分θb，即θ＝θa+θb；然后在表中分别查出θa的正、余弦值sinθa、cosθa；最后通过拟合公式计算角度值θ的正、余弦值，拟合公式为：

3.根据权利要求2所述的一种人形机器人一体化关节抗扰动控制方法，其特征在于，归一化处理后的电流环矢量控制算法的各步骤的计算公式为：

4.一种人形机器人一体化关节抗扰动控制系统，其特征在于，包括一软核处理器，用于完成控制节拍较低的速度环和位置环组成的外环实时控制任务，负责和上位机的各通讯模块、以及整个控制系统的实时监控；空间矢量控制从系统，用于完成如权利权利1～3任一项人形机器人一体化关节抗扰动控制方法中的电流环矢量控制算法，所述空间矢量控制从系统通过总线挂载到软核处理器上；

5.根据权利要求4所述的一种人形机器人一体化关节抗扰动控制系统，其特征在于，基于fpga的电流环模块包括运算核心单元、状态机、参数选择单元、中间变量存储器和内部连接模块；所述运算核心单元为以四个独立的乘法器和两个并行加法器组成的乘加计算单元，用于完成电流环矢量控制算法各步骤共享的两个基本运算公式：

技术总结
本发明公开了一种人形机器人一体化关节抗扰动控制方法及控制系统，改控制方法通过电流环矢量控制算法控制人形机器人的永磁同步电机，具体包括对电流环矢量控制算法的各步骤归一化处理；对归一化处理后的各步骤计算公式中的三角函数通过查表辅助的数值拟合方法求解；通过将占用硬件资源较大的三角函数常规运算处理改进为通过查表辅助的数值拟合方法，可以最大程度上提高求解速度和求解精度，同时降低查表深度和查表广度，从而在保证快速高精度求解三角函数的同时尽量减少对硬件资源的占用，实现电流环矢量控制算法高速运行来满足人形机器人一体化关节高动态抗扰动的性能要求。

技术研发人员：张树青,王宝磊,贾庆伟,王斐
受保护的技术使用者：台州清智数字科技有限公司
技术研发日：
技术公布日：2025/1/13