一种机器人运动控制方法及设备与流程

本发明实施例涉及机器人领域，特别是涉及一种机器人运动控制方法、设备及具有存储功能的装置。

背景技术：

现今，机器人在医疗、工业、生物等多个领域应用越来越广，对机器人，尤其是在液体中运动的微型机器人，运动控制精度和稳定性要求也越来越高。现有技术中，通常使用两部内外标定后的摄像机对液体中的微型机器人图像进行质心提取和主轴定位，继而完成其三维空间的位姿重建，该位姿决定了微型机器人的姿态和前进方向，然后将该重建结果反馈至位置闭环，在该位置闭环中计算该重建结果与预先给定的数据之间的误差，并将该误差传递给转向闭环，通过一个比例-积分控制器输出合适的磁场轴方向，利用该磁场轴方向，磁场驱动装置驱动3d亥姆霍兹线圈产生旋转的磁场，最终实现微型机器人位姿的闭环控制。

但是，上述方法中三维空间的位姿重建的精度依赖于摄像机标定的精度以及相关特征选取和计算的精度，该部分误差位于控制闭环外，无法通过闭环校正。此外，由于机器人运动过程中可能存在扰动，上述方法中的主轴方向未必是微型机器人实际的前进方向，则利用该主轴方向计算出的误差未必是微型机器人位姿的真正误差，因此上述方法存在控制模型失配的问题，稳定性不高。

技术实现要素：

本发明实施例主要解决的技术问题是提供一种机器人运动控制方法、设备及具有存储功能的装置，能够提高机器人运动控制的精度和稳定性。

为解决上述技术问题，本发明实施例采用的一个技术方案是：提供一种机器人运动控制方法，包括：对机器人的当前帧图像进行处理，以获取机器人在当前帧图像中的质心位置；获取机器人的移动方向，其中移动方向是机器人在前一帧图像和当前帧图像中的质心的移动方向；根据机器人的移动方向和预定移动方向的偏差，获取机器人的校正方向；利用该校正方向，控制驱动装置驱动机器人运动，以使得机器人的运动路径趋近预定路径。

为解决上述技术问题，本发明实施例采用的另一个技术方案是：提供一种机器人运动控制设备，包括：相互连接的处理器和通信电路；通信电路用于与外部设备进行通信；处理器用于执行指令以实现如上所述的方法。

为解决上述技术问题，本发明实施例采用的又一个技术方案是：提供一种具有存储功能的装置，存储有指令，该指令被执行时实现如上所述的方法。

本发明实施例的有益效果是：在本发明实施例的方法中，通过对机器人的当前帧图像进行处理，以获取机器人在当前帧图像中的质心位置，然后将机器人在前一帧图像和当前帧图像中的质心的移动方向，作为机器人的移动方向，根据机器人的移动方向和预定移动方向的偏差，获取机器人的校正方向，最后利用该校正方向，控制驱动装置驱动机器人运动，以使得机器人的运动路径趋近预定路径。上述实施例中，机器人的校正方向和反馈的移动方向均使用图像特征，从而可以直接在图像空间中完成闭环控制，无需三维重建，减少由于存在闭环外的误差导致控制精度下降的可能性，提高机器人运动控制的精度；而且使用时间上前后两帧机器人的质心的移动方向作为机器人的移动方向，该特征正是机器人移动方向的定义，因此不存在模型失配的问题，有效提高机器人运动控制的稳定性。

附图说明

图1是本发明机器人运动控制方法第一实施例的流程示意图；

图2是机器人在当前帧图像中的质心位置以及预设路径的场景示意图；

图3是本发明机器人运动控制方法采用的双闭环控制结构示意图；

图4是本发明机器人运动控制方法第二实施例的流程示意图；

图5是本发明机器人运动控制方法第三实施例的流程示意图；

图6是本发明机器人运动控制方法第四实施例的流程示意图；

图7是本发明机器人运动控制方法第五实施例的流程示意图；

图8是本发明机器人运动控制方法第六实施例的流程示意图；

