基于改进遗传算法的足球机器人自定位方法*

吴晓

(福州大学机械工程与自动化学院，福建福州350108)

机器人自定位是移动机器人研究中的一个重要课题，是智能移动机器人实现导航、运动规划等自主能力的基础，其任务是在具有地图的环境中，按一定的边界条件，寻找机器人在起始状态(包括位置及姿态)和到达目标状态(包括位置和姿态)时，机器人本身在世界绝对坐标中的位置和姿态［1］.

遗传算法(GA)模仿生物遗传及进化的步骤，引入选择、复制、交叉重组和变异等方法，并将进化论中适者生存的概念引入到算法中，可用于解决诸如控制及函数的优化、机器学习等方面的问题.微观上，GA是一种随机算法;宏观上，它又具有一定的方向性.因此，GA不同于一般的随机算法，它所使用的随机选择是有方向性的，是一个具有定向制导的随机搜索技术.正因为它的方向性，使得它比一般的随机搜索算法的效率要高［2］.

全自主机器人足球比赛涉及到诸多关键技术，如自定位技术、多任务实时控制技术、实时视觉技术、多机器人协同技术、路径规划技术等［3］.其中，机器人自定位问题要求实时性、精确性和鲁棒性均要好，是机器人自主化和智能化研究中最为重要的基本问题之一.常用的自定位方法有4类:提取黄、蓝球门、立柱等地标点，实现几何三角定位，由于新规则去掉了黄、兰门等，现在此方法已不能使用;使用Hough变换等方法提取场地白色标志线并使用球门、立柱信息判断直线归属，实现几何定位.如Luca［4］提出的利用视觉、声纳、红外或激光等提供的距离信息，通过Hough变换提取直线再与场地模板比较以确定机器人方位的方法;粒子滤波定位方法，也称为蒙特卡罗定位方法，如 Menegatti等［5］提出的利用机器人到最近颜色跃变点的距离来对机器人位姿进行估计的粒子滤波算法;基于匹配优化的自定位方法［6］.

以上方法均在过去的机器人比赛中发挥过重要作用，但存在诸如鲁棒性不强、实时性不足、实现效率不高、解决不了绑架问题等缺陷［7］.为了更有效解决足球机器人的自定位、绑架和跟踪问题，文中提出一种基于改进遗传算法的机器人自定位方法.

1 基于改进GA的机器人自定位方法

遗传算法是一种全局搜索方法，容易解决绑架问题，同时是一种并行搜索不易陷入局部极值、收敛速度快、处理时间相对较少的算法，具有可扩展性.因而遗传算法的鲁棒性强，高效，全面［8］.基于改进遗传算法的移动机器人自定位算法步骤如下:

步骤1 初始化进化代数，t=0.

步骤2 生成初始种群.

步骤3 计算t代目标函数.

步骤4 选择并判断代沟是否小于某设定值，是则交叉，变异;否则重新选择.

步骤5 对种群数量进行微分，自适应改变变异概率;t=t+1.

步骤6 判断总代数，达到总代数则继续步骤7;否则返回步骤3.

步骤7 估计k时刻机器人位姿，并判断是否达到轨迹终点，未到则进行步骤8;否则结束.

步骤8 根据位姿跟踪算法进行跟踪，得到机器人下一位姿，即下一初始种群，并返回步骤3.

根据机器人比赛运动的实时性、鲁棒性要求高，准确性相对要求低的特点，只对遗传算法的并行搜索、快速收敛和种群优化等方面进行研究.

1.1 编码

中型组机器人的全向视觉全景图像是场景通过组合镜面反射到摄像机得到的.组合镜面由不同形状的镜面组合而成，各部分镜面分别用于满足机器人对于相对自身不同方位物体的成像要求.根据足球机器人比赛对视觉系统的要求，文中使用的镜面由内至外分别为水平等比镜面、常值斜率镜面和平面镜面.其中等比镜面能使得图像像素间长度和现实场景中对应点间的长度关系是等比例的，即保持分辨率不变，它能保证6.5 m范围内水平场地的物体成像是等比例变化的;常值斜率和平面镜面分别能看到6.5m范围外的景象，但图像会产生畸变［9］.

