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

权利要求书

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以及加速度的大小；

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

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

(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^2\\ \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)

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

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

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

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

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

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

c)计算末端点到平行六面体面的距离ρ_f：同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进行比较得出机械臂末端点与平行六面体的最短距离ρ。