基于自适应动态规划的移动机器人视觉伺服跟踪控制

罗 彪 欧阳志华 易昕宁 刘德荣

随着移动机器人控制技术的发展,移动机器人如今已广泛应用于工业生产、国防军事以及生活服务等众多方面[1].视觉传感器近年来逐渐变得价格经济、易于获取,加上其本身具有获取环境信息丰富、外部感知能力强和适用范围广等特点,因而广泛装备于移动机器人和其他智能体系统[2].由于视觉传感器的作用,移动机器人的灵活性和智能性得到极大提高,可以适应更加复杂的环境,基于视觉的机器人的定位、环境感知与控制技术也得到了很大的发展[3].移动机器人视觉伺服控制主要有两大控制目标,一种是位姿校正[4],另一种是轨迹跟踪[5].在实际应用中,轨迹跟踪相比位姿校正往往更加复杂且常见,因此基于视觉的移动机器人轨迹跟踪吸引了大量学者的关注与研究.

经典的视觉伺服控制主要分为基于图像的、基于位置的和混合视觉伺服控制.基于图像的视觉伺服控制是通过在二维图像平面定义误差信号进行控制,不需要利用移动机器人三维位姿信息,且其对系统扰动具有鲁棒性,但是很难控制移动机器人的偏转位姿[6].基于位置的视觉伺服控制需要在三维欧氏空间定义误差信号,虽然需要进行三维重构,但这样直接对三维空间的误差进行控制可以保证其收敛性.由于移动机器人具有非完整约束特点以及单目相机深度信息的缺失造成系统带有不确定性参数,设计移动机器人的控制器存在很多困难[7].根据Brockett 定理,连续定常的控制器无法实现对具有非完整约束的移动机器人位姿校正控制[8].为了克服非完整约束,文献[9]提出了一种基于反步法的时变状态反馈跟踪控制方法.然而其并未将视觉传感器引入到控制中,并且一般假设系统状态是精确可测量的.但在移动机器人的视觉伺服跟踪控制中,由于视觉传感器的引入给系统带来了不确定性,以往的设计方法并不能直接应用于视觉伺服控制中.为了克服这些限制,目前已提出了许多非线性控制方法.文献[5]基于单应性技术设计了一种自适应控制器实现移动机器人的轨迹跟踪任务,其视觉反馈由搭载于机器人上的相机提供.为保持目标特征点在摄像机的视野范围内,文献[10]设计了一种时变连续的混合视觉伺服控制器实现对移动机器人一致性跟踪与位姿校正任务.为了在无需知道移动机器人位姿与速度信息下完成轨迹跟踪任务,文献[11]提出了一种自适应控制方法实时估计移动机器人的位姿与速度.近期,也有一系列关于移动机器人的视觉伺服跟踪控制的相关成果[12-15].在上述针对移动机器人的视觉伺服跟踪控制中,大多是为完成视觉跟踪任务,但考虑最优性能指标的移动机器人视觉伺服最优跟踪控制问题仍待研究.

考虑到移动机器人的视觉伺服轨迹跟踪控制问题的系统模型是一个时变仿射非线性系统.自适应动态规划(Adaptive dynamic programming,ADP)是解决非线性最优控制问题的一种有效的方法[16-21].ADP 作为一类智能控制方法,可以有效地解决传统动态规划中“维数灾”的问题,同时具备处理复杂约束和不确定性的能力.这类方法在处理具有强非线性、强耦合性的复杂非线性系统时,具有自适应性、最优性和稳定性[22].求解基于ADP 的移动机器人视觉伺服轨迹跟踪的最优控制,需要求解哈密顿-雅可比-贝尔曼(Hamilton-Jacobi-Bellman,HJB)方程.对于非线性定常系统来说,其HJB 方程是一个时不变的偏微分方程,无法求得其解析解,目前大部分求解方法都是设计合适的神经网络结构来近似未知项[23-24].文献[25]针对非线性不确定系统设计了三个神经网络分别近似系统状态、值函数和最优控制.在系统模型已知的情况下,文献[26]针对移动机器人控制问题提出了一种单网络自适应评价方法.与上述一般非线性系统不同的是,由于移动机器人系统模型固有的时变特性,最优控制问题中的HJB 方程含有两项偏微分项,且值函数是状态与时间的函数,这使得问题更加复杂.在现有的研究中针对时变系统的最优控制还较少.目前有两种神经网络的结构用于近似与时间相关的值函数: 1)带时变权值的神经网络[27-28];2)常数权值与时变激活函数的神经网络[29-30].

当前基于ADP 的移动机器人视觉伺服控制方法及理论仍然是一个开放性问题,有待深入研究.本文的贡献主要体现在以下几个方面.

