2. 西北工业大学 深圳研究院, 广州 深圳 518057;
3. 武警特警指挥学院, 北京 102202
移动机器人在复杂地形探测、战场侦查、地质勘探、仓储物流等方面具有重要的应用前景, 其控制器的设计水平是保证任务成功的关键[1]。机器人控制器的设计开始向着应用于复杂环境, 且能抵抗外部干扰的方向发展。由于能全向移动且转弯半径为零, 基于全向轮的移动机器人受到了广泛关注, 并且被应用于多种场景, 如仓库搬运机器人、全向轮椅等。全向移动机器人中, 麦克纳姆轮驱动的全向移动机器人受到越来越多的关注[2-3]。
由于麦克纳姆轮驱动的全向移动机器人存在诸多优势, 使其在工业界和学术界得到越来越多的关注, 诸多学者对其运动学建模和动力学建模展开了研究[3]。Muir等[4]对此类机器人进行了运动学建模, 从运动学方式上, 给出了基于麦克纳姆轮的全向机器人运动学公式。Tlale等[5-6]为此类机器人建立了动力学模型并从动力学角度对基于麦克纳姆轮的全向机器人展开分析。尽管此类机器人的运动学和动力学模型发展较为成熟, 针对应用于实际环境的移动机器人轨迹跟踪控制研究仍然方兴未艾。
由于4个麦克纳姆轮分别由4个直流电机驱动, 针对此类机器人如何设计多驱动电机的协调控制器成为至关重要的问题。实际工程应用中, 电机选型多选用带有速度伺服系统的电机, 由于电机的速度伺服系统多采用PID控制, 因此伺服控制系统会对整个系统带来更多的不确定性; 此外机器人所运行的环境也具有诸多不确定性, 因此控制器的鲁棒性和自适应性就显得至关重要。Shimada等[7]提出了一种基于位置误差的控制方法, 并研制了实验装置, 对所提出的方法进行了实验验证。Wang等[8]在不考虑扰动的情况下, 提出了一种用于轨迹规划的滑动模态控制器, Viet等[9]考虑了有扰动的情况, 在外加扰动的情况下针对全向移动机器人设计了滑模控制器, 其研究结果表明滑模控制器能有效抑制外部干扰。Fierro等[10]针对全向移动机器人的轨迹规划设计了一种神经网络控制器。另外, Xu等[11]针对全向移动机器人设计了一种基于神经网络的鲁棒滑模控制器。基于神经网络的控制器具有控制精度高等优点, 但需要大量学习, 因此具体实现上存在不便。以上控制器存在参数难以整定的问题, 如何设计一种高效的自适应控制器是解决此问题的关键。Wang等[12]针对移动机器人的轨迹规划设计了一种自适应控制器, 并证明了其稳定性。Sira-Ramirez等[13]设计了一种能够快速收敛的自适应控制器, 以应对直流电机的不确定性。
综上所述, 本文的研究对象具有确定的运动学模型, 适合采用基于模型的控制方法。常见的基于模型的控制方法有:LQR控制方法[14]以及结合LQR控制方法优点与卡尔曼滤波器的控制方法LQG控制[15], 然而LQR控制虽然具有良好的鲁棒性, 但其计算复杂且对模型精确度要求过高。而LQG控制鲁棒性较差, 并且该类方法多用于线性系统控制器设计。通过对移动机器人的运动学建模可知, 基于麦克纳姆轮的全向移动机器人是一类多输入多输出(MIMO)的非线性系统, 而滑动模态控制[16]是一种针对不确定条件且具有鲁棒性的非线性控制器。因此本文提出基于滑模控制的自适应鲁棒轨迹跟踪控制器。滑模控制器的常见问题为控制器输出的抖振问题, 马广富等[17]提出了一种多幂次滑模趋近律, 能够显著减小抖振, 并且通过仿真进行了验证, 但其不具有自适应性。为使系统具有更高的鲁棒性, 本文设计了一种比例-微分-积分形式的滑模面来提高系统的鲁棒性, 并且提出了一种多幂次趋近律来提高系统的动态性能, 最后为避免参数整定的麻烦操作, 本文设计了自适应律以提高系统的自适应性, 借此可以解决上述问题。
1 运动学模型麦克纳姆轮是一种在轮表面存在一些滚子(如图 1所示)。
这些滚子的转轴方向与整个轮的转轴方向呈45°夹角, 每个麦克纳姆轮由一个独立的直流电机驱动, 通过调节4只电机的不同转速, 即可实现全向的移动, 比如横向和纵向移动, 倾斜移动和零转弯角度转动(如图 2所示)。
对麦克纳姆轮驱动的全向移动机器人进行运动学建模, 首先要建立相关的坐标系(如图 3所示), Oixiyi为惯性坐标系(inertial coordinate system), Obxbyb为连体坐标系(body-fixed coordinate system), 其原点Ob为车体的几何中心, 其y轴始终指向车体纵轴, x轴与y轴垂直并组成右手坐标系, Owixwiywi为第i只轮子的轮系坐标系, 其原点位于麦克纳姆轮的几何中心, x轴与驱动电机输出轴平行, y轴与x轴垂直并组成右手坐标系。规定车体右前方的轮子为1号轮, 顺序按逆时针排序。
如图 1所示, 定义麦克纳姆轮滚子的转轴与Owixwiywi的x轴所呈夹角为ψ, 定义第i只轮Owixwiywi位置矢量pwi(i=1, 2, 3, 4)=[xwi, ywi, zwi]T, ωix为第i只轮的角速度, ωiy为第i只轮上的滚子角速度, ωiz为麦克纳姆轮绕过Owi且垂直于Owixwiywi平面转轴的角速度。Ri(i=1, 2, 3, 4)为第i只轮子的半径, ri为滚子半径, 轮系坐标系中轮子的速度矢量可以表示为:
(1) |
根据旋转矩阵, 可以获得如(2)式所示的速度计算公式:
(2) |
式中,
对雅可比矩阵分析可以得出如下结论:
1) 当且仅当ψi=0时, |Ji|=0。所以对于麦克纳姆轮来说, 此时雅可比矩阵是奇异的。
2) 值得注意的是, rank(Ji)=3, 所以每个轮子具有3个自由度。
在本文模型中, 每个麦克纳姆轮规格相同, 因此R1=R2=R3=R4=R, r1=r2=r3=r4=r, dw1xb=a, dw1yb=b, dw2xb=-a, dw2xb=b, dw3xb=-a, dw3yb=-b, dw4xb=a, dw4yb=-b, 针对每个轮子可以列写其雅可比矩阵如下:
(3) |
通过(2)式和(3)式, 移动机器人的微分运动学公式可以描述如下:
(4) |
式中, ωi(i=1, 2, 3, 4)是第i只轮的转速, 单位为rad/s。通过运动学公式不难发现, 基于麦克纳姆轮的移动机器人可以实现真正的全向移动。
闭环控制的反馈为静止坐标系下的位姿信息。进一步地, 根据旋转矩阵, 可得到连体坐标系下的速度Obxbyb和惯性坐标系下的速度Oixiyi的转换关系为:
(5) |
滑模控制器设计的第一个任务就是设计滑模面, 在控制系统工作的过程中, 保证系统始终工作在滑模面s(t)=0上。
首先定义误差向量为:e(t)=e1e2e3T, 轨迹的期望位姿为pd(t)=xdydφdT, 实际位姿为pi(t)=xiyiφiT, e(t)=pd(t)-pi(t)。显然地, 滑模面是关于误差的函数, 目前常用的滑模面是比例-微分形式的滑模面, 此前的研究表明, 滑模面中积分项的引入会显著地提高系统的鲁棒性, 因此本文选取一种比例-微分-积分形式的滑模面用于提高系统的鲁棒性[18], 同时可以适应不同的情况。滑模面设计如下:
(6) |
(7) |
在滑模控制器中, 常用的趋近律有:等速趋近律、指数趋近律、幂次趋近律、一般趋近律[12, 18]。其中幂次趋近律具有较好的收敛速度, 更进一步地, 为了保证较高的收敛速度, 并且尽量减小控制器的抖振问题, 本文设计一种多幂次滑模趋近律以达到控制目标, 并通过仿真验证其在系统动态响应过程中具有快速的收敛速度。本文选用的多幂次滑模趋近律如下:
(8) |
式中
(9) |
符号函数sgn()的引入会使系统的工作过程出现振荡。因此, 为了减小符号函数对系统的影响, 本文对趋近律进行如下改进, 引入饱和函数, 可有效削弱趋近律在滑模面附近的跳变, 以改善抖振问题:
(10) |
式中, δi为正小量:
(11) |
稳定性证明如下:首先选取李雅普诺夫函数
(12) |
(13) |
当且仅当si(t)=0, (i=1, 2, 3)时等号成立, 故该系统是李雅普诺夫稳定的。
2.3 自适应律设计由于环境的不确定性和电机速度伺服系统的不确定性, 自适应律的参数需要不断整定, 为避免后续的参数整定, 本文为所设计的滑模控制器设计了专门的自适应律, 以提高控制器的自适应性。公式(10)中的参数εi会对系统的性能产生比较大的影响。因此, 为该参数设计自适应律是十分必要的, 规定
(14) |
自适应律设计如下:
(15) |
稳定性证明如下:如(13)式所示已经证明了李雅普诺夫函数V1的稳定性, 现取李雅普诺夫函数如下:
(16) |
由于
(17) |
将公式(15)代入上式, 可得:
(18) |
稳定性得证。
对公式(6)两端同时求导, 可以得到:
(19) |
结合(14)式可以得到:
(20) |
进一步可以得到控制信号:
(21) |
结合公式(4)和公式(5), 可以得到各个轮的转速:
(22) |
图 4展示了本文所提控制器的设计流程。首先对机器人的运动学进行建模, 此后为保证对此类非线性耦合系统能够提供优越的控制性能, 本文设计以滑模控制器为基础; 为了提高控制器的鲁棒性, 选取了比例-积分-微分形式的滑模面; 为保证系统的动态性能, 选取了适当的趋近律; 为了避免整定参数, 同时使系统具有更好的自适应性, 设计了一种自适应律。然后针对所设计的控制器在仿真中加入扰动并与PID控制的普通滑模控制(SMC)的效果进行了对比, 最后通过实验验证的方式展示了本文所设计控制器的实用性。
3.2 仿真验证为验证本文所提的自适应滑动模态控制器的鲁棒性和动态性能, 首先设计了一条轨迹, 通过对标称轨迹的跟踪控制, 并且在其中加入了脉冲扰动, 与传统PID控制器和无自适应性的SMC控制器进行对比验证, 分析本文所设计控制器的性能。本文测试使用的标称轨迹可表示如下:
[xi yi φi]T=[0 m -0.2 m 0 m]T为系统的初始位置。同时, 为了验证系统的动态性能和鲁棒性, 实验中加入了扰动, 在x轴和y轴方向分别加入幅值为0.1 m, 频率为0.4 rad/s的正弦信号, 同时在10 s和30 s分别加入幅值为0.1 m和-0.2 m的阶跃信号。仿真测试的系数设置如下:
对于SMC控制器和ASMC控制器:
在SMC控制器中ε1=ε2=ε3=2。在ASMC控制器中ρ1=1.5, ρ2=4.5, ρ3=1.5,
仿真结果如图 5所示, 可以看出3种控制器均能有效地抑制干扰, 由于ASMC初始的系数
为了更加直观地分析3种控制器的控制效果, 对x轴和y轴方向的偏差进行统计分析, 如图 7和图 8所示。
通过图 7和图 8可以明显地看出, ASMC控制器虽然在控制的开始阶段, 系统的误差要大于其他两种控制器, 但是随着时间的推移, ASMC控制器的控制效果逐渐改良, 并取得了优于其他2种控制器的控制效果。滑模控制器的最大问题在于控制信号的抖振, 前文中已经提到, 多幂次趋近律的引入可以明显的改善系统抖振, 为验证其效果, 参见图 9和图 10。
为准确分析各控制器性能, 本文取x轴坐标误差和y轴坐标误差的均值作为控制器的控制效果, 取x轴速度和y轴速度的输出反映控制器的抖振情况统计结果见表格2和表格3。
偏差 | PID | SMC | ASMC |
x/m | 2.233 8×10-4 | -1.962 2×10-4 | -3.674 6×10-6 |
y/m | 1.934 3×10-4 | -2.024 0×10-4 | -6.164 6×10-5 |
对表 1进行分析, ASMC控制器的x轴偏差的平均值仅为PID控制器的1.64%, 为SMC控制器的1.87%, ASMC的y轴控制偏差为PID的31.87%, 为SMC控制器的30.46%, 证明本文设计的ASMC具有更好的控制效果。分析表 2, ASMC控制器的x轴输出方差为PID控制器的10.42%, ASMC的y轴输出方差为PID的57.03%, 证明本文ASMC具有更好的控制效果。此外, 本文设计的ASMC控制器的抖振情况优于PID控制器。
4 实验验证与分析本文的实验环境如图 11所示, 实验样机(轮式机器人)和实验所用软件如图 12所示。
本文实验验证设备电控流程图如图 13, 由Kinect深度相机连接PC端, 通过USB通信经过机器视觉处理获得机器人的x轴和y轴的坐标信息, 进而通过串口将信息传输给STM32VET6单片机, VET6单片机利用SPI通信借助NRF24L01通过2.4GHz通信, 将x轴和y轴的坐标信息传输到STM32ZET6单片机, 本文定义其为一号下位机, 而与一号下位机连接的还有IMU(MPU6050)用于提供机器人的φ角度信息, 一号下位机获得这些信息后, 通过与函数发生器对比产生偏差, 进而通过ASMC控制器产生速度控制信号, 并通过UART将信号传出给另外一片STM32ZET6, 本文定义其为二号下位机, 二号下位机接收到速度控制信号后经过逆运动学解算获得4只直流电机的期望速度, 通过PID控制器对电机速度进行控制, 达到控制目的。
实验验证过程中, 跟踪函数使用同3.2节中仿真所用类似的标称轨迹:
移动机器人初始位姿为[xi yi φi]T=[0 0 0]T。通过从kinect相机获得的机器人位置坐标, 绘制出轮式机器人在平面位置的跟踪曲线图如图 14所示。
并绘制距离误差-时间图像如图 15所示, 距离误差计算公式为
本文针对麦克纳姆轮驱动的全向移动机器人的轨迹跟踪问题, 在运动学建模的基础上设计了自适应滑模控制器。控制器设计分为3个步骤:滑模面设计、趋近律设计以及自适应律设计。通过上述3个步骤完成了自适应滑模控制器的设计并利用李雅普诺夫函数证明了其稳定性。存在脉冲扰动以及正弦信号扰动条件下对控制器进行了仿真验证, 并将结果与传统PID控制器和普通滑模控制器利用统计学的方法进行了对比, 统计结果表明本文设计的控制器具有响应快速, 鲁棒性强等优点。最后为验证控制器的实用性, 构建了专用的麦克纳姆轮全向移动机器人, 并利用Kinect深度摄像头和IMU搭建了实验平台对机器人进行了实际的轨迹跟踪控制实验, 结果表明该控制器能够高精度的控制机器人完成轨迹跟踪, 证明了文中所设计控制器的可行性与有效性。
[1] | Martins F N, Celeste W C, Carelli R, et al. An Adaptive Dynamic Controller for Autonomous Mobile Robot Trajectory Tracking[J]. Control Engineering Practice, 2008, 16(11): 1354-1363. DOI:10.1016/j.conengprac.2008.03.004 |
[2] | Rusu P, Petriu E M, Whalen T E, et al. Behavior-Based Neuro-Fuzzy Controller for Mobile Robot Navigation[J]. IEEE Trans on Instrumentation & Measurement, 2002, 52(4): 1335-1340. |
[3] | Tzafestas S G. Introduction to Mobile Robot Control[M]. London: Elsevier, 2014. |
[4] | Muir P F, Neuman C P. Kinematic Modeling for Feedback Control of an Omnidirectional Wheeled Mobile Robot[C]//Proceedings of the IEEE International Conference on Robotics and Automation, 1987: 1772-1778 |
[5] | Tlale N, De Villiers M. Kinematics and Dynamics Modelling of a Mecanum Wheeled Mobile Platform[C]//IEEE International Conference on Mechatronics and Machine Vision in Practice, 2008: 657-662 |
[6] | Wampfler G, Salecker M, Wittenburg J. Kinematics, Dynamics, and Control of Omnidirectional Vehicles with Mecanum Wheels[J]. Mechanics Based Design of Structures and Machines, 1989, 17(2): 165-177. DOI:10.1080/15397738909412814 |
[7] | Shimada A, Yajima S, Viboonchaicheep P, et al. Mecanum-Wheel Vehicle Systems Based on Position Corrective Control[C]//31st Annual Conference of IEEE Industrial Electronics Society, 2005: 2077-2082 |
[8] | Wang Z P, Yang W R, Ding G X. Sliding Mode Control for Trajectory Tracking of Nonholonomic Wheeled Mobile Robots Based on Neural Dynamic Model[C]//Second WRI Global Congress on Intelligent Systems, 2010: 270-273 |
[9] | Viet T D, Doan P T, Hung N, et al. Tracking Control of a Three-Wheeled Omnidirectional Mobile Manipulator System with Disturbance and Friction[J]. Journal of Mechanical Science and Technology, 2012, 26(7): 2197-2211. DOI:10.1007/s12206-012-0541-1 |
[10] | Fierro R, Lewis F L. Control of a Nonholonomic Mobile Robot Using Neural Networks[J]. IEEE Trans on Neural Networks, 1998, 9(4): 589-600. DOI:10.1109/72.701173 |
[11] | Xu D, Zhao D, Yi J, et al. Trajectory Tracking Control of Omnidirectional Wheeled Mobile Manipulators:Robust Neural Network-Based Sliding Mode Approach[J]. IEEE Trans on Systems Man & Cybernetics Part B, 2009, 39(3): 788-799. |
[12] | Wang J, Lu Z, Chen W, et al. An Adaptive Trajectory Tracking Control of Wheeled Mobile Robots[C]//Proceedings of the IEEE Conference on Industrial Electronics and Applications, 2011: 1156-1160 |
[13] | Sira-Ramirez H, Barrios-Cruz E, Marquez R J. Fast Adaptive Trajectory Tracking Control of a Completely Uncertain DC Motor via Output Feedback[C]//Proceedings of the IEEE Conference on Decision and Control, 2007: 4197-4202 |
[14] | Savkin A V, Pathirana P N, Faruqi F A. The Problem of Precision Missile Guidance: LQR and H∞ Control Frameworks[C]//Proceedings of the IEEE Conference on Decision and Control, 2001: 1535-1540 |
[15] | Nurdin H I, James M R, Petersen I R. Coherent Quantum LQG Control[J]. Automatica, 2009, 45(8): 1837-1846. DOI:10.1016/j.automatica.2009.04.018 |
[16] | Utkin V I, Guldner J, Shi J. Sliding Mode Control in Electro-Mechanical Systems[M]. Boca Raton CRC Press, 2009: 881-886. |
[17] | Zhang Y, Ma G F, Guo Y N, et al. A Multi Power Reaching Law of Sliding Mode Control Design and Analysis[J]. Acta Automatica Sinica, 2013. |
[18] |
刘金琨. 滑模变结构控制MATLAB仿真[M]. 北京: 清华大学出版社, 2012.
Liu Jinkun. Sliding Mode Control Design Matlab Simulation[M]. 2nd ed. Beijing: Tsinghua University Press, 2012. (in Chinese) |
2. Research Institute in Shenzhen, Northwestern Polytechnical University, Shenzhen 518057, China;
3. Special Police of China, Beijing 102202, China