基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制

第39卷第12期2022年12月控制理论与应用
Control Theory&Applications
V ol.39No.12
Dec.2022
基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制
李晨萍1,2,张雪波1†,王润花1,李宝全3,方勇纯1
(1.南开大学人工智能学院机器人与信息自动化研究所,天津300350;
2.南开大学新闻与传播学院,天津300350;
3.天津工业大学电气工程与自动化学院,天津300160)
摘要:针对移动机器人位姿镇定问题,本文提出基于视觉同时定位与建图(simultaneous localization and mapping, SLAM)–伺服框架的指令滤波反步控制策略.具体而言,通过加速度层控制器设计进而积分得到的光滑速度信号,减小SLAM视觉模块的预测位姿误差;继而应用指令滤波器简化控制器设计的复杂求导运算,减轻计算负担;此外, SLAM模块利用运动信息与视觉信息的融合解决未知尺度问题,降低未知深度造成的控制器设计复杂度.通过李雅普诺夫理论可以证明闭环系统的稳定性.仿真和实验结果最终验证了本文算法的有效性.
关键词:指令滤波;速度光滑;视觉伺服;同时定位与建图;移动机器人
引用格式:李晨萍,张雪波,王润花,等.基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制.控制理论与应用,2022,39(12):2233–2241
DOI:10.7641/CTA.2022.11260
Commandfilter backstepping control for mobile robots
based on visual SLAM and servoing framework
LI Chen-ping1,2,ZHANG Xue-bo1†,WANG Run-hua1,LI Bao-quan3,FANG Yong-chun1
(1.Institute of Robotics and Automatic Information System,College of Artificial Intelligence,
Nankai University,Tianjin300350,China;
2.School of Journalism and Communication,Nankai University,Tianjin300350,China;
3.School of Electrical Engineering and Automation,Tianjin Polytechnic University,Tianjin300160,China)
Abstract:For the pose regulation task of mobile robots,a commandfilter backstepping controller based on visual SLAM and servoing framework is proposed in this paper.Specifically,acceleration signals are designed and then integrals are computed to obtain smooth velocity commands,so that error of predicted pose can be reduced in the SLAM module. Then a commandfilter is introduced to simplify the computation process of derivation terms.Additionally,velocities designed by the controller are adopted in the SLAM module for scale estimation,thus reducing the complexity of controller design resulted from unknown depth.Stability of the closed-loop system is analyzed by Lyapunov-based techniques. Finally,simulation and experiments are implemented to verify the effectiveness of the proposed visual servoing controller.
Key words:commandfilter;smooth velocities;visual servoing;SLAM;mobile robots
Citation:LI Chenping,ZHANG Xuebo,WANG Runhua,et mandfilter backstepping control for mobile robots based on visual SLAM and servoing framework.Control Theory&Applications,2022,39(12):2233–2241
1引言
移动机器人具有运动空间大、运动灵活等优点;同时,低成本的单目相机能提供丰富的环境信息,因此利用单目相机反馈的图像信息来实现位姿镇定[1–2]或轨迹跟踪[3–4]的移动机器人视觉伺服发展迅速,在家庭服务、智能运输、自主停车等方面得到了广泛应用.
移动机器人本身的非完整约束给伺服控制器的设计带来了挑战,Brockett定理阐明了不存在光滑非时变的控制器完成移动机器人的位姿镇定[5],因此位姿镇定伺服控制器设计比轨迹跟踪控制器设计更加困
收稿日期:2021−12−23;录用日期:2022−05−26.
†通信作者.E-mail:*********************.cn;Tel.:+8622-23505706-809.
本文责任编委:苏剑波.
国家重点研发计划课题(2018YFB1307503),天津市杰出青年基金项目(19JCJQJC62100),天津市自然科学基金面上项目(19JCYBJC18500),中央高校基本科研业务费项目资助.
Supported by the National Key Research and Development Project(2018YFB1307503),Tianjin Science Fund for Distinguished Young Scholars(19JC JQJC62100),Tianjin Natural Science Foundation(19JCYBJC18500)and the Fundamental Research Funds for the Central Universities.
2234控制理论与应用第39卷
难.移动机器人的非完整约束和单目相机的未知深度
都是伺服控制器设计需要考虑的问题.文献[6]设计了分段连续控制策略,进而实现在部分视觉参数未知情况下的有限时间饱和镇定问题;文献[7]通过单应矩阵的元素构造状态变量,利用切换控制器将伺服镇定控制分原地旋转、移动至期望位姿正前方或正后方、直线运动三阶段进行;Shi 等人[8]利用先验信息投影预测的方法处理陆标被遮挡的特殊情况,同时利用强化学习调整相关参数进而保证伺服的效率;文献[9]提出一种模糊自适应控制策略,使用两个独立的伺服增益解耦平移和旋转;Zhang 等人[10]针对未标定的单目视觉伺服问题,提出一种自适应控制器实现移动机器人位姿镇定和轨迹跟踪.然而,上述方法都是运动学控制器,通过设计线速度和角速度进而实现控制目标.此类控制器在初始位置较大的误差信号下通常输出较大的速度,而由于惯性原因,设计的速度会与实际的速度有较大偏差,进而影响系统的性能.文献[11]从机械结构、执行器和能源3个子系统出发,设计了三级控制器,采用递阶控制器将3层控制器连接起来,最终实现移动机器人的动力学控制.但是该方法模型复杂,同时大部分商用移动机器人动力学参数未知且接口
未开放,因此限制了动力学控制器的实际应用.文献[12]通过设计加速度的伪动力学控制器,进而积分得到光滑的速度信号,但是动态面方法滤波产生的误差被忽略,最终会影响系统的控制精度;同时,反步法不可避免的复杂求导计算以及单目相机造成的未知位姿尺度自适应更新也增加了控制器的计算负担.
针对移动机器人单目视觉伺服,本文基于SLAM 的伺服框架基础[13],设计了图1所示的指令滤波反步控制策略.一方面,在SLAM 感知模块中利用已知的速度信息,对未知尺度在线估计和优化,进而减小伺服控制器设计的复杂度;另一方面,伺服控制模块通过加速度信号的设计积分得到光滑的速度命令,进而减小SLAM 的位姿预测模型误差;同时,指令滤波器的引入也避免了反步法的求导复杂计算.
论文其他部分安排如下:第2节对移动机器人视觉伺服位姿镇定问题进行了系统建模;第3节介绍了SLAM 感知模块;第4节设计了基于指令滤波反步法的位姿镇定控制器,并对其稳定性进行了分析;第5节通过仿真和实验验证了算法的有效性,同时与动态面方法的对比也体现了本方法的优势;最后一节对本文工作进行了总结.
图1系统概述Fig.1System overview
2系统建模
如图2所示,假定相机坐标系与移动机器人运动坐标系重合,即将相机光心垂直安装于移动机器人轮轴中心上方.令F 表示固定于相机光心的坐标系,其中x 轴沿光轴方向指向前,y 轴垂直指向左侧.F ∗和F c 分别表示固定于期望位姿和当前位姿的坐标系,θ表示F c 相对于F ∗的旋转角,(∗x c ,∗y c )是F c 原点在F ∗的坐标,∗T c =[∗R c ∗t c ]表示F c 相对于F ∗的变换矩阵,其中
∗
R c =[cos θ−sin θsin θcos θ],∗t c =[∗x c ∗y c
]
.
根据移动机器人的特性,可以得到其运动学方
程如下:
˙∗x c
=v cos θ,˙∗y
c =v sin θ,˙θ=ω.(1)
其中v 和ω分别表示移动机器人的线速度和角速度.定义(x,y )表示F ∗原点在F c 的坐标,c T ∗=[c R ∗
c t ∗
]表示F ∗相对于F c 的位姿变换矩阵.根据位姿矩阵变换关系,可知c R ∗=∗R T c ,c t ∗=−c R ∗∗t c
,即{
x =−∗x c cos θ−∗y c sin θ,
y =∗x c sin θ−∗y c cos θ.
(2)
对式(2)求导,结合式(1)得到
第12期
李晨萍等:基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制2235
˙y =ω(∗x c cos θ+∗y c sin θ)=−ωx,˙x =−v +ω(∗x c cos θ+∗y c sin θ)=−v +ωy.
(3)
为了减小机器人实际速度与设计速度信号的偏差,本文通过设计线加速度u 1和角加速度u 2的方法积分得到平滑的线速度和角速度信号,即
˙v =u 1,˙ω=u 2.
(4)
位置和姿态误差定义如下:
e x =x,e y =y,e θ=θ.
本文旨在设计u 1和u 2,驱动移动机器人运动至期望
位姿处,即令位置误差e x ,e y 和姿态误差e θ趋于零
.
图2视觉伺服坐标系关系
Fig.2Coordinate systems relationship in visual servoing
3SLAM 反馈信号解算
基于文献[13]提出的SLAM–伺服框架,本文在
图优化的SLAM 模块中,添加了位姿节点之间的运动约束(如图3所示),继而利用式(5)得到反馈位姿的尺度因子
s =arg min N −1∑
i =1
∥R i T t i +1−s t i m ∥2,(5)
其中:N 表示初始地图中的位姿节点数量,R i 表示
第i 个位姿节点相对于初始位姿的旋转矩阵,t i +1表示第i +1个位姿节点相对于初始位姿的平移向量,s 表示视觉模块的尺度因子,t i m 表示利用运动信息积分得到的第i +1个位姿节点相对于第i 个位姿节点平移向量:
t i m =[d i cos θi d i sin θi ]T ,
d i = t ∈[i,i +1]v t d t,
θi = t ∈[i,i +1]
ωt d t,
其中:v t 和ωt 分别表示两位姿节点的中间帧对应的
线速度和角速度
.
图3SLAM 位姿解算
Fig.3SLAM module for feedback signals estimation
伺服过程中,SLAM 模块应用前一帧的位姿和控制器输出的速度信息引导当前位姿的解算.具体而言,通过速度积分可以预测当前帧的位姿,进而预测地图点投影,辅助特征匹配.同时,位姿解算过程中,当前图像和地图点的3D-2D 匹配以及和前一帧之间的运动约束均参与位姿优化
T c =arg min(∥R l T t c −s ¯t
c ∥2+N ∑i =1
K δ(e 2(P i ,T c ))),
其中:R l T 表示前一帧图像的旋转矩阵的转置,t c 表
示当前图像的平移向量,s 表示尺度因子,¯t
c 表示前一帧速度信息积分得到的当前位姿相对前一帧的平移向量预测,N 表示当前帧观测到的3D 地图点数量,e (P i ,T c )表示第i 个地图点在当前帧的投影误差.为了提高对误匹配的鲁棒性,核函数K (·)定义如下:
K δ(e 2
(P i ,T c ))=
e 2(P i ,∗T c ),|e |<δ,2δe (P i ,∗T c )−δ2,|e | δ.关键帧插入后的局部地图优化中,定义状态量χ=[T 0···T N 1s P 0···P N 2],利用伺服控制的运动信息和地图点投影观测值优化状态量min χ
(
N 1∑i =1
∥R T i −1t i
−s ¯t
i ∥2+N 2∑j =0N j ∑k =0
K δ(e 2jk )),
其中:N 1是关键帧数量,s 代表尺度因子,¯t
i 表示根据运动信息得到的第i 个关键帧的预测平移向量,N 2表示关键帧观测的地图点数量,N j 表示观测到第j 个地图点的关键帧数目,e jk 表示第j 个地图点在第k 个关键帧上的投影误差.
利用伺服控制器设计的速度信息,SLAM 模块完成基于预测位姿引导定位的当前位姿解算.同时,SLAM 因子图中添加了位姿节点之间的运动约束,
2236控制理论与应用第39卷进而恢复视觉反馈位姿信号的尺度因子,避免了伺
服模型中因平移量的未知尺度带来的未知参数问
题.伺服控制器设计过程无须考虑未知参数的迭代
更新,SLAM模块恢复的尺度因子降低了控制器设
计的复杂度.
4基于指令滤波反步法的位姿镇定控制器
4.1控制器设计
视觉伺服系统中,平移子系统和旋转子系统的
开环误差模型分别是
˙e y=−ωe x,
˙e x=ωe y−v,
˙v=u1,
(6)
{
˙eθ=ω,
˙ω=u2.
(7)
根据指令滤波反步法,平移系统跟踪误差状态量定义如下:
˜e y=e y,
˜e x=e x−e x c,
˜v=v−v c,
(8)
其中:e x c和v c表示指令滤波αx和αv得到的信号,αx 和αv分别表示状态量e x和v的虚拟期望控制指令.
同时,为了补偿滤波误差,定义平移系统的误差补偿状态量如下:
v y=˜e y−ξy,
v x=˜e x−ξx,
v v=˜v,
(9)
其中:ξy和ξx初值为0,导数设计如下:
˙ξ
y=−k yω2ξ
y−ωξx−ω(e x c−αx),(10)
˙ξ
x=−k xξx−(v c−αv)+ωξy.(11) k y,k x∈R+为正的常数增益.
针对˜e y子系统,虚拟控制量αx设计如下:
αx=k yω˜e y.(12)将其作为指令滤波器的输入,滤波器的状态量z x,1
和z x,2设计如下:
{
˙z x,1=ωn z x,2,
˙z x,2=−2ζωn z x,2−ωn(z x,1−αx),
(13)其中:ζ∈(0,1]表示滤波器阻尼系数,ωn>0表示自然频率;初始状态z x,1(0)=αx(0),z x,2(0)=0.指令滤波器输出信号为e x c=z x,1和˙e x c=ωn z x,2.根据式(13),指令滤波器输出信号e x c与输入信号αx之间的传递函数为
e x c(s)
αx(s)
=
ωn
s2+2ζωn s+ω2n
.
可以看出,该指令滤波器具有稳态单位增益.同时,该指令滤波器可以根据αx直接求解得到˙e x c,避免了反步法导数求解的复杂计算.
由式(12)可得˜e y的闭环动态方程如下:
˙˜e
y=−ω[˜e x+αx+(e x c−αx)]=
−k yω2˜e y−ω˜e x−ω(e x c−αx).
(14)结合式(9)–(10)和式(14),闭环误差补偿状态量v y的动态方程如下:
˙v y=−k yω2v y−ωv x.(15)针对˜e x子系统,虚拟控制量αv设计如下:
αv=k x˜e x−˙e x c.(16)类似式(13),将αv作为指令滤波器的输入,无需求导计算,可直接得到v c和˙v c.
由式(16)可得˜e x的闭环动态方程如下:
˙˜e
x=ω˜e y−˙e x c−αv−˜v−(v c−αv)=
−k x˜e x+ω˜e y−˜v−(v c−αv).
(17)结合式(9)(11)和式(17),闭环误差补偿状态量v x的动态方程如下:
˙v x=−k x v x+ωv y−v v.(18)针对˜v子系统,设计线加速度控制输入u1如下:
u1=−k v˜v+˙v c+v x.(19)进而得到˜v的闭环动态方程如下:
˙˜v=u
1−˙v c=−k v˜v+v x.(20)结合式(9)和式(20),可得闭环误差补偿状态量v v的动态方程如下:
˙v v=−k v v v+v x.(21)
对于二阶旋转子系统,定义误差状态量如下:
{
˜eθ=eθ,
˜ω=ω−ωd.
(22)
针对状态量˜eθ,设计虚拟输入量
ωd=−kθ˜eθ,(23)其中:kθ∈R+是正的常数增益.进而可以得到˜eθ的闭环动态方程如下:
˙˜e
θ
=ωd+˜ω=−kθ˜eθ.(24)对于˜ω状态量,设计角加速度控制输入
u2=−kω˜ω−kθω−˜eθ+kξv y sin t,(25)其中:kω,kξ∈R+为正的常数增益.因此˜ω的闭环
第12期李晨萍等:基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制2237
动态方程如下:
˙˜ω=u 2+k θ˙˜e θ=−k ω˜ω−˜e θ+k ξv y sin t.(26)与反步法相比,引入指令滤波器避免了虚拟信号求导带来的复杂计算量.尤其是加速度u 1的计算过程中,如附录式(A.5)–(A.7)所示的反步法控制律需求解虚拟输入量的二次导数,计算繁琐;通过引入式(13)所示的指令滤波器,虚拟输入量的导数作为其输出信号可直接得到,避免了复杂的求导计算.4.2稳定性分析
定理1对于系统(6)–(9)和(22),控制律(12)–(13)(16)(19)(23)(25)可以使误差补偿状态量v y ,v x ,v v ,旋转子系统状态量e θ,ω以及平移系统误差˜v 均趋于0,即
lim t →∞v y =0,lim t →∞
v x =0,lim t →∞
v v =0,
lim t →∞
e θ=0,lim t →∞
ω=0,lim t →∞
˜v =0.
定理1的证明可以通过以下步骤完成:
步骤1证明平移子系统误差补偿状态量v y ,
v x ,v v ∈L ∞.
构造李雅普诺夫函数如下:
V =12v 2y +12v 2x +12
v 2
v 0.(27)对其求导,并将式(15)(18)和式(21)代入,整理后可得
˙V =−k y ω2v 2y −k x v 2x −k v v 2v 0.
(28)
根据李雅普诺夫理论可知v y ,v x ,v v ∈L ∞.步骤2
证明旋转子系统状态量e θ,ω∈L ∞.
根据模型(7)和(22),应用控制律(23)和(25),整
理可得旋转子系统状态空间如下:
[˙e θ˙ω]=[01−a −b ][e θω]+[0
1]k ξv y sin t,(29)其中:a =k θk ω+1>0,b =k θ+k ω>0,故系数矩阵特征值位于复平面左侧.v y ,sin t ∈L ∞,所以系统控制输入k ξv y sin t ∈L ∞.因此,旋转子系统状态量e θ,ω∈L ∞.
步骤3
证明平移子系统ωv y ,v x ,v v 趋于0.
根据v y ,v x ,v v ,e θ,ω∈L ∞和式(15)(18)(21)(29)可知˙v y ,˙v x ,˙v v ,˙e θ,˙ω∈L ∞.
由式(28)可知
k y ω2v 2
y
−˙V.
(30)
对式(30)两边积分可得
∞
k y ω2v 2
y V (0)−V (∞).
(31)
由式(27)–(28)可知,V (t )∈L ∞,因此ωv y ∈L 2.结合ωv y ∈L ∞,且(ωv y )′=˙ωv y +ω˙v y ∈L ∞,根据芭芭拉引理可知
lim t →∞
ωv y =0.
(32)
同样的方法,可以证明v x ,v v ∈L ∞∩L 2.结合˙v x ,˙v v ∈L ∞,进而得到
lim t →∞
v x =0,lim t →∞
v v =0.
(33)
步骤4证明平移子系统补偿状态量v y 趋于0.
定义f (t )=ωv y ,因此有
lim t →∞
f (t )=0.(34)
对f (t )求导整理可得
˙f (t )=f 1(t )+f 2(t ),(35)
其中:
f 1(t )=−(k θk ω+1)e θv y +k ξv 2
y sin t,
f 2(t )=−(k θ+k ω)ωv y −k y ω3v y −ω2v x .由lim t →∞
ωv y =0,lim t →∞v x =0,ω∈L ∞知
lim t →∞
f 2(t )=0.
(36)
对f 1(t )求导整理可得
˙f 1(t )=−(k θk ω+1)(ωv y +e θ˙v y )+
2k ξv y ˙v y sin t +k ξv 2
y cos t.
上式结合ω,v y ,e θ,˙v y ,sin t,cos t ∈L ∞可得
˙f 1(t )∈L ∞,(37)即f 1(t )一致连续.
根据扩展芭芭拉引理,由式(34)–(37)可得
lim t →∞
f 1(t )=0.
(38)
定义g (t )=f 1(t ),有
lim t →∞
g (t )=0.(39)
对g (t )求导并整理可得
˙g (t )=g 1(t )+g 2(t ),
(40)
其中:
g 1(t )=k ξv 2
y cos t,
g 2(t )=−(k θk ω+1)(ωv y +e θ˙v y )+
2k ξv y ˙v y sin t.
由lim t →∞
ωv y =0,lim t →∞
v x =0,ω∈L ∞和式(15),易知lim t →∞˙v y =0.
结合e θ,v y ,sin t ∈L ∞,可以得到
lim t →∞
g 2(t )=0.
(41)
2238控制理论与应用第39卷
对g 1(t
)求导整理可得˙g 1(t )=2k ξv y ˙v y cos t −k ξv 2y sin t.
上式结合v y ,˙v y ,cos t,sin t ∈L ∞,可以得到
˙g 1(t )∈L ∞,
(42)
即g 1(t )一致连续.
根据扩展芭芭拉引理,由式(39)–(42)可得
lim t →∞
g 1(t )=0.
(43)
由于g 1(t )=k ξv 2y cos t ,因此lim t →∞
v y
=0.步骤5
证明旋转子系统状态量e θ,ω趋于0.
由lim t →∞
v y =0可得lim t →∞
k ξv y sin t =0,即旋转子系统状态空间(29)的控制输入信号趋于0.结合其系数矩阵和控制矩阵可知
lim t →∞
e θ=0,lim t →∞
ω=0.
(44)
步骤6证明平移系统误差状态量˜v 趋于0.
根据式(9)以及lim t →∞
v v =0,可得
lim t →∞
˜v =0.
定理2跟踪误差状态量˜e y ,˜e x 最终一致有界.
证
构造李雅普诺夫函数如下:
V =12˜e 2y +12˜e 2x +12
˜v 2
0.(45)
对其求导,并将式(14)(17)和式(20)代入,整理后可得˙V
=−k y ω2˜e 2y −k x ˜e 2x −k v ˜v 2−ω˜y (e x c −αx )−˜e x (v c −αv )−˜e x ˜v +v x ˜v
−k y ω2˜e 2y −k x ˜e 2x −k v ˜v 2+|ω˜e y ||e x c −αx |+|˜e x ||v c −αv |+|˜e x ||˜v |+|v x ||˜v |
−(k y −1
2
)ω2˜e 2y −(k x −1)˜e 2x −(k v −1)˜v 2+12v 2x +12(e x c −αx )2+1
2
(v c −αv )2.(46)文献[14]证明了指令滤波器输出信号相对输入信号的偏差具有O (ϵ)数量级,即e x c −αx =O (ϵ),
其中ϵ=1
ωn
.O (ϵ)表示存在正常数k 和c 满足|e x c
−αx | k |ϵ|,∀|ϵ|<c .
由于初值条件e x c (0)=αx (0),v c (0)=αv (0),因此存在正常数c 1,c 2使得
1
2(e x c −αx )2 c 1,12
(v c −αv )2 c 2.(47)结合式(9)–(11),可知存在正常数c 3,满足12
v 2
x c 3.因此式(46)可以转化为
˙V −k 1ω2˜e 2y −k 2˜e 2x −k 3˜v
2+c.(48)
其中
k 1=k y −1
2
,k 2=k x −1,k 3=k v −1,
c =c 1+c 2+c 3.
(49)因此通过合理选择参数使k 1,k 2,k 3>0,可以保证跟踪误差最终一致有界.
证毕.
注1
文献[14]通过奇异摄动理论已证明指令滤波反
步法的跟踪误差相对于标准反步法(如附录所示)的跟踪误差也是O (ϵ)数量级,即最终一致有界的边界会随滤波器频率ωn 的增大而减小.
注2与动态面方法[12]相比,式(9)所示的滤波误差补
偿机制,使指令滤波反步法的跟踪误差更小.
5
仿真与实验
5.1
仿真
为了验证算法的有效性,本文通过MATLAB 对
算法进行仿真,并与文献[12]中的动态面方法进行比较.
以固定于期望位姿的坐标系F ∗作为参考坐标系,初始位姿为(−1.5m,−0.3m,22◦).指令滤波反步法的控制参数如下:
k y =3,k x =5,k v =1.2,k θ=2,k ω=1,k ξ=0.2.
滤波器阻尼系数ζ=1,带宽ωn =30.
动态面控制律参数设置如下:
k 1=2.4,k 2=0.4,k 3=1.15,γ0=0.7,γ1=0.2,k ξ=0.2.
滤波时间常数τ=0.03.
仿真结果如图所示.其中图4表示指令滤波反步法的跟踪误差状态量变化曲线,跟踪误差最终均收敛至0.图5表示根据设计的输入信号积分得到的平滑的速度信号
.
第
12
期李晨萍等:基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制2239
图4指令滤波反步法的跟踪误差
Fig.4
Tracking error signals of the command filter
backstepping method
图5机器人速度(指令滤波反步法)
Fig.5Velocities of the mobile robot in command fil-ter backstepping method
图6是指令滤波反步法和动态面方法对应的误差信号变化曲线
,
最终的稳态位姿误差如表
1所示.可以看出,指令滤波反步法比动态面方法收敛更快,精度更高.
图6误差信号曲线Fig.6Evolution of error signals
表1稳态位姿误差比较
Table 1Comparison of control errors
稳态位姿误差e x /m e y /m e θ/rad 动态面方法[12]9.37×10−5−0.0153−0.0036本文控制器
1.52×10−7
−2.58×10−7
−0.00053
5.2实验
本文通过实验验证了算法的有效性.如图7所示,实验平台是Pioneer3-DX 差分移动机器人,并于轮轴中心上方安装了RealSense D435i 相机(实验仅利用单目图像作为反馈信息),图像频率为30Hz,分辨率为1280×720像素.用于图像处理和控制器计算的笔记本电脑配置为2.6-GHz Intel Core i7-6700HQ CPU.以期望位姿作为参考坐标系,设置初始位姿为(−1.33m ,−0.25m ,10.3◦).实验环境是无人工陆标的实验室自然场景,期望图像、初始图像和伺服完成采集的图像如图8所示.
图9和图10分别表示控制器输出的加速度、速度命令曲线和误差变化曲线.通过简单的平移运动进行SLAM 的地图初始化和尺度初始化,该阶段角速度为0,设置固定的线加速度.之后通过两步法关联期望图像和SLAM 地图,进而获得误差反馈信号.可以看出,即使初始位置具有较大的位姿误差,速度依然平滑增长;同时,
期望位姿优化带来误差信号突变时,速度依然保持平滑变化,可以减小因惯性造成的执行误差引起的SLAM 模块预测误差.
图7移动机器人视觉伺服实验平台Fig.7Experimental platform of visual servoing
2240控制理论与应用第39卷
澳
图8视觉伺服图像(初始图像、期望图像、最终图像)Fig.8Images captured during visual servoing experi-ment (initial,desired and final
images)
图9加速度、速度命令曲线Fig.9Acceleration and velocity signals
6结论
针对移动机器人视觉伺服位姿镇定,本文设计
了基于视觉SLAM–伺服框架的指令滤波反步控制
策略,在解算反馈位姿的SLAM 模块中,利用速度信息对单目未知深度信息引起的位姿尺度因子进行在线估计和优化,进而减小伺服控制器设计的复杂度;同时,伺服控制模块通过设计加速度积分得到光滑的速度信号,减小执行误差造成的SLAM 位姿预测误差;基于指令滤波反步法的位姿镇定控制器,通过滤波误差补偿机制避免了动态面方法因滤波误差造成的系统误差.基于李雅普诺夫稳定性理论证明了闭环系统的稳定性,同时仿真和实验也验证了算法的有效性和误差补偿机制的优越性
.
图10反馈误差信号曲线
Fig.10Evolution of error signals in experiment
参考文献:
[1]XIAO H,LI Z,YANG C,et al.Robust stabilization of a wheeled
mobile robot using model predictive control based on neurodynam-ics optimization.IEEE Transactions on Industrial Electronics ,2017,64(1):505–516.[2]LI B,ZHANG X,FANG Y ,et al.Visual servo regulation of wheeled
mobile robots with simultaneous depth identification.IEEE Transac-tions on Industrial Electronics ,2018,65(1):460–469.[3]QIU Z,HU S,LIANG X.Disturbance observer based adaptive mod-el predictive control for uncalibrated visual servoing in constrained environments.ISA Transactions ,2020,106:40–50.[4]CHEN J,JIA B,ZHANG K.Trifocal tensor-based adaptive visual
trajectory tracking control of mobile robots.IEEE Transactions on Cybernetics ,2017,47(11):3784–3798.[5]BROCKETT R.The early days of geometric nonlinear control.Auto-matica ,2014,50(9):2203–2224.[6]CHEN Hua,WANG Chaoli,YANG Fang,et al.Finite-time saturated
stabilization of nonholonomic mobile robots based on visual servo-ing.Control Theory &Applications ,2012,29(6):817–823.
(陈华,王朝立,杨芳,等.基于视觉伺服非完整移动机器人的有限时间饱和镇定.控制理论与应用,2012,29(6):817–823.)[7]CAO Yu,LIU Shan.Switching control for homography-based mobile
robot visual servoing.Control Theory &Applications ,2017,34(1):109–119.
(曹雨,刘山.基于单应性矩阵的移动机器人视觉伺服切换控制.控制理论与应用,2017,34(1):109–119.)
第12期李晨萍等:基于视觉SLAM–伺服框架的移动机器人指令滤波反步控制2241
[8]SHI H,SUN G,WANG Y,et al.Adaptive image-based visual servo-
ing with temporary loss of the visual signal.IEEE Transactions on
Industrial Informatics,2018,15(4):1956–1965.
[9]SHI H,XU M,HWANG K S.A fuzzy adaptive approach to decou-
pled visual servoing for a wheeled mobile robot.IEEE Transactions
on Fuzzy Systems,2020,28(12):3229–3243.
[10]ZHANG K,CHEN J,LI Y,et al.Unified visual servoing tracking
and regulation of wheeled mobile robots with an uncalibrated cam-
era.IEEE/ASME Transactions on Mechatronics,2018,23(4):1728–
1739.
[11]ORTIGOZA R S,SANCHEZ J R G,GUZMAN V M H,et al.Tra-
jectory tracking control for a differential drive wheeled mobile robot
considering the dynamics related to the actuators and power stage.
IEEE Latin America Transactions,2016,14(2):657–664.
[12]ZHANG X,WANG R,FANG Y,et al.Acceleration-level pseudo-
dynamic visual servoing of mobile robots with backstepping and dy-
namic surface control.IEEE Transactions on Systems,Man,and Cy-
bernetics:Systems,2019,49(10):2071–2081.
[13]LI C,ZHANG X,GAO H,et al.Bridging the gap between visual ser-
voing and visual SLAM:A novel integrated interactive framework.
IEEE Transactions on Automation Science and Engineering,2021,
19(3):2245–2255.
[14]FARRELL J A,POLYCARPOU M,SHARMA M,et -
mandfiltered backstepping.IEEE Transactions on Automatic Con-
trol,2009,54(6):1391–1395.
附录
平移系统反步法状态量定义为
z1=e y,z2=e x−e x d,z3=v−v d,
其中e x d和v d表示虚拟输入量,可得平移系统状态模型如下:
˙z1=−ω(z2+e x d),
˙z2=ωz1−z3−v d−˙e x d,
˙z3=u1−˙v d.
(A.1)
针对z1,z2子系统,设计虚拟期望输入量e x d,v d如下:
e x d=k1ωz1,(A.2)
v d=k2z2−˙e x d,(A.3)其中k1,k2∈R+是正的常数增益.˙e x d可由式(A.2)求导得到
˙e x d=k1(˙ωz1+ω˙z1).(A.4)针对跟踪误差状态量z3,设计线加速度u1如下:
u1=−k3z3+˙v d+z2,(A.5)其中:
˙v d=k2˙z2−¨e x d,(A.6)¨e x d=−3k1ω˙ωz2−3k1ω˙ωe x d+k1¨ωz1−k1ω2˙e x d+ k1k2ω2z2+k1ω2z3−k1ω3z1.(A.7)旋转系统的反步法状态量定义为
z4=eθ,z5=ω−ωd,(A.8)其中:ωd表示虚拟角速度输入.因此旋转子系统状态模型为
{
˙z4=ωd+z5,
˙z5=u2−˙ωd,
(A.9)对于z4子系统,设计虚拟输入量ωd如下
ωd=−k4z4.(A.10)对其求导可得
˙ωd=−k4ω.(A.11)考虑跟踪误差状态量z5,设计角加速度u2如下:
u2=−k5z5−k4ω−z4+k′ξz1sin t.(A.12)定理3对于系统模型(A.1)和(A.9),反步控制律(A.2)–(A.7)和(A.10)–(A.12)可以使系统误差收敛至0,即
lim
t→∞
z1=0,lim
t→∞
z2=0,lim
t→∞
z3=0,
lim
t→∞
z4=0,lim
t→∞
z5=0.
构造李雅普诺夫候选函数如下:
V=
1
2
z21+
1
2
z22+
1
2
z23.(A.13)对其进行求导,并将式(A.1)–(A.5)代入,可得
˙V=−k
1ω
2z2
1−k2z
2
2−k3z
2
3 0.(A.14)定理3证明与定理1证明类似:
1)李雅普诺夫理论和式(A.13)–(A.14),可知z1,z2,z3∈L∞.
2)根据反步控制律可知式(A.15)所示的旋转子系统状态空间,其中c=k4k5+1>0,d=k4+k5>0.系数矩阵特征根位于复平面左侧,因此旋转子系统状态量eθ,ω∈L∞.
[
˙eθ
˙ω
]
=
[
01
−c−d
][
eθ
ω
]
+
[
1
]
k′ξz1sin t.(A.15)
3)由z1,z2,z3,eθ,ω∈L∞和式(A.2)–(A.12),可证明e xd, v d,˙e xd,˙v d,˙z1,˙z2,˙z3∈L∞.类似定理1的步骤3,根据式(A.13)(A.14)和芭芭拉引理可证
lim
t→∞
ωz1=0,lim
t→∞
z2=0,lim
t→∞
z3=0.(A.16)
4)平移子系统状态量z1趋于0.证明过程同定理1的步骤4,应用两次芭芭拉引理可证lim
t→∞
z1=0.
5)由lim
t→∞
z1=0可得lim
t→∞
k′ξz1sin(t)=0,即旋转子系
统的控制输入信号趋于0.结合式(A.15)可知lim
t→∞
eθ=0,
lim
t→∞
ω=0.根据式(A.8)(A.10)可得
lim
t→∞
z4=0,lim
t→∞
z5=0.
作者简介:
李晨萍博士,实验师,主要研究方向为移动机器人视觉伺服、视觉SLAM,E-mail:**********************;
张雪波教授,博士生导师,主要研究方向为移动机器人学、视觉伺服、运动规划、无人车自主导航、强化学习与智能博弈、多摄像机网络,E-mail:*********************.cn;
王润花讲师,硕士生导师,主要研究方向为移动机器人、视觉伺服、运动规划,E-mail:******************.cn;
李宝全教授,博士生导师,主要研究方向为移动机器人、计算机视觉、非线性控制、无人机,E-mail:*****************.cn;
方勇纯教授,博士生导师,主要研究方向为机器人视觉控制、无人机、欠驱动吊车、微纳米操作,E-mail:*****************.cn.。