一种用于遥操作人机交互的虚拟管道动态避障控制方法
【技术领域】
[0001] 本发明属于空间机器人遥操作人机交互领域，涉及一种用于遥操作人机交互的虚 拟管道动态避障控制方法。
【背景技术】
[0002] 从20世纪60年代空间机器人开始应用到今天，空间机器人的应用领域在不断地 扩大，从最初仅仅需要完成单纯的星球表面探测开始，到现在需要完成卫星的回收、释放、 舱内的科学实验以及空间站的在轨装配与维修等任务。同时空间机器人所承担任务的复杂 性也在不断增加：从结构化已知环境中的重复性操作到非结构化未知环境中的非重复性 操作，而且其精度要求也越来越高，需要完成一些诸如运动目标捕获、卫星装配以及卫星维 修之类的精密任务。为了适应操作环境的未知性、操作任务的复杂性，空间机器人需要具有 较强的环境适应能力，能在操作过程中动态的躲避障碍物。针对未知环境中的机器人，充分 利用操作者的智能，采用遥操作方式控制机器人完成操作任务是一种有效的手段。遥操作 的基本要求是在提高机器人操作性能的前提下，保证操作的安全性。然而，由于遥操作的安 全性与操作性能之间相互矛盾，尤其针对未知环境中的操作安全性难以保证，因此离线任 务规划和手动控制难以同时使用。
【发明内容】
[0003] 本发明的目的在于解决上述问题，提供一种用于遥操作人机交互的虚拟管道动态 避障控制方法，其中融合了视觉和人工势场法理论，使机械臂在空间操作中达到自动防撞 的目的。
[0004] 为了实现上述目的，本发明采用的技术方案包括以下步骤：
[0005] 步骤一：建立机械臂末端势场函数
[0006] 根据FIRAS函数建立人工斥力场，势场函数为：
[0008] 其中，η是一个限制参数，Pc表示机械臂末端的势场作用距离，P表示与障碍物 间的最短距离；P c的大小取决于机械臂末端移动的最大速度ν_以及加速度的大小；
[0009] 步骤二：在机械臂末端安装视觉传感器，实时计算机械臂末端与障碍物的距离；
[0010] 步骤三：计算机械臂末端势场对障碍物的虚拟力。
[0011] 进一步的，步骤一中，对（1)式关于P求梯度得到势场对障碍物的虚拟斥力为：
[0013] 其中，
表示势场中点与障碍物间距离的偏导数向量，定义如下：
[0015] (2)式中，F0^psrt的方向是UQ(x)的负梯度方向，当P > P。时，F ^psrt= 0,表示 障碍物未进入机械臂末端所形成势场；当P -〇时，FdPsp) - m，表示机械臂末端与障碍物 充分接近，产生较大的斥力。
[0016] 进一步的，步骤二中，传感器视场角为全向，视场半径为Rs，当动态障碍物与操作 对象的距离小于&时，传感器对障碍物位置进行采样，实时计算机械臂末端与障碍物的距 离。
[0017] 进一步的，步骤三中，计算机械臂末端势场对障碍物虚拟力的具体方法是：
[0018] (1)当障碍物较小时，近似看作一个质点；
[0019] 当障碍物进入视场范围后，视觉传感器对其位置进行采样，目标位置设为时间函 数f(t)，采用平方逼近的方法来近似预测；
[0020] 平方逼近表达式为：/(i) = ? +却+财2 .，其N点的逼近均方差为：
[0022] 最佳逼近通解为：
[0029] 根据⑷式得出f(k+l)的三点平方预测为：
[0030] f(k+l) = 3f (k)-3f (k-l)+f (k-2) (9)
[0031] 即根据视觉传感器采样得到前三个时刻的位置信息，便可通过平方预测方法来预 测下一时刻的位置，从而能够实时计算障碍物距离机械臂末端的最短距离P
[0032] (2)障碍物体积较大时，通过视觉传感器对其局部信息进行采样，先假设障碍物为 平行六面体形状，下边来求解机械臂末端到障碍物的最短距离；
[0033] 对三种距离进行比较判断得到最短距离P，即机械臂末端点到六面体顶点、棱和 面的距离：
[0034] a)计算到顶点的距离是将该平行六面体投影到二维平面，提取六面体的顶点，再 对上述顶点进行三维重建，得到相机视场中顶点到机械臂末端的三维距离；
[0035] b)计算末端点到六面体棱的距离：在对顶点三维重建后得到各顶点所在的棱，由 末端点向每条棱所在的直线作垂线；
[0036] 需要判断垂足0是否在六面体棱上，方法是根据末端点和六面体顶点坐标以及垂 直关系求出垂足〇的坐标，判断其三轴坐标值是否界于六面体顶点的对应坐标，如果判断〇 点不在六面体棱上，则将对应垂直距离舍去，属无效距离；
[0037] c)计算末端点到六面体面的距离：同b)中情况，由末端点向视场范围内的平面作 垂线；
[0038] 通过几何关系求解出垂足0的坐标0(x。，y。，z。)，判断0点是否在六面体面上；所求 平面法向量i经α，β旋转至与z轴平行，再经投影变换矩阵向xoy平面作正投影；变换矩 阵为：
[0040] 经投影之后问题转化为在二维平面内判断一个点是否落在所求区域内，通过曲线 规划判断该点的坐标是否满足描述该区域的不等式组；如果判断〇点不在六面体面上，则 将对应垂直距离舍去，属无效距离；
[0041] 将a)b)c)三种情况下得到的最短距离Pv、Pe、P f进行比较得出机械臂末端点与 六面体的最短距离P _。
[0042] 与现有技术相比，本发明具有以下有益效果：
[0043] 本发明针对未知环境，提出一种融合视觉的虚拟夹具动态避障方法，利用视觉对 环境中的障碍物进行识别，通过计算与障碍物间的实时距离来设计控制力，以保证遥操作 的安全性和操作性能。本发明应用场景是机械臂末端在空间遥操作中动态躲避障碍物并到 达指定位置。由于空间环境不确定或是一些非结构化环境因素导致操作不稳定，操作者难 以按照期望时间控制操作末端到达指定位置，故要求操作过程中，末端点动态避开障碍物 的同时实时更新最优路径，在提高操作效率的同时确保操作稳定性。
[0044] 本发明在机械臂末端添加视觉传感器，通过计算与动态障碍物间的距离来实现避 障。本发明与现有技术相比在提高了操作精度的同时确保了操作的安全性，能在空间实际 遥操作时动态躲避障碍物，从而实现了对遥操作机器人在未知环境中的高效操作。
[0045] 本发明考虑添加视觉辅助，即在机械臂末端安装视觉传感器。在操作过程中，传感 器对周围障碍物的信息进行采样，通过计算距离来进行避障。其中用人工势场函数来计算 对应的虚拟力。
【附图说明】
[0046] 图1为本发明三点平方逼近法求解距离的示意图；
[0047] 图2为本发明平行六面体与机械臂末端距离的示意图；
[0048] 图3为本发明机械臂末端到障碍物边界（棱）距离的示意图；
[0049] 图4为本发明机械臂末端到障碍物表面距离的示意图。
[0050] 具体实施方法
[0051] 下面结合附图对本发明做进一步详细的说明。
[0052] 参见图1-图4,本发明包括以下步骤：
[0053] 步骤一：机械臂末端势场函数的建立。
[0054] Khatib 提出的 FIRAS(Force Inducing an Artificial Repulsion from the Surface)函数建立人工斥力场，势场函数为：
[0056] 其中，η是一个限制参数，P c表示机械臂末端的势场作用距离，P表示与障碍物 间的最短距离。P c的大小取决于机械臂末端移动的最大速度Vniax以及加速度的大小。机 械臂末端点在人工势场中，考虑势场中点与障碍物间的作用，对（1)式关于P求梯度得到 势场对障碍物的虚拟斥力为：
[0058] 其中，
表示势场中点与障碍物间距离的偏导数向量，定义如下：
[0060] ⑵式中，F0^psri的方向是UQ(x)的负梯度方向，当P > Ρ。时，F0xpsri=O,表示障 碍物未进入机械臂末端所形成势场；当P -〇时，FdPsp) - m，表示机械臂末端与障碍物充 分接近，产生较大的斥力，为防止碰撞，这种情况在实际场景中是不允许发生的，故要求（2) 式中的限制参数η取适当的定值，以防止碰撞发生。
[0061] 步骤二：机械臂末端添加视觉传感器。
[0062] 如何实时确定障碍物与机械臂末端的最短距离，本发明在机械臂末端安装视觉传 感器，其视场角为全向，视场半径为Rs，当动态障碍物与操作对象的距离小于&时，传感器 就能对障碍物位置进行采样。实时计算机械臂末端与障碍物的距离。
[0063] 步骤三：计算机械臂末端势场对障碍物的虚拟力。
[0064] 针对障碍物大小不同进行分类讨论：
[0065] (1)当障碍物较小可近似看作一个质点。
[0066] 当障碍物进入视场范围后，视觉传感器对其位置进行采样，目标位置设为时间函 数f (t)，这里采用平方逼近的方法来近似预测。平方逼近表达式为：/(〇 = %+￥ + /)/，其 N点的逼近均方差
最佳逼近通解为：
[0069]式中：
[0074] 根据⑷式得出f(k+l)的三点平方预测为：
[0075] f(k+l) = 3f (k)-3f (k-l)+f (k-2) (9)
[0076] 即根据视觉传感器采样得到前三个时刻的位置信息，便可通过平方预测方法来预 测下一时刻的位置，从而可以实时计算障碍物距离机械臂末端的最短