CN105150210A - 一种用于遥操作人机交互的虚拟管道动态避障控制方法 - Google Patents

Abstract

本发明公开了一种用于遥操作人机交互的虚拟管道动态避障控制方法，利用视觉对环境中的障碍物进行识别，通过计算与障碍物间的实时距离来设计控制力，以保证遥操作的安全性和操作性能。本发明应用场景是机械臂末端在空间遥操作中动态躲避障碍物并到达指定位置。由于空间环境不确定或是一些非结构化环境因素导致操作不稳定，操作者难以按照期望时间控制操作末端到达指定位置，故要求操作过程中，末端点动态避开障碍物的同时实时更新最优路径，在提高操作效率的同时确保操作稳定性。

Description

一种用于遥操作人机交互的虚拟管道动态避障控制方法

技术领域

本发明属于空间机器人遥操作人机交互领域，涉及一种用于遥操作人机交互的虚拟管道动态避障控制方法。

背景技术

从20世纪60年代空间机器人开始应用到今天，空间机器人的应用领域在不断地扩大，从最初仅仅需要完成单纯的星球表面探测开始，到现在需要完成卫星的回收、释放、舱内的科学实验以及空间站的在轨装配与维修等任务。同时空间机器人所承担任务的复杂性也在不断增加:从结构化已知环境中的重复性操作到非结构化未知环境中的非重复性操作，而且其精度要求也越来越高，需要完成一些诸如运动目标捕获、卫星装配以及卫星维修之类的精密任务。为了适应操作环境的未知性、操作任务的复杂性，空间机器人需要具有较强的环境适应能力，能在操作过程中动态的躲避障碍物。针对未知环境中的机器人，充分利用操作者的智能，采用遥操作方式控制机器人完成操作任务是一种有效的手段。遥操作的基本要求是在提高机器人操作性能的前提下，保证操作的安全性。然而，由于遥操作的安全性与操作性能之间相互矛盾，尤其针对未知环境中的操作安全性难以保证，因此离线任务规划和手动控制难以同时使用。

发明内容

本发明的目的在于解决上述问题，提供一种用于遥操作人机交互的虚拟管道动态避障控制方法，其中融合了视觉和人工势场法理论，使机械臂在空间操作中达到自动防撞的目的。

为了实现上述目的，本发明采用的技术方案包括以下步骤：

步骤一：建立机械臂末端势场函数

根据FIRAS函数建立人工斥力场，势场函数为：

