机械臂安全边界控制方法和系统

技术领域

本发明涉及机械臂领域，特别涉及一种机械臂安全边界控制方法和系统。

背景技术

随着人工智能技术的发展，各类机器人在医疗领域、工业制造等领域得到越来越广泛的应用，机械臂是机器人的核心部件，人们通过对机械臂的操控来使机器人完成各种复杂且精细的操作，例如可进行外科手术、智能制造等操作。其中，如何实现对机械臂的精准控制是机器人领域的发展中最关键的核心技术之一。

在手术机器人领域，手术机器人一般包括控制台车、视觉定位追踪系统和机械臂，视觉定位追踪系统可以对机械臂的位置进行追踪，此外，机械臂的关节处还设置有编码器，可以反馈机械臂的位置和运动状态，控制台车可以根据视觉定位追踪系统传输的位姿信号和机械臂编码器反馈的机械臂的位置和运动状态信号对机械臂的位姿和运动状态进行操控，从而实现对机械臂的控制。

一般手术机器人在进行手术操作时，机械臂的运行范围会存在一个安全区域，机械臂只有在该安全区域内运行时，才可保证手术的正常进行，当机械臂因为各种原因超出该安全区域时，可能会出现机械臂碰撞到患者或者其他医疗设备等情况，造成人员受伤危险或者医疗设备损伤等问题，当机械臂超出安全区域后，只能关机将机械臂人为移动到安全区域内后重新开机，才可正常运行，这无疑增加了手术医生的干预次数和干预时间，影响了手术的正常进行，这无疑算是机器人的故障之一。因此，对机械臂的安全边界控制是机械臂控制过程中很重要的一环。而上段提到的控制方法在机械臂安全边界控制方面，会由于控制延时、位姿信号偏差等原因导致机械臂超出安全区的问题。首先，视觉定位追踪系统对机械臂的位置追踪主要基于光学追踪，当视觉定位追踪系统与机械臂之间存在遮挡时，会导致视觉定位追踪系统无法实时追踪到机械臂的位置，这样由于视觉定位遮挡的问题就会使得视觉定位追踪系统反馈的机械臂的位姿信号存在偏差，导致最终对机械臂的控制也存在偏差，导致机械臂在控制过程中可能会超出安全边界区域。另外，由于机械臂的编码器安装在机械臂的关节处，随着机械臂使用时间越来越长，关节处的磨损不可避免，也会导致编码器出现磨损，这样机械臂关节处的编码器反馈的位置和运动状态信号也会随着时间的推移存在误差，使得最终对机械别的控制也会出现偏差。而且，由于机械臂编码器的带宽不高，其反馈的位置和运动状态信号会存在一定的延迟，使得控制台车对机械臂的控制出现一定的延迟，也会导致机械臂超出安全区域。

需要说明的是，公开于该发明背景技术部分的信息仅仅旨在加深对本发明一般背景技术的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域技术人员所公知的现有技术。

发明内容

本发明的目的在于提供一种机械臂安全边界控制方法和系统，以解决现有技术中机械臂在控制过程中容易超出安全边界的问题。

为解决上述技术问题，本发明提供一种机械臂安全边界控制方法，包括：

S1：安装于机械臂关节处的IMU传感器采集机械臂的第一运动信号，安装于机械臂关节处的编码器采集机械臂的第二运动信号，视觉定位追踪系统采集机械臂的第三运动信号；

S2：获取机械表的安全边界；

S3：根据所述第一运动信号、所述第二运动信号、所述第三运动信号和所述机械臂的安全边界对所述机械臂的轨迹进行规划，获取安全警戒区域，以及对应的机械臂的临界运动状态T1；

S4：获取机械臂t时刻的实时运动状态Ti，当Ti≥T1时，判断机械臂超出安全警戒区域；

S5：当机械臂超出安全警戒区域时，根据IMU传感器采集的t时刻的第二实时运动信号获取前馈指令，并根据所述前馈指令对所述机械臂进行控制。

进一步的，所述S2中，所述第一运动信号包括机械臂的加速度信号和/或角度信号，所述第二运动信号包括机械臂的第二位姿信号，所述第三运动信号包括机械臂的第三位姿信号。

进一步的，所述S1具体包括：

建立IMU坐标系、机械臂坐标系和视觉坐标系，并将三个坐标系进行归一化，得到坐标转换矩阵；

