一种基于视觉导航移动设备的自适应路径跟踪方法及系统与流程

本发明属于移动设备控制领域，具体涉及一种基于视觉导航移动设备的自适应路径跟踪方法及系统。

背景技术：

随着通信技术、计算机技术、人工智能以及控制理论的不断发展，基于视觉的智能移动设备，尤其是机器人及自动驾驶汽车吸引了大量的关注，并且已成为当前国内外学者研究的热点问题。自动驾驶汽车，作为一种智能移动机器人，其技术主要涉及车辆网络技术，复杂的道路环境识别技术，自动路径规划技术和智能运动控制系统。在自动驾驶系统中，自动巡航控制是一重要功能。

自动巡航控制系统是最常见的技术，它可以帮助高速公路上的人类驾驶员减轻驾驶的负担。高性能的巡航控制系统能够显著改善交通状况，减少交通事故。以转向为主的巡航控制系统，也称为转向驾驶员模型。当前，很多该方面的方法能以相对较高的准确度实现路径跟随，但是高速情形下的小曲率跟踪任务效果较差，难以消除静态跟踪误差。

技术实现要素：

有鉴于此，本发明的目的在于提供一种基于视觉导航移动设备的自适应路径跟踪方法及系统，可解决现有基于视觉的移动设备在高速情况下实现各种曲率路况的高精度路径跟踪效果较差的缺陷。

本发明的一种基于视觉导航移动设备的自适应路径跟踪方法，包括如下步骤：

s1，建立包括自适应预瞄模型和自适应模糊转向控制器的自适应性路径跟踪模型；

s2，通过自适应预瞄模型根据期望路径和速度确定偏航误差及路径曲率：所述自适应预瞄模型包括多点预瞄策略和误差集成反馈模型，所述多点预瞄策略用于提供路径曲率；所述误差集成反馈模型用于提供偏航误差，并将其作为自适应模糊转向控制器的输入；

s3，通过自适应模糊转向控制器根据偏航误差及路径曲率计算视觉导航移动设备的前轮转角，并控制前轮转角：所述自适应模糊转向控制器包括模糊推理系统和自适应转向增益模型，所述自适应转向增益模型用于为模糊推理系统提供输出增益，所述模糊推理系统用于推理出转角输出变量，所述前轮转角为输出增益与转角输出变量的乘积；

s4，采用粒子群优化算法对自适应预瞄模型和自适应模糊转向控制器中的待定系数优化求解。

进一步地，步骤s2中偏航误差由预瞄信息计算获得，所述的预瞄信息包括多个预瞄点到视觉导航移动设备头部朝向上的直线的投影距离及投影线之间面积。

进一步地，所述预瞄信息通过如下步骤获得：

a)根据速度确定预瞄时间，计算方法如下：

式中，v为速度，tp为预瞄时间；

b)根据预瞄时间计算预瞄距离，计算公式如下：

dp＝tp·(ξ+α)·du(2)

式中，dp为预瞄距离，ξ是常数，α为预瞄距离调整因子且为待定系数，du是一维单位预瞄距离矩阵；

c)计算多个预瞄点到视觉导航移动设备头部朝向上的直线的投影距离及投影线之间面积；

d)根据多个预瞄点的位置，采用多点拟合圆法计算路径曲率。

进一步地，步骤d)中多点拟合圆法的计算步骤具体如下：

设定点(a,b)为实际转弯路径的圆心，p(x,y)为预瞄点，可得:

r＝(x-a)²+(y-b)²(3)

其中，r为圆半径；

经过转换，上述方程可写成：

x²+y²+ax+by+c＝0(4)

其中，

基于多个预瞄点坐标，通过lagrange方法和最小二乘法可以得到转弯圆的半径r为：

设定路径曲率为cl，则曲率cl可由如下公式求得：

进一步地，步骤s2中偏航误差包括侧向综合误差和预瞄点投影面积综合误差，所述侧向综合误差的计算公式如下：