$U_0\left(x\right)=\left\{\begin{array}{cc}\frac{1}{2}\mathrm{\η}{(\frac{1}{\mathrm{\ρ}}-\frac{1}{{\mathrm{\ρ}}_0})}^2,& \mathrm{\ρ}\≤{\mathrm{\ρ}}_0\\ 0,& \mathrm{\ρ}>{\mathrm{\ρ}}_0\end{array}\right.---\left(1\right)$

其中，η是一个限制参数，ρ₀表示机械臂末端的势场作用距离，ρ表示与障碍物间的最短距离；ρ₀的大小取决于机械臂末端移动的最大速度V_max以及加速度的大小；

步骤二：在机械臂末端安装视觉传感器，实时计算机械臂末端与障碍物的距离；

步骤三：计算机械臂末端势场对障碍物的虚拟力。

进一步的，步骤一中，对(1)式关于ρ求梯度得到势场对障碍物的虚拟斥力为：

$F_{(O,psp)}=-{\mathrm{gradU}}_0\left(x\right)=\left\{\begin{array}{cc}\mathrm{\η}(\frac{1}{\mathrm{\ρ}}-\frac{1}{{\mathrm{\ρ}}_0})\frac{1}{{\mathrm{\ρ}}^2}\frac{\∂\mathrm{\ρ}}{\∂x},& \mathrm{\ρ}\≤{\mathrm{\ρ}}_0\\ 0,& \mathrm{\ρ}>{\mathrm{\ρ}}_0\end{array}\right.---\left(2\right)$

其中，表示势场中点与障碍物间距离的偏导数向量，定义如下：

$\frac{\∂\mathrm{\ρ}}{\∂x}={\[\frac{\∂\mathrm{\ρ}}{\∂x}\frac{\∂\mathrm{\ρ}}{\∂y}\frac{\∂\mathrm{\ρ}}{\∂z}\]}^T$

(2)式中，F_(O,psp)的方向是U₀(x)的负梯度方向，当ρ＞ρ₀时，F_(O,psp)＝0，表示障碍物未进入机械臂末端所形成势场；当ρ→0时，F_(O,psp)→∞，表示机械臂末端与障碍物充分接近，产生较大的斥力。

进一步的，步骤二中，传感器视场角为全向，视场半径为R_s，当动态障碍物与操作对象的距离小于R_s时，传感器对障碍物位置进行采样，实时计算机械臂末端与障碍物的距离。

进一步的，步骤三中，计算机械臂末端势场对障碍物虚拟力的具体方法是：

(1)当障碍物较小时，近似看作一个质点；

当障碍物进入视场范围后，视觉传感器对其位置进行采样，目标位置设为时间函数f(t)，采用平方逼近的方法来近似预测；

平方逼近表达式为：其N点的逼近均方差为：

$E\left({\mathrm{\Δe}}_i^2\right)={\underset{i=1}{\overset{N}{\Σ}}\[f\left(t_i\right)-b_0-b_1{t}_i-b_2{t}_i^2\]}^2$

最佳逼近通解为：

$\left[\begin{array}{c}{b}_0\\ b_1\\ b_2\end{array}\right]=\frac{1}{\left|A\right|}\left[\begin{array}{c}B\\ C\\ D\end{array}\right]---\left(4\right)$

式中：

$B=c_{11}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{21}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{31}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(5\right)$

$C=c_{12}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{22}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{32}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(6\right)$

$D=c_{13}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{23}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{33}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(7\right)$

$A=\left[\begin{array}{ccc}N& \underset{i=1}{\overset{N}{\Σ}}{t}_i& \underset{i=1}{\overset{N}{\Σ}}{t}_i^2\\ \underset{i=1}{\overset{N}{\Σ}}{t}_i& \underset{i=1}{\overset{N}{\Σ}}{t}_i^2& \underset{i=1}{\overset{N}{\Σ}}{t}_i^3\\ \underset{i=1}{\overset{N}{\Σ}}{t}_i^2& \underset{i=1}{\overset{N}{\Σ}}{t}_i^3& \underset{i=1}{\overset{N}{\Σ}}{t}_i^4\end{array}\right]\left(\right|A|\≠0)---\left(8\right)$

根据(4)式得出f(k+1)的三点平方预测为：

f(k+1)＝3f(k)-3f(k-1)+f(k-2)(9)

即根据视觉传感器采样得到前三个时刻的位置信息，便可通过平方预测方法来预测下一时刻的位置，从而能够实时计算障碍物距离机械臂末端的最短距离ρ_min；

(2)障碍物体积较大时，通过视觉传感器对其局部信息进行采样，先假设障碍物为平行六面体形状，下边来求解机械臂末端到障碍物的最短距离；

对三种距离进行比较判断得到最短距离ρ，即机械臂末端点到六面体顶点、棱和面的距离：

a)计算到顶点的距离是将该平行六面体投影到二维平面，提取六面体的顶点，再对上述顶点进行三维重建，得到相机视场中顶点到机械臂末端的三维距离；

b)计算末端点到六面体棱的距离：在对顶点三维重建后得到各顶点所在的棱，由末端点向每条棱所在的直线作垂线；

需要判断垂足O是否在六面体棱上，方法是根据末端点和六面体顶点坐标以及垂直关系求出垂足O的坐标，判断其三轴坐标值是否界于六面体顶点的对应坐标，如果判断O点不在六面体棱上，则将对应垂直距离舍去，属无效距离；

c)计算末端点到六面体面的距离：同b)中情况，由末端点向视场范围内的平面作垂线；