根据设定的机械臂的运动区域以及坐标转换矩阵获取机械臂在视觉坐标系下的安全边界。

进一步的，所述S3包括：

S31：根据所述第一运动信号、所述第二运动信号、所述第三运动信号和所述坐标转换矩阵获取机械臂在视觉坐标系下的运动状态T0；

S32：根据所述机械臂在视觉坐标系下的运动状态T0以及所述安全边界对机械臂的轨迹进行规划，获取安全警戒区域，及对应的机械臂的临界运动状态T1。

进一步的，所述S32中，采用五次多项式插值法对机械臂的轨迹进行规划。

进一步的，所述机械臂的临界运动状态T1包括安全警戒区域内的至少一个临界位置，及机械臂在该临界位置对应的临界速度和/或临界加速度值。

进一步的，当Ti≥T1时，所述机械臂在t时刻运动至临界位置时对应的实时速度或实时加速度值大于等于机械臂在该临界位置对应的临界速度或临界加速度值。

进一步的，所述第二实时运动信号包括机械臂的加速度信号，所述S32中，根据所述机械臂的当前加速度获取所述前馈指令。

进一步的，所述第二实时运动信号包括机械臂的角度信号，所述S32中，根据所述机械臂的当前角度获取机械臂的当前速度，并根据所述当前速度获取所述前馈指令。

本发明还提供一种机械臂安全边界控制系统，包括：

IMU传感器，所述IMU传感器设置于机械臂的关节处，被配置为采集所述机械臂的第一运动信号；

编码器，所述编码器设置于所述机械臂的关节处，被配置为采集所述机械臂的第二运动信号；

视觉定位追踪系统，被配置为采集所述机械臂的第三运动信号；

驱动器，所述驱动器与所述IMU传感器和所述编码器通信连接，被配置为读取所述第一运动信号和所述第二运动信号；

控制模块，所述控制模块与所述IMU传感器、所述编码器、所述机械臂、所述视觉定位追踪系统和所述驱动器通信连接；

所述控制模块还被配置为：根据所述第一运动信号、所述第二运动信号、所述第三运动信号和机械臂的安全边界区域对所述机械臂的轨迹进行规划，获取安全警戒区域对应的机械臂的临界运动状态T1，并在机械臂运动超出安全警戒区域时，根据IMU传感器采集的t时刻的第二实时运动信号获取前馈指令，并根据所述前馈指令对所述机械臂进行控制。

综上，与现有技术相比，本发明提供的机械臂安全边界控制方法及系统具有以下优点：

本发明在机械臂的关节处设置IMU传感器实时采集机械臂的运动参数，由于IMU传感器的带宽相较于编码器更高，使得机械臂的运动参数能更快反馈至控制模块，有效降低了控制延迟。

并且，利用IMU传感器、编码器和视觉定位追踪系统采集的数据进行安全警戒区域规划，当机械臂超出安全警戒区域时，即发出预警信号，让控制模块提前对机械臂进行调控，这样可以有效抑制超调造成的运动过冲，避免机械臂非预期进入不安全区域，提高了系统的安全性和可靠性。

此外，本发明的方案中，当机械臂超出安全警戒区域时，利用IMU传感器采集的低延迟数据进行前馈控制，可以进一步提高系统的响应速度，降低机械臂的控制延迟，进一步抑制了机械臂的超调造成的运动过冲。而且，IMU传感器采集的数据不受视觉遮挡，也不会有驱动误差影响，也没有编码器磨损导致数据不准确的情况发生，可以使得机械臂的调控更加精确。

进一步的，本发明的机械臂安全边界控制方法中，直接以IMU采集的加速度信号作为前馈控制的输入，具有两个明显的优势，一是直接获取加速度信号，减少系统计算时间，提高系统响应速度；二是加速信号引入机械臂控制系统内环控制(电流环)，可以提高机械臂系统响应速度，有利于控制机械臂远离安全边界，保证手术安全。

附图说明

图1为本发明一实施方式的一种机械臂安全边界控制方法的示意图；

图2为本发明一实施方式的一种机械臂安全边界控制方法的流程示意图；

图3为本发明一实施方式的机械臂安全边界控制系统中视觉定位追踪系统与机械臂之间的对应关系图；

图4为本发明一实施方式中IMU传感器设置在机械臂上的结构示意图；

图5为本发明一实施方式中定位靶标设置在机械臂上的结构示意图；

