CN102788591B - 基于视觉信息的机器人沿引导线巡线导航方法 - Google Patents

Abstract

本发明公开基于视觉信息的机器人沿引导线巡线导航方法，包括如下步骤：（a）通过机器人上安装的视觉传感器获取环境图像；（b）从环境图像中获取路径信息，并将获取的路径信息传输到控制器；（c）控制器根据路径信息设定机器人的线速度和角速度。本发明能够使机器人沿指定线路行走。

Description

基于视觉信息的机器人沿引导线巡线导航方法

技术领域

本发明涉及一种导航方法，特别涉及一种基于视觉信息的机器人沿引导线巡线导航方法。

背景技术

导航技术是可移动智能设备完成很多复杂功能的基础，在工业生产、生活、国防等领域有着广泛的应用前景。例如，在环境恶劣的工作场合，可用于引导机器人完成危险任务；在生产车间、码头等场合，可用于完成运输任务；在交通领域，可用于实现自适应巡航等等。

目前常用的导航手段有以下几种：基于轨道等固定路径的导航、基于测距雷达的导航、基于惯性陀螺仪的导航、基于GPS的导航以及各种方法的混合导航。以上方法有各自的优缺点：基于固定路径的导航控制起来简单，但需要铺设轨道或是电磁线路，安装及变更时很不方便；基于测距雷达的导航受周围环境变化的影响较大，且需要设置大量反射标识；基于惯性陀螺仪的导航简单灵活，但受各种干扰的影响会发生误差累计，长期运行会因误差太大儿无法工作；基于GPS的导航使用方便，自由度大，但由于精度有限，不适合局部区域的精确导航。

近年来，随着高质量低成本视觉传感器的普及以及机器人处理能力的不断提高，视觉传感器已成为机器人感知外部环境的重要部件；这使得通过视觉实现导航成为可能。但是，在现有技术中还没有出现基于视觉信息的导航产品和方法，而基于视觉的导航有着很广阔的应用前景。那些需要在空间不大、环境较复杂、路径固定的区域内实现高精度导航的场所，例如工厂、港口、车站等需要完成大量运输任务的地方，基于视觉的导航能够发挥非常重要的作用。

发明内容

针对现有技术中存在的不足，本发明的目的在于提供一种能够使机器人沿指定线路行走的基于视觉信息的机器人沿引导线巡线导航方法。

本发明的技术方案是这样实现的：基于视觉信息的机器人沿引导线巡线导航方法，包括如下步骤：（a）通过机器人上安装的视觉传感器获取环境图像；（b）从环境图像中获取路径信息，并将获取的路径信息传输到控制器；（c）控制器根据路径信息设定机器人的线速度和角速度。

本发明的有益效果是：本发明只需在地面上绘制出引导线即可实现导航，不需要轨道、陀螺仪、GPS或电子地图等软硬件的支持，安装和变更都很方便。定位精度可以轻易达到厘米级。且由于参照物固定，避免了因各种外部因素而引入的累计误差。结合模式识别的技术，可以使机器人在检测到特定标识后自动执行相应动作，这样大大拓展了视觉引导线的应用范围。

本发明可以应用在那些需要在空间不大、环境较复杂、路径固定的区域内实现高精度导航的场所，例如工厂、港口、车站等需要完成大量运输任务的地方。

本发明仿真结果可以充分证明，只要参数合适，使用并行分配补偿模糊控制算法设计实现的控制器，能够取得很好的控制效果。即使在控制周期长的情况下，该控制器依然能够对机器人进行有效控制，且整个过程十分平稳。

附图说明

图1为环境图像；

图2将环境图像映射到HSV空间后的图像；

图3对H域做二值化后的图像；

图4对二值化后的图像做中值滤波后的图像；

图5对中值滤波后的图像进一步处理后的图像；

图6路径搜索的过程示意图；

图7路径搜索后的结果；

图8使线速度的设定值按低频率变化测得实际的线速度响应；

图9使线速度的设定值按高频率变化测得实际的线速度响应；

图10使角速度的设定值按低频率变化测得实际的线速度响应；

图11使角速度的设定值按高频率变化测得实际的线速度响应；

图12采用初始化后的参数得到的响应曲线；

图13一种较为理想的阶跃响应；

图14采用固定参数的PID算法时在第一组参数（V=1m/s,delta_t=16.67ms，delta_w=10°/s）下得到的仿真路线【实线为预先设定的引导线，圆点组成的虚线表示仿真路线】；