实际坐标和全局坐标的转化以自定位为中心，定位不成功将导致转化失去原有的意义.在编码之前，首先要确定3个坐标系统:图像坐标系I(u，v)、机器人坐标系R(x，y，z)和世界坐标系W(X，Y，Z).由于足球机器人用的是全向视觉系统，此系统是由摄像机与等比例反射镜构成的，其观测范围是360°，在6.5m半径范围内符合针孔成像模型，所以其坐标变换为

式中，R为旋转矩阵，T为平移矩阵.另外，由于全向视觉在同一平面上的模型为二维视觉，所以式(1)可以简化为

则

式中:s为一比例因子;au为u轴上的尺度因子，av为v轴上的尺度因子，(u0，v0)为图像中心点坐标.(XW，YW，φ)为机器人中心在世界坐标系中的坐标，即机器人自定位坐标;Wj表示第j个观测点的世界坐标，观测点在这里特指白线上的特征点(例如1、2、3)，如图1所示;(uj，vj)为第j个观测点的图像坐标;(Xj，Yj)为第j个观测点的世界坐标.由式(2)可知，机器人自定位参数确定后，直接可根据图像坐标求出目标的世界坐标.

这里必须特别说明的是:尽管6.5 m以外产生的图像是畸变的，但当图像与模型地图图1(b)比对时，是不需要“校正”的.这是因为目标函数是求每个“白线点”与模型线间距离总和的最小值，即距离总和小于一常数，所以图像无需校正，经多次迭代后误差最小，坐标接近真实值.因此，式(1)和(2)的坐标变换无需校正.

如图1(a)所示，场地尺寸为12m×8m，按50mm× 50mm栅格的分辨率，分球场模型为24×16个栅格，其中白色标识线含17条直线段和5个圆弧.通过机器人中心发出的辐射线与白线的交点即白线点j，将其作为与模型对应的匹配点.取这些白线点的判据是沿辐射线方向检测到的颜色按绿－白－绿的次序转换［10］.

用机器人自定位坐标(XW，YW，φ)的二进制数作为染色体进行编码，则种群表示为，，i=1，2，…，N，其中N为种群规模，个体编码为二进制数，如(111100，110010，100000)，表示机器人绝对坐标，解码后为(600 mm，500 mm，20°);设编码长度为m，则编码范围为［0，2m－1］.

1.2 适应度函数的建立

用dmin，j表示白线点j到场地模型线(共22根)中最近那根线的距离，所有点的最小距离之和为

式中:j=1，2，…，n，n为白线点数 (;Xi，Yi，φi)j表示第i个个体对应第j个观测点的绝对坐标，可由式(2)根据第i个个体的坐标及全景摄像机的内部参数求得 (;Xi，Yi，φi)jreal表示第j个观测点所对应地λ表示位于球场外的白线点数，μ表示白线点位于球场外的平均距离误差，取50cm.这样，该计算方法可补偿噪声及场地外白线点的影响［11］.图白线上最近点的绝对坐标.适应度函数为

1.3 初始种群的生成

在对机器人的位姿完全未知或机器人被绑架的情况下，使种群均匀分布在栅格地图上;否则根据先验知识给定种群初值，并以已知个别值为中心，按高斯分布给定其它个体的初值.在同一地图下，种群规模N太大则实时性不强，太小则精度不够，对于种群规模N的选择，必须根据定位精度和算法实时性两方面的要求进行折衷考虑.通常情况下，种群规模应与地图的大小成正比，否则种群的匮乏易导致定位失败［12］.合理的选择为:全景视觉图像与地图尺寸成比例的范围为6 m×6 m，按0.05 m×0.05 m为一个单元栅格，一个单元栅格为一个种群，则有6m×6m÷(0.05m×0.05m)=14400个种群，一般初始种群数为总种群数的百分之一，即N取140.

