基于改进导航圆算法的光伏阵列清扫机器人路径跟踪控制

丁 坤，王 立，张经炜，李辰阳，翁 帅

(1.河海大学机电工程学院，江苏 常州 213022；2.常州市光伏系统集成与生产装备技术重点实验室，江苏 常州 213022)

在户外环境中，光伏电站中的光伏组件表面极易堆积灰尘等不洁物，而灰尘因为具有反射、散射和吸收太阳辐射的作用，使得组件前盖透光率下降，降低了太阳辐射的通过率，从而导致光伏系统的发电效率和经济效益降低[1-2]。目前针对光伏组件表面的不洁物，主要存在4种清除方式：人工擦拭、人工水洗、半自动机械清洗、自动机械清洗[3-6]。其中采用人工擦拭、人工水洗的方式，清洁效率低，并且清洁过程难以控制。半自动机械清洗多采用带有滚刷臂的清扫卡车，其场地适应能力有限，无法在屋顶电站或者山地电站工作。自动机械清洗是将清洁装备技术与移动机器人技术结合，即移动机器人在光伏组件表面工作，通过自身配备的滚刷来清洁组件。由于光伏组件表面材料为玻璃，组件间存在边框，所以要求清扫机器人对组件表面的压强要小，并且具有一定的越障能力，而履带式移动结构因其与组件接触面积大，以及优秀的越障性能，十分契合光伏阵列清扫机器人的要求。光伏阵列清扫机器人具有清洁效率高，操作灵活和场地适应能力强等优势，以及巨大的市场潜力和经济价值，已引起许多专家和学者的重视[7]。

移动机器人的研究涉及诸多科学领域，其中智能控制是移动机器人的前沿课题，主要指移动机器人的定位导航、路径规划和路径跟踪等问题[8]。路径跟踪是指在平面坐标系下一条满足任务需求的理想几何路径，移动机器人从平面上某一点出发，按照某种控制规律到达该路径，并沿着该理想几何路径实现跟踪的控制运动[9]。对于路径跟踪问题，国外的Kanayama等[10]提出了一种回旋曲线跟踪路径；而对于国内学者，郝存明等[11]提出了基于滑模控制的室内移动机器人路径跟踪，尉成果等[12]提出了一种轮式移动机器人的轨迹跟踪算法，第一次提出了导航圆的概念。本文在导航圆算法的基础上进行改进，进一步提高移动机器人路径跟踪性能。

本文使用的光伏阵列清扫机器人采用履带式移动结构并带有滚刷，工作时行驶在光伏组件表面，在遇到凸起的组件边框或者在光滑组件表面上时容易出现打滑现象，导致偏离原始路径的问题。针对这一问题，首先给出机器人的运动学模型和适用的超宽带(ultra wide band，UWB)定位系统，然后提出改进的导航圆光伏阵列清扫机器人路径跟踪算法。该算法在原有导航圆算法的基础上，对其关键变量进行重新修正。验证结果表明，采用这一控制方法，机器人在短时间内即可调整到目标路径上，较导航圆算法有了显著提升。

1 清扫机器人的运动数学模型

1.1 机器人基本机构

为了适应不同的工作环境，一般室内移动机器人通常采用轮式移动机构，室外移动机器人为了适应野外环境的需要，多采用履带式移动机构[13]，所以本文机器人选择履带式移动机构，该机构相对轮式移动机构跨越障碍能力优越[14]，对组件表面压强小且行走方式更灵活。

1.2 机器人运动数学模型

为了了解机器人行走方式与电机转速的关系，从而更好地控制机器人的运动，需要对机器人进行运动分析。建立如图1所示的光伏阵列清扫机器人运动数学模型，假定机器人以P点为圆心从C1点移动至C2点，C为机器人几何中心。则机器人的运动数学模型[15]为

(1)

图1 光伏阵列清扫机器人运动数学模型 Fig.1 Kinematic model of photovoltaic array cleaning robot

式中：x、y——机器人的位置；vl——左带轮线速度；vr——右带轮线速度；L——左右履带间距；δ——机器人相对x轴的转角；v、ω——C点线速度和角速度。

2 清扫机器人的超宽带定位系统

UWB定位是一种基于极窄脉冲的无线技术，因其具有传输速率高(最高可达1 000 Mbps以上)、发射功率和功耗低，以及穿透性较强的特点而广泛应用于科学研究[16-17]。

UWB定位系统一般设有主动发射标签和多个接收基站。采用到达时间(time of arrival，TOA)算法或到达时间差(time difference of arrival，TDOA)算法来测距定位，接收基站接收来自发射标签的超宽带信号，过滤掉电磁波在传输过程中的噪声干扰，得到信号中的有效信息[18]，利用CPU对信息进行解码和计算，最终实现对发射标签的定位[19]。

为实现机器人的定位功能，建立如图2所示的光伏阵列清扫机器人超宽带定位系统，将1个主动发射标签设置在机器人的几何中心，将4个接收基站设置在组件阵列四周，采用TOA算法进行位置计算，之后通过无线通信上传到上位机中，用于后续跟踪算法，并在显示器上实时显示机器人的位置信息。设计的UWB系统定位精度在20 cm之内。