图9是图7中步骤s132的具体流程示意图；

图10是本发明机器人运动控制方法第七实施例的流程示意图；

图11是机器人移动到预设路径终点的场景示意图；

图12是本发明机器人运动控制设备第一实施例的结构示意图；

图13是本发明机器人运动控制设备第二实施例的结构示意图；

图14是本发明具有存储功能的装置一实施例的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明机器人运动控制方法第一实施例包括：

s11：对机器人的当前帧图像进行处理，以获取机器人在当前帧图像中的质心位置；

其中，该机器人可以是磁控微型机器人，也可以是其他类型的机器人，此处不做具体限定。本发明实施例中以磁控微型机器人为例进行说明。

具体地，在一个应用例中，利用摄像机拍摄机器人图像前，需要对摄像机进行初始配置，例如设置该摄像机的拍摄间隔，如每秒拍摄帧数等，然后利用该摄像机连续获取机器人的图像。本实施例中，磁控微型机器人在低雷诺数液体中运动，利用三维线圈产生的均匀磁场控制该磁控微型机器人的运动，该均匀磁场可绕空间中任一轴线旋转，在不考虑其他因素时，该轴线方向可以认为是该磁控微型机器人的前进方向。其中，该液体也可以是其他类型的液体，此处不做具体限定。

上述应用例中，获取机器人当前帧图像后，对该当前帧图像进行处理，例如分割处理和目标提取等，可以获取当前帧图像中机器人所在的区域，并且将该机器人所在的区域的质心作为机器人在当前帧图像中的质心位置。其中，分割处理可以是提取当前帧图像中一个区域的子图像，该子图像包括机器人所在的区域，该子图像可以是以用户输入的位置为中心选取的区域，也可以是利用图像识别算法识别出机器人所在区域后选取的区域。该机器人所在的区域的质心可以是该区域的中心位置，也可以是利用预先获取的机器人参数计算的质心位置，此处不做具体限定。该机器人在当前帧图像中的质心位置可以采用在图像中的相对坐标或者在图像中的位置标识等方式进行描述。

s12：获取机器人的移动方向，其中该移动方向是机器人在前一帧图像和当前帧图像中的质心的移动方向；

具体地，在一个应用例中，结合图2所示，当前帧图像中机器人20的质心位置为^kc，前一帧图像中机器人20的质心位置为^k-1c，则机器人20在相邻两帧图像中质心的移动方向为即机器人20的移动方向为

s13：根据机器人的移动方向和预定移动方向的偏差，获取机器人的校正方向；

其中，预定移动方向是预先设定的机器人的移动方向，可以通过用户预先输入的预定路径获得，该预定路径包括预定移动方向和位置信息。

用户预先输入移动路径后，三维线圈产生磁场控制机器人沿该移动路径运动，但由于外部干扰或控制误差等因素，机器人的实际移动路径容易偏离预先输入的移动路径，移动方向也容易偏离预定移动方向。本实施例中，通过计算机器人的移动方向与预定移动方向之间的偏差，获取校正方向，以利用该校正方向可以校正该机器人的移动方向和移动路径，使得机器人的运动路径趋近预定路径。

具体地，在上述应用例中，结合图2和图3所示，采用双闭环结构对机器人的移动方向进行校正，其中，外环控制器101从摄像机50获取机器人20当前帧的图像后，获取机器人20的移动方向为然后在用户预先输入的预定路径中，获取与机器人20当前位置最接近的一段预定路径为而该预定移动方向为预设的机器人的最佳移动方向，则该预定移动方向可以利用当前帧图像中该机器人的质心位置和最接近该质心位置的该段预设路径的方向计算得到，例如考虑机器人的质心^kc和该段预设路径之间的距离偏差，计算该预定移动方向具体计算方法可以视实际需求而定，此处不做具体限定。之后外环控制器101将该预定移动方向发送给内环控制器102，由内环控制器102计算机器人20的移动方向和预定移动方向之间的偏差，该偏差包括是方向向量和之间的角度偏差，利用该偏差，可以计算获得该机器人20的校正方向。例如，可以利用比例积分控制器计算两个方向向量和之间的角度偏差，以获得该校正方向。当然，在其他应用例中，该校正方向也可以是从向偏移的方向向量，偏移的角度可以利用该角度偏差获得，如该角度偏差的一半或三分之二等，此处不做具体限定。