通过几何关系求解出垂足O的坐标O(x₀,y₀,z₀)，判断O点是否在六面体面上；所求平面法向量经α,β旋转至与z轴平行，再经投影变换矩阵向xoy平面作正投影；变换矩阵为：

$T=T_{Ry}\·T_{Rx}\·T=\left[\begin{array}{cccc}cos\mathrm{\α}& -sin\mathrm{\α}& 0& 0\\ 0& \cos \mathrm{\β}& 0& 0\\ -sin\mathrm{\α}& -\cos \mathrm{\α}\·sin\mathrm{\β}& 0& 0\\ 0& 0& 0& 1\end{array}\right]---\left(10\right)$

经投影之后问题转化为在二维平面内判断一个点是否落在所求区域内，通过曲线规划判断该点的坐标是否满足描述该区域的不等式组；如果判断O点不在六面体面上，则将对应垂直距离舍去，属无效距离；

将a)b)c)三种情况下得到的最短距离ρ_v、ρ_e、ρ_f进行比较得出机械臂末端点与六面体的最短距离ρ_min。

与现有技术相比，本发明具有以下有益效果：

本发明针对未知环境，提出一种融合视觉的虚拟夹具动态避障方法，利用视觉对环境中的障碍物进行识别，通过计算与障碍物间的实时距离来设计控制力，以保证遥操作的安全性和操作性能。本发明应用场景是机械臂末端在空间遥操作中动态躲避障碍物并到达指定位置。由于空间环境不确定或是一些非结构化环境因素导致操作不稳定，操作者难以按照期望时间控制操作末端到达指定位置，故要求操作过程中，末端点动态避开障碍物的同时实时更新最优路径，在提高操作效率的同时确保操作稳定性。

本发明在机械臂末端添加视觉传感器，通过计算与动态障碍物间的距离来实现避障。本发明与现有技术相比在提高了操作精度的同时确保了操作的安全性，能在空间实际遥操作时动态躲避障碍物，从而实现了对遥操作机器人在未知环境中的高效操作。

本发明考虑添加视觉辅助，即在机械臂末端安装视觉传感器。在操作过程中，传感器对周围障碍物的信息进行采样，通过计算距离来进行避障。其中用人工势场函数来计算对应的虚拟力。

附图说明

图1为本发明三点平方逼近法求解距离的示意图；

图2为本发明平行六面体与机械臂末端距离的示意图；

图3为本发明机械臂末端到障碍物边界(棱)距离的示意图；

图4为本发明机械臂末端到障碍物表面距离的示意图。

具体实施方法

下面结合附图对本发明做进一步详细的说明。

参见图1-图4，本发明包括以下步骤：

步骤一：机械臂末端势场函数的建立。

Khatib提出的FIRAS(ForceInducinganArtificialRepulsionfromtheSurface)函数建立人工斥力场，势场函数为：