1.4 遗传操作设计

1.4.1 选择

遗传算法的主要操作为选择、交叉、变异和终止条件的设定等.选择的目的是为了从当前群中选出优良的个体，使它们有机会作为父代为下一代繁殖子孙.选择算子不当会增加群体中相近个体的数量，使得子代个体与父代个体相近，遗传失去多样性，产生早熟，导致进化停止.轮盘赌是一种回放式随机采样方法，应用比较广泛，但由于群体规模有限和随机操作等原因，使得这种选择方法误差比较大，有时甚至连适应度较高的个体也选不上.为防止过早收敛，保持群体多样性，算法的实现基于轮盘赌方法，剔除代沟最大的个体，具体选择流程如下:

步骤1 随机产生N个群体;

步骤2 采用轮盘赌的方法进行选择操作，选出优良子代群体;

步骤3 从子代群体中随机选取两个个体i1、i2作为父代;

步骤4 将i1、i2均匀交叉产生子群;

步骤5 计算代沟是否最大，如果最大则转步骤2，否则选择操作成功.

1.4.2 交叉

交叉算子是GA中最重要的操作算子之一，实际运行时根据交叉概率Pc进行下一代更新.Pc选得太大容易失去优良的个体，选得大小会降低搜索效率.针对上述问题，文中采用自适应分段概率与均匀交叉相结合的方法进行交叉操作，如式(3)所示，即按适应度值的大小进行排序更新:F1＜F2＜…＜FN，取中间值F3N/4作为分界点，小于F3N/4的适应度个体选用较大的交叉概率，大于F3N/4的适应度个体选用较小的交叉概率.这样，Pc能够随着目标函数适应度值的变化而自动调整，使目标函数适应度值大的个体取较小的Pc，目标函数适应度值小的个体取较大的Pc，从而使适应度值大的优良个体得到保存，而使适应度值小的个体被淘汰.同时适应度高的个体采用单点交叉，适应度低的采用均匀交叉.

式中，Fmax(t)为最大适应度.

De Jong等［13］已经证明，采用均匀交叉的方法能够扩大遗传算子的搜索子空间，使收敛结果更趋于最优解.

均匀交叉会产生一个随机的掩码与父代个体等长.若P1、P2表示两个父代，S1、S2表示两个子代，M表示掩码，例如，对于S1，当掩码中的位为1时，S1的对应位为P2的对应位，S2的对应位为P1的对应位;当掩码中的位为0时，S1的对应位为P1的对应位，S2的对应位为P2的对应位，即掩码为1则交叉，为0则不变.例如:

1.4.3 变异

变异操作主要用来改善遗传算法的局部搜索能力和维持群体的多样性，防止出现早熟现象.自适应有效基因突变的特点是最低有效基因个数自适应变化，且自适应调整变异概率Pm.Srinivas等［14］提出根据每代个体适应度的改变，自适应地改变Pc和Pm，在保护最优个体的同时，加快了较差个体的淘汰速率.

式(4)中，采用最大适应度Fmax、最小适应度Fmin、适应度平均值Fave来衡量群体适应度的集中程度，自适应地改变整个群体的Pm.其中，Fmin和Fmax的接近程度反映了整个群体的集中程度，二者越接近，遗传算法越可能陷于局部最优解，即越可能早熟.ε1、ε2为大于零的常数，σ2(t)为个体适应度的方差.

采用如下的自适应原则改变Pm:当机器人受到绑架时，调整概率的突变.实验表明，当机器人发生绑架时，种群的个体适应度会急剧下跌，即当 Fave时，自动增加Pm(t+1)值.当陷入早熟时，即σ2(t)≤ε2时，自动调整Pm.除上述情况外，Pm(t+1)=Pm(t).

1.5 控制参数的选择