e1＝k1·el1+k2·(e′l1+e′l1)+eli(8)

其中，e1为侧向综合误差，el1为第一个预瞄点的侧向误差，eli为剩余预瞄点其中一个的侧向误差，e′l1是第一个预瞄点上一采样时刻的侧向误差，e″l1为上上时刻第一个预瞄的侧向误差，k1与k2为待定系数；

所述预瞄点投影面积综合误差的计算公式如下：

e2＝k3·esi+k4·es(i+1)(9)

其中，e2为预瞄点投影面积综合误差，esi为第i-1个点和第i个点之间的投影面积误差，es(i+1)为第i个点和第i+1个点之间的投影面积误差，k3和k4为待定系数。

进一步地，步骤s3中模糊推理系统通过如下过程实现转角输出变量的输出：

s31，将侧向综合误差和预瞄点投影面积综合误差由清晰数值转化为模糊变量；

s32，通过如下模糊推理规则获得输出的模糊量：

1)由于左右转向的对称性，误差输入及控制输出相对称；

2)侧向综合误差和预瞄点投影面积综合误差的绝对值越小，控制输出绝对值也越小；

3)输入和输出呈反关系；

s33，通过解模糊将模糊推理结果清晰化，得到清晰化的转角输出变量。

进一步地，步骤s3中自适应转向增益模型用于为模糊推理系统提供的输出增益的计算公式如下：

f＝f1+f2+f3(10)

式中，f为输出增益，f1是常数，f2是速度的函数，f3是关于路径曲率的函数，其中：

f2通过多项式拟合获得，如下所示：

f2＝η1·v+η2·v²+η3·v³(11)

式中，η1、η2与η3为待定系数；

f3建模如下：

f3＝η4·(cl·k-m)(12)

式中，k和m为常数，η4为待定参数。

进一步地，预瞄距离调整因子α，待定系数k1与k2，待定系数k3与k4，待定系数η1、η2、η3和η4均通过粒子群优化算法求解。

进一步地，通过粒子群优化算法求解α、k1、k2、k3、k4和η1、η2、η3、η4具体步骤如下：

步骤1，选择carsim&simulink联合仿真平台；

步骤2，设置多条经典路径，涉及多种转弯半径的道路；

步骤3，设置多种模拟测试速度；

步骤4，将所有路径及所有测试速度下的跟踪误差作为评估指标；

步骤5，设置迭代回合次数和种群规模，每组解包含9个参数，所有参数的范围可以设置为[0,10]进行优化求解；

步骤6，进行实物测试。

本发明的另一个方面，还提供一种基于视觉导航移动设备的自适应路径跟踪系统，包括自适应预瞄模型和自适应模糊转向控制器：所述自适应预瞄模型包括多点预瞄策略和误差集成反馈模型，所述多点预瞄策略用于提供路径曲率；所述误差集成反馈模型用于提供偏航误差，并将其作为自适应模糊转向控制器的输入；所述自适应模糊转向控制器包括模糊推理系统和自适应转向增益模型，所述自适应转向增益模型用于为模糊推理系统提供输出增益，所述模糊推理系统用于推理出转角输出变量，所述前轮转角为输出增益与转角输出变量的乘积。

故此，本发明基于视觉导航移动设备的自适应路径跟踪方法及系统包括自适应预瞄模型和自适应模糊转向控制器：自适应预瞄模型能够提供偏航误差及期望路径曲率，自适应模糊转向控制器可根据偏航误差及路径曲率自适应控制前轮转角，具体地，自适应模糊转向控制器包括模糊推理系统和自适应转向增益模型，自适应转向增益模型用于为模糊推理系统提供输出增益，模糊推理系统用于推理出转角输出变量，前轮转角为输出增益与转角输出变量的乘积。本发明从预瞄方法、控制增益两个方面对传统的预瞄跟随算法的自适应能力进行改进，显著提升移动设备的自适应跟踪能力，改善其转向操作稳定性，为视觉导航移动设备实现高精度巡航驾驶、提高高速道路交通安全性。此外，上述基于视觉导航移动设备可为移动机器人、自动驾驶汽车等，适用范围亦较广，具有广阔的市场前景和应用价值。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明一种基于视觉导航移动设备的自适应路径跟踪方法一实施例的流程图；

