基于非奇异终端滑模的水下六足机器人姿态精确控制.pdf

无人系统技术Unmanned Systems Technology第6卷 第4期2023年8月Vol.6 No.4August 2023基于非奇异终端滑模的水下六足机器人姿态精确控制陈刚1*，施良才1，张晶晶2，施建伟2（1.浙江理工大学机械工程学院，杭州310018；2.浙江凯富博科科技有限公司，金华321035）摘要：水下机器人是开发海洋的重要工具，水下六足机器人由于其卓越的避障能力而受到格外关注。而水下六足机器人的姿态控制准确度是影响其避障能力的重要因素。为了解决水下六足机器人姿态精确控制问题，提出了一种基于非奇异终端滑模的姿态控制方法。首先建立水下六足机器人的运动学模型，提出了基于落足点的姿态控制方法。然后将落足点模型和非奇异终端滑模结合，采用三段式趋近律以加快趋近速度，并证明了稳定性。最后对水下六足机器人进行了仿真，并与传统滑模和PID进行了对比。阶跃跟踪和正弦跟踪仿真表明，改进的非奇异终端滑模响应速度提高了0.10.2 s，跟踪误差始终小于0.1。改进的非奇异终端滑模控制方法不仅收敛速度快于传统滑模和PID，跟踪精度也高于传统滑模和PID，进一步验证了基于非奇异终端滑模的姿态控制器的有效性。关键词：水下六足机器人；机器人学；运动学；非奇异终端滑模；姿态跟踪；联合仿真中图分类号：TP242.6 文献标识码：A 文章编号：2096-5915（2023）04-040-11DOI：10.19942/j.issn.2096-5915.2023.04.36Precise Attitude Control of Underwater Hexapod Robot Based on Nonsingular Terminal Sliding Mode ControlCHEN Gang1*，SHI Liangcai1，ZHANG Jingjing2，SHI Jianwei2（1.College of Mechanical Engineering，Zhejiang Sci-Tech University，Hangzhou 310018，China；2.Zhejiang Kaifu Boke Technology Co.，Ltd，Jinhua 321035，China）Abstract:Underwater robots are important tools for exploiting the oceans,and underwater hexapod robots are receiving extra attention due to their superior obstacle avoidance capabilities.The attitude control accuracy of underwater hexapod robots is an important factor affecting their obstacle avoidance ability.In order to solve the problem of accurate attitude control of underwater hexapod robot,an attitude control method based on nonsingular terminal sliding mode control is proposed.Firstly,the kinematics model of the underwater hexapod robot is established,and the attitude control method based on the foot dropping point is proposed.Then,the foot drop point model is combined with the nonsingular terminal sliding mode control,and a three-stage 收稿日期：2022-11-26；修回日期：2023-02-22基金项目：浙江省“尖兵”“领雁”研发攻关计划项目（2023C03015）；浙江省自然科学基金（LY20E050018）；国家自然科学基金（51875528，41506116）通讯作者：陈刚，博士，特聘教授。引用格式：陈刚，施良才，张晶晶，等.基于非奇异终端滑模的水下六足机器人姿态精确控制 J.无人系统技术，2023，6（4）：40-50.Chen G，Shi L C，Zhang J J，et al.Precise Attitude Control of Underwater Hexapod Robot Based on Nonsingular Terminal Sliding Mode Control J.Unmanned Systems Technology，2023，6（4）：40-50.第 4 期陈刚等：基于非奇异终端滑模的水下六足机器人姿态精确控制approach law is adopted to speed up the approach rate,and the stability is proved.Finally,the underwater hexapod robot is simulated and compared with the traditional sliding mode control and PID.The simulation of step tracking and sinusoidal tracking shows that the response speed of the improved nonsingular terminal sliding mode control is improved by 0.10.2 s,and the tracking error is always less than 0.1.The improved nonsingular terminal sliding mode control method not only has faster convergence rate than traditional sliding mode control and PID,but also has higher tracking accuracy than traditional sliding mode control and PID,which further verifies the effectiveness of the attitude controller based on nonsingular terminal sliding mode control.Key words:Underwater Hexapod Robot；Robotics；Kinematics；Non-singular Terminal Sliding Mode；Attitude Tracking；Joint Simulation1 引 言 21世纪是海洋的世纪，海洋拥有丰富的资源，对人类社会发展起着至关重要的作用，国家也倾注了很多资源在海洋探索上。潜水器是海洋探索中非常重要的工具，使得海洋探索向更深更广的领域进展。常说的水下机器人主要分为ROV（Remotely Operated Vehicle）和 AUV（Autonomous Underwater Vehicle）。除了这两种，还有一种为水下步行机器人，能够完成ROV和AUV不能完成的任务。足式具有越障能力强的优势，更加适合水底环境1-2。并且六足在行走中是采用三条腿站立，该运动的模式具有良好的稳定性3-5。对于海洋探索来说，海流会对机器人的稳定性造成巨大的影响。水下六足机器人可以根据海流的变化调整自身姿态，以减少受到的影响，提高稳定性。因此，首要解决的问题是如何保证水下机器人的姿态精确控制，以保证机器人的稳定性。滑模控制由于鲁棒性强、对干扰不敏感、对于水下复杂环境的适应性更好的特性而受到了广泛的关注。滑模控制是一种非线性控制方法，其通过在滑模面不断来回穿梭，使得系统误差逐渐收敛到0，因此对外界干扰不敏感。滑模控制是在系统渐进稳定的前提下设计出来，为了加快系统收敛速度，Zak等6提出了终端滑模，大大加快收敛速度，实现了有限时间收敛。但终端滑模存在奇异现象，Feng等7-9提出的非奇异终端滑模解决了奇异问题。高阶滑模控制通过设计连续的控制率，解决了传统滑模因符号函数的高频切换带来的抖振10-13。在水下机器人领域，Mobayen等14提出了二阶滑模控制，使得水下机器人的下潜深度得到了良好的控制。Zhou等15提出了基于Hurwitz的滑模控制方法，通过Hurwitz矩阵确定最佳滑模面，大大削弱了抖振。Ding等16通过将模糊和滑模控制相结合的方式，使得水下机器人能够在波浪的干扰下保持稳定。Hong等17提出了一种新型滑模控制方法，使得浮力对水下机器人的下潜深度和俯仰角影响大大降低。虽然滑模控制方法已有大量研究，但这些方法在水下足式机器人的应用上能否得到良好的控制效果，尚无明确的研究。因此本文针对水下六足机器人提出了一种基于非奇异终端滑模控制方法。2 六足机器人运动学模型分析 本文设计的水下机器人由机器人机体和六条腿组成，六条腿呈 3-3 制对称布局，如图 1（a）所示，图中数字为各条腿的编号。每条腿由根关节、髋关节、膝关节三个关节构成，腿的末端采用球形结构。机体由长方体框架和圆筒构成，电气元件置于密封的圆筒中，圆筒的引出线缆为上位机的控制线缆和电源线缆。机器人具有6个自由度，即3个平动、3个转动，保证机器人可以任意控制机体姿态。机器人质量主要集中于机体上，腿的质量远小于机体质量，并且机器人六条腿为均匀对称分布，因此可以认为机器人的重心和形心重合。2.1水下六足机器人D-H建模水下六足机器人的每条腿可以看作三自由度的三连杆机构，机体可以看作长方体框架和圆筒的组合。如图2所示，建立世界坐标系W、机器41第 6 卷无人系统技术图2水下六足机器人D-H模型Fig.2D-H model of underwater hexapod robot531642根关节髋关节膝关节(a)机器人模型(b)机器人实物模型 图1机器人模型图Fig.1Robot model diagram42第 4 期陈刚等：基于非奇异终端滑模的水下六足机器人姿态精确控制人机体坐标系B，其中世界坐标系W 与大地固定不动，机器人机体坐标系B与机器人本体固连。机器人机体坐标系B的坐标原点位于机器人重心，x轴指向3号腿并与机体横轴平行，z轴垂直向上于机体，y轴由右手定则确定。机器人腿部根关节坐标系U0j与机器人机体坐标系B平行，位于每条腿的根关节处。并且按照D-H建立腿部坐 标 系U1j、U2j、U3j，j=1，2，3，4，5，6。由于腿1、2坐标系建立方法相同，腿36坐标系建立方法相同，所以图2用腿2和腿5的坐标系来代表其他坐标系。水下六足机器人各腿的D-H参数如表12所示。2.2水下六足机器人正运动学机器人正运动学是通过机器人每条腿关节角去求解每条腿的末端位置。根据图2的D-H模型和建立的坐标系，假设已知每条腿的根关节原点坐标U0j(BxU0j，ByU0j，BzU0j)，大腿和小腿长度l2、l3，j腿的各个关节角度1j、2j、3j。那么可以得到坐标系之间的齐次变换矩阵。为了书写方便，定义sj，i=sinj，i，cj，i=cosj，i，i=1，2，3，下同。坐标系U1j相对于坐标系U0j的齐次变换矩阵为01Tj=cj，1-sj，10a0sj，1c0cj，1c0-s0-s0d1sj，1s0cj，1s0c0c0d10001 =cj，1-sj，10000-10-sj，1-cj，1000001（1）坐标系U2j相对于坐标系U1j的齐次变换矩阵为12Tj=cj，2-sj，20a1sj，2c1cj，2c1-s1-s1d2sj，2s1cj，2s1c1c1d20001 =cj，2-sj，2000010-sj，2-cj，2000001（2）坐标系U3j相对于坐标系U2j的齐次变换矩阵为23Tj=cj，3-sj，30a2sj，3c2cj，3c2-s2-s2d3sj，3s2cj，3s2c2c2d30001 =cj，3-sj，30l2sj，3cj，30000100001（3）那么坐标系U3j相对于坐标系U0j的齐次变换矩阵便可由如下公式得到03Tj=01Tj12Tj23Tj=cj，1cj，2，3-cj，1sj，2，3-sj，1l2cj，1cj，2sj，2，3cj，2，30l2sj，2sj，1cj，2，3-sj，1sj，2，3cj，1l2sj，1sj，20001 （4）式中，j，2，3=(j，2+j，3)。假设末端E点的位置为3PEj=l3001T，根据坐标变换理论，得到0PEj的位置为表1腿1、2的D-H参数Table 1D-H parameters of legs 1 and 2连杆j123连杆转角j-1/（）90-900连杆长度aj-1/m00l2连杆偏距dj/m000关节角j/（）（-120，-30）（-60，120）（-120，-0）表2腿3、4、5、6的D-H参数Table 2D-H parameters of legs 3,4,5 and 6连杆j123连杆转角j-1/（）90-900连杆长度aj-1/m00l2连杆偏距dj/m000关节角j/（）（-120，-30）（-120，-60）（0，120）43第 6 卷无人系统技术0PEj=03Tj3PEj =l3cj，1c(j，2+j，3)+l2cj，1cj，2l3sc(j，2+j，3)+l2sj，2l3sj，1c(j，2+j，3)+l2sj，1cj，21（5）假设末端 Ej以机器人机体坐标系B为参考系，那么其位置为 BxEj=BxU0j+l3cj，1c(j，2+j，3)+l2cj，1cj，2ByEj=ByU0j+l3sc(j，2+j，3)+l2sj，2BzEj=BzU0j+l3sj，1c(j，2+j，3)+l2sj，1cj，2（6）3 非奇异终端滑模控制器设计以及姿态闭环控制 3.1水下六足机器人姿态控制模型本文的水下六足机器人姿态控制模型建立在机器人的腿落足于海底陆地上，基于第2节得到的机器人D-H模型，可以得到机器人第j条腿的末端在世界坐标系W的位置为WPEj=WBRBPEj =WBR(BPU0j+BU0jRU0jPEj)（7）式中，WBR为机器人坐标系B相对于世界坐标系的三阶旋转矩阵，BU0jR为坐标系U0j相对于机器人机体坐标系B的旋转矩阵，UjPEj为第j腿的末端位置在坐标系U0j中的表示。由于坐标系U0j与机器人机体坐标系 B坐标轴平行，式（7）可改写为WPEj=WBR(BPU0j+U0jPEj)（8）对式（8）左右两边对时间求导可得WPEj=WBR(BPU0j+U0jPEj)+WBR(BPU0j+U0jPEj)（9）当机器人的腿落足于海底陆地上时，即支撑状态，其末端位置不变，即WPEj=0，并且坐标系U0j为固定坐标系，则BPU0j=0，那么式（9）可以改写为WBRU0jPEj+WBR(BPU0j+U0jPEj)=0（10）由式（7）和式（8）可以得到BPEj=BPU0j+U0jPEj（11）将式（11）代入式（10）可以得到WBRU0jPE+WBRBPEj=0（12）U0jPE可以由第j条腿关节角速度qj表示为U0jPE=Jjqj（13）式中，Jj为j条腿的雅可比矩阵。WBR可以表示为机器人姿态角速度与旋转矩阵WBR的叉乘WBR=HWBR（14）式中，H将世界坐标系W上的姿态角速度转换到机器人坐标系B上H=10-sin0cossincos0-sincoscos（15）将式（13）和式（14）代入式（12）可以得到 WBRJjqj+HWBRBPEj=WBRJjqj+S()WBRBPEj=0（16）式中，S()为H反对称矩阵。由此得到第j腿的姿态控制模型，并推广到机器人整体，得到水下六足机器人姿态控制模型为RJq+S()RPE=0（17）式中R=WBRc00000000WBRt3 3J=Jc00000000Jt3 3S()=S()c00000000S()t3 3（18）q=cq00000000tq3 3 44第 4 期陈刚等：基于非奇异终端滑模的水下六足机器人姿态精确控制PE=PcE00000000PtE3 3为落足支撑腿的数量。3.2基于非奇异终端滑模的水下六足机器人的姿态控制方法首先定义水下六足机器人姿态误差e为e=exp-act（19）式中，act=act act actT为机器人的实际姿态，exp=exp exp expT为机器人的目标姿态。然后，建立非奇异终端滑模面为s=ce+a|e|p/qsign(e)（20）式中，s=s1，s2，s3T，c=diag（c1，c2，c3），a=diag（a1，a2，a3），aj、cj均为正实数，pj、qj均为正奇数，且pj/qj1，j=1，2，3，e=e1，e2，e3T。采用双幂次趋近律结合负指数项的吸引子组成新的趋近律为s=-k1|s|r/gsign(s)+k2s+k3|s|o/usign(s)(1+|e|m/n-1)（21）式中，rj、gj为正奇数，且rj/gj 1，oj、uj为正奇数，且0 oj/uj 1，k1=diag(k11，k12，k13)，k2=diag(k21，k22，k23)，k3=diag(k31，k32，k33)，mj，nj为正奇数，且1 mj/nj 1时，k1|s|r/gsign(s)趋近速度明显比另外两段快，该趋近律起主要作用，当s 0，|s|o/usign(s)sT=so+u/u 0，因 此k1|s|r/g sign(s)sT+k2ssT+k3|so/usign(s)sT 0，得到V0，1 b 1。非奇异终端滑模参数为c=1.5，p/q=15，r/g=15/3，o/u=13/3，k1=1.2，k2=1.5，k3=3，m/n=17/11，滑模控制器参数为cr=2.1，k=1.8，b=5/7，PID参数为Kp=3.6，Ki=0，Kd=0.35。仿真中机器人运动的过程如图5所示。仿真结果如图6和图7所示。在阶跃3仿真实验中，基于非奇异终端滑模姿态控制航向角和横滚角响应时间比采用幂次趋近律的航向角和横滚角响应时间减少0.10.2 s，比PID姿态闭环的航向角和横滚角响应时间减少1.5 s。基于非奇异终端滑模姿态控制的俯仰角比采用幂次趋近律的俯仰角响应时间减少0.30.4 s，比PID姿态闭环俯仰角响应时间减少1.5 s。基于非奇异终端滑模姿态控制航向角稳态误差与采用幂次趋近律的航向角稳态误差相近，远小于PID姿态闭环航向角稳态误差，基图5机器人仿真运动图Fig.5Robot simulation motion diagram图6阶跃3姿态曲线Fig.6Step 3 attitude curve图7阶跃5姿态曲线Fig.7Step 5 attitude curve47第 6 卷无人系统技术于非奇异终端滑模姿态控制的横滚角和俯仰角稳态误差小于采用幂次趋近律和PID姿态闭环的横滚角和俯仰角稳态误差。在阶跃5的仿真实验中，采用幂次趋近律的阶跃响应速度有所加快，但仍明显小于基于非奇异终端滑模姿态控制的响应速度。基于非奇异终端滑模姿态控制和幂次趋近律的姿态控制航向角稳态误差相近，但基于非奇异终端滑模姿态控制的横滚角和俯仰角稳态误差明显优于采用幂次趋近律姿态闭环控制和PID姿态闭环控制的横滚角和俯仰角稳态误差。通过阶跃响应仿真结果可以发现，基于非奇异终端滑模姿态闭环控制器（NTSMC）的响应时间明显小于采用幂次趋近律滑模姿态闭环控制器（SMC）和PID姿态闭环控制器，并且稳态误差也得到一定的减小。4.2基于非奇异终端滑模的水下六足机器人的姿态正弦跟踪仿真采用3种正弦跟踪仿真实验，验证水下六足机器人非奇异终端滑模姿态控制（NTSMC）的鲁棒性。并将水下六足机器人的滑模控制器（SMC）和PID控制器进行对比，其中这些控制器的参数与阶跃仿真中完全一致，正弦跟踪曲线为，=Asin(t)，Asin(t)，Asin(t)（29）式中，A为幅值数值为2，为角频率数值为/10、/20以及/30。正弦跟踪仿真结果如图810所示。在相同幅图82sin(t/10)姿态曲线Fig.8Attitude curve of 2sin(t/10)图92sin(t/20)姿态曲线Fig.9Attitude curve of 2sin(t/20)48第 4 期陈刚等：基于非奇异终端滑模的水下六足机器人姿态精确控制值的条件下，正弦跟踪误差会随着周期的增加而减小。从图中可以得到，基于非奇异终端滑模姿态闭环控制的姿态正弦跟踪误差始终小于采用幂次趋近律的滑模姿态控制和PID姿态闭环控制的姿态正弦跟踪误差。幂次趋近律的姿态跟踪误差与基于非奇异终端滑模姿态闭环控制的姿态正弦跟踪误差的差值会随着周期的增加而减小，但幂次趋近律的姿态跟踪误差始终大于基于非奇异终端滑模姿态闭环控制的姿态正弦跟踪误差。基于PID的姿态闭环控制的正弦跟踪存在明显的时滞性。基于非奇异终端滑模姿态闭环控制具有良好的鲁棒性和精确性，其正弦跟踪误差小于0.1。5 结 论 本文基于提出的机器人运动学模型，建立了基于落足点的姿态控制方法，并与非奇异终端滑模控制器结合起来进行了稳定性证明，提出了适用于水下六足机器人的非奇异终端滑模姿态闭环控制器。非奇异终端滑模姿态闭环控制器采用三段式趋近律使得收敛速度得到进一步提升。最后经过仿真验证了非奇异终端滑模姿态闭环控制的姿态跟踪性能，同时阶跃跟踪和正弦跟踪仿真表明，其拥有比PID和传统滑模更快的响应速度、收敛速度和更好的跟踪性能，能够快速消除姿态误差，姿态误差始终小于0.1，响应速度提高0.10.2 s。仿真充分验证了所提出的非奇异终端滑模控制器对水下六足机器人姿态跟踪的有效性。后续在仿真实验基础上，将对水下六足机器人开展实物实验以验证本文所提出的控制器性能。由于趋近律参数对整个系统的收敛速度有较大影响，后续将自适应方法与非奇异终端滑模控制器结合，并根据系统误差调整趋近律参数，使系统收敛速度得到进一步提升。参 考 文 献1 Chai X，Gao F，Chenkun Q I，et al.Obstacle avoidance for a hexapod robot in unknown environmentJ.Science China Technological Sciences，2017，06：818-831.2 张秀丽，郑浩峻，陈恳，等.机器人仿生学研究综述J.机器人，2002，24（2）：188-192.3 Reina G，Foglia M.On the mobility of all-terrain roversJ.Industrial Robot，2013，40（2）：121-131.4 Shen-Chiang，Chen，Ke-Jung，et al.Quattroped：a leg-wheel transformable robotJ.IEEE/ASME Transactions on Mechatronics，2013，19（2）：730-742.5 Neumann M，Predki T，Heckes L，et al.Snake-like，tracked，mobile robot with active flippers for urban search-and-rescue tasksJ.Industrial Robot，2013，40（3）：246-250.6 Zak M.Terminal attractors for addressable memory in neural networks J.Physics Letters A，1988，133（1-2）：18-22.7 Feng Y，Yu X，Man Z.Non-singular terminal sliding 图102sin(t/30)姿态曲线Fig.10Attitude curve of 2sin(t/30)49第 6 卷无人系统技术mode control of rigid manipulatorsJ.Automatica，2002，38（12）：2159-2167.8 Shen J S，Zhou X C，Zhang H G.Research on sliding mode control for near-surface AUV depth regulation in waves circumstance C.2010 International Conference on Electrical and Control Engineerin，Wuhan，China，June 25-27，2010.9 Feng Y，Yu X，Han F.On nonsingular terminal sliding-mode control of nonlinear systemsJ.Automatica，2013，49（6）：1715-1722.10 Zhai J，Xu G.A novel non-singular terminal sliding mode trajectory tracking control for robotic manipulatorsJ.IEEE Transactions on Circuits and Systems II：Express Briefs，2020（99）：1.11 Zhang R，Sun C，Zhang J，et al.Second-order terminal sliding mode control for hypersonic vehicle in cruising flight with sliding mode disturbance observerJ.Control Theory，2013，11（2）：299-305.12 Mondal S，Mahanta C.Adaptive second order terminal sliding mode controller for robotic manipulatorsJ.Journal of the Franklin Institute，2014，351（4）：2356-2377.13 Van M.Higher-order terminal sliding mode controller for fault accommodation of lipschitz second-order nonlinear systems using fuzzy neural network J.Applied Soft Computing，2021，104（4）：107186.14 Mobayen S，Karami H，Fekih A.Adaptive nonsingular integral-type second order terminal sliding mode tracking controller for uncertain nonlinear systems J.International Journal of Control Automation and Systems，2021，19（7）：1539-1549.15 Zhou H Y，Feng L.State feedback sliding mode control without chattering by constructing hurwitz matrix for AUV movement J.International Journal of Automation&Computing，2011，8（2）：262-268.16 Ding H，Wang D S.Autonomous underwater vehicle heading control based on sliding mode fuzzy control C.Proceedings of the 2nd International Conference on Modelling and Simulation，Manchester，UK，May 21-22，2009.17 Hong E Y，Hong G S，Chitre M.Depth control of an autonomous underwater vehicle，starfish C.OCEANS 10 IEEE，Sydney，Australia，May 24-27，2010.50