$U_0\left(x\right)=\left\{\begin{array}{cc}\frac{1}{2}\mathrm{\η}{(\frac{1}{\mathrm{\ρ}}-\frac{1}{{\mathrm{\ρ}}_0})}^2,& \mathrm{\ρ}\≤{\mathrm{\ρ}}_0\\ 0,& \mathrm{\ρ}>{\mathrm{\ρ}}_0\end{array}\right.---\left(1\right)$

其中，η是一个限制参数，ρ₀表示机械臂末端的势场作用距离，ρ表示与障碍物间的最短距离。ρ₀的大小取决于机械臂末端移动的最大速度V_max以及加速度的大小。机械臂末端点在人工势场中，考虑势场中点与障碍物间的作用，对(1)式关于ρ求梯度得到势场对障碍物的虚拟斥力为：

$F_{(O,psp)}=-{\mathrm{gradU}}_0\left(x\right)=\left\{\begin{array}{cc}\mathrm{\η}(\frac{1}{\mathrm{\ρ}}-\frac{1}{{\mathrm{\ρ}}_0})\frac{1}{{\mathrm{\ρ}}^2}\frac{\∂\mathrm{\ρ}}{\∂x},& \mathrm{\ρ}\≤{\mathrm{\ρ}}_0\\ 0,& \mathrm{\ρ}>{\mathrm{\ρ}}_0\end{array}\right.---\left(2\right)$

其中，表示势场中点与障碍物间距离的偏导数向量，定义如下：

$\frac{\∂\mathrm{\ρ}}{\∂x}={\[\frac{\∂\mathrm{\ρ}}{\∂x}\frac{\∂\mathrm{\ρ}}{\∂y}\frac{\∂\mathrm{\ρ}}{\∂z}\]}^T$

(2)式中，F_(O,psp)的方向是U₀(x)的负梯度方向，当ρ＞ρ₀时，F_(O,psp)＝0，表示障碍物未进入机械臂末端所形成势场；当ρ→0时，F_(O,psp)→∞，表示机械臂末端与障碍物充分接近，产生较大的斥力，为防止碰撞，这种情况在实际场景中是不允许发生的，故要求(2)式中的限制参数η取适当的定值，以防止碰撞发生。

步骤二：机械臂末端添加视觉传感器。

如何实时确定障碍物与机械臂末端的最短距离，本发明在机械臂末端安装视觉传感器，其视场角为全向，视场半径为R_s，当动态障碍物与操作对象的距离小于R_s时，传感器就能对障碍物位置进行采样。实时计算机械臂末端与障碍物的距离。

步骤三：计算机械臂末端势场对障碍物的虚拟力。

针对障碍物大小不同进行分类讨论：

(1)当障碍物较小可近似看作一个质点。

当障碍物进入视场范围后，视觉传感器对其位置进行采样，目标位置设为时间函数f(t)，这里采用平方逼近的方法来近似预测。平方逼近表达式为：其N点的逼近均方差

$E\left({\mathrm{\Δe}}_i^2\right)={\underset{i=1}{\overset{N}{\Σ}}\[f\left(t_i\right)-b_0-b_1{t}_i-b_2{t}_i^2\]}^2,$ 最佳逼近通解为：

$\left[\begin{array}{c}{b}_0\\ b_1\\ b_2\end{array}\right]=\frac{1}{\left|A\right|}\left[\begin{array}{c}B\\ C\\ D\end{array}\right]---\left(4\right)$

式中：

$B=c_{11}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{21}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{31}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(5\right)$

$C=c_{12}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{22}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{32}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(6\right)$

$D=c_{13}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{23}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{33}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(7\right)$

$A=\left[\begin{array}{ccc}N& \underset{i=1}{\overset{N}{\Σ}}{t}_i& \underset{i=1}{\overset{N}{\Σ}}{t}_i^2\\ \underset{i=1}{\overset{N}{\Σ}}{t}_i& \underset{i=1}{\overset{N}{\Σ}}{t}_i^2& \underset{i=1}{\overset{N}{\Σ}}{t}_i^3\\ \underset{i=1}{\overset{N}{\Σ}}{t}_i^2& \underset{i=1}{\overset{N}{\Σ}}{t}_i^3& \underset{i=1}{\overset{N}{\Σ}}{t}_i^4\end{array}\right]\left(\right|A|\≠0)---\left(8\right)$

根据(4)式得出f(k+1)的三点平方预测为：

f(k+1)＝3f(k)-3f(k-1)+f(k-2)(9)

即根据视觉传感器采样得到前三个时刻的位置信息，便可通过平方预测方法来预测下一时刻的位置，从而可以实时计算障碍物距离机械臂末端的最短距离ρ_min，如图1所示。

(2)障碍物体积较大时，我们通过视觉传感器对其局部信息进行采样，这里先假设障碍物为简单的平行六面体形状，下边来求解机械臂末端到障碍物的最短距离。

这里需要对三种距离进行比较判断得到最短距离ρ，即机械臂末端点到六面体顶点、棱和面的距离，如图2所示：

a)计算到顶点的距离是将该平行六面体投影到二维平面，提取六面体的顶点，再对上述顶点进行三维重建，得到相机视场中顶点到机械臂末端的三维距离；

b)计算末端点到六面体棱的距离：在对顶点三维重建后得到各顶点所在的棱，由末端点向每条棱所在的直线作垂线，如图3所示；

