一种机器人驱控系统及方法与流程

1.本发明涉及电机控制技术领域，具体而言，涉及一种机器人驱控系统及方法。


背景技术：

2.微创手术可视化技术将手术诊疗技术大大提高，该技术有着伤口小、疼痛轻、恢复快的优点，在手术过程中医生与助手可以清楚地观察病灶位置，相互配合完成手术。医生控制台所使用的控制器，为了更好的体现真实环境，控制器一般要求反馈器械末端的受力情况或对器械末端受到的限位给出提示，方便医生在观察屏幕的同时能够感知手术效果。
3.控制器反馈部分一般由驱动控制回路实现，现有的驱动控制回路往往反馈控制不精确，导致操作体验与真实工况存在偏差。


技术实现要素：

4.本发明所要解决的问题是如何提高驱动控制回路的控制精确。
5.为解决上述问题，一方面，本发明提供了一种机器人驱控系统，包括依次相连的主控单元、驱动器、控制线路和关节电机；
6.所述控制线路包括依次串联的多个pi控制器，所述控制线路的输入一端与所述驱动器相连，所述控制线路的输出一端与所述关节电机相连；且多个所述pi控制器分别与所述关节电机相连，以构成多个反馈环，多个所述反馈环之间相互嵌套，外层反馈环的输出信息作为内层反馈环的输入信息。
7.可选地，多个所述反馈环包括电流环、速度环和位置环，所述位置环的pi控制器输出的修正位置作为所述速度环的pi控制器的输入，所述速度环的pi控制器输出的修正速度作为所述电流环的pi控制器的输入，所述电流环的pi控制器输出的修正电流发送给所述关节电机。
8.可选地，所述主控单元通过多个控制线程与所述驱动器相连。
9.可选地，所述主控单元与所述驱动器之间采用ethercat通信方式。
10.另外一方面，本发明还提供了一种机器人驱控方法，基于上述机器人驱控系统，所述驱控方法包括:
11.主控单元向驱动器发送控制信息；
12.所述驱动器根据所述控制信息向控制线路发送位置指令；
13.控制线路中的多个反馈环获取关节电机的实时运行信息和所述位置指令；
14.多个所述反馈环根据所述实时运行信息和所述位置指令，向所述关节电机发送修正信息，修正所述关节电机的运行状态。
15.可选地，所述多个所述反馈环根据所述实时运行信息和所述位置指令，向所述关节电机发送修正信息，修正所述关节电机的运行状态包括：
16.所述实时运行信息包括实时位置信息、实时速度信息和实时电流信息；
17.位置环中的pi控制器根据所述实时位置信息和所述位置指令，向速度环中的pi控
制器发送修正位置信息；
18.所述速度环中的所述pi控制器根据所述修正位置信息、所述实时速度信息和速度补偿信息，向电流环中的pi控制器发送修正速度信息；
19.所述电流环中的所述pi控制器根据所述修正速度信息、所述实时电流信息和电流补偿信息，向所述关节电机发送修正电流信息。
20.可选地，当所述pi控制器输出的控制量大于或等于控制量阈值时，停止向所述pi控制器发送输入信息。
21.可选地，所述驱动器根据所述控制信息驱动关节电机运行之后，所述机器人驱控方法还包括：
22.主控单元对所述关节电机进行心跳检测、故障检测和飞车检测；
23.所述飞车检测包括：
24.在飞车检测的一个检测周期内，根据所述实时电流信息和理论电流信息，得到所述电流差值；
25.若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值；
26.若一个所述检测周期内所有所述超限电流差值的总和大于电流总差阈值时，则判定控制出错，切换至错误处理机制。
27.可选地，所述若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值之后，包括：
28.在一个所述检测周期结束后，将所述检测周期与所述关节电机一次反馈调节的耗时相除，得到调节次数；
29.若所述电流超限次数大于所述调节次数的一半时，则判定控制出错，切换至错误处理机制。
30.可选地，所述若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值之后，包括：
31.在所述检测周期进行到一半时，开启两条控制线程；
32.当任意所述控制线程的所述检测周期结束时，若所述电流超限次数大于所述调节次数，则判定控制出错，切换至错误处理机制。
33.与现有技术相比，本发明具有如下有益效果：
34.本发明提供的一种机器人驱控系统及方法，主控单元向驱动器发送控制信息，驱动器根据所述控制信息驱动关节电机运行，关节电机的实时运行信息被控制线路中的多个pi控制器获取，多个pi控制器与关节电机组成多个反馈环，反馈环之间层层嵌套，其中，外层反馈环根据关节电机的实时运行信息和主控单元输出的控制信息，输出一个修正信息，并且外层反馈环的输出信息作为其相邻内层反馈环的输入信息，内层反馈环再结合关节电机的实时运行信息，输出另一个修正信息，依次类推，达到多个反馈环层层修正，最终输出精度更高的修正信息，使得关节电机的运行精度更高，让用户可以感受到真实的操作工况，增强操作体验感。
附图说明
35.图1示出了本发明实施例中一种机器人驱控系统的结构示意图；
36.图2示出了本发明实施例中控制线路的原理示意图；
37.图3示出了本发明实施例中一种机器人驱控方法的流程示意图；
38.图4示出了本发明实施例中pi控制器的改进示意图。
具体实施方式
39.为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
40.要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。
41.在本说明书的描述中，参考术语“实施例”、“一个实施例”和“一个实施方式”等的描述意指结合该实施例或实施方式描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或实施方式中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或实施方式。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或实施方式以合适的方式结合。
42.图1示出了本发明实施例中一种机器人驱控系统的结构示意图，所述机器人驱控系统，包括依次相连的主控单元、驱动器、控制线路和关节电机；
43.如图2所示，所述控制线路包括依次串联的多个pi控制器，所述控制线路的输入一端与所述驱动器相连，所述控制线路的输出一端与所述关节电机相连；且多个所述pi控制器分别与所述关节电机相连，以构成多个反馈环，多个所述反馈环之间相互嵌套，外层反馈环的输出信息作为内层反馈环的输入信息。
44.具体地，如图1所示，主控单元与驱动器相连，关节驱动器共有4个，分别记为关节驱动器1、关节驱动器2、关节驱动器3、关节驱动器4。由于机器人使用环境和空间限制，可以设置关节驱动器1负责驱动4个关节电机，其余驱动器各负责驱动1个关节电机，当然，如果机器人的关节更多，还可以使用其余驱动器驱动更多的关节电机。主控单元可以选用plc或者单片机，也可以是cpu或者mcu芯片。
45.如图2，主控单元向驱动器发送控制信息(即为图2中的位置指令)；驱动器根据所述控制信息驱动关节电机运行，关节电机的实时运行信息被控制线路中的多个pi控制器获取，多个pi控制器与关节电机组成多个反馈环，反馈环之间层层嵌套，其中，外层反馈环根据关节电机的实时运行信息和主控单元输出的控制信息(即想要关节电机达到的理论运行信息)，输出一个修正信息，并且外层反馈环的输出信息作为其相邻内层反馈环的输入信息，然后外层的相邻内层反馈环再结合关节电机的另一实时运行信息以及接收到的外层反馈环输出的一个修正信息，从而输出另一个修正信息，依次类推，外层反馈环的输出信息作为内层反馈环的输入信息，再结合关节电机的实时运行信息，达到多个反馈环层层修正，最终输出精度更高的修正信息，关节电机的运行精度更高，让操作者感觉到机器人如臂使指，想让机器人到达某个位置时，机器人就能够立刻到达某个位置，用户可以立马感受到当前
机器人真实所在的操作环境以及机器人当前遇到的操作阻力，增强操作体验感。
46.在本发明的一种实施例中，多个所述pi控制器与所述关节电机之间构成电流环、速度环和位置环，所述位置环、所述速度环和所述电流环的输入信息分别为所述关节电机的实时位置信息、实时速度信息和实时电流信息，所述位置环的pi控制器输出的修正位置作为所述速度环的pi控制器的输入，所述速度环的pi控制器输出的修正速度作为所述电流环的pi控制器的输入，所述电流环的pi控制器输出的修正电流发送给所述关节电机。
47.具体地，如图2，主控单元的控制信息通过驱动器形成位置指令发送给关节驱动器，关节电机被驱动器驱动运转，关节电机的实时运行信息被三个pi控制器获得，其中pi控制器1获得位置指令和实时位置信息，输出修正位置信息，并将该修正位置信息发送给pi控制器2，pi控制器2还获得关节电机的实时速度信息和速度补偿信息，使得pi控制器2输出修正速度信息，并将修正速度信息发送给pi控制器3，pi控制器3还获得实时电流信息和电流补偿信息，并输出电流修正信息，并将该电流修正信息发送给关节电机，修正关节电机的运行，使得关节电机达到主控单元想要关节电机到达的位置，增加机器人的操作精度，使得机器人如臂使指，增加操作者的操作真实感。
48.在本发明的一种实施例中，如图1，所述主控单元通过多个控制线程与所述驱动器相连。具体地，主控单元通过一分二主板(接线板)与关节驱动器相连，其中主控单元使用一束主控线束与主板相连，然后主板的两个输出接线口均与四个关节驱动器相连，形成双线程控制，使用一分二主板之后，使得主控线束的数量减少。另外，所述主控单元与所述驱动器之间采用ethercat(ethernet for control automation technology，控制自动化技术)通信方式，通信速度可以达到100m。ethercat有着速度快、线束少的优点，使得驱动器电路整体体积很小，对结构空间需求更少，使得机器人的剩余设计空间变大，为外观部件预留更多空间。在主控单元与关节驱动器进行数据传输的过程中，过程数据通过pdo传输，配置数据通过sdo传输，使得控制和配置分开，确保运动过程中不会因为参数改变而引起误动作。
49.图3示出了本发明实施例中一种机器人驱控方法的流程示意图，所述机器人驱控方法包括：
50.s1：所述主控单元向驱动器发送控制信息；例如，控制信息为图2中的位置指令，也可以说是关节电机应该达到的理论位置信息。
51.s2：所述驱动器根据所述控制信息向控制线路发送位置指令；不同的关节驱动器驱动相应的关节电机。
52.s3：控制线路中的多个反馈环获取关节电机的实时运行信息和所述位置指令；
53.s4：多个所述反馈环根据所述实时运行信息和所述位置指令，向所述关节电机发送修正信息，修正所述关节电机的运行状态。
54.具体地，主控单元向驱动器发送控制信息，驱动器根据所述控制信息驱动关节电机运行，关节电机的实时运行信息被控制线路中的多个pi控制器获取，多个pi控制器与关节电机组成多个反馈环，反馈环之间层层嵌套，其中，外层反馈环根据关节电机的实时运行信息和主控单元输出的控制信息，输出一个修正信息，并且外层反馈环的输出信息作为其相邻内层反馈环的输入信息，内层反馈环再结合关节电机的实时运行信息，输出另一个修正信息，依次类推，达到多个反馈环层层修正，最终输出精度更高的修正信息，使得关节电机的运行精度更高，让用户可以感受到真实的操作工况，增强操作体验感。
55.在本发明的一种实施例中，所述多个所述反馈环根据所述实时运行信息和所述位置指令，向所述关节电机发送修正信息，修正所述关节电机的运行状态包括：
56.所述实时运行信息包括实时位置信息、实时速度信息和实时电流信息；
57.位置环中的pi控制器根据所述实时位置信息和位置指令，向速度环中的pi控制器发送修正位置信息；
58.所述速度环中的所述pi控制器根据所述修正位置信息、所述实时速度信息和速度补偿信息，向电流环中的pi控制器发送修正速度信息；
59.所述电流环中的所述pi控制器根据所述修正速度信息、所述实时电流信息和电流补偿信息，向所述关节电机发送修正电流信息。
60.具体地，采用pi控制器主要用于消除稳态误差，使得控制更精准。多个pi控制器与关节电机之间形成位置环、速度环和电流环，其中位置环处于最外层，速度层居中，电流环在最内层，外层反馈环的输出作为次一级内层反馈环的输入，同时结合关节电机的实时运行信息，达到多环修正，另外还加入速度补偿以及电流补偿，用于上层前馈控制，增加补偿后可以改善用户体验，电流环主要针对受力情况的反馈，使得用户可以感受到操作阻力，操作中的相关限位提示，可通过改变补偿参数进行操作。
61.在本发明的一种实施例中，当所述pi控制器输出的控制量大于控制量阈值时，停止向所述pi控制器发送输入信息。
62.具体地，如图4，假设输出控制量为u，输入控制量为i，当输出控制量大于或等于预设的控制量阈值时，此时就暂停i的作用或者直接将i置为0，防止超调的问题出现，使得轨迹更平滑，体验效果更好。
63.在本发明的一种实施例中，所述驱动器根据所述控制信息驱动关节电机运行之后，所述机器人驱控方法还包括：
64.主控单元对所述关节电机进行心跳检测、故障检测和飞车检测。
65.具体地，所述心跳检测为了检测主控单元以及驱动器是否在线，增加周期性检测信号，当主控单元和驱动器均不在线时，产生触发信号，用于启动安全处理机制。关于驱动器的故障检测包括过流、过载、通信中断、过温、编码器等故障的检测，会实时传递给主控单元。另外主控单元中的sto(停止)功能用于紧急停止操作。
66.在本发明的一种实施例中，所述飞车检测包括：
67.在飞车检测的一个检测周期内，根据所述实时电流信息和理论电流信息(目标电流)，得到所述电流差值；
68.若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值；
69.若一个所述检测周期内所有所述超限电流差值的总和大于电流总差阈值时，则判定控制出错，切换至错误处理机制。
70.具体地，预先设定电流差阈值a和电流总差阈值d，规定控制周期(关节电机一次反馈调节的耗时)为δt；
71.在一个检测周期t内，若(实时电流信息-理论电流信息＝电流差值)＞电流差阈值a，计算超限电流差值c＝实时电流-理论电流，同时累加电流超限次数e，若电流差值＜电流差阈值a，超限电流差值c＝0，并在每个周期内累加超限电流差值c的值，得到一个所述检测
周期内所有所述超限电流差值的总和b。第一判断条件为，超限电流差值的总和b超过电流总差阈值d，则进入错误处理程序，保证系统安全。该方法以加法运算替代复杂的发散计算，简化计算，可以降低cpu成本，加快计算速度。
72.在本实施例中，所述若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值之后，包括：
73.在一个所述检测周期结束后，将所述检测周期与所述关节电机一次反馈调节的耗时相除，得到调节次数；
74.若所述电流超限次数大于所述调节次数的一半时，则判定控制出错，切换至错误处理机制。
75.具体地，另一判断条件为，检测周期t计时结束时，计算检测周期t/控制周期δt＝一个检测周期内对关节电机的调节次数；若电流超限次数e＞调节次数的一半，则进入错误处理程序，保证系统安全。
76.在本实施例中，所述若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值之后，包括：
77.在所述检测周期进行到一半时，开启两条控制线程；
78.当任意所述控制线程的所述检测周期结束时，若所述电流超限次数大于所述调节次数，则判定控制出错，切换至错误处理机制。
79.具体地，为了防止一个检测周期内有数据传输遗漏情况，可在t/2时刻开启另一线程进行相同检测，对两个线程进行同时监控，两个线程中会传输相同的数据，进行相同的计算和判断，但是如果有一个线程中数据传输丢失，另一个线程中数据可能该数据没有丢失，保证最终判断结果的准确性。因此增加另一判断条件，在任意线程检测周期t结束时刻，若电流超限次数e＞调节次数，则进入错误处理程序，保证系统安全。从检测周期一半时间点才开启的另一线程，其实际调节次数是一直运行线程的调节次数的一半。
80.虽然本发明披露如上，但本发明并非限定于此。任何本领域技术人员，在不脱离本发明的精神和范围内，均可作各种更动与修改，因此本发明的保护范围应当以权利要求所限定的范围为准。