图15采用固定参数的PID算法时在第二组参数（V=3m/s,delta_t=16.67ms，delta_w=10°/s）下得到的仿真路线【实线为预先设定的引导线，圆点组成的虚线表示仿真路线】；

图16采用固定参数的PID算法时在第三组参数（V=1m/s delta_t=100ms，delta_w=10°/s）下得到的仿真路线【实线为预先设定的引导线，圆点组成的虚线表示仿真路线】；

图17为图14对应的角速度曲线；

图18为图15对应的角速度曲线；

图19为图16对应的角速度曲线；

图20采用基于并行分配补偿的模糊控制算法时在第一组参数（v=1m/s,delta_t=16.67m，delta_w=10°/s）下得到的仿真路线【实线为预先设定的引导线，圆点组成的虚线表示仿真路线】；

图21采用基于并行分配补偿的模糊控制算法时在第二组参数（v=1m/s,delta_t=100ms，delta_w=10°/s）下得到的仿真路线【实线为预先设定的引导线，圆点组成的虚线表示仿真路线】；

图22为图20对应的角速度曲线；

图23为图21对应的角速度曲线；

图24为控制算法的结构框图。

1-引导线，2-路面，3-Pioneer3-AT型移动机器人，4-视觉传感器获取环境图像的范围。

具体实施方式

本实施例采用MobileRobots公司的Pioneer3-AT型移动机器人，该型号的机器人为四轮驱动，转向机制与履带车相似；配有CCD可变焦摄像头、声纳环、激光雷达、陀螺仪等传感器，并具备车载电脑，主频1.5GHz。车载电脑使用的操作***是Windows XP，属于多任务非实时操作***。用户通过AIRA(ActivMedia RoboticsInterface for Application)应用程序接口与机器人进行交互，例如设定速度、读取状态等。AIRA是用面向对象的C++编写的，支持多线程。用户既可以通过底层命令直接驱动机器人，也可通过加载自定义的任务实现更高级的功能。引导线为道路上画出的黄色实曲线或白色实曲线。

本实施例基于视觉信息的机器人沿引导线巡线导航方法，包括如下步骤：

（a）通过机器人上安装的视觉传感器获取环境图像；

（b）从环境图像中获取路径信息，并将获取的路径信息传输到控制器；

首先采用基于最小生成树的分割算法从环境图像中提取路径，步骤如下：先将环境图像（如图1）映射到HSV空间（如图2），在H域中做进一步处理：由于实验环境的背景较复杂，在引导线附近有很多色调与引导线接近的杂点，如果直接在H域进行图像分割，区域的扩散现象会很严重；先根据引导线的色调值对H域做二值化（如图3），之后再对二值化的图像做中值滤波（如图4），最后采用基于最小生成树的快速图分割算法将引导线对应的区域分离；对分离后的各个区域的黑点数进行统计，将黑点比例大于一定阈值的区域选定为引导线对应的区域，将其染成黑色，最终的结果如图5所示；

然后采用路径搜索算法得到环境图像中的路径点，用以这些路径点为折点的折线段表示路径；这样可以减小运算量，同时增强抗干扰能力；步骤如下：第一步：将点的序号num置为1；第二步：从最近一行开始逐行扫描，当在某一行检测到宽度符合引导线宽度范围的黑色区域时，将该区域的中点作为第一个搜索到的路径点，将位置P(num)记录下来；将该点的预测方向pre_th(num)设为90度，状态state(num)记为1，之后进入第二步；如果没有搜索到宽度符合引导线宽度范围的黑色区域，则重复第一步；第三步：根据当前的路径点P(num)判断下一个点是否有可能超出图像范围；若超出则转向第三步，若没有超出，则以P(num)为圆心，以一定像素长度为半径，在预测角度pre_th(num)左右各一定角度的范围内进行搜索；若搜索到宽度符合引导线宽度范围的黑色区域，则将黑色区域的中点记为下一个路径点P(num+1)；若搜索到多个路径点，选择最接近预测方向pre_th(num)的路径点，将P(num)到P(num+1)的方向角作为第num+1个路径点的预测角度，并将state(num+1)的状态记为1；若没有搜索到路径点，将预测方向上的搜索点作为第num+1个路径点，将其状态state(num+1)记为0，其预测方向pre_th(num+1)记为pre_th(num)；重复第三步；第四步：搜索完毕；路径搜索的过程如图6所示，路径搜索后的结果如图7所示。