需要判断垂足O是否在六面体棱上，方法是根据末端点和六面体顶点坐标以及垂直关系求出垂足O的坐标，判断其三轴坐标值是否界于六面体顶点的对应坐标，如果判断O点不在六面体棱上，则将对应垂直距离舍去，属无效距离。

c)计算末端点到六面体面的距离：同b)中情况，由末端点向视场范围内的平面作垂线，如图4所示；

通过几何关系求解出垂足O的坐标O(x₀,y₀,z₀)，下边判断O点是否在六面体面上。所求平面法向量经α,β旋转至与z轴平行，再经投影变换矩阵

$T=T_{Ry}\·T_{Rx}\·T=\left[\begin{array}{cccc}cos\mathrm{\α}& -\sin \mathrm{\α}& 0& 0\\ 0& \cos \mathrm{\β}& 0& 0\\ -sin\mathrm{\α}& -\cos \mathrm{\α}\·sin\mathrm{\β}& 0& 0\\ 0& 0& 0& 1\end{array}\right]---\left(10\right)$

向xoy平面作正投影。经投影之后问题转化为在二维平面内判断一个点是否落在所求区域内，可以通过曲线规划判断该点的坐标是否满足描述该区域的不等式组。如果判断O点不在六面体面上，则将对应垂直距离舍去，属无效距离。

综上，将a)b)c)三种情况下得到的最短距离ρ_v、ρ_e、ρ_f进行比较得出机械臂末端点与六面体的最短距离ρ_min。

实施例：

本发明的一个具体实施例子是在虚拟场景中设计动态障碍物，通过视觉传感器与人工势场来实现机械臂末端的动态避障。具体过程如下：

1.构造实验场景：

实验中主端是一个6自由度的PHANTOM手控器，从端是在CHAI3D虚拟三维空间中规划的虚拟管道，以代理点来代替机械臂末端点，设计动态障碍物O，O的位置变化用函数f(t)表示。

2.代理点处势场的建立：

由势场函数的定义，设η是一个限制参数，实验中取η＝0.5；ρ₀表示机械臂末端的势场作用距离，ρ₀的大小取决于机械臂末端移动的最大速度V_max以及加速度的大小，实验中取ρ₀＝30mm；ρ表示代理点与障碍物间的最短距离。

3.虚拟管道代理点位置添加视觉传感器计算与障碍物间的距离：

在代理点处安装视觉传感器，其视场角为全向，视场半径为R_s，当动态障碍物与操作对象的距离小于R_s时，传感器就能对障碍物位置进行采样。

4.通过距离实时计算代理点处势场对障碍物的虚拟排斥力：

因障碍物的大小不同而考虑两组实验：

(1)障碍物较小可近似看作一个质点，此时根据视觉传感器采样得到前三个时刻的位置信息，通过平方预测方法来预测下一时刻的位置，从而可以实时计算障碍物距离机械臂末端的最短距离ρ。

当ρ＞30mm时，表示障碍物未进入势场范围，虚拟排斥力为0；当0＜ρ＜30mm时，将ρ值代入虚拟排斥力的表达式(2)进行求解，得出所需的排斥力的大小。

(2)障碍物体积较大时，通过视觉传感器对其局部信息进行采样。三种情况下分别进行实验：

a)计算到末端点到顶点的距离：将平行六面体投影到二维平面，提取六面体的顶点，再对上述顶点进行三维重建，得到相机视场中顶点到机械臂末端的三维距离ρ_v。

b)计算末端点到六面体棱的距离：在对顶点三维重建后得到各顶点所在的棱，由末端点向每条棱所在的直线作垂线，若判断垂足在六面棱上，则保留对应的距离ρ_e。