1)针对移动机器人视觉伺服控制,提出了基于ADP 的跟踪控制方法.与现有视觉伺服控制方法相比,本文设计的ADP 控制方法是基于最优控制理论,算法收敛后,可得到近似最优控制器.

2)现有的大多数工作针对定常系统设计,而移动机器人视觉伺服控制系统为时变系统,得到的HJB方程是时变的.因而,本文的工作与现有ADP 方法在理论分析与实现存在显著区别,这是本文主要解决的理论难题.同时,本文所提的基于ADP 的跟踪控制方法对于一般性的时变仿射非线性系统的跟踪控制问题具有普适性和通用性.

3)针对时变HJB 方程,本文设计了带时变权值的神经网络结构,给出了一种新的权值更新律,证明了神经网络权值的收敛性和闭环系统的稳定性.同时,在实验仿真中验证了该类结构具有较小的误差和快速收敛性.

针对时变非线性系统的最优控制仍然与时不变系统存在不同之处,同时带来许多挑战.本文针对移动机器人视觉跟踪控制问题,提出了一种新颖的基于ADP 的跟踪控制方法.考虑移动机器人系统模型的时变特性与非完整约束,采用带时变权值的神经网络近似值函数,并给出了一种新的权值更新律.运用Lyapunov 稳定性理论证明了权值的收敛性以及闭环系统状态是一致最终有界的.

1 问题描述

如图1 所示,本文考虑带有单目相机的轮式移动机器人,假设移动机器人的坐标系与相机坐标系相同.当前相机坐标系记为Fc,坐标系Fc的原点为移动机器人中心,x轴方向为与移动机器人轮轴平行,z轴方向为相机光轴方向.由右手坐标系规则,y轴垂直于移动机器人运动平面xz向下.坐标系Fd与F*分别表示移动机器人在期望位姿和在固定参考位姿处的坐标系,其x,y,z轴的定义规则与坐标系Fc相同.在移动机器人的轨迹跟踪任务中,期望轨迹在Fd中由一系列关于特征点的图像描述.F*为固定参考坐标系,由一张关于特征点的图像表示.θd和θ分别表示Fd与Fc相对参考坐标系F*绕y轴的旋转角.基于以上的坐标系定义,本文的目标是设计一种视觉伺服跟踪控制器以确保移动机器人完成轨迹跟踪任务,即当t→∞时,Fc→Fd.

图1 视觉伺服轨迹跟踪任务描述Fig.1 Visual servoing trajectory tracking task

1.1 欧氏重构

如图1 所示,假设移动机器人仅做平面运动,坐标系F*在坐标系Fc中的位姿信息可以由[x,z,θ]T表示.其中,x和z分别表示沿着x轴和z轴方向的平移,θ表示绕y轴的旋转.同样,坐标系F*在坐标系Fd中的位姿信息可以由[xd,zd,θd]T表示.考虑N个共面的静止特征点Oi(i=1,2,···,N),其在坐标系Fc,Fd,F*的三维坐标分别为定义为

由针孔相机模型可以得到特征点归一化三维欧氏坐标与可测量齐次像素坐标的关系为

其中,Hc,Hd ∈R3×3分别为当前位置、期望位置与参考位置之间的单应性矩阵.通过直接线性变换算法,可以将单应性矩阵Hc分解得到当前相机的尺度化平移和旋转角θ;同样地,将单应性矩阵Hd分解得到期望的尺度化平移和旋转角θd[31].具体定义为

其中,d*为深度信息常量,表示固定参考坐标系F*的原点到特征点平面的距离,即d*=由前文的描述可知,当t→∞时,若则表示移动机器人完成了轨迹跟踪任务.

1.2 移动机器人运动学模型

移动机器人Fc的位姿信息在坐标系F*中记为[x*,z*,θ*].经典的移动机器人连续时间运动学模型可以表达为

其中,v和ω分别为移动机器人的线速度与角速度.通过坐标变换可以得到 [x,z,θ]T与[x*,z*,θ*]T之间的关系,即

对式 (9)求关于时间t的导数,并将式 (8)代入求导结果,可得

将式 (7)代入式 (10)和式 (11),可得

1.3 跟踪误差系统模型

移动机器人轨迹跟踪误差定义为

其中,ex,ez表示平移误差,eθ为旋转误差.由式 (14)可知,当e→0 时,则因此上述问题转换为设计一反馈控制律使得e→0,则表示移动机器人完成了轨迹跟踪任务.对式 (14)求关于时间的导数,并将式 (12)和式 (13)代入,可得

为了便于后续对跟踪控制问题控制器的设计,受文献[32]启发,使用如下输入变换:

其中,uv,uω为新的输入控制量,可见上述变换也是可逆的.由此可以推导出新的误差模型,即

为估计深度参数信息,深度参数d*的估计更新律设计采用与文献[33]类似方法,且其收敛性已在文献中得到证明.

2 控制器设计

在移动机器人跟踪控制任务中,期望轨迹的速度vd(t),ωd(t)是随时间变化的,导致在误差模型 (17)中含有时变项.本节基于ADP 方法设计最优控制器以保证移动机器人完成轨迹跟踪任务,采用评价神经网络结构近似最优值函数,求解最优控制输入.

2.1 最优控制设计

由误差动力学方程 (17)可知,该系统是一个连续时间仿射非线性系统,可以表示为

其中,

注意到系统状态方程 (18)中控制输入为二维,而系统状态为三维,可见该系统是一个欠驱动系统.

针对此系统的最优控制问题,本文的目标是设计一个反馈控制策略使得下列定义的值函数最小,即

其中,效用函数取为L(e,u,t)=Q(e)+uTRu,Q(e)是正定的,即对于∀e0,Q(e)＞0 且e=0⇒Q(e)=0,R∈R2×2为对称正定矩阵.沿着系统轨迹(18),对值函数求时间微分,可得如下时变Lyapunov方程[34]

注意到与其他仿射非线性系统无穷时域最优控制情况不同,式 (21)是与时间t直接相关的.根据最优性原理,当=0 时,可得最优控制为

将式 (22)代入式 (20),可得时变HJB 方程

注意到HJB 方程 (23)是一个偏微分方程,很难直接求得此方程的解析解,因此这里使用神经网络近似估计最优值函数V*(e,t),以期望求得近似最优的反馈控制.

2.2 神经网络近似

与时不变仿射非线性系统最优控制问题不同的是,这里的最优值函数V*(e,t)与时间变量t相关.受文献[35]启发,带时变权值的神经网络结构可以用来近似一致连续时变函数.假设V*(e,t)是光滑的,则最优值函数V*(e,t)可由下列神经网络形式表示

其中,W(t)∈RL为真实的神经网络权值向量,L ∈R,L＞0为隐含层节点数,随着神经网络隐含层节点数增加,逼近误差会不断减小[35],即当L→∞,ε(e,t)→0.ϕ(e)=[ϕ1(e),···,ϕL(e)]T∈RL为与状态相关的连续可微的激活函数,ε(e,t)为神经网络逼近误差.对最优值函数V*(e,t)求分别关于e和t的偏导

假设 2.神经网络逼近误差ε(e,t)以及其分别关于状态e与时间t的偏导数∇eε(e,t),∇tε(e,t)是有界的.真实的神经网络权值W(t)以及其对时间的导数是有界的,激活函数ϕ(e)以及其对e的偏导数∇eϕ(e)是有界的: ‖ε(e,t)‖≤εM,‖∇eε(e,t)‖≤εeM,‖∇tε(e,t)‖≤εtM,‖W(t)‖≤WM,≤WtM,‖ϕ(e)‖≤ϕM,‖∇eϕ(e)‖≤ϕeM.

将式 (25)代入式 (22)中,最优控制输入可以表示为

其中,D1(e)=g(e)R-1gT(e).根据文献[36],随着神经网络隐含层节点数L→∞,HJB 方程残差εhjb(e,t)→0,即对于∀εh＞0,∃L:‖εhjb(e,t)‖＜εh.

2.3 神经网络权值更新

由于真实的神经网络权值W是未知的,设计一个评价神经网络近似最优值函数 (24),即

定义真实最优控制输入与近似最优控制输入之差为

将式 (27)和式 (31)代入式 (32)中,可得

将式 (30)和式 (31)分别作为近似最优值函数和近似最优控制代入哈密顿函数 (21)中,可得

根据式 (34),定义目标误差函数为

为使目标误差函数E不断减小,结合梯度下降法的思想设计权值更新律为

根据式 (36),可得

将等式 (38)左边第2 项移到右边,可得

由式 (39),可得:

根据式 (31),可得

3 稳定性分析

定理 1.针对式 (18)描述的非线性时变仿射系统,以式 (31)为控制输入,式 (36)为评价神经网络的权值更新律,则闭环系统的状态e与评价网络的权值估计误差是一致最终有界的.

证明.选择Lyapunov 函数形式为

将式 (18)、式 (25)和式 (26)代入式 (48),可得

将式 (31)代入式 (49),可得

根据HJB 方程 (28),可得

将式 (52)代入式 (51)中,可得

将式 (47)和式 (53)相加,可得Lyapunov 函数的导数为

根据假设1 和假设2,可得