最后利用三阶透视变换矩阵得到各个路径点对应的地理位置：将路径点映射到以机器人中心为原点，纵向为Y轴的地面坐标系中；设任意一个路径点P在图像中的坐标是(x,y),在地理坐标系中的坐标是(u,v),则存在一个矩阵mat使得对于任意的P都存在相应的t满足：

(u*t,v*t,t)^T=mat*(x,y,1)^T     （2-1）

T是线性代数中矩阵的转置，是一个公知的数学运算符；为了确定透视变换矩阵mat，可以从图像中选择四个标定点，分别记录下相应的地理坐标；将这四个标定点带入矩阵方程（2-1），形成一个矩阵方程组，通过该矩阵方程组即可求出mat；当mat矩阵确定以后，对于在图像中搜索出的每个路径点P(num)，都可以用公式（2-1）求出相应的地理坐标GP(num)；将环境图像中的路径用若干个离散点构成的折线表示，设共搜索到point_num个点，这些点对应的地理坐标为GP(num),状态为state(num)。

经过实际测试，本实施例的路径提取与表示方法在背景复杂且受到各种干扰的情况下依然能够可靠工作，准确地将引导线分离出来。当图片分辨率为160*120，CPU主频为1.5GHz时，整个算法的运行时间（包括了从图像采集到路径表达的整个过程）为200ms左右。与机器人100ms的控制周期相比，该算法的运行时间可以接受。为了解决路径信息的采集频率与机器人控制频率不同步的问题，将某一时刻得到的路径信息用于连续的几个控制周期，在每个控制周期内会根据机器人的状态变化对当前的路径信息进行估计，用这个估计值计算控制量。

（c）控制器根据路径信息设定机器人的线速度和角速度。

机器人的运动模型：

Pioneer3-AT型移动机器人的转向机理和履带式车辆一致，即左侧和右侧的车轮速度分别相等。当两侧车轮的速度保持不变时，机器人的运动轨迹为直线或圆弧。该移动机器人可简化成两轮运动模型，两轮的轴线在机器人前后轮轴线的中间。

为了测试机器人对线速度和角速度设定值的动态响应特性，本实施例作了如下实验：使线速度的设定值按低频率和高频率两种方式变化，测得实际的线速度响应如图8和图9所示：由以上结果可知，若线速度的设定值与实际值差别较大，则第一个控制周期的修正量较小，从第二个控制周期开始以恒定的最大速度对线速度进行修正，当设定值与实际值差别小于阈值时线速度的动态响应与一阶惯性***相似。

使角速度的设定值按低频率和高频率两种方式变化，测得实际的线速度响应如图10和图11所示：由以上结果可知，与线速度的动态响应类似，若角速度设定值与实际值差别较大，则第一个控制周期的修正量较小，从第二个控制周期开始以恒定的最大速度对角速度进行修正，当设定值与实际值差别小于阈值时角速度的动态响应与一阶惯性***相似。

根据前面的结论建立近似的离散模型，如下所示：

$V=\frac{\mathrm{Vl}+\mathrm{Vr}}{2}---(3-1)$

$W=\frac{\mathrm{Vr}-\mathrm{Vl}}{\mathrm{width}}---(3-2)$

$x(t+1)=x\left(t\right)+V\left(t\right)*\mathrm{delta}\_t*\cos (\mathrm{angle}\left(t\right)+\frac{W\left(t\right)*\mathrm{delta}\_t}{2})---(3-3)$

$y(t+1)=y\left(t\right)+V\left(t\right)*\mathrm{delta}\_t*\sin (\mathrm{angle}\left(t\right)+\frac{W\left(t\right)*\mathrm{delta}\_t}{2})---(3-4)$

