一种基于视觉和超声波组合的智能轮椅床对接控制方法与流程

本发明属于视觉和运动控制领域，尤其涉及一种基于视觉和超声波组合的智能轮椅床对接控制方法。

背景技术：

人口老龄化的快速增长以及对病残人的护理成为了社会的一个重要问题。在对老龄人和病残人的护理中，会将老人和病残人从床和轮椅进行多次的转移，因此需要一种床椅一体化系统。智能轮椅床系统不仅可以降低护理人员的工作量，而且有利于增强老人及病残人的独立生活能力。这对解决老人及病残人的护理问题具有重要的意义。

轮椅和床体组合时存在对接问题。不同的轮椅床结构及传感器也有着不同的自动对接策略。申请号201410682694.1的专利公开了一种利用红外人工路标实现自动对接的方法。该方法是将红外光源按一定的几何形状安装在床体，然后使用装有滤光片的相机采集图像并进行滤波和边缘提取，最后通过对人工路标的计算实现定位对接。此方法中使用的相机需要安装滤光片，这样就会增加传感器设计的复杂度。此外，红外人工路标的提取不仅算法复杂而且也会存在提取误差，这样就会降低对接的准确度。

技术实现要素：

针对当前的轮椅床对接问题，本发明提出一种基于视觉系统和超声波组合的自动对接方法，引入人工路标-棋盘格作为视觉引导物和超声波测距反馈组合实现床椅系统的对接。此方法不需要对床体结构和传感器进行特殊设计，同时降低算法的复杂度，具有一定的鲁棒性和有效性。

应用该方法的轮椅床系统：人工路标安装在床体，轮椅可以利用相机识别到人工路标；相机安装在轮椅的左扶手侧，具有红外功能；三个超声波安装在轮椅的左侧；其中，人工路标使用的是棋盘格。

一种基于视觉系统和超声波组合的轮椅床对接方法，包括以下步骤：

步骤1：相机标定获得相机的内参及去畸变系数，采用张正友平面标定法进行摄像机的标定；

步骤2：通过视觉系统采集轮椅左侧的图像，并提取图像中的人工路标，目标引导物为对接的重要人工标志，需要将轮椅放在可以观测到目标引导物的区域。当相机视野观测不到路标时，可以通过转动找到目标引导物；

步骤3：分别在人工路标、相机和轮椅上建立世界、相机和轮椅坐标系系统；

步骤4:根据建立的坐标系系统计算轮椅相对人工路标的位置和偏转角；

步骤5：当轮椅相对人工路标的距离小于设定阈值采用超声波完成对接。

同现有的轮椅床对接方法相比，本发明的优势如下：

(1)本发明使用的人工路标制作简单，特征明显。

(2)本发明采用的传感器及床椅对接环境设计简单，具有较好的实用性。

(3)人工路标的提取及算法复杂度低且具有较好的鲁棒性。

附图说明

图1为轮椅床对接示意图；

图2为本发明的总体流程图；

图3为基于视觉和超声波组合的对接流程图；

图4为建立世界、相机和轮椅坐标系系统；

图5为世界、相机和轮椅坐标系位姿变换关系；

图6(a)-图6(d)为实际环境图像的采集及位姿计算结果：图6(a)轮椅和床相距50cm,轮椅的底座长边和辅助床的长边夹角为0度的结果；图6(b)轮椅和床相距100cm，轮椅的底座长边和辅助床的长边夹角为0度的结果；图6(c)轮椅和床相距100cm，轮椅相对床向里偏转，轮椅的底座长边和辅助床的长边夹角为20度的结果；图6(d)轮椅和床相距100cm，轮椅相对床向外偏转，轮椅的底座长边和辅助床的长边夹角为20度的结果。

具体实施方式

下面结合附图和具体实施方式对本发明作详细说明。

本发明提出基于视觉引导和超声波传感器组合的方法完成泊位对接，对接示意图如图1所示。本发明是将制作好的人工路标固定在辅助床，轮椅通过自主运动可以找到相机视野中的人工路标。工控机根据视觉系统计算的轮椅相对床的位置和姿态角控制轮椅向辅助床移动，当视觉系统计算轮椅相对人工路标的距离小于设定的阈值时，由于相机视角原因，工控机不能从相机采集到的图像中识别人工路标从而采用超声波闭环方式完成对接。

上述人工路标使用的是一张尺寸为长14.8cm、宽10.6cm的棋盘格,棋盘格上特征点的间隔为13mm。制作方法是以a4纸的1/4打印标准的棋盘格。

上述超声波传感器安装在轮椅的左侧且分布在同一水平直线。轮椅从前向后安装超声波1、超声波2和超声波3。超声波1和轮椅底座前边界距离为0.07m,超声波1和超声波2距离为0.26m，超声波2和超声波3距离为0.34m，超声波3和轮椅底座后边界距离为0.07m。

上述相机固定在轮椅的左扶手侧，安装在轮椅左侧中点且距离地面高度为0.375m。相机采集到的图像通过usb端口上传到工控机。工控机完成图像校正、人工路标识别位姿计算以及控制策略。

如图2和如图3所示，智能轮椅的自动对接具体过程如下：