在其他实施例中，该预定路径也可以采用曲线方程离散化取点或者从用户输入的点列文件等方式获得，则该预定移动方向可以是机器人当前位置与曲线方程中最接近的点的切线方向；该预定方向也可以直接采用与当前帧图像中该机器人的质心位置最接近的部分预设路径的方向。校正方向也可以直接采用该预定移动方向的垂直方向，或者该机器人移动方向的垂直方向等，该预定方向也可以综合考虑机器人的质心位置与预定路径之间的距离以及机器人移动方向和该预定路径之间的角度偏差，此处不做具体限定。

s14：利用该校正方向，控制驱动装置驱动机器人运动，以使得机器人的运动路径趋近预定路径。

其中，该驱动装置可以包括磁场驱动装置和三维线圈，也可以根据机器人的类型选择其他类型的驱动装置，此处不做具体限定。

具体地，在一个应用例中，结合图3所示，内环控制器102将该校正方向输出给磁场驱动装置30，磁场驱动装置30获取该校正方向后，根据该校正方向向量，产生相应的模拟电压信号，再将该模拟电压信号输入电流控制器进行线性变换，输出一电流控制信号至3d亥姆霍兹线圈40产生相应的磁场，控制机器人20运动，使得机器人20的运动路径趋近预设路径。其中，利用上述步骤循环校正机器人的运动路径，可以使得机器人的运动路径趋近预定路径，降低机器人的运动偏差。

本实施例中，机器人的校正方向和反馈的移动方向均使用图像特征，从而可以直接在图像空间中完成闭环控制，无需三维重建，减少由于存在闭环外的误差导致控制精度下降的可能性，提高机器人运动控制的精度；而且使用时间上前后两帧机器人的质心的移动方向作为机器人的移动方向，该特征正是机器人移动方向的定义，因此不存在模型失配的问题，有效提高机器人运动控制的稳定性。

如图4所示，本发明机器人运动控制方法第二实施例是在本发明机器人运动控制方法第一实施例的基础上，步骤s11之前，进一步包括：

s10：获取用户输入的预定路径，以根据该预定路径控制机器人运动。

其中，该预设路径采用点列方式存储。

具体地，在一个应用例中，摄像机拍摄机器人图像前，用户可以利用鼠标预先输入机器人的预定路径，例如利用鼠标在显示屏或监视器上先后点击多个位置，该多个位置按照点击顺序连成的线段即为该预定路径，其中该显示屏或监视器用于显示摄像机拍摄的机器人的图像。

结合图2所示，位置¹p、²p……^n-1p、ⁿp、ⁿ⁺¹p、ⁿ⁺²p……^n+mp之间的连线即为用户输入的预定路径，该预定路径以点列方式存储，例如l＝(¹p，²p，……，^n+mp)，其中l表示预定路径，¹p，²p，……，^n+mp表示用户顺序输入的多个位置点。

当然，在其他实施例中，用户也可以输入一个连续的曲线后，再利用采样的方式获取该多个位置点，还可以通过插值的方式向该预定路径中增加部分位置点，从而改变该预定路径，此处不做具体限定。

上述应用例中，驱动装置获取该预定路径后，可以根据该预定路径，产生控制该机器人运动的磁场，使得该机器人沿该预设路径运动。其中，该驱动装置可以是磁场驱动装置，也可以是其他类型的驱动装置，例如电场驱动装置等，此处不做具体限定。

如图5所示，本发明机器人运动控制方法第三实施例是在本发明机器人运动控制方法第一实施例的基础上，步骤s11进一步包括：