图6为本发明一实施方式的一种机械臂安全边界控制方法中机械臂的安全边界的示意图；

图7为本发明一实施方式中采用加速度作为前馈指令的控制模式图；

图8为本发明一实施方式中采用速度信号作为输入的控制模式图。

其中，10-视觉定位追踪系统；20-机械臂；21-IMU传感器；22-定位靶标。

具体实施方式

以下结合附图和具体实施方式对本发明提出的一种机械臂安全边界控制方法和系统作进一步详细说明。根据下面说明，本发明的优点和特征将更清楚。

需要说明的是，附图采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施方式的目的。为了使本发明的目的、特征和优点能够更加明显易懂，请参阅附图。须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容能涵盖的范围内。

本发明的目的在于提供一种机械臂安全边界控制方法和系统，以解决现有技术中机械臂在控制过程中容易超出安全边界的问题。

图1是本发明提供的一种机械臂安全边界控制方法的示意图，图2是机械臂安全边界控制方法的流程示意图，其中，本发明提供的机械臂安全边界控制方法具体包括如下步骤：

S1：如图3所示，安装于机械臂20关节处的IMU传感器21采集机械臂的第一运动信号，安装于机械臂20关节处的编码器(图中未示出)采集机械臂20的第二运动信号，视觉定位追踪系统10采集机械臂20的第三运动信号。在机械臂20的关节处一般会安装编码器，该编码器可采集机械臂20的位姿信号，为了描述方便，在此将编码器采集的位姿信号定义为第二运动信号，该第二运动信号包括机械臂20的第二位姿信号(为方便将运动信号与位姿信号对应起来，以避免后续描述时混淆，此处直接将第二运动信号对应的位姿信号的编号定义为第二位姿信号，在本发明的方案中，不存在第一位姿信号)，该第二位姿信号反映的是通过机械臂20的编码器记录的机械臂20相对于初始状态时的位姿信息，编码器可将该第二运动信号反馈给控制模块，以对机械臂20的运动运动状态进行监控。此外，视觉定位追踪系统10也会对机械臂20进行实时追踪，采集机械臂20的第三运动信号，该第三运动信号包括机械臂20的第三位姿信号，反映的是机械臂20在视觉坐标系下的位姿状态，包括机械臂20与周围物体之间的位置距离，例如机械臂20相对于病患或者其他靶标之间的位姿状态，以及根据视觉定位追踪系统10采集的图像信息得到的机械臂的速度信号等信息，视觉定位追踪系统10可以将该第三位姿信号反馈给控制模块。在本发明的方案中，还通过设置在机械臂20关节处的IMU传感器21实时采集机械臂20的运动信号，该运动信号可以定义为第一运动信号，该第一运动信号可以包括机械臂20的加速度信号和/或角度信号，例如可以利用加速度传感器采集机械臂20的加速度信号，也可利用角速度传感器等采集机械臂20的角度信号。IMU传感器21采集的该第一运动信号可反馈给控制模块。

S2：获取机械臂的安全边界；机械臂在工作时，会根据实际的操作流程划定机械臂的正常运行范围，该范围即为机械臂的安全边界。机械臂的安全边界由两个部分组成，包括静态安全边界和动态安全边界，静态安全边界一般根据CT影像在术前规划给出，如图3至图6所示，以手术机器人为例，会根据临床的手术需求，设定术式需求空间，例如设定的术式需求空间为以病患为中心的300*300*300mm的立方体空间，手术机器人的机械臂20的末端即在该立方体空间内带动手术器械运动以进行手术操作，即机械臂20末端在视觉定位追踪系统10的视觉坐标系下的空间坐标范围为(x,y,z)＝(±150mm，±150mm，±150mm)，如图6中虚线的机械臂所示的位置即为机械臂的静态安全边界。当机械臂20的末端超出该范围时，可能会碰撞到其他医疗设备或医务操作人员，导致设备损坏或人员受伤。因此，必须保证机械臂在静态安全边界内运动，才能保障手术的正常进行。一般来说，机械臂的静态安全边界在工作之前，就会根据实际情况进行划定，但该静态安全边界并不是设定后就完全不可变动，而是可以根据实际情况进行更新，例如在手术过程中，当病患移动时，该机械臂20的静态安全边界也可重新规划设定。除静态安全边界外，机械臂的安全边界还包括动态安全边界部分，该部分主要包括机械臂的实时运动状态，动态安全边界反映的是机械臂当前的惯量，当前机械臂运动越快，则代表机械臂从运动状态到静止状态所需时间越长，机械臂超出界限的可能性也就越高，该部分是后续设定警戒区域的重要参考指标。具体来说，动态安全边界对应的机械臂的运动状态例如可以包括机械臂当前的实时速度和加速度，该部分可根据上述S1步骤采集的第一运动信号、第二运动信号和/或第三运动信号计算得到。由此，可将机械臂的安全边界P表征为：P＝k