$\left\{\begin{array}{cc}V(t+1)=v\_\mathrm{para}*V\_\mathrm{set}\left(t\right)+V\left(t\right)& \left(\right|V\_\mathrm{set}\left(t\right)-V\left(t\right)|<\mathrm{delta}\_v)(3-5)\\ V(t+1)=V\left(t\right)+\mathrm{delta}\_v& (V\_\mathrm{set}\left(t\right)-V\left(t\right)>\mathrm{delta}\_v)(3-6)\\ V(t+1)=V\left(t\right)-\mathrm{delta}\_v& (V\_\mathrm{set}\left(t\right)-V\left(t\right)<-\mathrm{delta}\_v)(3-7)\end{array}\right.$

$\left\{\begin{array}{cc}W(t+1)=w\_\mathrm{para}*W\_\mathrm{set}\left(t\right)+W\left(t\right)& \left(\right|W\_\mathrm{set}\left(t\right)-W\left(t\right)|<\mathrm{delta}\_w)(3-8)\\ W(t+1)=W\left(t\right)+\mathrm{delta}\_w& (W\_\mathrm{set}\left(t\right)-W\left(t\right)>\mathrm{delta}\_w)(3-9)\\ W(t+1)=W\left(t\right)-\mathrm{delta}\_w& (W\_\mathrm{set}\left(t\right)-W\left(t\right)<-\mathrm{delta}\_w)(3-10)\end{array}\right.$

其中V是线速度，Vl、Vr分别为左、右轮的速度，v_para是和机器人运动***惯性相关的车轮线速度系数。W为角速度，x(t)、y(t)为机器人的坐标，angle(t)为机器人的方向角（以x轴正方向为起始线，逆时针为正）,w_para是和机器人运动***惯性相关的车轮角速度系数。V_set和W_set分别为线速度和角速度的设定值。

由上面的分析可以得到以下结论：在理想情况下，如果位置确定，机器人在未来时刻内的运动轨迹完全由V和W决定，因此对小车运动状态的控制可通过控制V和W来实现。由于V和W在100ms的控制周期内变化不大，可以认为机器人在一个控制周期内以恒定的V和W运动，轨迹为圆弧。

此外，当V和W的设定值与实际值差别较大时，V和W的实际值分别以恒定速度逼近设定值，当V和W的设定值与实际值差别较小时，实际值的响应类似惯性***。当机器人的负载在额定范围内变化时，***参数的变化较小，可以忽略。由实验数据可知，线速度的最大变化速度是300mm/s，角速度的最大变化速度是100°/s，惯性较大。同时，机器人的运动***存在不确定的时滞，时滞一方面来自与机器人本身的命令响应***，另一方面来自多线程环境所导致的时序的不确定。

首先采用与路径弯曲度相关的速度控制算法对机器人速度大小进行控制【考虑到视觉引导的主要任务是使机器人巡线，对速度的要求不高】，方法如下：连接GP(num)与GP(num+1)的折线段与地理坐标系中x轴正方向的夹角为Gth(num)，则弯曲程度可以表示为：

$W={\mathrm{\&Sigma;}}_{i=1}^n\left|\mathrm{\&theta;i}\right|---(3-11)$

其中θ1=Gth(1),θi=Gth(i)-Gth(i-1)(i≥2)；根据W将速度设定值划分为停、低、中和高四个档位；

然后控制器再采用基于并行分配补偿的模糊控制算法对机器人速度方向进行控制：在每个控制周期内，选取前方引导线上的一点，并求出该点处的切线；根据模糊控制算法计算出使机器人尽快逼近该切线的控制量，从而达到使机器人平稳巡线的目的；机器人属于非线性***，可以用两个线性***的加权来近似；分别利用线性二次型法对这两个线性***设计最优控制器，这两个最优控制器的加权就是最终的控制器；具体包括如下步骤：

步骤1：选取机器人移动的差速度cur_vlr，机器人车体与切线的夹角l_angle以及机器人车体中心与切线的距离l_y三个参数作为状态量；利用线性二次型法抽象出机器人移动的状态方程；其具体执行过程如下：

(1a)机器人角速度的动态响应模型用一阶惯性模型来近似；

(1b)选取机器人移动的差速度cur_vlr，机器人车体与切线的夹角l_angle以及机器人车体中心与切线的距离l_y三个参数作为状态量；

(1c)利用线性二次型法抽象出机器人移动的状态方程，如公式（3-16）至（3-18）所示：

cur_vlr(t+1)=v_para*u(t)+(1-v_para)*cur_vlr(t)    (3-16)

$l\_\mathrm{angle}(t+1)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)}{\mathrm{width}}*\mathrm{delta}\_t---(3-17)$

$l\_y(t+1)=l\_y\left(t\right)+v*\mathrm{delta}\_t*\sin (l-\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}})---(3-18)$

其中，cur_vlr是左右轮的差速度，v是机器人的线速度，width是车体宽度，delta_t是控制周期；

步骤2：由于机器人的运动模型是非线性的，包含sin函数；根据机器人移动的状态方程，抽象出两个简化的状态空间线性模型来近似；其具体执行过程如下：

(2a)由于机器人的运动模型包含sin函数，所以是非线性的；