技术特征：
1.一种机器人驱控系统，其特征在于，包括依次相连的主控单元、驱动器、控制线路和关节电机；所述控制线路包括依次串联的多个pi控制器，所述控制线路的输入一端与所述驱动器相连，所述控制线路的输出一端与所述关节电机相连；且多个所述pi控制器分别与所述关节电机相连，以构成多个反馈环，多个所述反馈环之间相互嵌套，外层反馈环的输出信息作为内层反馈环的输入信息。2.根据权利要求1所述的机器人驱控系统，其特征在于，多个所述反馈环包括电流环、速度环和位置环，所述位置环的pi控制器输出的修正位置作为所述速度环的pi控制器的输入，所述速度环的pi控制器输出的修正速度作为所述电流环的pi控制器的输入，所述电流环的pi控制器输出的修正电流发送给所述关节电机。3.根据权利要求1所述的机器人驱控系统，其特征在于，所述主控单元通过多个控制线程与所述驱动器相连。4.根据权利要求1所述的机器人驱控系统，其特征在于，所述主控单元与所述驱动器之间采用ethercat通信方式。5.一种机器人驱控方法，其特征在于，基于权利要求1-4任一项所述的机器人驱控系统，所述驱控方法包括：主控单元向驱动器发送控制信息；所述驱动器根据所述控制信息向控制线路发送位置指令；控制线路中的多个反馈环获取关节电机的实时运行信息和所述位置指令；多个所述反馈环根据所述实时运行信息和所述位置指令，向所述关节电机发送修正信息，修正所述关节电机的运行状态。6.根据权利要求5所述的机器人驱控方法，其特征在于，所述多个所述反馈环根据所述实时运行信息和所述位置指令，向所述关节电机发送修正信息，修正所述关节电机的运行状态包括：所述实时运行信息包括实时位置信息、实时速度信息和实时电流信息；位置环中的pi控制器根据所述实时位置信息和所述位置指令，向速度环中的pi控制器发送修正位置信息；所述速度环中的所述pi控制器根据所述修正位置信息、所述实时速度信息和速度补偿信息，向电流环中的pi控制器发送修正速度信息；所述电流环中的所述pi控制器根据所述修正速度信息、所述实时电流信息和电流补偿信息，向所述关节电机发送修正电流信息。7.根据权利要求6所述的机器人驱控方法，其特征在于，当所述pi控制器输出的控制量大于或等于控制量阈值时，停止向所述pi控制器发送输入信息。8.根据权利要求5所述的机器人驱控方法，其特征在于，所述驱动器根据所述控制信息驱动关节电机运行之后，还包括：所述主控单元对所述关节电机进行心跳检测、故障检测和飞车检测；所述飞车检测包括：在飞车检测的一个检测周期内，根据所述实时电流信息和理论电流信息，得到所述电
流差值；若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值；若一个所述检测周期内所有所述超限电流差值的总和大于电流总差阈值时，则判定控制出错，切换至错误处理机制。9.根据权利要求8所述的机器人驱控方法，其特征在于，所述若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值之后，包括：在一个所述检测周期结束后，将所述检测周期与所述关节电机一次反馈调节的耗时相除，得到调节次数；若所述电流超限次数大于所述调节次数的一半时，则判定控制出错，切换至错误处理机制。10.根据权利要求9所述的机器人驱控方法，其特征在于，所述若所述电流差值大于电流差阈值时，对电流超限次数进行累加，并将所述电流差值标记为超限电流差值之后，包括：在所述检测周期进行到一半时，开启两条控制线程；当任意所述控制线程的所述检测周期结束时，若所述电流超限次数大于所述调节次数，则判定控制出错，切换至错误处理机制。

技术总结
本发明提供了一种机器人驱控系统及方法，涉及电机控制领域，系统包括依次相连的主控单元、驱动器、控制线路和关节电机；所述控制线路包括依次串联的多个PI控制器，所述控制线路的输入一端与所述驱动器相连，所述控制线路的输出一端与所述关节电机相连；且多个所述PI控制器分别与所述关节电机相连，以构成多个反馈环，多个所述反馈环之间相互嵌套，外层反馈环的输出信息作为内层反馈环的输入信息。主控单元向驱动器发送控制信息，驱动器驱动关节电机运行，关节电机的实时运行信息被控制线路中的多个PI控制器获取，多个反馈环层层嵌套，达到层层修正，使得关节电机的运行精度更高，让用户可以感受到真实的操作工况，增强体验感。增强体验感。增强体验感。


技术研发人员：周德刚 张海柱 苏衍宇 邹建宇 张星亮
受保护的技术使用者：哈尔滨思哲睿智能医疗设备股份有限公司
技术研发日：2023.05.29
技术公布日：2023/8/21