图2为本发明一种基于视觉导航移动设备的自适应路径跟踪系统的设计框图；

图3是本发明多点拟合圆法求取曲率的示意图(1为圆心，2为拟合的圆，3为期望路径)；

图4-a为本发明模糊推理系统输入侧向综合误差e1的模糊集；

图4-b为本发明模糊推理系统输入预瞄点投影面积综合误差e2的模糊集；

图4-c为本发明模糊推理系统输出前轮转角δ的模糊集；

图5为本发明模糊推理规则表；

图6为本发明模糊推理面；

图7为本发明控制输出计算实施流程图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

需要说明的是，为更好地理解本发明，特对如下部分进行解释：

lagrange方法(拉格朗日乘数法)，是一种寻找变量受一个或多个条件所限制的多元函数的极值的方法，用于解决最优问题；

carsim&simulink联合仿真：carsim是专门针对车辆动力学的仿真软件，simulink是用来建模、仿真和分析动态多维系统的交互工具，两者联合模拟真实、可操控性强。

图1是本发明一实施例的基于视觉导航移动设备的自适应路径跟踪方法的流程图。如图1所示，该实施例的自适应路径跟踪方法包括如下步骤：

s1，建立包括自适应预瞄模型和自适应模糊转向控制器的自适应性路径跟踪模型。

s2，通过自适应预瞄模型根据期望路径和速度确定偏航误差及路径曲率：所述自适应预瞄模型包括多点预瞄策略和误差集成反馈模型，所述多点预瞄策略用于提供路径曲率；所述误差集成反馈模型用于提供偏航误差，并将其作为自适应模糊转向控制器的输入。

优选地，上述偏航误差由预瞄信息计算获得，该预瞄信息包括多个预瞄点到视觉导航移动设备头部朝向上的直线的投影距离及投影线之间面积，具体地，预瞄信息通过如下步骤获得：

a)根据速度确定预瞄时间，计算方法如下：

式中，v为速度，tp为预瞄时间。

需要说明的是，根据预瞄时间tp的建模公式可知，其取值随着速度v变化在0.5～1.5之间变化，当基于视觉导航移动设备运动速度v较高时，预瞄时间tp较长，反之较短。因此，提高了预瞄的自适应性。

b)根据预瞄时间计算预瞄距离，计算公式如下：

dp＝tp·(ξ+α)·du(2)

式中，dp为预瞄距离，ξ是常数，α为预瞄距离调整因子且为待定系数，它能修正预瞄时间tp的建模偏差，du是一维单位预瞄距离矩阵。

以n为预瞄点个数，取n＝9，n个点的的预瞄控制中，du被设为：[0,3,6,9,12,15,18,21,24]，dp则表示九个预瞄点的距离，dp和du的单位均是米。通过预瞄距离dp建模可知，dp具有人类驾驶的特点，当速度v较高时，看得更远；当速度v较低时，看得更近，使得系统具备智能性。

c)计算多个预瞄点到视觉导航移动设备头部朝向上的直线的投影距离及投影线之间面积。

需要说明的是，该部分(直线的投影距离及投影线之间面积)可通过视觉导航移动设备上传感器反馈的预瞄点坐标求得，或是由传感器直接提供。

d)根据多个预瞄点的位置，采用多点拟合圆法计算路径曲率。

图3即为多点拟合圆求取曲率的示意图。如图3所示，设有定点(a,b)为转弯路径的圆心，p(x,y)为预瞄点，圆的半径的半径r，上述多点拟合圆法求取曲率的计算方法如下：