遗传算法控制参数的选择十分关键，选取不同的控制参数会影响算法的性能和整个算法的收敛性.这些参数包括群体规模N、选择常数q、二进制编码长度、变异概率Pm、交叉概率Pc等.

交叉概率大可使各代充分交叉，但群体中的优良模式也易遭到破坏，产生代沟，使搜索走向随机化;交叉概率太小，复制增加搜索会陷入停滞状态.变异算子增加了群体的多样性，防止遗传算法过早收敛到局部最优.但变异概率过大时，搜索会产生随机化;过小时，一旦陷入局部极值则很难跳出来，易产生未成熟收敛.

交叉运算是GA区别于其它进化运算的重要特征，是产生新个体的主要方法，属全局搜索;而变异运算是GA产生新生个体的辅助方法，属局部搜索.交叉概率Pc一般选0.40～0.99;变异概率Pm一般选0.0001～0.1000.

群体规模的大小直接影响到GA的收敛性和计算效率，规模过大则搜索效率会降低，规模过小则容易收敛到局部最优解.根据实际情况，群体规模一般选择10～200.

1.6 位姿跟踪

在进行若干代更新后，个体收敛到最佳值，可以求得机器人的自定位世界坐标值，但为了增加自定位坐标的鲁棒性，这里采用加权平均的方法，即对最佳个体的前3名求平均值，将其作为机器人的估计坐标:

式中，i'表示按自适应度排序后的个体序号，k'表示进化结束的时刻.

由于机器人是连续定位的，在k+1时刻的自定位可以通过重复k时刻的步骤求得，也可以在k时刻自定位的基础上，利用其位姿的估计信息结合里程计运动信息和观测信息的导数求得:

式中:ΔXW、ΔYW和Δφ为选取的步长;观测点的绝对坐标为(X，Y)，第j个白线点的绝对坐标Wj可直接由式(2)求得，那么观测点与真实点的距离误差为f(X，Y).按Prewitt算子计算梯度，则

其中，

另外，假设观测数据符合高斯分布［15］.

图1 足球场地及坐标Fig.1 Soccer field and coordinate

2 仿真与实验结果分析

2.1 仿真结果与分析

试验的软、硬件条件为:Window XP操作系统平台，VC++6.0环境下编程;酷睿双核笔记本电脑.

为验证改进遗传算法的有效性，通过两组仿真的白线点数据进行算法验证，主要过程是机器人经历从绿(图2(a)下部点)到红(图2(a)上部点)的移动过程，直接用给定的白线点坐标代替全景白线点数据，为了表达的真实性，在给定的白线点坐标中加入观测噪声，其符合正态分布N(0，0.02(x，y))，步长为(0.05，0.05，0.1).图2(a)所示为预先设定的规则辐射线与白线图像框的交点(白线点)，若机器人的自定位坐标设定为(0，0，0)、(－0.1，0.15，0.05)、(－0.5，8，μ/6)，则可直接求出各白线点的相对位置坐标.反过来以各白线点的相对位置坐标为参数，用上述遗传算法迭代求出机器人的自定位坐标，分析与设定坐标的误差，并与蒙特卡罗定位法比较，过程为由绿点到红点.然后再按图2(b)所示，以随机选取的已知白线点的相对坐标为参数，用遗传算法与蒙特卡罗定位法再次比较，过程为黄点－(图2(b)下部点)到白线点(图2(b)上部点).

图2 实验数据示意图Fig.2 Diagram of experimental data

采用全局搜索，种群数为50，初始值为与视觉图像成比例的6.5m范围内的方位值，选择参数q= 0.5，步数为50，初始交叉概率为0.8，初始变异概率为0.07，终止条件为适应度函数值大于95%.当迭代次数大于总次数的25%而适应度函数差分小于10%时，个体值置位重新计算，以避免进入局部最优.结果如表1、2所示，其中a为一组具备直线特征的数据，b为一组模拟的随机数据.从表1中可知，采用遗传算法连续自定位的误差远小于采用蒙特卡罗算法产生的误差，其中随机白线点产生的自定位误差又小于辐射线上白线点的自定位产生的误差.

