[发明专利]基于视觉信息的机器人沿引导线巡线导航方法

权利要求书

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

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

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

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

2.根据权利要求1所述的基于视觉信息的机器人沿引导线巡线导航方法，其特征在于，在步骤（b）中：首先采用基于最小生成树的分割算法从环境图像中提取路径，然后采用路径搜索算法得到环境图像中的路径点，最后利用三阶透视变换矩阵得到各个路径点对应的地理位置。

3.根据权利要求2所述的基于视觉信息的机器人沿引导线巡线导航方法，其特征在于，在步骤（c）中：控制器首先采用与路径弯曲度相关的速度控制算法对机器人速度大小进行控制，然后控制器再采用固定参数的PID算法或基于并行分配补偿的模糊控制算法对机器人速度方向进行控制。

4.根据权利要求3所述的基于视觉信息的机器人沿引导线巡线导航方法，其特征在于，在步骤（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)。

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

W=Σi=1n|θi|---(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_angle(t+1)=l_angle(t)-cur_vlr(t)width*delta_t---(3-17)]]>

l_y(t+1)=l_y(t)+v*delta_t*sin(l-angle(t)-cur_vlr(t)*delta_t2*width)---(3-18)]]>

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

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

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

设p(t)=l_angle(t)-cur_vlr(t)*delta_t2*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=1-speed_para00-delta_twidth10-V*delta_t22*widthV*delta_t1]]>

B1=(speed_para 0 0)

A2=1-speed_para00-delta_twidth10-g*V*delta_t22*widthg*V*delta_t1]]>

B2=(speed_para 0 0)

g=0.01/pi

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

w1(p(t))=sin(p(t))-g*p(t)p(t)*(1-g)---(3-22)]]>

w2(p(t))=(p(t))-sin(p(t))p(t)*(1-g)---(3-23)]]>

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

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

其中：

A(p(t))=w1(p(t))*A1+w2(p(t))*A2w1(p(t))+w2(p(t))---(3-25)]]>

B(p(t))=w1(p(t))*B1+w2(p(t))*B2w1(p(t))+w2(p(t))---(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(t)=w1(p(t))*f1+w2(p(t))*f2w1(p(t))+w2(p(t))*x(t)---(3-29)]]>

W(t)=-u(t)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)，直到达到最佳控制效果。