设定点(a,b)为转弯路径的圆心，p(x,y)为预瞄点，可得:

r＝(x-a)²+(y-b)²(3)

其中，r为圆的半径；

经过转换，上述方程可写成，

x²+y²+ax+by+c＝0(4)

其中，

基于多个预瞄点坐标，通过lagrange方法和最小二乘法可以得到转弯圆的半径r为：

设定路径曲率为cl，曲率cl为圆半径r的倒数，则曲率cl可由如下公式求得：

上述多点拟合圆法计算方法简洁高效，满足视觉导航移动设备的实时性要求。

在进一步的技术方案中，偏航误差包括侧向综合误差和预瞄点投影面积综合误差，侧向综合误差的计算公式如下：

e1＝k1·el1+k2·(e′l1+e″l1)+eli(8)

其中，e1为侧向综合误差，el1为第一个预瞄点(t时刻距离为0m的预瞄点)的侧向误差，eli为剩余余预瞄点其中一个的侧向误差，e′l1是第一个预瞄点上一采样时刻的侧向误差，e″l1为上上时刻第一个预瞄的侧向误差，k1与k2为待定系数；

上述公式主要采用了两个预瞄点的信息获得。由于目前已有理论证明远近两点预瞄的控制效果和多点预瞄的控制效果一致，因此，在综合误差建模部分，考虑计算的简便性而取两点计算。

上述建模公式考虑了0米处的误差及其两个采样周期的积分量，能够有效克服跟踪的静态偏差，克服传统预瞄算法存在的控制滞后、静态偏差难以消除的难题。

预瞄点投影面积综合误差的计算公式如下：

e2＝k3·esi+k4·es(i+1)(9)

其中，e2为预瞄点投影面积综合误差，esi为第i-1个点和第i个点之间的投影面积误差，es(i+1)为第i个点和第i+1个点之间的投影面积误差，当i等于1时，esi取0，k3和k4为待定系数。

s3，通过自适应模糊转向控制器根据偏航误差及路径曲率计算视觉导航移动设备的前轮转角，并控制前轮转角：所述自适应模糊转向控制器包括模糊推理系统和自适应转向增益模型，所述自适应转向增益模型用于为模糊推理系统提供输出增益，所述模糊推理系统用于推理出转角输出变量，所述前轮转角为输出增益与转角输出变量的乘积。

优选地，步骤s3中模糊推理系统通过如下过程实现转角输出变量的输出：

s31，将侧向综合误差e1和预瞄点投影面积综合误差e2由清晰数值转化为模糊变量。

需要说明的是，以上两个变量e1和e2的模糊集均有7个模糊隶属度函数构成，7个模糊隶属度函数表示“负大”、“负中”、“负小”、“零”、“正小”、“正中”、“正大”，它们分为对应着：“nb”,“nm”,“ns”，“zo”，“ps”，“pm”，“pb”，具体如如图4-a、4-b、4-c所示。其中，为了提高控制器对微小偏差的敏感度，e1中的隶属度函数稍微往中间靠，其他两个变量只需要常规设置。此外，e1和e2在模糊化之前，先被映射到[-4，4]之间。

s32，通过如下模糊推理规则获得输出的模糊量：

1)由于左右转向的对称性，误差输入及控制输出相对称；

2)侧向综合误差e1和预瞄点投影面积综合误差e2的绝对值越小，控制输出绝对值也越小；

1)输入和输出呈反关系；如输入为负，则输出为正，否则反之。

图5即为建立的模糊推理规则表，得到的模糊推里面如图6所示。需要说明的是，上述模糊推理规则主要依照专家经验建立。

s33，通过解模糊将模糊推理结果清晰化，得到清晰化的转角输出变量δ‘。

需要说明的是，在本步骤解模糊过程用到输出模糊集。该输出模糊集的构建和输入模糊集类似。δ’解模糊之后的范围优选为[-3,3]。