设 $p\left(t\right)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}}---(3-19);$

(2b)当p(t)在零附近时，线性模型如公式（3-20）所示：

x(t+1)=A1*x(t)+B1*u(t)      (3-20)；

(2c)当p(t)在正负pi附近时，线性模型如公式（3-21）所示：

x(t+1)=A2*x(t)+B2*u(t)      (3-21)；

(2d)公式（3-20）与公式（3-21）中：

$A1=\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& V*\mathrm{delta}\_t& 1\end{array}\right)$

B1=(speed_para 0 0)

$A2=\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{g*V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& g*V*\mathrm{delta}\_t& 1\end{array}\right)$

B2=(speed_para 0 0)

g=0.01/pi

Speed_para的含义：Speed_para是和机器人运动***惯性相关的整体线速度系数。

(2e)设w1(p(t))、w2(p(t))为p(t)对于两个线性模型的隶属度函数，如公式（3-22）、公式（3-23）所示：

$w1\left(p\left(t\right)\right)=\frac{\sin \left(p\left(t\right)\right)-g*p\left(t\right)}{p\left(t\right)*(1-g)}---(3-22)$

$w2\left(p\left(t\right)\right)=\frac{\left(p\left(t\right)\right)-\sin \left(p\left(t\right)\right)}{p\left(t\right)*(1-g)}---(3-23)$

则模糊模型如公式（3-24）所示：

x(t+1)=A(p(t))*x(t)+B(p(t))*u(t)       （3-24）

其中：

$A\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*A1+w2\left(p\left(t\right)\right)*A2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-25)$

$B\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*B1+w2\left(p\left(t\right)\right)*B2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-26)$

当-179.4270<p(t)<179.4270时，该模糊模型精确的表示了非线性模型；

步骤3：针对两个线性模型分别用线性二次型方法设计最优控制器，其具体执行过程如下：

(3a)针对两个线性模型分别用线性二次型方法，得到控制率如公式(3-27)、公式(3-28)所示：

当p(t)在零附近时，

u(t)=f1*x(t)    (3-27)

当p(t)在正负pi附近时

u(t)=f2*x(t)    (3-28)；

(3b)最终控制量如公式（3-30）所示：

$u\left(t\right)=\frac{w1\left(p\left(t\right)\right)*f1+w2\left(p\left(t\right)\right)*f2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}*x\left(t\right)---(3-29)$

$W\left(t\right)=-\frac{u\left(t\right)}{\mathrm{width}}---(3-30);$

步骤4：在用线性二次型方法设计最优控制器时，需要给定加权矩阵Q和R，通过离线求解Riccati微分方程得到控制器的参数；Q和R对控制效果的影响非常大；为了提高整定参数的效率，通过仿真确定参数的大致范围，再通过实际测试对参数作出微调；其具体执行过程如下：

(4a)在MATLAB中建立了一个在速度恒定的情况下机器人的各个状态对角速度设定值的响应模型；Q和R分别反应了机器人移动的差速度cur_vlr，机器人车体与切线的夹角l_angle，机器人车体中心与切线的距离l_y和控制量对控制品质的贡献大小，因此根据机器人在给定初始状态下的响应结果可以确定参数的调整方向。例如，采用初始化后的参数可以得到图12所示的响应曲线。由结果可知位置偏差收敛的速度较慢，可以适当增加Q矩阵中l_y对应的权值，继续测试。总体原则是，哪个状态收敛较慢，就适当增加该状态在Q矩阵中对应的权值，直到响应指标达到要求为止。图13显示了一种较为理想的阶跃响应，可以看出机器人的位置偏差很快趋于0，且整个过程较稳定。此时相应的参数为该速度对应的准优化参数。

需要指出的是：为了便于计算Riccati微分方程，需要假定线速度为恒定值。当线速度变化时，控制器的最优参数值也会发生变化。为了解决这个问题，需要离线计算若干个线速度对应的最优控制器参数，通过数据拟合估算出其他线速度对应的参数。这种方法工作量较大，但可以达到最好的控制效果。本实施例为了均衡复杂度和控制性能，没有采用这种方法。

(4b)为了便于验证算法的有效性，同时为整定参数提供便利，使用MATLAB GUI构建一个仿真环境：该环境实时显示机器人运动过程中的各个参数值，可以随意暂停，返回上一个状态，并能对历史数据进行显示和分析；

(4c)进行仿真实验：

固定参数PID算法的实验结果如图14至图19所示：

