具体实施方式

现结合实施例、附图对本发明作进一步描述：

如图1所示。本发明所采用的技术方案是，首先，分别建立视觉控制与人机交互控制的模型，计算出二者的控制输出，再由模糊控制器计算权重分配，将融合后的控制量输入到机器人关节伺服控制系统中，控制机器人运动。

步骤1:对视觉控制模型建模，计算视觉控制器输出的控制量；

步骤2：对人机交互控制模型建模，计算人机交互控制器输出的控制量；

步骤3：根据视觉控制器和人机交互控制器的参数，计算模糊控制器的输入参数，通过模糊控制方法分配控制权重，合成加权控制量；

步骤4：将步骤3中合成的加权的机器人控制量，作为机器人输入的期望末端笛卡尔空间位姿变化量，进行关节伺服跟踪，控制机器人末端位置和姿态产生变化，使得图像伺服系统能够辅助人的控制操作和对目标物体观察。

本发明具体实施步骤如下：

步骤1主要是完成视觉控制器的设计，获得对机器人末端位姿的控制量。

定义末端固定相机的六自由度机器人，其末端坐标系和相机坐标系重合，末端相对于基座坐标系的初始位置和姿态为ξ＝[x y z θ_x θ_y θ_z]^T，本发明中都使用2-3-1旋转顺序，设相机模型的参数为[f ρ_u ρ_v u₀ v₀]。

本发明适用于具有三个及以上特征点目标的伺服跟踪，本发明中讨论的问题限定下面以目标为矩形为例，选取矩形的四个顶点为特征点进行说明。对于每个特征点，都可测得其初始的像素坐标p＝[u v]^T，单个图像特征变化率和末端位姿变化率具有线性关系

即

其中υ_p＝[v_x v_y v_z ω_x ω_y ω_z]^T＝[v_p ω_p]^T为末端相对于基座坐标系的位移和旋转速度，J_p即为图像雅可比矩阵，根据透视投影方程和相机模型参数，假设该点在真实空间相对于相机的坐标为[X Y Z]^T,则可以得到图像雅可比矩阵的计算表达式：

对于任意点，假设其期望的像素坐标位置p_T＝[u_T v_T]^T,设计特征点像素的跟踪误差e_p

e_p＝p_T-p (3)

设计比例控制器，比例系数为K_pp

u_p＝K_ppe_p (4)

选取四个特征点中的三组点特征，堆叠其雅可比矩阵，求解机器人基座坐标系下，为了使得图像特征在像素平面内到达期望位置，末端位姿所需运动的速度和角速度量

其中J_Pi和u_pi表示为第i个特诊点的图像雅可比矩阵和控制输入。

在本发明中，使用反光流法恢复特征点相对于相机坐标系的深度信息，即求解坐标中的Z值。此处的深度估计器设计如下：使用υ_r＝[v_r ω_r]^T表示机器人末端相对于基座坐标系的真实位移和旋转速度，代入(2)中，整理可得到

使用最小二乘法进行在线辨识参数估计，在任意设定初值的情况下，该深度估计值都可以在系统初始阶段较快地跟踪上真值，且该方法在实际应用上对Z值的误差不敏感，初始偏差的存在对系统的性能影响不大。

步骤2主要是完成人机交互控制器的设计，计算人机交互控制对机器人末端位姿的控制量。人机互控制器采用三自由度或者六自由度手控器等具有相应位置速度传感器的人机交互设备采集控制输入。以六自由度并联手控器为例，假设采集的输入为手控器末端点，在手控器基座坐标系下的三维空间位置和变化速度：

e＝[v_hx v_hz v_hz ω_hx ω_hy ω_hz]^T，

设计比例控制律

给定速度和角速度从手控器基座坐标系转换到机器人基座坐标系的变换矩阵T_v和T_ω，通过计算

则可以将控制量从手控器基座坐标空间转换到机器人基座坐标空间。

同时利用视觉伺服控制量的三个速度分量作为反馈，在手控器内设计力反馈进行交互，使得操作者能够感受到视觉伺服控制的方向。交互力的设计为

F＝K_τT_v ^-1v_p (9)

其中权重K_τ由根据下一步骤进行计算。

步骤3利用模糊控制方法，得到控制输入的权重分配。本发明采用线性加权和将两个控制量进行合成，设视觉控制的权重设置为K_α，合成的控制量为

υ＝K_αυ_p+(1-K_α)υ_h (10)

其中K_α和上一步骤中的K_τ使用模糊控制器进行求解。

设计模糊控制器为双输入控制器，两个模糊控制输入分别定义为：n个特征点的像素特征偏量的归一化参数Δp和控制量速度矢量夹角的归一化参数Δθ，按照如下公式进行计算

根据先验知识，将两个归一化参数输入和两个输出权重进行模糊化，分别设计模糊集为很小(VS)、小(S)、偏小(PS)、偏大(PB)、大(B)、很大(VB)，并确定相应的隶属度函数。根据“像素偏差大的时候，视觉控制权值应当小，力反馈参数应当大”；和“速度夹角偏差大的时候，视觉控制权值应当小，力反馈参数应当大”两个先验模糊准则，建立模糊关系S₁和S₂。其中

S₁＝(Δp×Δθ)→K_α (13)

S₁具体如下表所示

S₂＝(Δp×Δθ)→K_τ (14)

S₂具体如下表所示

则根据模糊规则，由一次输入计算量的模糊化Δp^*和Δθ^*，可以分别计算出

K_α ^*＝(Δp^*×Δθ^*)οS₁ (15)

K_τ ^*＝(Δp^*×Δθ^*)οS₂ (16)

将输出进行反模糊，得到满足需求的控制权重中K_α和K_τ。

步骤4.将步骤3中合成的加权的机器人控制量，作为机器人输入的期望末端笛卡尔空间位姿变化量，进行关节伺服跟踪，控制机器人末端位置和姿态产生变化，使得图像伺服系统能够辅助人的控制操作和对目标物体观察。

选用六自由度旋转机器人模型，DH参数已知，设其当前关节角q＝[q₁ q₂ q₃ q₄ q₅q₆]，根据其参数和旋转角，可以获得任意两相邻关节坐标系的齐次变换矩阵A_i，和机器人总变换矩阵

A＝A₁A₂A₃A₄A₅A₆ (17)

根据机器人笛卡尔空间位置速度变化和关节空间角度变化间的映射关系满足

J_r为机器人笛卡尔空间位置速度变化和关节空间角度变化间的雅各比矩阵，可以求解出关节角控制量

对各个关节角进行伺服控制，并能根据变化后的关节角值计算出被控后的末端位置旋转矩阵

T＝A(q) (20)

从而获得被控后的末端位置和姿态坐标。

本发明的关键在于，基于单目视觉利用反光流深度恢复构建了机器人的视觉伺服框架，利用力反馈引导机制构建了人机交互控制框架，设立了两个评估参数，通过模糊控制方法，利用先验经验建立规则，使用评估参数进行决策，获得控制权重分配系数和力反馈引导系数，最终能够实现两种控制模式的融合。