s111：对当前帧图像进行处理，获取当前帧图像的连通区域；

具体地，在一个应用例中，获取当前帧图像后，对当前帧图像进行边界识别，例如对当前帧图像的像素点中，当相邻两个像素点的像素值之间的差值大于预定值(如80)时，则在该相邻两个像素点之间划分边界，从而将由边界分割出的封闭区域作为连通区域。若当前帧图像中无法获得连通区域，则重新获取一帧图像后，再对重新获取的该帧图像进行处理，获取该帧图像的连通区域。

在其他实施例中，也可以只对当前帧图像的部分区域进行边界识别，以减少计算量。

s112：判断该连通区域是否与机器人相匹配；

具体地，获取该连通区域后，例如多个连通区域时，则分别将每个连通区域与该机器人的参数进行比较，以判断连通区域是否与该机器人的参数相匹配。其中，该机器人的参数包括机器人的面积、形状、轮廓等。若连通区域的面积与该机器人的面积相同或差别小于阈值(如1个像素面积)，则判定该连通区域与机器人相匹配，否则判定为不匹配。

该机器人的参数可以是预先保存的数据，也可以是用户输入的数据，还可以是通过摄像机初始拍摄的图像处理后获得的数据，此处不做具体限定。

在其他实施例中，当存在多个连通区域时，也可以选择其中与机器人的参数最接近的连通区域作为匹配的连通区域。

s113：若判断结果为匹配，则选取该连通区域的质心作为机器人在当前帧图像中的质心位置。

具体地，在一个应用例中，当一个连通区域与机器人相匹配时，计算该连通区域的中心位置，将该中心位置作为机器人在当前帧图像中的质心位置。在其他应用例中，也可以结合该机器人的参数，例如密度分布等数据，计算该连通区域的质心，作为机器人在当前帧图像中的质心位置，其中该质心可以偏离该连通区域的面积中心位置。

本实施例还可以与本发明机器人运动控制方法第二实施例相结合。

如图6所示，本发明机器人运动控制方法第四实施例是在本发明机器人运动控制方法第三实施例的基础上，步骤s111进一步包括：

s1111：以机器人的初始位置/机器人在前一帧图像中的质心位置为中心，获取当前帧图像中预定区域的子图像；

其中，机器人的初始位置可以是用户根据显示屏或监视器中显示的机器人图像，手动输入的机器人的初始位置，也可以直接选择用户预先输入的预设路径中的起始点作为该初始位置，此处不做具体限定。该预定区域是预先设置的从当前帧图像中获取的子图像的范围，其具体数据可以视实际情况设置，此处不做具体限定。

具体地，在一个应用例中，获取的当前帧图像是摄像机拍摄的第一帧图像，或者没有检测到存在前一帧图像，又或者没有检测到机器人在前一帧图像中的质心位置时，获取用户输入的初始位置，以该初始位置为中心，获取一个600*600像素点的区域，作为该子图像。当存在前一帧图像时，则以前一帧图像中机器人的质心位置为中心，获取一个600*600像素点的区域，作为该子图像。

s1112：对该子图像进行二值化操作，以获取该子图像的二值图；

其中，二值图是每个像素只有两个可能取值(如0和1)的数字图像。

具体地，在一个应用例中，将获取的子图像中的每个像素点的灰度值与预设灰度值(如90)进行比较，当像素点的灰度值大于预设灰度值时，则将该像素点的像素值设置为0，否则将该像素点的像素值设置为1，从而获得该子图像的二值图。

s1113：对该二值图进行开操作，并获取处理后的该二值图中的连续为预定值的区域，作为当前帧图像的连通区域。

其中，先腐蚀后膨胀的操作称为开操作，可以使操作对象的轮廓变得光滑，断开狭窄的间断和消除细的突出物。

具体地，在一个应用例中，对该二值图进行开操作后，可以消除该二值图中的噪声，然后选择该二值图中像素值成块为1的区域作为连通区域，获取该二值图中的所有连通区域，以便进行后续与机器人进行匹配的步骤。