图14至图16是采用固定参数的PID算法时在3组参数下分别得到的仿真路线，图17至图19是对应的角速度曲线，其中横坐标为控制周期数、纵坐标为角速度。在每组参数下，都已将PID参数整定到较合适的值。结果表明，控制周期较短时，该方法比较有效。而当控制周期较长时，即使调整参数，控制效果依然很差。由于机器人的标准控制周期为100ms，该方法理论上不可用。

基于并行分配补偿的模糊算法的实验结果如图20至图23所示：

图20和图21是采用基于并行分配补偿的模糊控制算法时在2组参数下分别得到的仿真路线，图22和图23是对应的角速度曲线。

仿真结果可以充分证明，只要参数合适，该算法就能够取得很好的控制效果；即使在控制周期长的情况下，该算法依然能够对机器人进行有效控制，且整个过程十分平稳；

由仿真实验的结果可以得到如下结论：

（1）固定参数的PID算法控制效果十分有限，不宜采用。

（2）基于并行分配补偿的模糊算法可以取得较好的控制效果。

步骤5：通过实际测试对参数做出微调，其具体执行过程如下：

(5a)将(4c)得到的仿真数据应用到真实实验中；

(5b)根据真实实验，调整个别参数的值；

(5c)重复步骤(5a)～(5b)，直到达到最佳控制效果。

固定参数PID法和基于并行分配补偿的模糊算法实验结果如下：

（1）固定参数的PID算法

固定参数的PID算法在速度很低（0.3m/s左右）的情况下有效，速度稍高时就会不稳定；并且参数的整定比较麻烦。

（2）基于并行分配补偿的模糊算法

采用基于并行分配补偿的模糊算法时，需要调整较多参数，对控制器参数的确定需要做较多实验。为了提高效率，可以先在MATLAB仿真的状态空间模型中确定出准优化参数，然后通过实际测试对参数进行微调。为了便于比较，在实际测试中统一将机器人起始时刻的纵向与引导线平行，机器人的中心与引导线偏离30cm。根据机器人稳定巡线前经历的过渡时间和震荡程度评估控制品质，并对参数作出相应的调整。

在参数较为合理的情况下，机器人会快速稳定地过渡到巡线状态，整个过程没有震荡。当机器人的实际线速度接近参数对应的理想线速度时，该算法的控制效果最优。但是当机器人的实际线速度与参数对应的理想状态偏差较大时，该算法的效果会严重下降。为了解决这个问题，需要针对低、中和高三个档位分别测定控制参数，用数据拟合的方法得到各档位速度对应的控制参数。

固定参数的PID算法：根据机器人当前的状态从图像中选择一行（即机器人前方的某一行），将引导线在该行的位置作为机器人运动的参考点。以该点与行中点的差值E作为偏差，用增量PID公式计算出角速度设定值。计算公式如（3-12）所示：

W_set(t+1)=W_set(t)+Kp*(E(t)-E(t-1))+Ki*E(t)+Kd*(E(t)+E(t-2)-2*E(t-1))（3-12）

该算法的最大特点是不需要知道被控对象的模型，运算简单，缺点是控制效果完全依赖于参数，当被控对象的状态（如线速度、角速度、前方路径）不同时最佳参数往往不同。对于控制目标是使机器人在速度不高的情况下巡线，对控制品质没有太高要求，固定参数PID算法同样适用。

Claims (2)

1.基于视觉信息的机器人沿引导线巡线导航方法，其特征在于，包括如下步骤：

(a)通过机器人上安装的视觉传感器获取环境图像；

(b)从环境图像中获取路径信息，并将获取的路径信息传输到控制器；

(c)控制器根据路径信息设定机器人的线速度和角速度；

在步骤(b)中：首先采用基于最小生成树的分割算法从环境图像中提取路径，然后采用路径搜索算法得到环境图像中的路径点，最后利用三阶透视变换矩阵得到各个路径点对应的地理位置；

在步骤(c)中：控制器首先采用与路径弯曲度相关的速度控制算法对机器人速度大小进行控制，然后控制器再采用固定参数的PID算法或基于并行分配补偿的模糊控制算法对机器人速度方向进行控制；

在步骤(b)中：

采用基于最小生成树的分割算法从环境图像中提取路径的步骤如下：先将环境图像映射到HSV空间，在H域中做进一步处理：先根据引导线的色调值对H域做二值化，之后再对二值化的图像做中值滤波，最后采用基于最小生成树的快速图分割算法将引导线对应的区域分离；对分离后的各个区域的黑点数进行统计，将黑点比例大于一定阈值的区域选定为引导线对应的区域，将其染成黑色；