表1 自定位实验数据Table 1 Test results of self-localization %

表2 绑架实验数据Table 2 Test results of kidnapping %

在绑架仿真过程中，突然改变机器人坐标所对应的白线点数据，相当于在实战中的机器人被绑架.从表2中可以看出，绑架后的自定位误差比连续自定位的误差有所增加，但通过连续迭代趋于收敛，完全能够满足机器人的自定位需求.

由于蒙特卡罗算法与遗传算法在实际操作中都是以统计概率或随机数据为依据，所以随机参数取得的白线点自定位总是更有效，但在机器人比赛实践中，随机白线点是无法求得的，只有求辐射线上的白线点或有规律的白线点，才有可能进行应用操作.

2.2 实验结果与分析

针对上述遗传算法，采用地图为12 m×8 m的标准足球比赛实验场地进行足球机器人实验验证.为了提高算法的实时性，以50mm×50 mm为一单元栅格，分地图为24×16个单元栅格.遗传算法的种群初始化、选择、交叉、变异操作及适应度的计算等步骤均基于栅格地图进行.

为说明算法的性能，比较了相同参数下采用传统遗传算法和改进遗传算法进行分析的结果.算法的参数为:初始化种群大小为50，q=0.5，Pc=0.8，Pm=0.01.经过80代进化后最小误差变化曲线见图3.由图3可看出，改进遗传算法的最小误差变化曲线收敛得更快，求解自定位坐标和寻优效率均优于传统遗传算法，更有利于产生最优解.

图3 最小误差变化曲线Fig.3 Change curves of minimum error

在实验中，假设机器人初始位姿为绝对坐标原点(图2中“O”)，在机器人自由运动后能够实现全局定位.为了模拟“绑架”现象，当机器人运行到第40步时，将被人为转移到一个未知位姿，前后位姿之间的实际变化量为(－1.3m，+4m，μ/6).这就要求机器人能够发现“绑架”现象，并能从中恢复，正确实现全局重定位.“绑架”发生后机器人继续进行正常比赛.改进的遗传算法与蒙特卡罗算法的结果比较见图4.从图4中可看出，改进的遗传算法在起步阶段有较快的收敛速度，在11步内实现机器人的快速全局定位;在第40步遭遇机器人“绑架”时，该算法也能够迅速收敛，快速实现机器人的重定位;在定位过程的其它阶段，算法具有较高的平均位姿跟踪精度，其平均跟踪误差约为(0.046m，0.22°);且改进的遗传算法的定位精度较高.

图4 定位误差曲线Fig.4 Curves of localization errors

由上述结果可以看出:(1)改进遗传算法与传统遗传算法相比，其对目标函数的收敛速度快，鲁棒性好，同步数目标误差更小;(2)改进遗传算法与蒙特卡罗算法相比，收敛速度快，平稳无震荡且定位误差更小，且在被人为绑架后能够更快速地自定位; (3)仿真辐射线、绑架辐射线自定位精度分别达到98.74%和96.58%，仿真随机白线点、绑架随机白线点自定位精度分别达98.98%和96.92%.(4)与文献［6］、［15］中结果相比，其多组实验自定位平均误差基本在0.059～0.390m和2.5°～4.2°，而文中平均跟踪误差约为(0.046m，0.22°).

3 结语

文中针对足球机器人在比赛过程中的自定位问题进行了分析.利用改进遗传算法，以白线点为参数进行自定位坐标的求取，通过仿真实验与实地验证，发现采用改进遗传算法进行全局自定位高效、鲁棒，局部梯度搜索快速、精确;采用改进的遗传算法进行移动机器人的自定位收敛速度快，11次迭代便达到了较高的定位精度;在足球场地实验中，当人为绑架机器人后，能够在11步之内快速实现重新自定位，从而证明了该算法在连续定位和绑架情况下的自定位均能满足足球机器人比赛的要求，为机器人的自定位提供了一种新方法.

