一种上肢康复机器人的虚拟避障方法及系统与流程

1.本发明属于康复机器人领域，具体涉及一种上肢康复机器人的虚拟避障方法及系统。


背景技术：

2.在上肢康复过程中，用户借助康复机器人与虚拟训练环境进行交互，以达到训练上肢的目的。在虚拟训练环境中，包含了一系列不可接近或者穿越的障碍物(比如石头、围栏等)。当用户操作康复机器人移动虚拟物体接近虚拟障碍物时，康复机器人需要能够帮助用户避开这些障碍物。
3.例如，所述上肢康复机器人(后面简称机器人)的整体硬件结构可参考专利cn113244578a。
4.上肢康复训练是指，在现实环境中，用户握住机器人末端手柄，主动发力或者在机器人的带动下完成康复训练动作。在虚拟环境中，被操控对象根据机器人的轨迹做相应的运动。其中，虚拟环境中包含一系列的障碍物，比如石头，围墙，河流等，这些障碍物在虚拟环境被设定为不可穿越或者不可接近的。由于用户的肌肉力量较弱，当虚拟环境中的被操控对象接近障碍物时难以避开这些虚拟障碍物，因此需要机器人主动帮助用户远离这些障碍物。
5.而cn113081666a专利中只说明了如何防止机器人达到限位位置，并未涉及虚拟环境中其他障碍物的避障方法。限位位置都是预先设定好的，无法根据虚拟环境实时进行修改。


技术实现要素：