c)计算末端点到六面体面的距离：同b)中情况，由末端点向视场范围内的平面作垂线，若判断垂足在六面体面上，则保留对应的距离ρ_f。

比较三种情况下得到的最短距离ρ_v、ρ_e、ρ_f，得出机械臂末端点与六面体的最短距离ρ_min，将ρ值代入虚拟排斥力的表达式(2)进行求解得出所需的排斥力的大小。

实验结果表明了动态避障的可行性和有效性。根据以上发明内容和具体实施例，一种视觉辅助和添加人工势场的虚拟夹具动态避障方法在空间遥操作中将有重要应用。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (4)

1.一种用于遥操作人机交互的虚拟管道动态避障控制方法，其特征在于，包括以下步骤：

步骤一：建立机械臂末端势场函数

根据FIRAS函数建立人工斥力场，势场函数为：

$U_0\left(x\right)=\left\{\begin{array}{cc}\frac{1}{2}\mathrm{\η}{(\frac{1}{\mathrm{\ρ}}-\frac{1}{{\mathrm{\ρ}}_0})}^2,& \mathrm{\ρ}\≤{\mathrm{\ρ}}_0\\ 0,& \mathrm{\ρ}>{\mathrm{\ρ}}_0\end{array}\right.---\left(1\right)$

其中，η是一个限制参数，ρ₀表示机械臂末端的势场作用距离，ρ表示与障碍物间的最短距离；ρ₀的大小取决于机械臂末端移动的最大速度V_max以及加速度的大小；

步骤二：在机械臂末端安装视觉传感器，实时计算机械臂末端与障碍物的距离；

步骤三：计算机械臂末端势场对障碍物的虚拟力。

2.根据权利要求1所述的用于遥操作人机交互的虚拟管道动态避障控制方法，其特征在于：所述步骤一中，对(1)式关于ρ求梯度得到势场对障碍物的虚拟斥力为：

$F_{(O,psp)}=-{\mathrm{gradU}}_0\left(x\right)=\left\{\begin{array}{cc}\mathrm{\η}(\frac{1}{\mathrm{\ρ}}-\frac{1}{{\mathrm{\ρ}}_0})\frac{1}{{\mathrm{\ρ}}^2}\frac{\∂\mathrm{\ρ}}{\∂x},& \mathrm{\ρ}\≤{\mathrm{\ρ}}_0\\ 0,& \mathrm{\ρ}>{\mathrm{\ρ}}_0\end{array}\right.---\left(2\right)$

其中，表示势场中点与障碍物间距离的偏导数向量，定义如下：

$\frac{\∂\mathrm{\ρ}}{\∂x}={\[\frac{\∂\mathrm{\ρ}}{\∂x}\frac{\∂\mathrm{\ρ}}{\∂y}\frac{\∂\mathrm{\ρ}}{\∂z}\]}^T$

(2)式中，F_(O,psp)的方向是U₀(x)的负梯度方向，当ρ＞ρ₀时，F_(O,psp)＝0，表示障碍物未进入机械臂末端所形成势场；当ρ→0时，F_(O,psp)→∞，表示机械臂末端与障碍物充分接近，产生较大的斥力。

3.根据权利要求1所述的用于遥操作人机交互的虚拟管道动态避障控制方法，其特征在于：所述步骤二中，传感器视场角为全向，视场半径为R_s，当动态障碍物与操作对象的距离小于R_s时，传感器对障碍物位置进行采样，实时计算机械臂末端与障碍物的距离。

4.根据权利要求1所述的用于遥操作人机交互的虚拟管道动态避障控制方法，其特征在于：所述步骤三中，计算机械臂末端势场对障碍物虚拟力的具体方法是：

(1)当障碍物较小时，近似看作一个质点；

当障碍物进入视场范围后，视觉传感器对其位置进行采样，目标位置设为时间函数f(t)，采用平方逼近的方法来近似预测；

平方逼近表达式为：其N点的逼近均方差为：

$E\left({\mathrm{\Δe}}_i^2\right)={\underset{i=1}{\overset{N}{\Σ}}\[f\left(t_i\right)-b_0-b_1{t}_i-b_2{t}_i^2\]}^2$

最佳逼近通解为：

$\left[\begin{array}{c}{b}_0\\ b_1\\ b_2\end{array}\right]=\frac{1}{\left|A\right|}\left[\begin{array}{c}B\\ C\\ D\end{array}\right]---\left(4\right)$

式中：

$B=c_{11}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{21}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{31}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(5\right)$

$C=c_{12}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{22}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{32}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(6\right)$

$D=c_{13}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)+c_{23}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i+c_{33}\underset{i=1}{\overset{N}{\Σ}}f\left(t_i\right)t_i^2---\left(7\right)$

$A=\left[\begin{array}{ccc}N& \underset{i=1}{\overset{N}{\Σ}}{t}_i& \underset{i=1}{\overset{N}{\Σ}}{t}_i^2\\ \underset{i=1}{\overset{N}{\Σ}}{t}_i& \underset{i=1}{\overset{N}{\Σ}}{t}_i^2& \underset{i=1}{\overset{N}{\Σ}}{t}_i^3\\ \underset{i=1}{\overset{N}{\Σ}}{t}_i^2& \underset{i=1}{\overset{N}{\Σ}}{t}_i^3& \underset{i=1}{\overset{N}{\Σ}}{t}_i^4\end{array}\right]\left(\right|A|\≠0)---\left(8\right)$

根据(4)式得出f(k+1)的三点平方预测为：

f(k+1)＝3f(k)-3f(k-1)+f(k-2)(9)

即根据视觉传感器采样得到前三个时刻的位置信息，便可通过平方预测方法来预测下一时刻的位置，从而能够实时计算障碍物距离机械臂末端的最短距离ρ_min；

(2)障碍物体积较大时，通过视觉传感器对其局部信息进行采样，先假设障碍物为平行六面体形状，下边来求解机械臂末端到障碍物的最短距离；

对三种距离进行比较判断得到最短距离ρ，即机械臂末端点到六面体顶点、棱和面的距离：

a)计算到顶点的距离是将该平行六面体投影到二维平面，提取六面体的顶点，再对上述顶点进行三维重建，得到相机视场中顶点到机械臂末端的三维距离；