其中，P代表机械臂的安全边界，Ps代表的是术前规划的机械臂的静态安全边界，Vi是机械臂当前的实时速度信号(可以是机械臂当前的实时速度，也可以包括机械臂当前的实时加速度)，k1和k2为常量边界系数。具体地，V

本领域技术人员应当理解的是，本发明的机械臂并不局限于手术机器人的机械臂，还可以是其他领域的机械臂，例如可以是智能制造工厂中的机械手等，即本发明的安全边界控制方法可应用于任何需要进行机械臂安全边界控制的领域中。

S3：根据所述第一运动信号、所述第二运动信号、所述第三运动信号和所述机械臂20的安全边界对所述机械臂20的轨迹进行规划，获取机械臂的安全警戒区域，得到机械臂在安全警戒区域对应的机械臂的临界运动状态T1。当采集到三个运动信号后，就可以对机械臂20当前的运动状态进行确定，并且安全边界对机械臂20的轨迹进行规划，设定机械臂20的安全警戒区域，机械臂20的安全警戒区域包括静态位置区域和对应的机械臂20的动态临界速度部分。机械臂20的安全警戒区域代表的是：当机械臂20到达安全警戒区域的位置时，此时机械臂20的运动速度若大于安全警戒区域设定的临界速度，若不进行额外的干预，按照原有的趋势继续运动，机械臂20就有极大的概率最终超出安全边界，因此，提前设定安全警戒区域可以提前获知机械臂20是否有超出安全边界的风险，从而方便后续对机械臂20的运动进行干预，防止其超出安全边界。机械臂20的安全警戒区域的设定与机械臂20当前的运动状态，机械臂20的质量以及控制系统的信号响应速度有关，当机械臂20运动越快，机械臂20的质量越大，控制系统的信号响应速度越慢，则代表机械臂20越不容易从当前的运动状态降低速度，也越容易超出安全区域，则安全警戒区域的静态位置区域设置得就更小，离安全边界的位置就越远，以留出足够大的响应空间来使机械臂降速。需要注意的是，机械臂20的安全警戒区域是根据机械臂20当前的实时运动状态和安全边界进行设定，因此并不是完全固定不变的，当机械臂20的运动状态发生改变时，该安全警戒区域也可以随之变动。在本发明的方案中，机械臂20的运动状态一般包括机械臂20当前的位置，以及机械臂20当前的速度和/或加速度值，安全警戒区域对应的机械臂20的临界运动状态T1可以是机械臂20的临界速度，也可以是加速度值，或同时包含速度值和加速度值。需要注意的是，机械臂20的安全警戒区域可以同时包含多个临界位置及其对应的机械臂20的临界运动状态，即可以在机械臂20的安全警戒区域内选取若干临界位置，每个临界位置对应机械臂20的一个临界速度，这样组成一组包含临界位置及对应临界速度的集合，在后续对机械臂20的运动状态进行监控时，可以根据需要提前对机械臂20是否超出安全警戒区域进行判定，即只要当机械臂20后续运动到对应的一个临界位置时，发现其实时运动速度大于该临界位置对应的临界速度时，则可判定机械臂20有超出安全警戒区域的风险。