6.为了克服上述缺陷，本发明提出了一种上肢康复机器人的虚拟避障方法及系统。可以根据虚拟环境的中的障碍物变化动态地调整机器人的运动轨迹，以达到虚拟避障的效果。
7.为了实现上述目标，本发明的第一方面采用如下的技术方案：一种上肢康复机器人的虚拟避障方法，其特征在于，包括如下步骤：
8.障碍物坐标被发送至机器人控制器；
9.机器人控制器检测当前机器人末端的速度，确定避障半径r；
10.根据避障半径确定触点坐标，并计算触点是否接触到障碍物；
11.根据触点接触情况计算避障所需的反向力；
12.机器人控制器发送反向力信号给电机驱动器，驱动电机实现避障。
13.用户手持机器人末端，运动模式为2种：1、用户主动发力(用户上肢主动运动带动机器人末端移动)；2、在用户主动运动过程中，机器人提供一定的助力。
14.避障半径r与机器人末端的运行速度呈正相关关系，
15.r＝0.1v
16.其中，r的单位为mm，v的单位为mm/s，v为机器人末端的运行速度。这里的0.1指0.1s；例如，当v＝50mm/s，r＝5mm。
17.如果检测到仅有一个触点与障碍物边界接触，则从圆心o点到该触点ci的向量与障碍物边界相交于点pi；计算向量p
ici
在障碍物边界的法向量vi上的投影的向量的模长di；根据模长di计算避障反向力；
[0018][0019]
其中，i＝1、2、3
…
n，n指一共有n个触点，fi指第i个触点与障碍物接触时避障所需的反向力，指第i个触点的向量p
ici
在障碍物边界的法向量vi上的投影的向量的模长；r为避障半径；本文中，i即i；r即r(避障半径)。
[0020]
将反向力信号发送给电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动；
[0021]
如果有多个触点同时与障碍物边界接触，则计算所有触点的避障所需的反向力的合力f
total
；
[0022][0023]
其中，αi表示第i个触点是否与障碍物边界接触；当第i个触点与障碍物接触时，αi＝1；当第i个触点没有与障碍物接触时，αi＝0；将计算得到的合力信号发送给电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动。
[0024]
本发明的第二个方面是提供一种上肢康复机器人的虚拟避障系统，包括机器人控制器，其特征在于，所述的机器人控制器包括：
[0025]
障碍物坐标信息接收模块，用于接收上位机(例如，运行游戏的电脑)发送的障碍物坐标信息；
[0026]
避障半径模块，用于实时接收机器人末端的运行速度信息，并根据机器人末端的实时运行速度计算避障半径r；
[0027]
障碍物接触判定模块，根据避障半径模块计算得到的避障半径以及确定所有触点的坐标，并计算触点是否接触到障碍物；
[0028]
避障反向力计算模块，根据触点接触情况计算避障所需的反向力；
[0029]
反向力信号模块，使反向力信号被输出至电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动。
[0030]
虚拟环境可以是游戏；游戏中的场景会随着游戏过程的进行而实时变化，这里的实时指的是随时都可能变化。障碍物坐标是事先设计好的，由上位机发送给下位机(下位机即机器人控制器)。
[0031]
每次刷新坐标的时候会把当前地图上所有的障碍物坐标都发送给下位机。刷新坐标是指：障碍物信息发生变化；比如石头之类的障碍物是可以被敲碎的，然后这个障碍物就不存在了，这个时候上位机就会给下位机发送障碍物更新信息。当前地图是整个游戏地图中的一部分(可以认为是一帧)。
[0032]
如果检测到仅有一个触点与障碍物边界接触，则从圆心o点到该触点ci的向量与障碍物边界相交于点pi；计算向量p
ici
在障碍物边界的法向量vi上的投影的向量的模长di；根据模长di计算避障反向力；
[0033][0034]
其中，i＝1、2、3
…
n，n指一共有n个触点，fi指第i个触点与障碍物接触时所需的反向力，指第i个触点的向量p
ici
在障碍物边界的法向量vi上的投影的向量的模长；
[0035]
将反向力信号发送给电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动；
[0036]
如果有多个触点同时与障碍物边界接触，则计算所有与障碍物边界接触的触点的反向力的合力f
total
；
[0037][0038]
其中，αi表示第i个触点是否与障碍物边界接触；当第i个触点与障碍物接触时，αi＝1；当第i个触点没有与障碍物接触时，αi＝0；将计算得到的合力信号发送给电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动。
[0039]
针对多个触点，障碍物可以为一个或不止一个。针对特定的触点，其对应的障碍物边界为其对应的障碍物的与就该触点接触的那部分边界。
[0040]
避障半径r与机器人末端的运行速度呈正相关关系，
[0041]
r＝0.1v
[0042]
其中，r的单位为mm，v的单位为mm/s，v为机器人末端的运行速度。
[0043]
例如，如果n个触点中有若干触点与不止一个障碍物边界接触，例如，有部分触点与一个内部障碍物边界接触，同时有另外的部分触点与一个外部障碍物边界接触，则计算所有与外部障碍物接触的触点的反向力的合力f
total外
和所有与内部障碍物接触的触点的反向力的合力f
total内
，f
total外
加上f
total内
得到最终的合力信号，将最终的合力信号发送给电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动。
[0044]
同理，如果n个触点中有若干触点与不止一个内部障碍物边界接触，也计算所有接触障碍物边界的触点的反向力的合力信号；如果n个触点中有若干触点与不止一个外部障碍物边界接触，也计算所有接触障碍物边界的触点的反向力的合力信号。
[0045]
本发明的第三个方面是提供一种处理器，处理器用于运行计算机程序，计算机程序运行时执行如前所述的上肢康复机器人的虚拟避障方法。
[0046]
本发明的第四个方面是提供一种终端，包含存储器、处理器及存储在存储器并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如前所述的上肢康复机器人的虚拟避障方法。
[0047]
本发明的第五个方面是提供一种计算机可读介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行可实现如前所述的上肢康复机器人的虚拟避障方法。
[0048]
相对于现有技术，本发明的有益效果为：本发明可针对任意多边形表示的障碍物
进行主动避障，可以实时的修改障碍物的坐标。
附图说明
[0049]
图1为以设备当前的位置为圆心，以参数r(数值可调)为半径画圆并取点的示意图；
[0050]
图2为触点与内、外障碍物示意图；
[0051]
图3和图4为避障反向力计算示意图；
[0052]
图5为控制方案示意图；
[0053]
图6为本发明的上肢康复机器人虚拟避障流程图。
具体实施方式
[0054]
以下结合附图和具体实施例对本发明作具体的介绍。本实施例中，未作详细描述的技术内容均为现有技术。
[0055]
在用户训练过程中，机器人控制器(后面简称控制器)实时(与控制器的控制周期一致)检测设备(指机器人末端)当前的位置(通过电机的编码器或者其他传感器检测到，其他传感器包括但不限于霍尔传感器，电位计以及光栅尺等)。以机器人末端当前的位置为圆心o，以参数r(数值可调，r为避障半径)为半径画一个圆。在圆上均匀的取n个触点c
1-cn(n越大检测越精确，但是会增加计算量，一般取8或者16)，如图1所示，以n＝8为例。分别判断这n个点是否已经接触到了障碍物。对于不可从外往里穿越的障碍物(比如石头)，需要判断n个点是否在障碍物里面。对于不可从里往外穿越的障碍物(比如围墙)，需要判断n个点是否在障碍物的外面。例如，对于不可从里往外穿越的障碍物(比如围墙)，当n个点都不在障碍物的外面时，则无需提供反向力；只要有1个点在障碍物的外面，即需计算反向力。判断点是否在障碍物内外均为现有技术。例如判断点是否在多边形障碍物内有多种方法，本方案采用的判断方法为winding number算法，是公开方法，不作详细赘述。
[0056]
所述障碍物由任意多边形构成。
[0057]
设备的坐标(机器人末端的坐标)通过机器人控制器发送到游戏界面中，控制游戏中的元素进行运动。针对上肢康复机器人，采用游戏进行康复训练，为现有技术。
[0058]
所述训练过程是指，用户主动发力带动机器人运动或者由机器人帮助用户运动，相应的机器人会分别给用户提供阻力和助力。机器人将自身的坐标信息实时传送至虚拟训练环境中，虚拟环境中的被控制对象按照机器人的坐标进行移动以完成某些训练动作(比如画一个直线或者曲线)。其中用户的发力能被机器人上的力传感器(不仅限于此检测方式)检测到，随后该力信息被用于控制电机的输出力。本实施例中，虚拟环境是一个游戏，游戏中用户控制一个车辆在屏幕上进行移动，因此这里的被控制对象就是屏幕上的车辆。在其他实施例中，被控制对象也可以是别的东西，比如一个虚拟的手掌，一支虚拟的笔等等。
[0059]
一种可行的控制方案如图5所示。其中，fd表示期望力，ff表示前馈补偿力，x表示机器人的位置，表示机器人的速度，表示机器人的加速度，fe表示外部力(即用户对机器人末端施加的力)。力控制器可以是pi控制器，pd控制器以及阻抗控制器等。力控制器保证用户与机器人末端(例如手柄)之间的接触力稳定在期望力，前馈控制器用于计算机器人的逆动力学模型得到前馈补偿力。增加前馈补偿力能够提高系统的响应能力，保证接触力的精
度。力控制器、前馈控制器均为前述的机器人控制器的一部分。
[0060]
参数r与机器人末端的运行速度呈正相关关系，即速度越大，r越大。一种可行的计算公式如下：
[0061]
r＝0.1v
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0062]
其中，r的单位为mm，v的单位为mm/s，r为避障半径。
[0063]
只考虑已经接触到障碍物的点，然后根据下面的方法计算反向力/反向合力。未接触到的点不做任何处理(即相应反向力为0)。
[0064]
假如某触点与障碍物边界接触(指触点与圆心o的连线与障碍物边界有交叉，该触点为ci，i＝1、2、3
……
n，)，则从该圆心o点到触点ci的向量与障碍物边界相交于点pi。根据下列公式计算向量p
ici
在边界的法向量vi上的投影的向量p
ic′i的模长di。以机器人末端当前的位置为圆心o。
[0065][0066]
根据模长di计算避障反向力fi(单位为n)，如下述公式(3)，避障反向力又称为反向力。
[0067]
其中反向力的方向沿vi方向，与速度方向相反。
[0068][0069]
其中，i＝1、2、3
…
n，n指n个触点，fi指第i个触点与障碍物接触时所需的反向力；指第i个触点的向量p
ici
在障碍物边界的法向量vi上的投影的向量p
ic′i的模长，单位为mm；100的单位是n*mm；
[0070]
例如检测到有一个触点与障碍物边界接触(指触点与圆心o的连线与障碍物边界有交叉，这里以c5点为例)，则从该圆心o点到触点c5的向量与障碍物边界相交于点p5。根据下列公式计算向量p5c5在边界的法向量v5上的投影的向量p5c5′
的模长d5(如图3和4所示)。根据模长d5计算避障反向力f5(单位为n)，避障反向力又称为反向力。
[0071][0072]
其中反向力的方向沿v5方向，与速度方向相反。反向力fi的大小与的长度呈正相关关系，即的数值越大，反向力越大。根据公式(3)，触点c5的反向力计算公式为：
[0073][0074]
以上为公式(5)，其中公式(5)中，f5指第5个触点与障碍物接触时所需的反向力，指第5个触点的向量p5c5在边界的法向量v5上的投影的向量的模长，向量p5c5中p5指：从圆心o点到端点c5的向量与障碍物边界相交于点p5。
[0075]
将反向力发送给电机驱动器，驱动电机向远离障碍物的方向移动，达到避障的效
果。
[0076]
在主动训练模式下(用户主动发力带动机器人运动，机器人不给用户提供任何助力)，控制器计算出来的反向力作为力控制器的控制目标(即fd)。在不需要避障的情况下，力控制器的控制目标是0，即机器人末端跟随用户上肢进行运动。当需要进行避障的时候，力控制器的控制目标fd被修改为反向力值，即需要沿用户运动方向的反向对用户施加一个阻力，阻止用户继续接近障碍物。
[0077]
在助力模式下(用户主动发力，且机器人给用户提供一定的助力)，控制器计算出来的反向力作为力控制器的控制目标(即fd)。在不需要避障的情况下，力控制器的控制目标是助力值(即机器人给用户提供的助力的值)，即机器人末端提供一定的助力。当需要进行避障的时候，力控制器的控制目标fd被修改为反向力值，即需要沿用户运动方向的反向对用户施加一个阻力，阻止用户继续接近障碍物。
[0078]
如果有多个触点同时与障碍物边界接触，则计算所有与障碍物边界接触的触点的反向力的合力f
total
。即
[0079][0080]
以上为公式(6)；
[0081]
其中，αi表示第i个触点是否与障碍物边界接触；当第i个触点与障碍物接触时，αi＝1；当第i个触点没有与障碍物接触时，αi＝0，当αi＝0时，其相应的α
ifi
＝0，且无需用公式(3)计算fi，相应的fi直接设为0(即该触点没有反向力)；
[0082]
将计算得到的合力信号发送给电机驱动器，由电机驱动器驱动电机向远离障碍物的方向移动，达到避障的效果。
[0083]
在主动训练模式下(用户主动发力带动机器人运动，机器人不给用户提供任何助力)，控制器计算出来的合力信号作为力控制器(例如pi控制器)的控制目标fd。在不需要避障的情况下，力控制器的控制目标是0，即机器人末端跟随用户上肢进行运动。当需要进行避障的时候，力控制器的控制目标被修改为反向力的合力值，即需要沿用户运动方向的反向对用户施加一个阻力，阻止用户继续接近障碍物。
[0084]
在助力模式下(用户主动发力，且机器人给用户提供一定的助力)，控制器计算出来的合力信号作为力控制器的控制目标(即fd)。在不需要避障的情况下，力控制器的控制目标是助力值(即机器人给用户提供的助力的值)，即机器人末端提供一定的助力。当需要进行避障的时候，力控制器的控制目标被修改为反向力的合力值，即需要沿用户运动方向的反向对用户施加一个阻力，阻止用户继续接近障碍物。
[0085]
本发明的关键点包括通过电机提供反向驱动力的方法达到虚拟避障的方法；计算反向力的方法。
[0086]
本发明可针对任意多边形表示的障碍物进行主动避障，可以实时的修改障碍物的坐标。
[0087]
本行业的技术人员应该了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。