步骤1，相机标定获得内参和去畸变系数。

(1)打印一张尺寸为a4的靶标模板，靶标上特征点的间隔为30mm。

(2)从多个角度拍摄靶标。拍摄时，应尽量让靶标占满屏幕，并保证靶标的每一个角都在屏幕中，一共拍摄10张靶标图像,每张图像分辨率为1920*1080。

(3)将采集到的10张靶标图像使用张正友平面标定法进行摄像机的标定。

(4)通过相机标定获得相机的内参矩阵及去畸变系数：

其中，(fx,fy)是相机的归一化焦距，(u0,v0)是主点坐标，γ是u轴和v轴的不垂直因子。k1,k2为径向畸变系数，k3,k4为切向畸变系数。

步骤2，视觉系统采集图像并提取人工路标。

(1)假定轮椅放在与辅助床相对距离合理范围内，在范围内轮椅通过调整姿态可以观测到人工路标”。

(2)相机采集轮椅左侧的图像，并传到工控机进行图像处理。

(3)利用步骤1获得的去畸变系数对原始图像进行畸变校正。

(4)使用图像处理中的棋盘格角点检测方法提取图像中的人工路标。

(5)当人工路标不在相机视野时，工控机控制轮椅以0.02m/s的速度以一定的方向原地转动一定的角度停止。

(6)重复步骤(2)～(5),直到提取到人工路标。

步骤3，分别在人工路标、相机和轮椅上建立世界、相机和轮椅坐标系系统，如图4所示。

(1)根据步骤2获得人工路标。

(2)根据人工路标建立世界坐标系，棋盘格的特征点之间的距离可以通过测量得到。ow-xwywzw表示为世界坐标系，原点ow为棋盘格的第一行(从下往上为一、二、三)与第五列的交点，xw沿辅助床体长边水平指向(如图4所示方向)，yw垂直向上，zw指向前方。

(3)oc-xcyczc表示为相机坐标系，原点oc为相机的光学中心，zc轴沿相机的光轴指向正前方，xc轴和yc轴与像素坐标系的u轴和v轴平行。

(4)o-xyz表示为轮椅坐标系，原点o为轮椅的左侧中点且与相机等高，x轴、y轴和z轴分别与相机坐标系的xc轴、yc轴、zc轴平行。

步骤4，根据坐标系系统计算轮椅相对人工路标的位置和偏转角，如图5所示。具体计算方法如下：

(1)摄像机坐标系和世界坐标系可以用旋转矩阵r和平移矩阵t来描述。用齐次坐标和矩阵表示上述关系为：

其中，r为3*3正交单位矩阵，t为3*1三维平移向量，0^t为(0,0,0)。

(2)第i个特征点pi在两个坐标系下的坐标分别为:ci＝(xci,yci,zci)^t和wi＝(xwi,ywi,zwi)^t,则两个坐标系间的变换表达式为：

ci＝rwi+t

世界坐标系的原点到摄像机坐标的转换表达式为：

c＝r0w+t0

相机坐标系转换到轮椅坐标系的表达式为：

s＝r1c+t1

世界坐标系转换到轮椅坐标系的表达式为：

s＝r2w+t2

其中，s＝(x,y,z)^t为轮椅坐标，r2＝r1r0,t2＝r1t0+t1,r1＝i。

(3)从平移矩阵t2提取x值和z值。

(4)根据(2)计算得到的旋转矩阵计算轮椅和人工路标的姿态角。

轮椅的对接问题只存在偏航角的变化，因此，只需要根据旋转矩阵计算偏航角。计算公式如下：

当r31不等于±1时，表达式为：

θ＝-arcsin(r31)

当r31＝+1时，表达式为：

θ＝π/2

当r31＝-1时，表达式为：

θ＝-π/2

步骤5，深度值小于设置的阈值，采用超声波对接。

(1)根据步骤4得到的深度值小于设置的阈值时，视觉对接切换到超声波对接。

(2)利用超声波1和超声波3的距离差调整姿态角。

(3)轮椅控制系统根据超声波的数据控制轮椅向辅助床移动

(4)重复(2)(3),超声波测量距离小于设置的阈值，完成对接。

为验证本发明所述方法的有效性，下面给出应用本发明所述方法在室内真实环境下进行位姿计算的一个实验实例。

轮椅床系统使用的是主频为2.5ghz的mio-5290研华工控机，图像采集使用的是颜色分辨率为1920*1080的高清摄像机。在不同距离和不同角度进行采集图像并计算轮椅相对人工路标的位置和姿态角，如图6所示。(a)轮椅和床相距50cm,轮椅的底座长边和辅助床的长边夹角为0度的结果；(b)轮椅和床相距100cm，轮椅的底座长边和辅助床的长边夹角为0度的结果；(c)轮椅和床相距100cm，轮椅相对床向里偏转，轮椅的底座长边和辅助床的长边夹角为20度的结果；(d)轮椅和床相距100cm，轮椅相对床向外偏转，轮椅的底座长边和辅助床的长边夹角为20度的结果。

实验结果表明，本发明所述方法能够确定轮椅和人工路标的相对位姿，且具有较好的精确度和鲁棒性。