S4：获取机械臂t时刻的实时运动状态Ti，并将其与阈值状态T2进行比较，当Ti≥T2时，判断机械臂20超出安全警戒区域，其中T2≤T1。当确定好安全警戒区域后，即可对机械臂20后续的运动状态进行监控，当发现机械臂超出安全警戒区域，即代表需要对机械臂20的运动进行干预，防止其超出安全边界。但由于传感器采集的机械臂的实时速度可能会存在一定的误差，且控制系统对机械臂20的控制不可避免的存在一定的延迟，因此，如果完全按照安全警戒区域对应的临界速度作为干预调控的标准，可能会出现控制延迟的情况，导致机械臂20超出安全边界，因此，可以设定一阈值状态T2，该阈值状态T2下，机械臂20的速度或加速度值小于等于安全警戒区域对应的机械臂20的临界速度或加速度值。可以将机械臂20的实时运动状态Ti与该阈值状态T2进行比较，当Ti≥T2时，代表所述机械臂20在t时刻的位置对应的速度或加速度值大于等于阈值状态下机械臂20在该位置对应的速度或加速度值，此时即判定机械臂20在t时刻超出安全警戒区域，需要对机械臂20的运动进行调控，防止其超出安全边界。

S5：当机械臂20超出安全警戒区域时，根据IMU传感器21采集的t时刻的第二实时运动信号获取前馈指令，并根据所述前馈指令对所述机械臂20进行控制。在本发明的方案中，当检测到机械臂20超出安全警戒区域后，即对机械臂20的运动状态进行调节，防止在后续的运动过程中超出安全边界，具体来说，会根据IMU传感器21实时采集的第二实时运动信号生成前馈指令，控制模块以该前馈指令对机械臂20进行控制，例如IMU传感器21会实时采集机械臂20当前的运动参数，当机械臂20运动超出安全警戒区域时，表明当前的运动参数使机械臂20的运动过快了，此时控制模块会根据机械臂20当前的运动参数生成一个前馈指令，通过该前馈指令控制机械臂20的运动速度慢下来，使机械臂20提前降速，防止机械臂20超出安全边界。

在本发明的方案中，在机械臂20的关节处设置IMU传感器21实时采集机械臂的运动参数，由于IMU传感器21的带宽相较于编码器更高，使得机械臂20的运动参数能更快反馈至控制模块，有效降低了控制延迟。并且，利用IMU传感器21、编码器和视觉定位追踪系统10采集的数据进行安全警戒区域规划，当机械臂20超出安全警戒区域时，即发出预警信号，让控制模块提前对机械臂20进行调控，这样可以有效抑制超调造成的运动过冲，避免机械臂20非预期进入不安全区域，提高了系统的安全性和可靠性。此外，本发明的方案中，当机械臂20超出安全警戒区域时，利用IMU传感器21采集的低延迟数据进行前馈控制，可以进一步提高系统的响应速度，降低机械臂20的控制延迟，进一步抑制了机械臂20的超调造成的运动过冲。而且，IMU传感器21采集的数据不受视觉遮挡，也不会有驱动误差影响，也没有编码器磨损导致数据不准确的情况发生，可以使得机械臂20的调控更加精确。

进一步的，由于IMU传感器21，机械臂的编码器以及视觉定位追踪系统10采集的数据不在同一坐标系下，为了便于计算，减少机械臂控制系统的计算时长，缩短系统的响应时间，可以对采集的三个运动信号的数据进行归一化处理，具体来说，在所述S1步骤中，可以先建立IMU坐标系、机械臂坐标系和视觉坐标系，并将三个坐标系进行归一化，得到坐标转换矩阵；然后再根据设定的机械臂的运动区域以及坐标转换矩阵获取机械臂在视觉坐标系下的安全边界。并且，后续IMU传感器以及编码器采集的运动参数也可根据坐标转换矩阵转换至视觉坐标系下。例如，如图5所示，可以将一定位靶标22设置在机械臂20的定位特征点上，可以是机械臂20的末端，也可以是机械臂20的其他位置。然后利用视觉定位追踪系统10追踪定位靶标22，即可得到机械臂20在视觉坐标系下的位置，由于IMU传感器21安装在机械臂20的关节处，其与机械臂20的位置关系，以及与定位靶标22的位置关系都是确定的，进而可以得到IMU传感器21在视觉坐标系下的位置。

其中，为了便于计算，减少机械臂控制系统的计算时长，在本实施例中，在装配时，可以使IMU传感器21的轴与机械臂20的转轴重合，这样就可以将IMU坐标系和机械臂坐标系的Z轴重合。在进行坐标归一化处理时，可采集机械臂在视觉坐标系下的多组姿态数据

进一步的，所述S3包括：

S31：根据所述第一运动信号、所述第二运动信号、所述第三运动信号和所述坐标转换矩阵获取机械臂20在视觉坐标系下的运动状态T0；IMU传感器21和编码器采集的运动信号，都可以根据机械臂20的动力学模型和雅可比矩阵等转换至机械臂末端的运动信号，然后通过坐标转换矩阵转换成视觉坐标系下机械臂20的实时位姿以及速度。