b)计算末端点到六面体棱的距离：在对顶点三维重建后得到各顶点所在的棱，由末端点向每条棱所在的直线作垂线；

需要判断垂足O是否在六面体棱上，方法是根据末端点和六面体顶点坐标以及垂直关系求出垂足O的坐标，判断其三轴坐标值是否界于六面体顶点的对应坐标，如果判断O点不在六面体棱上，则将对应垂直距离舍去，属无效距离；

c)计算末端点到六面体面的距离：同b)中情况，由末端点向视场范围内的平面作垂线；

通过几何关系求解出垂足O的坐标O(x₀,y₀,z₀)，判断O点是否在六面体面上；所求平面法向量经α,β旋转至与z轴平行，再经投影变换矩阵向xoy平面作正投影；变换矩阵为：

$T=T_{Ry}\·T_{Rx}\·T=\left[\begin{array}{cccc}cos\mathrm{\α}& -\sin \mathrm{\α}& 0& 0\\ 0& \cos \mathrm{\β}& 0& 0\\ -sin\mathrm{\α}& -\cos \mathrm{\α}\·sin\mathrm{\β}& 0& 0\\ 0& 0& 0& 1\end{array}\right]---\left(10\right)$

经投影之后问题转化为在二维平面内判断一个点是否落在所求区域内，通过曲线规划判断该点的坐标是否满足描述该区域的不等式组；如果判断O点不在六面体面上，则将对应垂直距离舍去，属无效距离；

将a)b)c)三种情况下得到的最短距离ρ_v、ρ_e、ρ_f进行比较得出机械臂末端点与六面体的最短距离ρ_min。