图2 光伏阵列清扫机器人超宽带定位系统示意图Fig.2 Schematic map of ultra-wide band positioning system for photovoltaic array cleaning robot

3 改进的导航圆路径跟踪算法

3.1 导航圆路径跟踪算法

导航圆路径跟踪算法，是将获取的位置信息转化成与目标路径的距离偏差，与当前行进方向和目标路径方向的角度偏差综合成一个角度，对该角度进行比例积分微分 (proportional-integral-derivative，PID)调节，以使机器人回到路径上。尉成果等[12]提出的导航圆算法，本质上是将一个假想圆作为机器人的导航范围来计算路径跟踪时的纠偏角度，通过PID控制调节纠偏角，直至机器人回归直线。具体算法如图3所示，导航圆的圆心C为机器人的几何中心，半径为R0；机器人处于远离目标直线的位置C1，逐渐调整至C2位置，D1和D2分别表示当前时刻机器人的目标点。

图3 直线跟踪示意图Fig.3 Sketch map of straight-line tracking

如图3所示，在机器人逐渐靠近目标直线的过程中，纠偏角(机器人当前方向与目标方向的夹角)γ逐渐减小，当γ=0时，即可实现目标跟踪。

已知机器人到目标直线y=k*x+b的距离h[12]为

(2)

式中：xc、yc——机器人实时位置坐标；k*、b——目标直线的斜率和纵截距。

由几何关系可得各角度信息[12]，各角度方向均如图3中所示：

θ=α-arcsin(h/R0)

(3)

γ=θ-β=α-arcsin(h/R0)-β

(4)

式中：α——目标直线路径倾角；β——机器人前进方向与x轴的夹角，即偏航角；θ——预测偏航角。

利用PID控制器对γ反馈调节，直至γ为0，机器人回归目标直线。PID控制的原理为

(5)

式中：u(t)——控制量；Kp——比例系数；TI——积分常数；TD——微分常数；e(t)——纠偏角γ;t——时间。

对γ采用增量式PID控制，将PID控制算法离散化为

Δu(k)=Kp[e(k)-e(k-1)]+KIe(k)+KD[e(k)-2e(k-1)+e(k-2)]

(6)

其中KI=KPT/TIKD=KPTD/T

式中：KI——积分系数；KD——微分系数；T——采样周期。由式(6)得

u(k)=u(k-1)+Δu(k)

(7)

则机器人在纠偏过程中任意k时刻速度分配为

(8)

对于目标路径为圆弧的导航圆跟踪算法，其思想与直线跟踪相同，只是由跟踪已知直线变为跟踪动态直线，动态直线是跟踪圆弧上的切线，但光伏阵列清扫机器人主要是直线运动，所以不具体分析。

假设路径跟踪的目标直线l：y=500 cm，机器人左右履带间距L=50 cm，起点为(50，700)，初始速度为0.1 m/s，初始前进方向为x轴正方向。若使导航圆算法成立，则导航圆与目标直线必须存在交点，即R0≥h=200 cm，设R0为200 cm和300 cm，采用Matlab分别模拟仿真，导航圆算法直线跟踪仿真结果如图4所示。

图4 导航圆算法直线跟踪仿真结果Fig.4 Simulation results of straight-line tracking based on navigation round algorithm

由图4(a)位置变化过程可知，当R0=200 cm时，所需调节距离为533 cm，当R0=300 cm，所需调节距离为924 cm。由图4整体结果可知，在h>80 cm时，两种参数下的β均收敛迅速，在h<80 cm时，两种参数下的β均较为平滑收敛。可得出导航圆算法在R0≥h条件下，导航圆R0越小，收敛速度越快，调节距离越短，而且在接近目标直线时变化缓慢，不会产生超调量。

3.2 改进的导航圆路径跟踪算法

由仿真结果可知，在满足R0≥h的条件下，R0越小，调节距离越短；不同R0条件下，当h<80 cm时，β均较为平滑收敛。所以动态地调整导航圆半径可以有效地改善调节速度。另外机器人在最终接近目标直线时，需要使机器人缓慢靠近目标直线。因此当h>80 cm时，令R0=h，即arcsin(h/R0)=90°，机器人垂直地靠近目标直线；当h≤80 cm时，令R0=80 cm，机器人平缓安全地接近目标直线，则改进后的γ为

(9)

对首次改进导航圆算法进行模拟仿真，其中机器人自身物理参数和初始姿态参数与导航圆算法相同，如图5(a)所示，发现x轴方向的调节距离大幅缩短至172 cm，但是如图5(b)所示，β相对导航圆算法变化剧烈，容易使机器人在行进时发生抖动，导致机器人稳定性变差。

图5 首次改进导航圆算法直线跟踪仿真结果Fig.5 Simulation results of straigh-line tracking based on improved navigation circle algorithm for first time