［1］ Lu H M，Zhang H，Xiao J H，et al.Arbitrary ball recognition based on omni-directional vision for soccer robots［C］∥RoboCup 2008:Robot Soccer World Cup XII.Berlin:Springer-Verlag，2009:133-144.

［2］ Chang P，Chen S H，Liu C H.Sub-population genetic algorithm with mining gene structures for flow shop scheduling problems［J］.Expert Systems with Applications，2007，33 (3):762-771.

［3］ Melville P，Mooner J.Creating diversity in ensembles using artificial data［J］.Information Fusion，2005，6(1): 99-111.

［4］ Luca Iocchi.Robust color segmentation through adaptive color distribution transformation［C］∥Advances in Cryptology Crypto 91.Berlin-Heidelberg:Springer-Verlag，2006: 287-295.

［5］ Menegatti E，Pretto A，Scarpa A，et al.Omnidirectional vision scan matching for robot localization in dynamic environments［J］.IEEE Transactions on Robotics，2006，22 (3):523-535.

［6］ 卢惠民，张辉，杨绍武，等.一种鲁棒的基于全向视觉的足球机器人自定位方法［J］.机器人，2010，32(4): 553-559.Lu Hui-min，Zhang Hui，Yang Shao-wu，et al.A robust self-localization method based on omnidirectional vision for soccer robots［J］.Robot，2010，32(4):553-559.

［7］ 卢惠民，刘斐，郑志强.一种新的用于足球机器人的全向视觉系统［J］.中国图象图形学报，2007，12(7): 1243-1248.Lu Hui-min，Liu Fei，Zheng Zhi-qiang.A novel omni-vision system for soccer robots［J］.Journal of Image and Graphics，2007，12(7):1243-1248.

［8］ 刘建立，左保齐.基于遗传算法的织物印花图案的分割［J］.计算机工程与设计，2008，29(15):3966-3967.Liu Jian-li，Zuo Bao-qi.Segmentation of pattern of printed fabric based on genetic algorithm［J］.Computer Engineering and Design，2008，29(15):3966-3967.

［9］ 刘伟.RoboCup中型组机器人全景视觉系统设计与实现［D］.长沙:国防科技大学机电工程与自动化学院，2004.

［10］ 吴晓，曹其新.基于权值模板匹配算法的全自主足球机器人目标识别［J］.厦门大学学报:自然科学版，2008，47(6):819-822.Wu Xiao，Cao Qi-xin.Target Recognition of an autonomous robot soccer based on weight template matching algorithm［J］.Journal of Xiamen University:Natural Science，2008，47(6):819-822.

［11］ Weiss C，Masselli A，Tamimi H，et al.Fast outdoor robot localization using integral invariants［C］∥Proceedings of the 5th International Conference on Computer Vision Systems.Bielefeld:Applied Computer Science Group，2007.

［12］ 贺锋，秦晓丽，方勇纯.一种基于遗传算法的移动机器人自定位方法［J］.模式识别与人工智能，2009，22 (1):142-147.He Feng，Qin Xiao-li，Fang Yong-chun.A genetic algorithm based autonomous localization strategy for mobile robots［J］.Pattern Recognition and Aritificial Intelligence，2009，22(1):142-147..

［13］ De Jong K A，Spears W M.A formal analysis the role of multi-point crossover in genetic algorithms［J］.Mathematics and Artifical，1992，5(1):1-26.

［14］ Srinivas M，Patnaik L M.Adaptive probabilities of crossover and mutation in genetic algorithms［J］.IEEE Transactions on System，Man and Cybemeties，1994，24 (4):656-667.

［15］ Heinemann P，Haase J，Zell A.A novel approach to efficient Monte-Carlo localization in RoboCup［C］∥Robo-Cup 2006:Robot Soccer World Cup X.Berlin，Germany: Springer-Verlag，2007:322-329.