S32：根据所述机械臂20在视觉坐标系下的运动状态T0以及所述安全边界对机械臂20的轨迹进行规划，获取安全警戒区域，及对应的机械臂20的临界运动状态T1。具体来说，可先根据设定的安全边界，确定机械臂末端的位置，然后再通过齐次变换矩阵进行逆运算，求解视觉坐标系下，机械臂各关节在安全边界范围内转动的角度α＝[α1、α2、α3、α4、...、αi]，然后根据机械臂20在视觉坐标系下的实时位姿和速度等获取机械臂20当前的转动角度，再通过矩阵变换转换到机械臂坐标系下，然后进行机械臂轨迹规划，例如可以采用五次多项式插值法对机械臂的轨迹进行规划，。此时，通过机械臂20的轨迹规划，结合机械臂20自身的参数(质量等参数)以及控制系统的响应时间等可以得到所述安全警戒区域，在所述安全警戒区域对应的临界位置，对应了机械臂临界运动状态下的速度信息，包括机械臂的位置θ(t)＝[θ1,θ2,θ3,θ4,…,θi]、及对应的速度

作为本发明的一种实现方式，所述第二实时运动信号包括机械臂20的加速度信号，在所述S32中，根据所述机械臂20的当前加速度获取所述前馈指令。具体来说，当机械臂20超出安全警戒区域后，可以将IMU传感器21采集的机械臂20的当前加速度信号直接作为前馈指令，控制模块根据该当前加速度与阈值状态T2对应的加速度值生成控制指令，传递给机械臂，使机械臂的加速度降低至阈值状态T2对应的加速度值以下，使机械臂20提前减速，远离安全警戒区域，防止机械臂20超出安全边界，保证手术顺利进行。其控制模式如图7所示，包括内环控制和外环控制，电机电气及电机机械环节组成机械臂基础硬件系统，电流校准和电机电气环节、电流传感器组成该控制模式的内环控制(即电流环控制)，此环通过电流传感器(如电磁电流传感器、电子电流传感器等)检测机械臂运动过程中驱动器给电机的各相的输出电流，负责反馈的电流校正环节进行PID调节，从而达到输出电流尽量接近等于设定电流。而速度校准、电机机械环节、速度传感器及内环共同组成控制模式的外环控制(即速度环控制)，通过检测的伺服电机编码器的信号来进行反馈PID调节，它的环内PID输出直接就是电流环的设定。整个控制目的保证机械臂准确移动所需要的位置。当机械臂运动进入安全警戒区域时，IMU加速度信号作为机械臂内环控制的输入，形成新的机械臂外环控制模式，控制机械臂远离安全警戒区或停止运动到安全警戒区域。直接以IMU采集的加速度信号作为前馈控制的输入，具有两个明显的优势，一是直接获取加速度信号，减少系统计算时间，提高系统响应速度；二是加速信号引入机械臂控制系统内环控制(电流环)，可以提高机械臂系统响应速度，有利于控制机械臂远离安全边界，保证手术安全。

在本发明的另一种实施方式中，所述第二运动信号包括机械臂的角度信号，所述S32中，根据所述机械臂的当前角度获取机械臂的速度，并根据所述速度获取所述前馈指令。具体来说，当机械臂超出安全警戒区域后，驱动器读取IMU采集的机械臂的角度信号，并传递给控制模块，控制模块通过微分计算，利用角度信号可以获取机械臂的速度值，该速度值一般为角速度，然后将该速度信号导入到控制系统中，控制模块基于该速度信号生成前馈指令，对机械臂进行控制。其控制模式如图8所示，该控制模式同样包括内环控制和外环控制，内环控制与上述描述基本相同，电流校准和电机电气环节、电流传感器组成该控制模式的内环控制(即电流环控制)，此环通过电流传感器(如电磁电流传感器、电子电流传感器等)检测机械臂运动过程中驱动器给电机的各相的输出电流，负责反馈的电流校正环节进行PID调节，从而达到输出电流尽量接近等于设定电流。速度校准、电机机械环节、速度传感器及内环共同组成控制模式的外环控制(即速度环控制)，通过检测的伺服电机编码器的信号来进行反馈PID调节，它的环内PID输出直接就是电流环的设定。整个控制目的保证机械臂准确移动所需要的位置。当机械臂运动超出安全警戒区域时，该速度信号与阈值状态T2对应的速度值之间的差值信号作为机械臂控制模式的外环输入，形成新的机械臂外环控制模式，从而降低机械臂运行速度，距离边界越近，速度下降越快，直至下降为0，从而保证机器人一直运行在安全区域。

