CN102871784B - 定位控制装置和方法 - Google Patents

Abstract

一种定位控制装置包括：激光发射器，用于向外发射光路相同的可见激光和红外激光；第一摄像头和第二摄像头，用于拍摄红外图像；第三摄像头，用于拍摄彩色图像；图像识别模块，用于识别出红外图像中满足预设条件的红外激光光斑；平面定位模块，用于以红外图像中心为原点，得到红外激光光斑的坐标；空间定位模块，用于根据红外激光光斑的坐标，以及第一摄像头和第二摄像头的间距和焦距，计算得到红外激光光斑的空间坐标。本发明还提供一种采用的方法。上述装置和方法通过可见激光和不可见的红外激光配合运用，用户只需简单的利用头部晃动，将可见激光照射到需要操作的物体上，便实现了定位功能，对四肢不便的用户来说，机械假肢的控制变得更加简单。

Description

定位控制装置和方法

技术领域

本发明涉及一种人机交互与控制技术，特别是涉及一种用于控制机械臂的定位控制装置和方法。

背景技术

根据世界卫生组织的统计数据，运动功能障碍群体已成为世界上最大的少数群体，大约有10％的世界人口即6.5亿生活在各类身体残疾中。“帮助残疾人康复”已成为我国政府要解决的重大民生问题之一。假肢的研究与开发，对帮助运动功能缺失的残疾人康复或进行正常生活的具有重要意义。电动机械假肢可以看成是一种与人体紧密接触的机器人。与传统假肢不同，电动机械假肢具备自行运动的能力，更有助于帮助患者进行正常的生活。

如今，电动机械假肢(机械臂)机构制作技术较为成熟。但是，这些电动机械假肢机构都依赖于人对假肢的操作控制，而对于有些迫切需要这类机械假肢的残疾人，他们双臂都失去了行动能力，无法完成基于操纵杆的复杂控制操作。

有一种较为热门的技术是：目标肌肉神经分布重建(Targeted MuscleRe-innervations,TMR)技术，即采用接驳神经信号的方法，对残疾人剩余肢体的肌电信号进行提取和识别，恢复肢体控制信号，进行假肢控制。

但是，由于人体神经信号差异较大而且不稳定，难以实现精确的控制动作，且使用前需要长期的适应性训练。而其他大部分的电动机械假肢的控制方法依赖于残疾人仍然能运动的肢体，对于行动能力缺失较为严重的残疾人不太适用，而这些人恰恰是对电动机械假肢需求最大的人群。

发明内容

基于此，有必要提供一种操作更方便的用于控制机械臂的定位控制装置和方法。

一种定位控制装置，包括激光发射器、第一摄像头、第二摄像头、第三摄像头、图像获取模块、图像识别模块、平面定位模块、空间定位模块，

所述激光发射器用于向外发射光路相同的可见激光和红外激光；

所述第一摄像头和所述第二摄像头用于拍摄红外图像；

所述第三摄像头用于拍摄彩色图像，且所述第一摄像头、所述第二摄像头、所述第三摄像头的光轴平行；

所述图像获取模块用于获取所述第一摄像头和所述第二摄像头拍摄的所述红外图像；

所述图像识别模块用于识别出所述第一摄像头和所述第二摄像头拍摄的所述红外图像中满足预设条件的红外激光光斑；

所述平面定位模块用于以所述红外图像中心为原点，得到所述第一摄像头和所述第二摄像头拍摄的两幅所述红外图像中红外激光光斑的坐标；

所述空间定位模块用于根据两幅所述红外图像中所述红外激光光斑的坐标，以及所述第一摄像头和所述第二摄像头的间距，所述第一摄像头和所述第二摄像头焦距，计算得到所述红外激光光斑的空间坐标。

其中一个实施例中，所述第一摄像头、所述第二摄像头、所述第三摄像头的光轴平行且焦距相同，并位于一条直线上，且所述第三摄像头位于所述第一摄像头和所述第二摄像头的中间。

其中一个实施例中，所述红外激光光斑空间坐标是以所述第一摄像头和所述第二摄像头中心连线的中点作为空间坐标原点O，所述第一摄像头和所述第二摄像头中心连线为X轴，垂直向上为Y轴，向前为Z轴。

其中一个实施例中，所述图像识别模块用于提取出包含红外激光点的二值图像I_bin(x,y)，并找到二值图像I_bin(x,y)中的所有连通域，且通过下列公式识别二值图像中红外激光光斑对应的连通域：