采用路径搜索算法得到环境图像中的路径点，用以这些路径点为折点的折线段表示路径，步骤如下：第一步：将点的序号num置为1；第二步：从最近一行开始逐行扫描，当在某一行检测到宽度符合引导线宽度范围的黑色区域时，将该区域的中点作为第一个搜索到的路径点，将位置P(num)记录下来；将该点的预测方向pre_th(num)设为90度，状态state(num)记为1，之后进入第二步；如果没有搜索到宽度符合引导线宽度范围的黑色区域，则重复第一步；第三步：根据当前的路径点P(num)判断下一个点是否有可能超出图像范围；若超出则转向第三步，若没有超出，则以P(num)为圆心，以一定像素长度为半径，在预测角度pre_th(num)内进行搜索；若搜索到宽度符合引导线宽度范围的黑色区域，则将黑色区域的中点记为下一个路径点P(num+1)；若搜索到多个路径点，选择最接近预测方向pre_th(num)的路径点，将P(num)到P(num+1)的方向角作为第num+1个路径点的预测角度，并将state(num+1)的状态记为1；若没有搜索到路径点，将预测方向上的搜索点作为第num+1个路径点，将其状态state(num+1)记为0，其预测方向pre_th(num+1)记为pre_th(num)；重复第三步；第四步：搜索完毕；

利用三阶透视变换矩阵得到各个路径点对应的地理位置：将路径点映射到以机器人中心为原点，纵向为Y轴的地面坐标系中；设任意一个路径点P在图像中的坐标是(x,y),在地理坐标系中的坐标是(u,v),则存在一个矩阵mat使得对于任意的P都存在相应的t满足：

(u*t,v*t,t)^T＝mat*(x,y,1)^T         (2-1)，

T是线性代数中矩阵的转置，是一个公知的数学运算符；为了确定透视变换矩阵mat，从图像中选择四个标定点，分别记录下相应的地理坐标；将这四个标定点带入矩阵方程(2-1)，形成一个矩阵方程组，通过该矩阵方程组即可求出mat；当mat矩阵确定以后，对于在图像中搜索出的每个路径点P(num)，都可以用公式(2-1)求出相应的地理坐标GP(num)；将环境图像中的路径用若干个离散点构成的折线表示，设共搜索到point_num个点，这些点对应的地理坐标为GP(num),状态为state(num)。

2.根据权利要求1所述的基于视觉信息的机器人沿引导线巡线导航方法，其特征在于，在步骤(c)中：采用与路径弯曲度相关的速度控制算法对机器人速度大小进行控制的方法如下：连接GP(num)与GP(num+1)的折线段与地理坐标系中x轴正方向的夹角为Gth(num)，则弯曲程度表示为：

$W={\mathrm{\&Sigma;}}_{i=1}^n\left|\mathrm{\&theta;i}\right|---(3-11)$

其中θ1＝Gth(1),θi＝Gth(i)-Gth(i-1)，i≥2；W为角速度，根据W将速度设定值划分为停、低、中和高四个档位；

然后控制器再采用基于并行分配补偿的模糊控制算法对机器人速度方向进行控制：在每个控制周期内，选取前方引导线上的一点，并求出该点处的切线；根据模糊控制算法计算出使机器人尽快逼近该切线的控制量，从而达到使机器人平稳巡线的目的；机器人属于非线性***，用两个线性***的加权来近似；分别利用线性二次型法对这两个线性***设计最优控制器，这两个最优控制器的加权就是最终的控制器；具体包括如下步骤：

步骤1：选取机器人移动的差速度cur_vlr，机器人车体与切线的夹角l_angle以及机器人车体中心与切线的距离l_y三个参数作为状态量；利用线性二次型法抽象出机器人移动的状态方程；其具体执行过程如下：

(1a)机器人角速度的动态响应模型用一阶惯性模型来近似；

(1b)选取机器人移动的差速度cur_vlr，机器人车体与切线的夹角l_angle以及机器人车体中心与切线的距离l_y三个参数作为状态量；

(1c)利用线性二次型法抽象出机器人移动的状态方程，如公式(3-16)至(3-18)所示：

cur_vlr(t+1)＝v_para*u(t)+(1-v_para)*cur_vlr(t)             (3-16)