考虑到机器人采用履带式移动机构，该机构因其具有转向特性，即在转向过程中存在较大的转向阻力，导致机器人轨迹可控性差[20]，所以当机器人β变化剧烈时，车身会出现比较大的抖动，因此在机器人靠近目标直线时偏航角需要平缓变化，避免因抖动惯性导致震荡的发生。

更进一步发现γ=θ-β=α-arcsin(h/R0)-β中，α和β是不可更改量，影响γ的主要因素是反正弦函数arcsin(h/R0)，而R0又受h的制约，因此尝试修正γ的计算公式，并引入一个新的调节参数ω，使γ为

γ=α-arctan(h/ω)-β

(11)

如图6所示，ω不受h的制约，在[0，100]的范围内反正切函数arctan(h/ω)均大于反正弦函数arcsin(h/R0)；并且ω越小，反正切函数越大，则纠偏角变化越大，调节距离越短。考虑到机器人的履带移动机构特性以及运动惯性，ω的范围设定在[50，80]之间，一般情况下ω取值为60，使得机器人既能快速调节，又符合实际要求。

图6 反正切函数与反正弦函数对比Fig.6 Comparison between arctan function and arcsine function

采用最终改进的导航圆算法进行Matlab仿真，x轴方向调节距离如图7(a)所示，调节距离为180 cm，β变化过程如图7(b)所示，相对导航圆算法的首次改进，位置和β变化更为平缓，机器人在跟踪直线时可以更加稳定和安全。

图7 改进后的导航圆算法直线跟踪仿真结果Fig.7 Simulation results of straight-line tracking based on improved navigation round algorithm

图8 光伏清扫机器人跟踪系统工作流程Fig.8 Workflow of photovoltaic cleaning robot tracking system

4 跟踪系统工作流程

根据所述原理，在硬件上有机器人机械本体、各类传感器以及控制电路，软件上有机器人控制算法、超宽带定位算法，以及改进后的导航圆算法，建立如图8所示的基于改进导航圆算法的光伏阵列清扫机器人路径跟踪系统工作流程。该流程解释如下：在超宽带定位系统和姿态传感器使能的情况下，判断当前机器人车身状态。当机器人车身状态为前进，以每200 ms的间隔计算γ，之后利用PID控制，循环修正目标偏航角，最终实现对目标路径的跟踪。

对于PID的KI、KP和KD参数整定，首先将KI和KD取零，即消除微分和积分的作用，使用纯比例控制，不断增大KP的数值，观察机器人调节响应速度，直至达到一定范围的超调。针对机器人出现的静态误差，不断增大KI直至消除静差，发现机器人超调量增大且有振荡，引入KD并不断微调KI、KP直至效果理想，机器人最终选择PID参数分别为KP=16、KI=6、KD=2。

5 试 验 验 证

为了验证改进导航圆算法的优势和正确性，如图9所示，令机器人工作在光伏组件表面上，设置两种不同的试验条件，并通过超宽带定位系统得到机器人的实时位置信息，最后对位置信息进行对比分析。

图9 试验平台Fig.9 Experimental platform

试验一：机器人以坐标(30，700)处为起点，以0.1 m/s的速度沿着x轴正方向前行，分别采用导航圆算法和改进导航圆算法跟踪目标直线y=500 cm，调节结果如图10所示。通过分析试验结果，可知采用导航圆算法跟踪目标路径，x轴方向的调节距离为513 cm，而改进后的导航圆算法在相同的条件下，x轴方向的调节距离为247 cm。由试验可得，改进后的导航圆算法调节距离为原导航圆算法调节距离的48.1%，机器人迅速回到目标路径上，并且几乎无超调量的产生。

图10 直线路径跟踪对比实验结果Fig.10 Experimental results of linear path tracking

试验二：机器人起点为(30，500)，以0.1 m/s的速度沿x轴正方向前行，在行进中人为转动30°，模拟机器人在组件上出现打滑的现象，分别采用两种算法跟踪目标直线y=500 cm，扰动下的调节结果如图11所示，采用导航圆算法调节，调节距离为246 cm，而采用改进的导航圆算法调节，调节距离为86 cm，可知改进后的导航圆算法调节距离为原导航圆算法的34.9%。

图11 扰动下直线路径跟踪对比实验结果Fig.11 Experimental results of linear path tracking under disturbance

2种试验方法均表明，改进后的导航圆算法相对原算法有更好的调节性，调节距离缩减50%以上，清扫机器人可以更为迅速地回到目标路径上完成相应的工作。

6 结 语

本文在原有导航圆算法的基础上，根据对算法原理的进一步分析，提出了一种改进后的导航圆路径跟踪算法。首先给出针对清扫机器人的运动学模型和定位系统，之后分析原有导航圆算法，提出假设并改进算法，同时对算法进行Matlab仿真。最终的对比试验结果表明，改进后的导航圆算法较原有算法具有调节距离短、前后时刻偏航角变化量小的特点，机器人可以更加迅速和安全地回到原始路径上。