$\left\{\begin{array}{c}{\mathrm{TH}}_{\mathrm{area}1}<S.\mathrm{area}<{\mathrm{TH}}_{\mathrm{area}2}\\ \frac{|S.\mathrm{width}-S.\mathrm{height}|}{|S.\mathrm{width}+S.\mathrm{height}|}<{\mathrm{TH}}_{\mathrm{wh}}\\ |\frac{4\&times;S.\mathrm{area}}{p\&times;S.\mathrm{width}\&times;S.\mathrm{height}}-1|<{\mathrm{TH}}_{\mathrm{round}}\end{array}\right.,$

公式中，S.area为连通域的面积，用像素点个数表示；S.width和S.height分别为连通域宽度和高度的像素；TH_area1、TH_area2为连通域面积大小阈值；TH_wh为连通域长宽比阈值；TH_round为判断连通域是否圆形的阈值。

其中一个实施例中，假设所述第一摄像头和第二摄像头分别拍到的红外图像中的红外激光光斑坐标为：(x_L,y_L)和(x_R,y_R)，设两个摄像头间距为d毫米，焦距为f，所述空间定位模块是通过以下公式得到红外激光光斑的空间坐标位置(X,Y,Z)：

$\left\{\begin{array}{c}Z=\frac{d\&times;f}{|x_L-x_R|}\\ Y=\frac{Z\&times;(y_L+y_R)}{2f}\\ X=\frac{Z\&times;(x_L+x_R)}{2f}\end{array}\right..$

其中一个实施例中，还包括图像提取模块、图像定位模块、物体识别模块和大小计算模块，

所述图像提取模块用于获取所述第三摄像头拍摄的彩色图像。

所述图像定位模块用于根据所述第一摄像头、所述第二摄像头、所述第三摄像头的位置关系，以及所述红外激光光斑的空间坐标，得到所述红外激光光斑在所述彩色图像中的位置坐标；

所述物体识别模块用于在所述彩色图像中，以所述红外激光光斑在彩色图像中的位置坐标为中心，提取预设范围内的局部图像，统计颜色直方图得到颜色分量最大的RGB颜色值，并用漫水法在所述彩色图像中扩张填充，得到扩充区域；

所述大小计算模块用于获取所述扩充区域的像素长宽参数，并根据该像素长宽参数，所述红外激光光斑的空间坐标、所述第三摄像头的焦距计算得到物体的实际长宽。

一种定位控制方法，包括如下步骤：

通过激光发射器发射光路相同的可见激光和红外激光；

通过两个红外摄像头拍摄红外激光光斑所在区域的红外图像；

通过一个彩色摄像头拍摄可见激光光斑所在区域的彩色图像；

获取所述两个红外摄像头拍摄的两幅所述红外图像；

识别出所述红外图像中的红外激光光斑；

以所述红外图像中心为原点，得到两幅所述红外图像中所述红外激光光斑的坐标。

根据所述两幅红外图像中所述红外激光光斑的坐标，所述两个红外摄像头的间距，以及所述两个红外摄像头的焦距，计算得到所述红外激光光斑的空间坐标。

其中一个实施例中，还包括如下步骤：

获取所述彩色摄像头拍摄的彩色图像；

根据所述二个红外摄像头和所述彩色摄像头的位置关系，以及所述红外光斑的空间坐标，得到所述红外激光光斑在所述彩色图像中的位置坐标；

在所述彩色图像中，以所述红外激光光斑在彩色图像中的位置坐标为中心，提取预设范围内的局部图像，统计颜色直方图得到颜色分量最大的RGB颜色值，并用漫水法在彩色图像中扩张填充，得到扩充区域；

获取所述扩充区域的像素长宽参数，并根据该像素长宽参数，所述红外激光光斑的空间坐标、所述彩色摄像头的焦距计算得到物体的实际长宽。

其中一个实施例中，所述两个红外摄像头和彩色摄像头在同一直线上且等间隔布置，彩色摄像头位于两个红外摄像头中间，三个摄像头的光轴平行，焦距相同。

其中一个实施例中，所述识别出所述红外图像中的红外激光光斑的步骤是：

先通过亮度阈值去除背景，提取出包含所述红外激光点的二值图像I_bin(x,y)；

再找到二值图像I_bin(x,y)中的所有连通域，并通过下列公式识别二值图像中所述红外激光光斑对应的连通域：

$\left\{\begin{array}{c}{\mathrm{TH}}_{\mathrm{area}1}<S.\mathrm{area}<{\mathrm{TH}}_{\mathrm{area}2}\\ \frac{|S.\mathrm{width}-S.\mathrm{height}|}{|S.\mathrm{width}+S.\mathrm{height}|}<{\mathrm{TH}}_{\mathrm{wh}}\\ |\frac{4\&times;S.\mathrm{area}}{p\&times;S.\mathrm{width}\&times;S.\mathrm{height}}-1|<{\mathrm{TH}}_{\mathrm{round}}\end{array}\right.,$

公式中，S.area为连通域的面积，用像素点个数表示；S.width和S.height分别为连通域宽度和高度的像素；TH_area1、TH_area2为连通域面积大小阈值；TH_wh为连通域长宽比阈值；TH_round为判断连通域是否圆形的阈值。

上述定位控制装置和方法通过可见激光和不可见的红外激光配合运用，使得操作和计算都变得更加简单。用户可以通过可见激光准确指定所需操作的位置，但是在环境复杂变化的场景下，去识别可见激光点，计算非常复杂，而且容易出错，为此通过红外激光和红外摄像头的组合，大大减少了激光光斑识别和计算量。

且上述三个摄像头在一条直线上等间隔排列的方式也有效地降低了后续的计算复杂度。

用户在使用上述定位控制方法时只需简单的利用头部晃动，将可见激光照射到需要操作的物体上，便实现了定位功能，特别是对四肢不便的用户来说，机械假肢的操作控制变得更加简单。

附图说明

图1为一实施例的定位控制装置的功能模块图；

图2为机械假肢的控制方式示意图；

图3为一实施例的定位控制方法的步骤流程图。

具体实施方式

如图1所示，其为一实施例的定位控制装置10的功能结构示意图，包括：激光发射器110、开关120、眼镜130、第一摄像头210、第二摄像头220、第三摄像头230、图像获取模块310、图像识别模块311、平面定位模块312、空间定位模块313、图像提取模块410、图像定位模块411、物体识别模块412和大小计算模块323。

所述激光发射器110用于向外发射光路相同的可见激光和红外激光。

所述光路相同是指可见激光点和红外激光点聚集的位置要求尽可能重合，光斑大小尽可能一致。所述可见激光可以是红色，绿色等颜色，便于用户肉眼发现。所述红外激光虽然肉眼看不见，但有利于后续图像处理。

开关120用于通过有线或者无线的方式控制激光发射器110的发光和熄灭。本实施例中，开关120为小型防水轻触式开关，可以放置于口中，可用舌头或牙齿来控制激光发射器110发光。

所述激光发射器110装配在眼镜130上，以便用户带上眼镜130后，可以让激光发射方向和视线方向同步移动。其他实施例中，也可以用帽子，头箍等其他佩戴方式将激光发射器110固定在用户头上。

第一摄像头210和第二摄像头220用于拍摄红外图像。

如第一摄像头210和第二摄像头220都安装红外滤波片，构成双目红外捕捉系统，用于捕捉激光发射器110发出的红外激光反光点。

第三摄像头230用于拍摄彩色图像，且第一摄像头210、第二摄像头220、第三摄像头230的光轴平行。

本实施例中，第一摄像头210、第二摄像头220、第三摄像头230位于一条直线上，且第三摄像头230位于第一摄像头210和第二摄像头220的中间。第一摄像头210、第二摄像头220、第三摄像头230的焦距相同。

图像获取模块310用于获取第一摄像头210和第二摄像头220拍摄的红外图像。

图像识别模块311，用于识别出第一摄像头210和第二摄像头220拍摄的红外图像中满足预设条件的红外激光光斑。

如：由于红外滤波片已经滤除了非红外发光的部分，可以先通过亮度阈值去除背景，提取出包含所述红外激光点的二值图像I_bin(x,y)；

再找到二值图像I_bin(x,y)中的所有连通域，并通过下列公式识别二值图像中所述红外激光光斑对应的连通域：

$\left\{\begin{array}{c}{\mathrm{TH}}_{\mathrm{area}1}<S.\mathrm{area}<{\mathrm{TH}}_{\mathrm{area}2}\\ \frac{|S.\mathrm{width}-S.\mathrm{height}|}{|S.\mathrm{width}+S.\mathrm{height}|}<{\mathrm{TH}}_{\mathrm{wh}}\\ |\frac{4\&times;S.\mathrm{area}}{p\&times;S.\mathrm{width}\&times;S.\mathrm{height}}-1|<{\mathrm{TH}}_{\mathrm{round}}\end{array}\right.,$

公式中，S.area为连通域的面积，用像素点个数表示；S.width和S.height分别为连通域宽度和高度的像素；TH_area1、TH_area2为连通域面积大小阈值由激光光斑大小决定，此处可以分别取5和20；；TH_wh为连通域长宽比阈值；TH_round为判断连通域是否圆形的阈值，此处可取0.1。

满足上公式的连通域，判断为圆形红外激光光斑对应连通域，其中心对应的图像坐标(x，y)即为红外图像中红外激光光斑的位置。

平面定位模块312用于以红外图像中心为原点，得到两幅红外图像中红外激光光斑的坐标。

如：第一摄像头210和第二摄像头220分别拍到的红外图像中的红外激光光斑坐标分别为：(x_L,y_L)和(x_R,y_R)，一般有y_L＝y_R。

空间定位模块313用于根据两幅红外图像中红外激光光斑的坐标，以及第一摄像头210和第二摄像头220的间距，第一摄像头210和第二摄像头220焦距，计算得到红外激光光斑的空间坐标。

所述红外激光光斑空间坐标是以第一摄像头210和第二摄像头220中心连线的中点作为空间坐标原点O，第一摄像头210和第二摄像头220中心连线为X轴，垂直向上为Y轴，向前为Z轴。

如：第一摄像头210和第二摄像头220分别拍到的红外图像中的红外激光光斑坐标为：(x_L,y_L)和(x_R,y_R)，设两个摄像头间距为d毫米，焦距为f，(f依照镜头成像关系转化为像素单位)，红外激光光斑的空间坐标位置(X,Y,Z)可由下式求得：

$\left\{\begin{array}{c}Z=\frac{d\&times;f}{|x_L-x_R|}\\ Y=\frac{Z\&times;(y_L+y_R)}{2f}\\ X=\frac{Z\&times;(x_L+x_R)}{2f}\end{array}\right..$

图像提取模块410用于获取第三摄像头230拍摄的彩色图像。

图像定位模块411用于根据第一摄像头210、第二摄像头220、第三摄像头230的位置关系，以及红外激光光斑的空间坐标，得到红外激光光斑在彩色图像中的位置坐标。

本实施例中，由于第一摄像头210、第二摄像头220、第三摄像头230位于一条直线上，且第三摄像头230位于第一摄像头210和第二摄像头220的中间，那么红外激光光斑在彩色图像中的位置坐标(x,y)等于：

$x=\frac{x_L+x_R}{2},y=\frac{y_L+y_R}{2}.$

物体识别模块412用于在彩色图像中，以红外激光光斑在彩色图像中的位置坐标为中心，提取预设范围内的局部图像，统计颜色直方图得到颜色分量最大的RGB颜色值，并用漫水法在彩色图像中扩张填充，得到扩充区域。

如：在彩色图像中，以(x,y)为中心提取20*20像素区域，统计颜色直方图，提取最大的颜色分量对应的RGB颜色值，作为选定操作物体的参考颜色值。假设物体颜色基本一致且与背景颜色不同，可采用漫水算法进行适当扩张填充，提取出物体的整体。

大小计算模块323用于获取扩充区域的像素长宽参数，并根据该像素长宽参数，红外激光光斑的空间坐标、第三摄像头230的焦距计算得到物体的实际长宽。

如：假设物体在彩色图像中所占象素区域宽高为(w,h)，则物体的实际宽高为(W,H)：

$\left\{\begin{array}{c}W\&ap;\frac{Z\&times;w}{f}\\ H\&ap;\frac{Z\&times;h}{f}\end{array}\right.,$

其中Z为红外激光光斑的空间坐标的Z坐标值，f为第三摄像头230的焦距。

将上述红外激光光斑的空间坐标提供给机械假肢，机械假肢便可找到红外激光光斑照射的位置，将上述物体的实际宽高提供给机械假肢，机械假肢便可知道带操作物体的带下，从而能准确的完成抓起等动作。

上述定位控制装置10通过可见激光和不可见的红外激光配合运用，使得操作和计算都变得更加简单。用户可以通过可见激光准确指定所需操作的位置，但是在环境复杂变化的场景下，去识别可见激光点，计算非常复杂，而且容易出错，为此通过红外激光和红外摄像头的组合，大大减少了激光光斑识别和计算量。且上述事实中三个摄像头在一条直线上等间隔排列的方式也有效地降低了后续的计算复杂度。用户在使用上述定位控制装置10时，只需简单的利用头部晃动，将可见激光照射到需要操作的物体上，便实现了定位功能，特别是对四肢不便的用户来说，机械假肢(机械臂)的操作控制变得更加简单。

关于机械手执行何种操作，可以通过很多种方式实现，如声控，按键控制等，本发明中，定位/启动、抓起、放下、推按、拉、停止这几个常见控制命令，控制命令通过人控制激光点闪烁或者控制激光点划出特定运动轨迹来实现，计算机通过图像处理，识别不同的激光点的闪烁方式或者运动轨迹，进而识别不同的控制命令。

如：在通过激光点闪烁或者运动轨迹的方式来发送控制命令时，用户输入不同控制命令的方式可以是下表所列方式：

如图2所示，其为一实施例的机械假肢控制示意图，所示机械假肢50的几何结构满足Pieper准则的6自由度机械臂制作。根据D-H参数和连杆坐标系，6自由度机械臂的运动学方程描述为：

T_end＝T₁T₂T₃T₄T₅T₆

其中：T_i＝R_z(θ_i)T_z(d_i)T_x(a_i)R_x(α_i)(i＝1,2,...,6)，a_i为连杆长度，α_i为连杆扭角，d_i为连杆偏置，θ_i为关节变量，T_end为机械臂末端的位姿矩阵。其中，连杆参数d_i，a_i和α_i已知，T_end由物体的空间定位确定。可以采用如下的方法计算各个关节的目标控制角度θ_i：

令L(i,j)和R(i,j)分别表示4*4矩阵L和R的第i行,第j列个元素,求解其逆运动学问题的步骤为：由如下公式

$L=T_1^{-1}{T}_{\mathrm{end}}=T_2{T}_3{T}_4{T}_5{T}_6=R$

L(3,4)＝R(3,4),L(1,4)＝R(1,4),

L(2,4)＝R(2,4)

可分别求出θ₁和θ₃在区间[-180°,180°]两个解，同样由如下公式

$\left\{\begin{array}{c}L={\left(T_1{T}_2{T}_3\right)}^{-1}{T}_{\mathrm{End}}=T_4{T}_5{T}_6=R,\\ L\left(\mathrm{1,4}\right)=R\left(\mathrm{1,4}\right),\\ L\left(\mathrm{3,4}\right)=R\left(\mathrm{3,4}\right),\\ L\left(\mathrm{1,3}\right)=R\left(\mathrm{1,3}\right),\\ L\left(\mathrm{2,3}\right)=R\left(\mathrm{2,3}\right)\end{array}\right.$

可分别求出θ₂在区间[-180°,180°]一个解和θ₄在区间的两个解，由如下公式

$\left\{\begin{array}{c}L={\left(T_1{T}_2{T}_3{T}_4\right)}^{-1}{T}_{\mathrm{End}}=T_5{T}_6=R,\\ L\left(\mathrm{1,3}\right)=R\left(\mathrm{1,3}\right),\\ L\left(\mathrm{2,3}\right)=R\left(\mathrm{2,3}\right)\end{array}\right.$

可分别求出θ₅在区间[-180°,180°]一个解，由公式

$\left\{\begin{array}{c}L={\left(T_1{T}_2{T}_3{T}_4{T}_5\right)}^{-1}{T}_{\mathrm{End}}=T_6=R,\\ L\left(\mathrm{1,3}\right)=R\left(\mathrm{1,3}\right),\\ L\left(\mathrm{2,3}\right)=R\left(\mathrm{2,3}\right)\end{array}\right.$

可求出θ₆在区间[-180°,180°]一个解。

在求得机械假肢各关节目标姿态之后，选择适当的控制路径，由工控机系统控制自动完成动作。

如图3所示，其为一实施例的定位控制方法的步骤流程图，包括如下步骤：

步骤S301，通过激光发射器发射光路相同的可见激光和红外激光。

步骤S302，通过两个红外摄像头拍摄红外激光光斑所在区域的红外图像。

步骤S303，通过一个彩色摄像头拍摄可见激光光斑所在区域的彩色图像。

一实施例中，所述两个红外摄像头和彩色摄像头在同一直线上且等间隔布置。彩色摄像头位于两个红外摄像头中间，三个摄像头的光轴平行，焦距相同。

步骤S304，获取两幅红外图像。

步骤S305，识别出红外图像中的红外激光光斑。

如：由于红外滤波片已经滤除了非红外发光的部分，可以直通过亮度阈值去除背景，提取出包含所述红外激光点的二值图像I_bin(x,y)；

再找到二值图像I_bin(x,y)中的所有连通域，并通过下列公式识别二值图像中所述红外激光光斑对应的连通域：

$\left\{\begin{array}{c}{\mathrm{TH}}_{\mathrm{area}1}<S.\mathrm{area}<{\mathrm{TH}}_{\mathrm{area}2}\\ \frac{|S.\mathrm{width}-S.\mathrm{height}|}{|S.\mathrm{width}+S.\mathrm{height}|}<{\mathrm{TH}}_{\mathrm{wh}}\\ |\frac{4\&times;S.\mathrm{area}}{p\&times;S.\mathrm{width}\&times;S.\mathrm{height}}-1|<{\mathrm{TH}}_{\mathrm{round}}\end{array}\right.,$

公式中，S.area为连通域的面积，用像素点个数表示；S.width和S.height分别为连通域宽度和高度的像素；TH_area1、TH_area2为连通域面积大小阈值由激光光斑大小决定，此处可以分别取5和20；；TH_wh为连通域长宽比阈值；TH_round为判断连通域是否圆形的阈值，此处可取0.1。

满足上公式的连通域，判断为圆形红外激光光斑对应连通域，其中心对应的图像坐标(x，y)即为红外图像中红外激光光斑的位置。

步骤S306，以红外图像中心为原点，得到两幅红外图像中红外激光光斑的坐标。

步骤S307，根据两幅红外图像中红外激光光斑的坐标，两个红外摄像头的间距，以及两个红外摄像头的焦距，计算得到红外激光光斑的空间坐标。

所述红外激光光斑空间坐标是以两个红外摄像头中心连线的中点作为空间坐标原点O，两个红外摄像头中心连线为X轴，垂直向上为Y轴，向前为Z轴。

如：两个红外摄像头分别拍到的红外图像中的红外激光光斑坐标为：(x_L,y_L)和(x_R,y_R)(均以图像中心点为坐标原点，以像素为单位)，设两个摄像头间距为d毫米，焦距为f，(f依照镜头成像关系转化为像素单位)，红外激光光斑的空间坐标位置(X,Y,Z)可由下式求得：

$\left\{\begin{array}{c}Z=\frac{d\&times;f}{|x_L-x_R|}\\ Y=\frac{Z\&times;(y_L+y_R)}{2f}\\ X=\frac{Z\&times;(x_L+x_R)}{2f}\end{array}\right.$

步骤S308，获取所述彩色摄像头拍摄的彩色图像。

步骤S309，根据二个红外摄像头和彩色摄像头的位置关系，以及红外光斑的空间坐标，得到红外激光光斑在彩色图像中的位置坐标。

本实施例中，由于两个红外摄像头和彩色摄像头位于一条直线上，且彩色摄像头位于两个红外摄像头的中间，那么红外激光光斑在彩色图像中的位置坐标(x,y)等于：

$x=\frac{x_L+x_R}{2},y=\frac{y_L+y_R}{2}.$

步骤S310，在彩色图像中，以红外激光光斑在彩色图像中的位置坐标为中心，提取预设范围内的局部图像，统计颜色直方图得到颜色分量最大的RGB颜色值，并用漫水法在彩色图像中扩张填充，得到扩充区域。

如：在彩色图像中，以(x,y)为中心提取20*20像素区域，统计颜色直方图，提取最大的颜色分量对应的RGB颜色值，作为选定操作物体的参考颜色值。假设物体颜色基本一致且与背景颜色不同，可采用漫水算法进行适当扩张填充，提取出物体的整体。

步骤S311，获取扩充区域的像素长宽参数，并根据该像素长宽参数，红外激光光斑的空间坐标、彩色摄像头的焦距计算得到物体的实际长宽。

如：假设物体在彩色图像中所占象素区域宽高为(w,h)，则物体的实际宽高为(W,H)：

$\left\{\begin{array}{c}W\&ap;\frac{Z\&times;w}{f}\\ H\&ap;\frac{Z\&times;h}{f}\end{array}\right.,$

其中Z为红外激光光斑的空间坐标的Z坐标值，f为彩色摄像头的焦距。

将上述红外激光光斑的空间坐标提供给机械假肢，机械假肢便可找到红外激光光斑照射的位置，将上述物体的实际宽高提供给机械假肢，机械假肢便可知道带操作物体的带下，从而能准确的完成抓起等动作。

上述定位控制方法通过可见激光和不可见的红外激光配合运用，使得操作和计算都变得更加简单。用户可以通过可见激光准确指定所需操作的位置，但是在环境复杂变化的场景下，去识别可见激光点，计算非常复杂，而且容易出错，为此通过红外激光和红外摄像头的组合，大大减少了激光光斑识别和计算量。且上述实施例中三个摄像头在一条直线上等间隔排列的方式也有效地降低了后续的计算复杂度。用户在使用上述定位控制方法时只需简单的利用头部晃动，将可见激光照射到需要操作的物体上，便实现了定位功能，特别是对四肢不便的用户来说，机械假肢的操作控制变得更加简单。

关于机械手执行何种操作，可以通过很多种方式实现，如声控，按键控制等，本发明中，定位/启动、抓起、放下、推按、拉、停止这几个常见控制命令，控制命令通过人控制激光点闪烁或者控制激光点划出特定运动轨迹来实现，计算机通过图像处理，识别不同的激光点的闪烁方式或者运动轨迹，进而识别不同的控制命令。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种定位控制装置，其特征在于，包括激光发射器、第一摄像头、第二摄像头、第三摄像头、图像获取模块、图像识别模块、平面定位模块、空间定位模块，

所述激光发射器用于向外发射光路相同的可见激光和红外激光；

所述第一摄像头和所述第二摄像头用于拍摄红外图像；

所述第三摄像头用于拍摄彩色图像，且所述第一摄像头、所述第二摄像头、所述第三摄像头的光轴平行；

所述图像获取模块用于获取所述第一摄像头和所述第二摄像头拍摄的所述红外图像；

所述图像识别模块用于识别出所述第一摄像头和所述第二摄像头拍摄的所述红外图像中满足预设条件的红外激光光斑；

所述平面定位模块用于以所述红外图像中心为原点，得到所述第一摄像头和所述第二摄像头拍摄的两幅所述红外图像中红外激光光斑的坐标；

所述空间定位模块用于根据两幅所述红外图像中所述红外激光光斑的坐标，以及所述第一摄像头和所述第二摄像头的间距，所述第一摄像头和所述第二摄像头焦距，计算得到所述红外激光光斑的空间坐标。

2.根据权利要求1所述的定位控制装置，其特征在于，所述第一摄像头、所述第二摄像头、所述第三摄像头的光轴平行且焦距相同，并位于一条直线上，且所述第三摄像头位于所述第一摄像头和所述第二摄像头的中间。

3.根据权利要求1所述的定位控制装置，其特征在于，所述红外激光光斑空间坐标是以所述第一摄像头和所述第二摄像头中心连线的中点作为空间坐标原点O，所述第一摄像头和所述第二摄像头中心连线为X轴，垂直向上为Y轴，向前为Z轴。

4.根据权利要求1所述的定位控制装置，其特征在于，所述图像识别模块用于提取出包含红外激光点的二值图像I_bin(x,y)，并找到二值图像I_bin(x,y)中的所有连通域，且通过下列公式识别二值图像中红外激光光斑对应的连通域：

$\left\{\begin{array}{c}{\mathrm{TH}}_{\mathrm{area}1}<S.\mathrm{area}<{\mathrm{TH}}_{\mathrm{area}2}\\ \frac{|S.\mathrm{widtg}-S.\mathrm{height}|}{|S.\mathrm{width}+S.\mathrm{height}|}<{\mathrm{TH}}_{\mathrm{wh}}\\ |\frac{4\&times;S.\mathrm{area}}{p\&times;S.\mathrm{width}\&times;S.\mathrm{height}}-1|<{\mathrm{TH}}_{\mathrm{round}}\end{array}\right.,$

公式中，S.area为连通域的面积，用像素点个数表示；S.width和S.height分别为连通域宽度和高度的像素；TH_area1、TH_area2为连通域面积大小阈值；TH_wh为连通域长宽比阈值；TH_round为判断连通域是否圆形的阈值。

5.根据权利要求1所述的定位控制装置，其特征在于，假设所述第一摄像头和第二摄像头分别拍到的红外图像中的红外激光光斑坐标为：(x_L,y_L)和(x_R,y_R)，设两个摄像头间距为d毫米，焦距为f，所述空间定位模块是通过以下公式得到红外激光光斑的空间坐标位置(X,Y,Z)：

$\left\{\begin{array}{c}Z=\frac{d\&times;f}{|x_L-x_R|}\\ Y=\frac{Z\&times;(y_L+y_R)}{2f}\\ X=\frac{Z\&times;(x_L+x_R)}{2f}\end{array}\right..$

6.根据权利要求1所述的定位控制装置，其特征在于，还包括图像提取模块、图像定位模块、物体识别模块和大小计算模块，

所述图像提取模块用于获取所述第三摄像头拍摄的彩色图像；

所述图像定位模块用于根据所述第一摄像头、所述第二摄像头、所述第三摄像头的位置关系，以及所述红外激光光斑的空间坐标，得到所述红外激光光斑在所述彩色图像中的位置坐标；

所述物体识别模块用于在所述彩色图像中，以所述红外激光光斑在彩色图像中的位置坐标为中心，提取预设范围内的局部图像，统计颜色直方图得到颜色分量最大的RGB颜色值，并用漫水法在所述彩色图像中扩张填充，得到扩充区域；

所述大小计算模块用于获取所述扩充区域的像素长宽参数，并根据该像素长宽参数，所述红外激光光斑的空间坐标、所述第三摄像头的焦距计算得到物体的实际长宽。

7.一种定位控制方法，其特征在于，包括如下步骤：

通过激光发射器发射光路相同的可见激光和红外激光；

通过两个红外摄像头拍摄红外激光光斑所在区域的红外图像；

通过一个彩色摄像头拍摄可见激光光斑所在区域的彩色图像；

获取所述两个红外摄像头拍摄的两幅所述红外图像；

识别出所述红外图像中的红外激光光斑；

以所述红外图像中心为原点，得到两幅所述红外图像中所述红外激光光斑的坐标；

根据所述两幅红外图像中所述红外激光光斑的坐标，所述两个红外摄像头的间距，以及所述两个红外摄像头的焦距，计算得到所述红外激光光斑的空间坐标。

8.根据权利要求7所述的定位控制方法，其特征在于，还包括如下步骤：

获取所述彩色摄像头拍摄的彩色图像；

根据所述二个红外摄像头和所述彩色摄像头的位置关系，以及所述红外光斑的空间坐标，得到所述红外激光光斑在所述彩色图像中的位置坐标；

在所述彩色图像中，以所述红外激光光斑在彩色图像中的位置坐标为中心，提取预设范围内的局部图像，统计颜色直方图得到颜色分量最大的RGB颜色值，并用漫水法在彩色图像中扩张填充，得到扩充区域；

获取所述扩充区域的像素长宽参数，并根据该像素长宽参数，所述红外激光光斑的空间坐标、所述彩色摄像头的焦距计算得到物体的实际长宽。

9.根据权利要求7所述的定位控制方法，其特征在于，所述两个红外摄像头和彩色摄像头在同一直线上且等间隔布置，彩色摄像头位于两个红外摄像头中间，三个摄像头的光轴平行，焦距相同。

10.根据权利要求7所述的定位控制方法，其特征在于，所述识别出所述红外图像中的红外激光光斑的步骤是：

先通过亮度阈值去除背景，提取出包含所述红外激光点的二值图像I_bin(x,y)；

再找到二值图像I_bin(x,y)中的所有连通域，并通过下列公式识别二值图像中所述红外激光光斑对应的连通域：

$\left\{\begin{array}{c}{\mathrm{TH}}_{\mathrm{area}1}<S.\mathrm{area}<{\mathrm{TH}}_{\mathrm{area}2}\\ \frac{|S.\mathrm{widtg}-S.\mathrm{height}|}{|S.\mathrm{width}+S.\mathrm{height}|}<{\mathrm{TH}}_{\mathrm{wh}}\\ |\frac{4\&times;S.\mathrm{area}}{p\&times;S.\mathrm{width}\&times;S.\mathrm{height}}-1|<{\mathrm{TH}}_{\mathrm{round}}\end{array}\right.,$

公式中，S.area为连通域的面积，用像素点个数表示；S.width和S.height分别为连通域宽度和高度的像素；TH_area1、TH_area2为连通域面积大小阈值；TH_wh为连通域长宽比阈值；TH_round为判断连通域是否圆形的阈值。