因为Q(e)是正定的,存在一个λq使得eTλqe≤Q(e).根据文献[36],随着神经网络隐含层节点数L的增大,HJB 方程残差εhjb(e,t)会逐渐收敛到零.假设存在一正数εhM,选择合适的神经网络隐含层节点数L,HJB 方程残差满足:εhjb≤εhM.因而,根据式 (54)和式 (55),可得

其中,

式中,I为合适维度的单位矩阵.选择参数使H1＞0,根据式 (56),如果

在这里,有必要讨论一下本文与相关工作[16,37-39]的区别.在文献[37-38] 中,采用基于策略迭代方法设计控制器,与此不同,本文则是基于ADP设计自适应控制器,因而在实现方法与理论分析上存在着显著的差异.在文献[16,39]中,考虑的是非线性离散时间系统的跟踪控制问题,与本文方法的区别主要体现在: 文献[16,39]考虑的是定常系统,也就是说,虽然期望轨迹为时变的,但是针对的系统是定常系统,也即时不变系统;与文献[16,39]不同,本文考虑移动机器人视觉伺服跟踪控制问题,不仅期望轨迹是时变的,而且系统也是时变的.因此,文献[16,39]与本文所针对的问题是完全不同的.

4 仿真研究

为了验证本文提出的控制方法的有效性,本节利用计算机进行仿真实验.选择4 个共面特征点作为视觉目标点,以便能通过单应性矩阵分解成对应的平移与旋转量.相机标定矩阵为

图2～9 展示了实验仿真结果.由图2 和图3可以看出,在本文设计的控制方法下系统状态最终收敛到零并且控制输入也随着时间最终趋于零.神经网络的权值最终如期望的一样收敛于常数值如图4 所示,最终收敛权值为=[0.0445,0.0458,0.0214,0.0048,0.0180,-0.0005,0.0652,0.0174,0.0004,0.0430,0.0292,0.0021,-0.0021,-0.0011,-0.0009,0.0077,0.0105,0.0048,0.0003,0.0012,-0.0001]T.移动机器人的线速度和角速度如图5 所示,可以看出,当前线速度和角速度与期望轨迹的线速度和角速度逐渐一致.

图2 系统响应Fig.2 System response

图3 控制输入Fig.3 Control input

图4 评价神经网络权值的收敛Fig.4 Convergence of critic neural network weights

图5 移动机器人期望轨迹速度与实际运动速度Fig.5 Desired and real velocities of the mobile robot

在仿真过程中,为了验证本文所提出算法的效果,在保证选取同样网络层数和隐含层节点个数的前提下,将本文提出的时变权值神经网络结构与文献 [38] 中提出的时变激活函数NN 结构进行对比.在最优控制问题中,HJB 方程的近似误差可以用于表征控制器对性能指标优化程度的好坏.如图6 所示,在两种方法下HJB 方程的残差最终均能收敛至零,但本文所提方法的收敛速度相对要快很多.

图6 HJB 方程残差Fig.6 The residual error of HJB equation

在两种方法下的移动机器人期望轨迹与实际运动轨迹如图7 和图8 所示.可见,在本文所提的时变权值的神经网络结构方法下,机器人的跟踪效果更好、跟踪误差更小.此外,特征点的二维图像轨迹如图9 所示.其中,红色虚线表示期望图像轨迹,实心圆点表示初始的期望图像,五角星表示最终的期望图像;蓝色实线表示当前的真实图像轨迹,空心圆点表示初始的真实图像,方形表示最终的真实图像.由图9 可知,当前实际图像轨迹与期望的图像轨迹逐渐一致.

图7 利用本文时变权值神经网络结构方法的移动机器人期望轨迹与实际运动轨迹Fig.7 Desired and real trajectories of the mobile robot using time-varying weights neural network structure method in this paper

图8 利用时变激活函数神经网络结构方法的移动机器人期望轨迹与实际运动轨迹Fig.8 Desired and real trajectories of the mobile robot using time-varying activation neural network structure method

图9 特征点二维图像轨迹Fig.9 2D image trajectories of the feature points

5 结束语

本文设计了一种新的基于ADP 的跟踪控制方法来解决移动机器人视觉伺服轨迹跟踪最优控制问题.与以往控制对象不同的是移动机器人视觉伺服轨迹跟踪的误差系统模型是一个时变仿射非线性系统,针对此系统的最优控制问题需要设计具有时变权值的神经网络近似值函数以求解时变的HJB 方程.运用Lyapunov 稳定性理论证明了在本文提出的控制方法作用下神经网络权值的收敛性以及闭环系统的稳定性.仿真实验结果验证了所提出方法的有效性与可行性.