$l\_\mathrm{angle}(t+1)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)}{\mathrm{width}}*\mathrm{delta}\_t---(3-17)$

$l\_y(t+1)=l\_y\left(t\right)+v*\mathrm{delta}\_t*\sin (l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}})---(3-18)$

其中，v_para是和机器人运动***惯性相关的系数，cur_vlr是左右轮的差速度，v是机器人的线速度，width是车体宽度，delta_t是控制周期；

步骤2：由于机器人的运动模型是非线性的，包含sin函数；根据机器人移动的状态方程，抽象出两个简化的状态空间线性模型来近似；其具体执行过程如下：

(2a)由于机器人的运动模型包含sin函数，所以是非线性的；

设 $p\left(t\right)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}}---(3-19);$

(2b)当p(t)在零附近时，线性模型如公式(3-20)所示：

x(t+1)＝A1*x(t)+B1*u(t)       (3-20)；

(2c)当p(t)在正负pi附近时，线性模型如公式(3-21)所示：

x(t+1)＝A2*x(t)+B2*u(t)        (3-21)；

(2d)公式(3-20)与(3-21)中：

$\text{A1=}\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& V*\mathrm{delta}\_t& 1\end{array}\right)$

B1＝(speed_para 0 0)

$A2=\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{g*V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& g*V*\mathrm{delta}\_t& 1\end{array}\right)$

B2＝(speed_para 0 0)

g＝0.01/pi

(2e)设w1(p(t))、w2(p(t))为p(t)对于两个线性模型的隶属度函数，如公式(3-22)、公式(3-23)所示：

$w1\left(p\left(t\right)\right)=\frac{\sin \left(p\left(t\right)\right)-g*p\left(t\right)}{p\left(t\right)*(1-g)}---(3-22)$

$w2\left(p\left(t\right)\right)=\frac{p\left(\text{t}\right)-\sin \left(p\left(t\right)\right)}{p\left(t\right)*(1-g)}---(3-23)$

则模糊模型如公式(3-24)所示：

x(t+1)＝A(p(t))*x(t)+B(p(t))*u(t)         (3-24)

其中：

$A\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*A1+w2\left(p\left(t\right)\right)*A2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-25)$

$B\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*B1+w2\left(p\left(t\right)\right)*B2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-26)$

当-179.4270<p(t)<179.4270时，该模糊模型精确的表示了非线性模型；

步骤3：针对两个线性模型分别用线性二次型方法设计最优控制器，其具体执行过程如下：

(3a)针对两个线性模型分别用线性二次型方法，得到控制率如公式(3-27)、公式(3-28)所示：

当p(t)在零附近时，

u(t)＝f1*x(t)            (3-27)

当p(t)在正负pi附近时

u(t)＝f2*x(t)            (3-28)；

(3b)最终控制量如公式(3-30)所示：

$u\left(t\right)=\frac{w1\left(p\left(t\right)\right)*f1+w2\left(p\left(t\right)\right)*f2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}*x\left(t\right)---(3-29)$

$W\left(t\right)=-\frac{u\left(t\right)}{\mathrm{width}}---(3-30)$

步骤4：在用线性二次型方法设计最优控制器时，需要给定加权矩阵Q和R，通过离线求解Riccati微分方程得到控制器的参数；Q和R对控制效果的影响非常大；通过仿真确定参数的大致范围；其具体执行过程如下：

(4a)在MATLAB中建立了一个在速度恒定的情况下机器人的各个状态对角速度设定值的响应模型；Q和R分别反应了机器人移动的差速度cur_vlr，机器人车体与切线的夹角l_angle，机器人车体中心与切线的距离l_y和控制量对控制品质的贡献大小；根据机器人在给定初始状态下的响应结果可以确定参数的调整方向；

(4b)使用MATLAB GUI构建一个仿真环境：该环境实时显示机器人运动过程中的各个参数值，可以随意暂停，返回上一个状态，并能对历史数据进行显示和分析；

(4c)进行仿真实验：仿真结果可以充分证明，只要参数合适，该算法就能够取得很好的控制效果；即使在控制周期长的情况下，该算法依然能够对机器人进行有效控制，且整个过程十分平稳；

步骤5：通过实际测试对参数做出微调，其具体执行过程如下：

(5a)将(4c)得到的仿真数据应用到真实实验中；

(5b)根据真实实验，调整个别参数的值；

(5c)重复步骤(5a)～(5b)，直到达到最佳控制效果。