如图7所示，本发明机器人运动控制方法第五实施例是在本发明机器人运动控制方法第三实施例的基础上，步骤s112进一步包括：

s1121：判断连通区域的轮廓和/或面积是否与机器人的轮廓和/或面积相匹配；

s1122：若判断结果为匹配，则判定该连通区域与机器人相匹配。

具体地，在一个应用例中，获取当前帧图像中的所有连通区域后，逐一将连通区域与机器人进行匹配，比较该连通区域的轮廓和/或面积是否与该机器人的轮廓和/或面积一致，或者二者之差是否小于误差容忍值，如轮廓线的平均偏差距离小于1个像素距离，和/或面积之差小于1个像素面积，则判定该连通区域与机器人相匹配，否则判定该连通区域与机器人不匹配。

在其他实施例中，若存在多个机器人，需要逐一匹配每个机器人的连通区域，匹配方法可以参考上述方法，此处不再重复。

本实施例也可以与本发明运动控制方法第四实施例相结合。

如图8所示，本发明机器人运动控制方法第六实施例是在本发明机器人运动控制方法第一实施例的基础上，步骤s13进一步包括：

s131：选取与机器人在当前帧图像中的质心位置距离最近的部分预设路径；

s132：利用该质心位置和部分预设路径，计算预定移动方向；

具体地，结合图2所示，在一个应用例中，机器人20在当前帧图像中的质心位置为^kc，预设路径l中与该质心位置直线距离最近的部分预设路径为

可选地，如图9所示，步骤s132进一步包括：

s1321：利用公式(1)，计算该预定移动方向：

其中，表示该预定移动方向，表示与部分预设路径平行的方向，表示与部分预设路径垂直、且满足的方向，利用下述公式(2)计算：

其中，d表示该质心位置到部分预设路径之间的最短距离，m表示比例参数。

该比例参数m是预先设定的参数值，其取值范围为50～200个像素距离。其中，当m取值越小，机器人的质心越接近预设路径，而当m取值越大，机器人的移动方向越平行于预设路径。

在其他实施例中，该预定移动方向也可以根据实际需求采用其他方式计算，例如采用除d之外的其他图像特征计算权重等，此处不做具体限定。

s133：将机器人的移动方向和预定移动方向的偏差进行比例积分调节，获取机器人的校正方向。

具体地，在上述应用例中，结合图2所示，利用前后相邻两帧图像中机器人的质心位置^k-1c和^kc，做差并归一化，可以得到机器人的移动方向再计算与预定移动方向之间的偏差，例如计算两个向量和的外积，然后进行比例积分调节，即将该偏差输入一个比例-积分调节器，该比例-积分调节器则可输出得到机器人的校正方向。

在其他实施例中，也可以之间将该偏差作为机器人的校正方向，或者将该偏差进行比例-积分-微分调节，再输出该机器人的校正方向。

本实施例也可以与本发明运动控制方法第二至第五中任一个实施例或其不冲突的组合相结合。

如图10所示，本发明机器人运动控制方法第七实施例是在本发明机器人运动控制方法第一实施例的基础上，进一步包括：

s15：计算在当前帧图像中机器人的质心位置与该部分预设路径终点之间的距离；

其中，预设路径采用分段点列方式存储，该部分所述预设路径是整个预设路径中与机器人在当前帧图像中的质心位置距离最近的相邻两个路径点组成的线段，例如图11所示的相邻点^n+m-1p和点^n+mp组成的线段

s16：判断该距离是否小于预设阈值；

s17：若判断结果为小于，则判断该部分预设路径是否为该预设路径的最后两个相邻路径点组成的线段；

s18：若判断结构为否，则对该机器人的下一帧图像进行处理，继续执行获取机器人的移动方向的步骤。