此外，值得提及的是，步骤s3中自适应转向增益模型用于为模糊推理系统提供的输出增益的计算公式如下：

f＝f1+f2+f3(10)

式中，f为输出增益，f1是常数，f2是速度的函数，f3是关于路径曲率的函数，其中：

f2通过多项式拟合获得，由于速度v越快，转向操作应越加灵敏。在此，采用多项式拟合的方式对f2进行建模，其中权衡建模精度和实时性，取前三项，如下所示：

f2＝η1·v+η2·v²+η3·v³(11)

式中，η1、η2与η3为待定系数；

对于f3的建模，考虑路径存在往左和往右两种情形，f3的取值应该在某个值的周围变化，f3建模如下：

f3＝η4·(cl·k-m)(12)

式中，k和m为常数，η4为待定参数，需说明的是，k为放大系数，由于路径的曲率往往非常小，应适度放大，m用于偏置作用。

故此，依据步骤s3中提及的前轮转角为输出增益与转角输出变量的乘积，用公式表达则为：

δ＝f*δ′(13)

式中，δ为前轮转角，即为控制器最后的控制输出。

s4，采用粒子群优化算法对自适应预瞄模型和自适应模糊转向控制器中的待定系数优化求解。

具体地，前述预瞄距离调整因子α，待定系数k1与k2，待定系数k3与k4，待定系数η1、η2、η3和η4均通过粒子群优化算法求解。通过步骤s4可进一步提升视觉导航移动设备的自适应跟踪能力。

优选地，通过粒子群优化算法求解α、k1、k2、k3、k4和η1、η2、η3、η4具体步骤如下：

步骤1，选择carsim&simulink联合仿真平台；

步骤2，设置多条经典路径，涉及多种转弯半径的道路，如半径可选250m、300m和350m；

步骤3，设置多种模拟测试速度，如以100km/h、110km/h、120km/h作为测试速度；

步骤4，将所有路径及所有测试速度下的跟踪误差作为评估指标；

步骤5，设置迭代回合次数和种群规模，每组解包含9个参数，所有参数的范围可以设置为[0,10]进行优化求解；

需要说明的是，本实施例中迭代回合次数可选1000，种群规模可选100；

步骤6，进行实物测试。

从上可知：本发明一种基于视觉导航移动设备的自适应路径跟踪方法整体实施可等同为两部分：第一部分为计算控制输出，包括步骤s1、s2和s3，具体流程如图7所示；第二部分为待定系数优化求解，为步骤s4。本发明从预瞄方法、控制增益两个方面对传统的预瞄跟随算法的自适应能力进行改进，显著提升移动设备的自适应跟踪能力，改善其转向操作稳定性，为移动设备实现高精度巡航驾驶、提高高速道路交通安全性，具有广阔的市场前景和应用价值。

另一方面，如图2所示，本发明还提供一种基于视觉导航移动设备的自适应路径跟踪系统，包括自适应预瞄模型和自适应模糊转向控制器：所述自适应预瞄模型包括多点预瞄策略和误差集成反馈模型，所述多点预瞄策略用于提供路径曲率；所述误差集成反馈模型用于提供偏航误差，并将其作为自适应模糊转向控制器的输入；所述自适应模糊转向控制器包括模糊推理系统和自适应转向增益模型，所述自适应转向增益模型用于为模糊推理系统提供输出增益，所述模糊推理系统用于推理出转角输出变量，所述前轮转角为输出增益与转角输出变量的乘积。

上述基于视觉导航移动设备的自适应路径跟踪系统包括自适应预瞄模型和自适应模糊转向控制器：自适应预瞄模型能够提供偏航误差及路径曲率；自适应模糊转向控制器可根据偏航误差及路径曲率自适应控制前轮转角，显然具有显著提升视觉导航移动设备的自适应跟踪能力，此处不再赘述。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。