本发明还提供了一种机械臂安全边界控制系统，包括：

IMU传感器21，所述IMU传感器21设置于机械臂20的关节处，被配置为实时采集所述机械臂20的第一运动信号；

编码器，所述编码器设置于所述机械臂20的关节处，被配置为实时采集所述机械臂20的第二运动信号；

视觉定位追踪系统10，被配置为实时采集所述机械臂20的第三运动信号；

驱动器，所述驱动器与所述IMU传感器21和所述编码器通信连接，被配置为读取所述第一运动信号和所述第二运动信号；通过驱动器直接读取IMU传感器21采集的数据，并将该数据通过Ethcart(也可通过can等传输数据)传送到控制模块中，可以大大降低数据传输的延迟。

控制模块，所述控制模块与所述IMU传感器21、所述编码器、所述机械臂20、所述视觉定位追踪系统10和所述驱动器通信连接；

所述控制模块还被配置为：根据所述第一运动信号、所述第二运动信号、所述第三运动信号和机械臂的安全边界区域对所述机械臂的轨迹进行规划，实时获取安全警戒区域，并在机械臂运动超出所述安全警戒区域时，根据实时采集的所述第二运动信号获取前馈指令，并根据所述前馈指令对所述机械臂进行控制。

基于同一发明构思，本发明一实施方式还提供了一种电子设备，所述电子设备包括处理器和存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现上文所述的机械臂安全边界控制方法。由于本发明提供的电子设备与本发明提供的机械臂安全边界控制方法属于同一发明构思，因此本发明提供的电子设备至少具有本发明提供的机械臂安全边界控制方法的所有有益效果，具体可以参考上文中有关本发明提供的机械臂安全边界控制方法所具有的有益效果的相关描述，故在此不再对本发明提供的电子设备所具有的有益效果进行赘述。

本发明还提供了一种可读存储介质，所述可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时可以实现上文所述的机械臂安全边界控制方法。由于本发明提供的可读存储介质与本发明提供的机械臂安全边界控制方法属于同一发明构思，因此本发明提供的可读存储介质具有本发明提供的机械臂安全边界控制方法的所有有益效果，具体可以参考上文中有关本发明提供的机械臂安全边界控制方法的有益效果的相关描述，故在此不再对本发明提供的可读存储介质所具有的有益效果进行赘述。

综上，与现有技术相比，本发明提供的机械臂安全边界控制方法及系统具有以下优点：

本发明在机械臂的关节处设置IMU传感器实时采集机械臂的运动参数，由于IMU传感器的带宽相较于编码器更高，使得机械臂的运动参数能更快反馈至控制模块，有效降低了控制延迟。

并且，利用IMU传感器、编码器和视觉定位追踪系统采集的数据进行安全警戒区域规划，当机械臂超出安全警戒区域时，即发出预警信号，让控制模块提前对机械臂进行调控，这样可以有效抑制超调造成的运动过冲，避免机械臂非预期进入不安全区域，提高了系统的安全性和可靠性。

此外，本发明的方案中，当机械臂超出安全警戒区域时，利用IMU传感器采集的低延迟数据进行前馈控制，可以进一步提高系统的响应速度，降低机械臂的控制延迟，进一步抑制了机械臂的超调造成的运动过冲。而且，IMU传感器采集的数据不受视觉遮挡，也不会有驱动误差影响，也没有编码器磨损导致数据不准确的情况发生，可以使得机械臂的调控更加精确。

进一步的，本发明的机械臂安全边界控制方法中，直接以IMU采集的加速度信号作为前馈控制的输入，具有两个明显的优势，一是直接获取加速度信号，减少系统计算时间，提高系统响应速度；二是加速信号引入机械臂控制系统内环控制(电流环)，可以提高机械臂系统响应速度，有利于控制机械臂远离安全边界，保证手术安全。

上述描述仅是对本发明较佳实施方式的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于权利要求书的保护范围。显然，本领域的技术人员可以对发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。