其中，该预设阈值是预先设定的结束采用当前帧图像数据校正该机器人的运动的触发条件，当机器人的质心位置与当前帧图像中的与机器人在当前帧图像中的质心位置距离最近的部分预设路径终点之间的距离小于该预设阈值时，则表示该机器人足够接近该部分预设路径终点，此时，则可以不用再利用该部分预设路径继续校正该机器人的运动，此时若该部分预设路径是整个预设路径的最后两个相邻路径点组成的线段，即该部分预设路径包括整个预设路径的终点时，则表示机器人已经足够接近整个预设路径的终点，可以结束整个机器人控制流程，否则载入下一段预设路径进行后续方向校正。其中，该预设阈值的具体取值可以根据实际需求设置，例如根据机器人的体宽设置(如20～40个像素距离范围内)，此处不做具体限定。

具体地，结合图11所示，在一个应用例中，在当前帧图像中，获取与机器人20的质心位置ⁿc最近的该部分预设路径后，计算该质心位置ⁿc和该部分预设路径的终点^n+mp之间的距离d^*，然后判断该距离d^*是否小于预设阈值(如20个像素距离)，若该距离d^*小于该预设阈值，则表示该机器人20足够接近该部分预设路径终点^n+mp，此时，继续判断该部分预设路径是否为整个预设路径l的最后两个相邻路径点组成的线段，也就是说，该段预设路径的其中一个端点是否为整个预设路径l的终点，若判断结果为是，即点^n+mp是整个预设路径l的终点，则表示该机器人20已经足够接近整个预设路径l的终点，已经完成针对整个预设路径的方向校正，可以结束整个运动控制流程。若判断结果为否，则表示还有部分预设路径没有进行方向校正，则获取机器人的下一帧图像，进行处理，并继续执行后续获取机器人的移动方向的步骤，即载入下一段预设路径，继续校正该机器人的运动方向。

本实施例步骤的执行可以在步骤s11之前，也可以在步骤s14之后，本实施例还可以与本发明运动控制方法第二至第六中任一个实施例或其不冲突的组合相结合。

如图12所示，本发明机器人运动控制设备第一实施例包括：

相互连接的处理器801和通信电路802。

通信电路802用于与外部设备进行通信，该通信电路802包括但不限于天线、射频电路、蓝牙等元件。

处理器801控制通信设备的操作，处理器801还可以称为cpu(centralprocessingunit，中央处理单元)。处理器801可能是一种集成电路芯片，具有信号的处理能力。处理器801还可以是通用处理器、数字信号处理器(dsp)、专用集成电路(asic)、现成可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

处理器801用于执行指令以实现如本发明机器人运动控制方法第一至第七任一个实施例或其不冲突的组合所提供的方法。

当然，在其他实施例中，机器人运动控制设备还可以包括存储器、显示器(图未示)等其他部件，此处不做具体限定。

本实施例中的机器人运动控制设备可以是手机、电脑、平板、服务器等，也可以是可集成于上述设备中的独立部件，例如芯片。

如图13所示，本发明机器人运动控制设备第二实施例是在本发明机器人运动控制设备第一实施例的基础上，进一步包括：

摄像机803、机器人804、磁场驱动装置805和三维线圈806。

其中，该机器人804是磁控微型机器人。

该摄像机803连接该处理器801，用于拍摄机器人804的图像。

磁场驱动装置805连接该三维线圈806和该通信电路802，用于获取该校正方向后，驱动该三维线圈806产生磁场，以控制机器人804的运动。

该三维线圈806可以是3d亥姆霍兹线圈，用于产生磁场，以控制该机器人804的运动。

处理器801和通信电路802的工作过程具体可以参考本发明机器人运动控制设备第一实施例，此处不再重复。

如图14所示，本发明具有存储功能的装置一实施例中，具有存储功能的设备90内部存储有程序901，该程序901被执行时实现如本发明机器人运动控制方法第一至第七实施例中任一个以及任意不冲突的组合所提供的方法。

其中，具有存储功能的设备90可以是便携式存储介质如u盘、光盘，也可以是手机、服务器或可集成于上述设备中的独立部件，例如芯片等。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。