液压四足机器人机电液控感仿真平台搭建方法

1.本发明属于液压四足机器人仿真技术领域，特别是一种液压四足机器人机电液控感仿真平台搭建方法。


背景技术：

2.足式机器人作为目前重点发展领域机器人技术的一个分支，落足点是离散的而非连续的，能在很大程度上实现了机器人对地形的灵活适应性。因此足式机器人广泛应用于复杂地形、货物运输等领域。随着液压伺服技术、智能控制技术、新型制造技术和逆向工程等技术的高速发展，在复杂环境下，能够负重运输物资，并且能够快速响应的液压四足机器人成为当下的热点。液压机器人多是仿生设计，参照四足哺乳动物的运动模式对其进行步态规划，实现人类想要达到的运动状态，最终实现辅助人类完成复杂作业或代替人类完成对人类有危害的作业的目标。
3.液压四足机器人是一个复杂的系统，不仅如此，使用者对其可靠性、适应性以及其他的性能都有极高的要求，因此反复地设计和优化是机器人设计中的必要过程。在传统的设计方法中，需要制造样机、试验分析及修改方案等过程。这一过程成本高、周期长，而虚拟仿真技术的出现解决了这一问题。虚拟仿真系统结合计算机计算技术和图形处理技术，围绕机械的运动学、动力学和控制理论提供了一种新的设计方法。利用仿真系统，对机器人在不同工况下进行不断地测试、不断地改进，最终得到相对完美的设计方案。因此，虚拟仿真技术一经出现，就引起了各国学者广泛地关注，并将虚拟仿真技术引入到机器人研究中。如何能够更好地规划机器人的运动状态，使机器人按照自己想要的方式运动，成为广大学者的研究方向。因此，基于以上背景，寻求一种液压四足机器人机电液控感仿真平台搭建方法以实现对液压四足机器人机电液控感仿真平台的搭建是十分必要且又相当迫切的。


技术实现要素：

4.本发明针对上述现有技术中的缺陷，提出一种液压四足机器人机电液控感仿真平台搭建方法。该方法包括通过足端轨迹规划模块，对液压四足机器人的足端轨迹进行规划，通过运动学模块得到液压四足机器人d-h坐标角，通过关节空间与液压驱动单元行程映射模块得到液压驱动单元期望行程，通过液压驱动单元位置控制模块得到液压驱动单元实际旋转角度和实际行程，在matlab simmechanics工具箱中建立动力学模型模块，根据各模块间信号流转与各关节驱动单元受力与负载力之间的关系，共同构建得到液压四足机器人机电液控感仿真平台，进而验证所建立的液压四足机器人机电液控感仿真平台的行程和速度结果的准确性。本发明借助matlab中simmechanics工具箱，对液压四足机器人进行模块化仿真设计，缩短设计和优化周期。
5.本发明提供一种液压四足机器人机电液控感仿真平台搭建方法，其包括以下步骤：
6.s1、通过液压四足机器人的足端轨迹规划模块，对液压四足机器人的足端轨迹进
行规划，使其足端形成连续轨迹；
7.s11、选取液压四足机器人的实验步态，由五次多项式对足端进行轨迹规划；
8.s12、基于matlab simmechanics工具箱，利用足端轨迹规划方程搭建足端轨迹规划模块；
9.s2、以液压四足机器人足端坐标{x,y,z}为运动输入量，通过运动学模块得到液压四足机器人d-h坐标角{ct1,ct2,ct3}；
10.s21、根据液压四足机器人各关节关系建立d-h坐标系，并确定相应参数；
11.s22、根据d-h坐标系和腿部结构参数，获得液压四足机器人运动学正解；
12.s23、根据d-h坐标系、腿部结构参数和液压四足机器人运动学正解，获得液压四足机器人运动学逆解；
13.s24、以杆件长度a、足端坐标{x,y,z}为变量，利用函数关系，在matlab simmechanics工具箱中完成运动学模块搭建，得到液压四足机器人d-h坐标角{ct1,ct2,ct3}；
14.s3、以液压四足机器人d-h坐标角{ct1,ct2,ct3}作为驱动输入量，通过关节空间与液压驱动单元行程映射模块，得到液压驱动单元期望行程{ct1,l2,l3}；
15.s31、根据运动学逆解得到的液压四足机器人d-h坐标角，得到液压驱动单元的行程；
16.s32、以θ1、θ2和θ3为变量，利用函数关系，在matlab simmechanics工具箱中完成对关节空间与液压驱动单元行程映射模块的搭建，得到液压驱动单元的期望行程{ct1,l2,l3}；
17.s4、以关节空间与液压驱动单元行程映射模块得到液压驱动单元的期望行程{ct1,l2,l3}、动力学模型采集回的力矩torque与力force作为负载力为实际驱动输入量，输入液压驱动单元位置控制模块，并得到液压驱动单元实际旋转角度xc和实际行程{xp2,xp3}；
18.s5、根据液压四足机器人的机械结构，构建液压四足机器人的动力学模型模块，绘制液压四足机器人的三维模型，并设置材料和质量的相关属性，在matlab simmechanics工具箱中建立液压四足机器人的动力学模型模块；
19.s6、根据液压四足机器人的所述足端轨迹规划模块、运动学模块、关节空间与液压驱动单元行程映射模块、液压驱动单元位置控制模块和动力学模型模块间信号流转与各关节驱动单元受力与负载力之间的关系，共同构建得到液压四足机器人机电液控感仿真平台；
20.s7、验证所建立的液压四足机器人机电液控感仿真平台的行程和速度结果的准确性；
21.s71、对理论计算得到的液压驱动单元期望行程、液压驱动单元位置控制模块得到的液压驱动单元实际行程和机电液控感仿真平台测量得到的仿真行程进行验证；
22.s72、对液压四足机器人理论计算速度和仿真测量速度结果进行对比验证。
23.进一步，所述步骤s11具体包括以下步骤：
24.s111、支撑相时，液压四足机器人足端轨迹为：
[0025][0026]
其中，c
zlj
表示着地相液压四足机器人足端z方向轨迹的第j次多项式系数，j＝0,1,
…
,5；t表示液压四足机器人单腿向前迈一步的周期；z
l
(t)表示支撑相时t时刻液压四足机器人足端z向坐标；x
l
(t)表示支撑相时t时刻液压四足机器人足端x向坐标；
[0027]
s112、根据液压四足机器人trot步态特点，式(1)的边界条件为：
[0028][0029]
其中，z
l
(0),表示支撑相时液压四足机器人足端z向初始坐标、初始速度和初始加速度；s表示步长；分别表示支撑相时时刻液压四足机器人足端z向坐标、速度和加速度；v
t
表示着地相腿的速度；
[0030]
s113、根据边界条件，计算得到液压四足机器人支撑相时的足端轨迹为：
[0031][0032]
s114、摆动相时，液压四足机器人足端轨迹为：
[0033][0034]
其中，c
zsj
表示摆动相液压四足机器人足端z方向轨迹的第j次多项式系数；c
xs1j
表示摆动相液压四足机器人足端x方向抬腿轨迹的第j次多项式系数；c
xs2j
表示摆动相液压四足机器人足端x方向落腿轨迹的第j次多项式系数；zs(t)表示摆动相时t时刻液压四足机器人足端z向坐标；xs(t)表示摆动相时t时刻液压四足机器人足端x向坐标；
[0035]
s115、根据液压四足机器人trot步态特点，式(4)的边界条件为：
[0036][0037]
其中，分别表示摆动相时刻液压四足机器人足端z向坐标、速度和加速度；zs(t),分别表示摆动相t时刻液压四足机器人足端z向坐标、速度和加速度；分别表示摆动相时刻液压四足机器人足端x向坐标、速度和加速度；分别表示摆动相时刻液压四足机器人足端x向坐标、速度和加速度；xs(t),分别表示摆动相t时刻液压四足机器人足端x向坐标、速度和加速度；
[0038]
s116、根据边界条件，计算得到液压四足机器人摆动相时足端轨迹为：
[0039][0040]
其中，h表示步高；
[0041]
s117、式(3)和式(6)组成液压四足机器人足端的一个步行周期轨迹，进而计算液压四足机器人trot步态下的足端轨迹。
[0042]
可优选的，所述步骤s22具体包括以下步骤：
[0043]
s221、根据d-h坐标系内容，规定连杆关系：a
i-1
表示从z
i-1
到zi沿x
i-1
的距离；α
i-1
表示从z
i-1
到zi沿x
i-1
旋转的角度；di表示从x
i-1
到xi沿zi的距离；θi表示从x
i-1
到xi沿zi旋转的角度；2b表示机器人机身长度；2w表示机器人机身宽度；h表示机器人机身高度；
[0044]
s222、根据d-h坐标系中连杆关系，当i＞1时，连杆i与连杆i-1间的位姿转换关系式
[0045][0046]
s223、将液压四足机器人前腿单腿机械部分参数代入式(7)，得到液压四足机器人单腿足端至过渡坐标{0}运动学正解位姿变换矩阵为：
[0047][0048]
其中，规定简写符号分别表示为s1＝sinθ1,c1＝cosθ1,s
23
＝sin(θ2+θ3),c
23
＝cos(θ2+θ3)；
[0049]
s224、从液压四足机器人机身过渡坐标系{0}至足端坐标系{4}的足端位置变换关系为：
[0050][0051]
其中，分别表示液压四足机器人机身过渡坐标系{0}至足端坐标系{4}的足端x、y、z位置变换关系；
[0052]
所述步骤s23具体包括以下步骤：
[0053]
s231、根据d-h坐标系，定义液压四足机器人足端坐标系{4}相对于液压四足机器人机身过渡坐标系{0}位姿变换关系式
[0054][0055]
其中，分别表示足端坐标系{4}中i单位主矢量相对于过渡坐标系{0}的方向余弦；分别表示足端坐标系{4}中j单位主矢量相对于过渡坐标系{0}的方向余弦；分别表示足端坐标系{4}中k单位主矢量相对于过渡坐标系{0}的方向余弦；
[0056]
s232、用和左乘得到：
[0057][0058]
即：
[0059][0060]
s233、根据式(12)解得θ1、θ2和θ3，分别为：
[0061][0062]
可优选的，所述步骤s31具体包括以下步骤：
[0063]
s311、求解驱动液压四足机器人髋关节纵摆运动的液压驱动单元ac的长度，由于：
[0064]
∠abc＝90
°
+θ
2-∠cbg+∠abm
ꢀꢀꢀ
(14)
[0065]
其中，∠abc表示ab与bc的夹角；ab表示髋结合两个铰点之间的距离；bc表示髋结合和膝缸体之间的铰点到髋缸和膝缸体之间的铰点的距离；∠cbg表示bc与bg的夹角；bg表示髋结合与膝缸体之间的铰点到膝缸体与小腿之间的铰点的距离；∠abm表示ab与bm的夹角；bm表示过髋结合与膝缸体之间的铰点的水平参考线；
[0066]
在δabc中，由对ac边使用余弦定理得：
[0067][0068]
s312、求解驱动液压四足机器人膝关节纵摆运动的液压驱动单元de的长度，由于：
[0069]
∠fgd＝180
°‑
θ
3-∠fgh-∠bgd
ꢀꢀꢀ
(16)
[0070]
其中，∠fgd表示fg与gd的夹角；fg表示膝缸体与小腿之间的铰点到连杆和小腿之间的铰点的距离；gd表示膝缸体与小腿之间的铰点到膝缸体内部活塞缸无杆腔底部中心的距离；∠fgh表示fg与gh的夹角；gh表示膝缸体与小腿之间的铰点到小腿足端的距离；∠bgd表示bg与gd的夹角；
[0071]
且满足几何关系：
[0072]
de
2-2df*cos∠fde*de+df
2-ef2＝0
ꢀꢀꢀ
(17)
[0073]
其中，df表示膝缸体无杆腔底部到连杆与小腿之间的铰点的距离；ef表示连杆的长度，即连杆与膝缸体活塞杆之间的铰点到连杆与小腿之间的铰点的距离；∠fde表示fd与de的夹角；
[0074]
考虑液压四足机器人单腿几何关系的限制，在δdef中，∠efd始终为锐角，由一元二次方程的求根公式得到de较小的一个解作为所求de的值：
[0075][0076]
可优选的，所述步骤s4具体包括以下步骤：
[0077]
s41、通过对液压驱动单元的机理分析，选取6个线性无关的系统变量作为控制系统的状态变量，其分别为：x1＝x
p
,x3＝xv,x5＝p1,x6＝p2，得到方程：
[0078][0079]
其中，分别表示x1,x2,x3,x4,x5,x6的一阶导数；x
p
表示伺服缸缸杆位移；xv表示伺服阀阀芯位移；分别表示x
p
,xv的一阶导数；p1表示无杆腔压力；p2表示有杆腔压力；ζ表示伺服阀阻尼比；ω表示伺服阀固有频率，单位为rad/s；k表示负载刚度，单位为n/m；k1表示与伺服缸无杆腔对应的伺服阀流量系数；k2表示与伺服缸有杆腔对应的流量系数；k
x
表位移传感器增益；k
axv
表示伺服阀增益；βe表示有效体积弹性模量，单位为pa；v1表示无杆腔容积；v2表示有杆腔容积；c
ip
表示伺服缸内泄漏系数，单位为m3/(s
·
pa)；c
ep
表示伺服缸外泄漏系数，单位为m3/(s
·
pa)；a1表示非对称伺服缸无杆腔面积，单位为m2；a2表示非对称伺服缸有杆腔面积，单位为m2；m
t
表示折算到伺服缸活塞上总质量，单位为kg；b
p
表示负载及阀控非对称缸的黏性阻尼系数，单位为n
·
s/m；u1,u2分别表示第一输入量和第二输入量，且有u1＝l，u2＝f
p
，l表示伺服缸活塞总行程，f
p
表示动力机构的驱动力；
[0080]
s42、由式(20)在matlab simmechanics工具箱中完成对液压驱动单元位置控制模块的搭建。
[0081]
可优选的，所述步骤s5具体包括以下步骤：
[0082]
s51、根据设计需求在solidworks三维设计软件中绘制液压四足机器人的三维模型，并添加各零件的相关属性；
[0083]
s52、在solidworks软件装配体环境中，根据液压四足机器人机械结构各部件的相互运动关系施加构件间的约束；
[0084]
s53、利用solidworks软件的simscape multibody link工具，将液压四足机器人机械结构的装配体文件导出为一个.xml文件和多个.stl文件；
[0085]
s54、在matlab命令窗口中运行“smimport”命令，matlab自动读取相对应的.xml文件和.stl文件，完成液压四足机器人的动力学模型模块的搭建。
[0086]
可优选的，所述步骤s6具体包括以下步骤：
[0087]
s61、根据仿真平台中的坐标系及实际需要，完成对重力在内基本参数的设置；
[0088]
s62、在动力学模型模块中，添加接触力模块，并通过第一坐标变换模块transform1与第二坐标变换模块transform2使得液压四足机器人的足端与地面相接触，获
得液压四足机器人的足端和地面的接触点fc；
[0089]
s63、将xc、xp2、xp3输入动力学模型，测量得到torque和force；并将torque和force作为负载力输入液压驱动单元位置控制模块，完成液压四足机器人机电液控感仿真平台搭建；
[0090]
s64、设置f模块作为液压四足机器人机身与腿部结构的结合模块，将腿部髋缸结构与髋活塞杆结构通过第一移动副模块相连接，所述第一移动副模块以第一条腿的第二个关节的输入信号xp
12
信号为输入量，以第一位置信号p1、第一速度信号v1、第一力信号f1为输出量，完成对髋关节液压驱动单元的实时控制与监测，并将第一力信号f1作为负载力输入液压系统，完成髋关节液压驱动单元的闭环控制；腿部膝缸体结构与膝活塞杆结构通过第二移动副模块相连接，所述第二移动副模块以第一条腿的第三个关节的输入信号xp
13
信号为输入量，以第二位置信号p2、第二速度信号v2、第二力信号f2为输出量，完成膝关节液压驱动单元的实时控制与监测，并将第二力信号f2作为负载力输入液压系统，完成膝关节液压驱动单元的闭环控制；完成对液压四足机器人机电液控感仿真平台的搭建；
[0091]
s65、运行液压四足机器人机电液控感仿真平台。
[0092]
可优选的，所述液压四足机器人机电液控感仿真平台包括足端轨迹规划模块、运动学模块、关节空间和液压驱动单元行程映射模块、液压驱动单元位置控制模块和动力学模型模块；所述足端轨迹规划模块根据足端轨迹模型构建，所述足端轨迹模型采用数学建模的方式构建得到；所述运动学模块根据液压四足机器人正运动学和逆运动学模型构建，所述液压四足机器人正运动学和逆运动学模型由d-h坐标系和液压四足机器人几何结构决定，并采用数学建模的方式构建得到；所述关节空间和液压驱动单元行程映射模块由关节空间与液压驱动单元映射关系构建，关节空间与液压驱动单元映射关系由液压四足机器人腿部几何结构决定，并采用数学建模的方式构建；所述液压驱动单元位置控制模块根据液压驱动单元位置控制模型构建，所述液压驱动单元位置控制模型由液压驱动单元的结构决定，并采用机理建模的方式构建得到；所述动力学模型模块根据所述液压四足机器人机械结构构建；所述液压四足机器人机电液控感仿真平台根据所述足端轨迹规划模块、运动学模块、关节空间与液压驱动单元行程映射模块、液压驱动单元位置控制模块和动力学模型模块间的信号流转共同搭建液压四足机器人机电液控感仿真平台；所述足端轨迹规划模块以x0、y0、z0、s、t、h、t、tt为输入量，以足端轨迹坐标x、y、z为输出量，x0,y0,z0分别表示初始状态下x、y、z方向上的足端坐标；tt表示实际时间。
[0093]
可优选的，所述实验步态包括trot步态、walk步态、pace步态和jump步态。
[0094]
与现有技术相比，本发明的技术效果为：
[0095]
1、本发明设计的一种液压四足机器人机电液控感仿真平台搭建方法，借助matlab中的simmechanics工具箱，对液压四足机器人的足端轨迹规划、运动学、关节空间和液压驱动单元行程映射、液压驱动单元位置控制和动力学模型进行了模块化仿真设计，能够验证四足机器人控制理论，简化四足机器人设计和优化过程，缩短设计和优化周期。
[0096]
2、本发明设计的一种液压四足机器人机电液控感仿真平台搭建方法，相比于传统的设计方法，所提方法能够更好地规划机器人的运动状态，避免了大量样机试错的周期，大大降低了研发成本；此外，可以提前在仿真平台上验证程序设计的正确性，防止技术不成熟时直接应用在样机上，导致发生意外事故。
附图说明
[0097]
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本技术的其它特征、目的和优点将会变得更明显。
[0098]
图1是本发明的液压四足机器人机电液控感仿真平台搭建方法流程图；
[0099]
图2是本发明的液压四足机器人机电液控感仿真平台及各个模块信号流转示意图；
[0100]
图3是本发明的液压四足机器人trot步态足端触地顺序图；
[0101]
图4是本发明的理论计算四足机器人足端轨迹示意图；
[0102]
图5是本发明的液压四足机器人d-h坐标系示意图；
[0103]
图6a是本发明的液压四足机器人单腿三维机械结构示意图；
[0104]
图6b是本发明的液压四足机器人单腿三维机械结构简图；
[0105]
图7是本发明的液压四足机器人液压驱动单元位置控制模块示意图；
[0106]
图8是本发明的液压四足机器人动力学模型模块示意图；
[0107]
图9是本发明的液压四足机器人单腿动力学模型仿真的系统模块示意图；
[0108]
图10是本发明的在simmechanics环境中虚拟平台运行状态下图像处理示意图；
[0109]
图11是本发明的液压四足机器人髋关节液压驱动单元位置控制与仿真验证示意图；
[0110]
图12是本发明的液压四足机器人膝关节液压驱动单元位置控制与仿真验证示意图；
[0111]
图13是本发明的液压四足机器人速度仿真验证示意图。
具体实施方式
[0112]
下面结合附图和实施例对本技术作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
[0113]
需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本技术。
[0114]
图1示出了本发明的液压四足机器人机电液控感仿真平台搭建方法，该方法包括以下步骤：
[0115]
液压四足机器人机电液控感仿真平台包括足端轨迹规划模块、运动学模块、关节空间和液压驱动单元行程映射模块、液压驱动单元位置控制模块和动力学模型模块；足端轨迹规划模块根据足端轨迹模型构建，足端轨迹模型采用数学建模的方式构建得到；运动学模块根据液压四足机器人正运动学和逆运动学模型构建，液压四足机器人正运动学和逆运动学模型由d-h坐标系和液压四足机器人几何结构决定，并采用数学建模的方式构建得到；关节空间和液压驱动单元行程映射模块由关节空间与液压驱动单元映射关系构建，关节空间与液压驱动单元映射关系由液压四足机器人腿部几何结构决定，并采用数学建模的方式构建；液压驱动单元位置控制模块根据液压驱动单元位置控制模型构建，液压驱动单元位置控制模型由液压驱动单元的结构决定，并采用机理建模的方式构建得到；动力学模型模块根据液压四足机器人机械结构构建。
[0116]
液压四足机器人机电液控感仿真平台根据足端轨迹规划模块、运动学模块、关节空间与液压驱动单元行程映射模块、液压驱动单元位置控制模块和动力学模型模块间的信号流转共同搭建液压四足机器人机电液控感仿真平台。在一个具体实施例中，如图2所示为液压四足机器人机电液控感仿真平台及信号在各个模块间里流转示意图。
[0117]
s1、通过液压四足机器人的足端轨迹规划模块，对液压四足机器人的足端轨迹进行规划，使其足端形成连续轨迹。
[0118]
s11、选取液压四足机器人的实验步态，如图3所示，由五次多项式对足端进行轨迹规划。实验步态包括trot步态、walk步态、pace步态和jump步态。
[0119]
s111、支撑相时，液压四足机器人足端轨迹为：
[0120][0121]
其中，c
zlj
表示着地相液压四足机器人足端z方向轨迹的第j次多项式系数，j＝0,1,
…
,5；t表示液压四足机器人单腿向前迈一步的周期；z
l
(t)表示支撑相时t时刻液压四足机器人足端z向坐标；x
l
(t)表示支撑相时t时刻液压四足机器人足端x向坐标。
[0122]
s112、根据液压四足机器人trot步态特点，式(1)的边界条件为：
[0123][0124]
其中，z
l
(0),表示支撑相时液压四足机器人足端z向初始坐标、初始速度和初始加速度；s表示步长；分别表示支撑相时时刻液压四足机器人足端z向坐标、速度和加速度；v
t
表示着地相腿的速度。
[0125]
s113、根据边界条件，计算得到液压四足机器人支撑相时的足端轨迹为：
[0126][0127]
s114、摆动相时，液压四足机器人足端轨迹为：
[0128][0129]
其中，c
zsj
表示摆动相液压四足机器人足端z方向轨迹的第j次多项式系数；c
xs1j
表示摆动相液压四足机器人足端x方向抬腿轨迹的第j次多项式系数；c
xs2j
表示摆动相液压四足机器人足端x方向落腿轨迹的第j次多项式系数；zs(t)表示摆动相时t时刻液压四足机器人足端z向坐标；xs(t)表示摆动相时t时刻液压四足机器人足端x向坐标。
[0130]
s115、根据液压四足机器人trot步态特点，式(4)的边界条件为：
[0131][0132]
其中，分别表示摆动相时刻液压四足机器人足端z向坐标、速度和加速度；zs(t),分别表示摆动相t时刻液压四足机器人足端z向坐标、速度和加速度；分别表示摆动相时刻液压四足机器人足端x向坐标、速度和加速度；分别表示摆动相时刻液压四足机器人足端x向坐标、速度和加速度；xs(t),分别表示摆动相t时刻液压四足机器人足端x向坐标、速度和加速度。
[0133]
s116、根据边界条件，计算得到液压四足机器人摆动相时足端轨迹为：
[0134][0135]
其中，h表示步高。
[0136]
s117、式(3)和式(6)组成液压四足机器人足端的一个步行周期轨迹，进而计算液压四足机器人trot步态下的足端轨迹。在一个具体实施例中，液压四足机器人trot步态下的足端轨迹类似于“馒头”，轨迹规划结果如图4所示。trot步态的机器人迈腿时，机器人的机身速度等于着地相腿的速度v
t
，为了避免足端与地面存在相对速度而发生剧烈碰撞，因此，机器人足端与地面接触和离开瞬间，足端相对于机身的速度为-v
t
。
[0137]
上述针对机器人trot步态，规划了其腿部z方向和x方向的足端轨迹，根据该步态需求，也可规划出机器人腿部y方向的足端轨迹。同理，采用上述相同的方法，亦可规划诸如walk步态、pace步态和jump步态的足端轨迹，本发明不再赘叙。
[0138]
s12、基于matlab simmechanics工具箱，利用足端轨迹规划方程搭建足端轨迹规划模块；足端轨迹规划模块以x0、y0、z0、s、t、h、t、tt为输入量，以足端轨迹坐标x、y、z为输出量，x0,y0,z0分别表示初始状态下x、y、z方向上的足端坐标；tt表示实际时间。在一个具体实施例中，为表达简洁，已将液压四足机器人的trot步态足端坐标用集合信号线表示，如图2所示。
[0139]
s2、以液压四足机器人足端坐标{x,y,z}为运动输入量，通过运动学模块得到液压四足机器人d-h坐标角{ct1,ct2,ct3}。
[0140]
s21、根据液压四足机器人各关节关系建立d-h坐标系，并确定相应参数；在一个具体实施例中，建立相应的d-h坐标系结果，如图5所示。
[0141]
s22、根据d-h坐标系和腿部结构参数，获得液压四足机器人运动学正解，为后续仿真平台搭建做准备。
[0142]
本具体实施例中机器人单腿运动学正解、逆解均基于图5中腿2(即右前腿)推导，其余各个单腿运动学关系可根据以下推到内容类似得出。
[0143]
已知机器人各关节角度的数值求解机器人足端相对于机身坐标系的位置问题属于足式机器人运动学正解问题。
[0144]
s221、根据d-h坐标系内容，规定连杆关系：a
i-1
表示从z
i-1
到zi沿x
i-1
的距离；α
i-1
表示从z
i-1
到zi沿x
i-1
旋转的角度；di表示从x
i-1
到xi沿zi的距离；θi表示从x
i-1
到xi沿zi旋转的角度；2b表示机器人机身长度；2w表示机器人机身宽度；h表示机器人机身高度；
[0145]
s222、根据d-h坐标系中连杆关系，当i＞1时，连杆i与连杆i-1间的位姿转换关系式
[0146][0147]
s223、根据图5中坐标位姿定义关系，将液压四足机器人前腿单腿机械部分参数代入式(7)，得到液压四足机器人单腿足端至过渡坐标{0}运动学正解位姿变换矩阵为：
[0148][0149]
其中，规定简写符号分别表示为s1＝sinθ1,c1＝cosθ1,s
23
＝sin(θ2+θ3),c
23
＝cos(θ2+θ3)。
[0150]
s224、从液压四足机器人机身过渡坐标系{0}至足端坐标系{4}的足端位置变换关系为：
[0151][0152]
其中，分别表示液压四足机器人机身过渡坐标系{0}至足端坐标系{4}的足端x、y、z位置变换关系。
[0153]
s23、根据d-h坐标系、腿部结构参数和液压四足机器人运动学正解，获得液压四足机器人运动学逆解，为后续仿真平台搭建做准备。
[0154]
s231、根据d-h坐标系，定义液压四足机器人足端坐标系{4}相对于液压四足机器人机身过渡坐标系{0}位姿变换关系式
[0155][0156]
其中，分别表示足端坐标系{4}中i单位主矢量相对于过渡坐标系{0}的方向余弦；分别表示足端坐标系{4}中j单位主矢量相对于过渡坐标系{0}的方向余弦；分别表示足端坐标系{4}中k单位主矢量相对于过渡坐标系{0}的方向余弦。
[0157]
s232、用和左乘得到：
[0158][0159]
即：
[0160][0161]
s233、根据式(12)解得θ1、θ2和θ3，分别为：
[0162][0163]
由于反余弦函数值域的非负性，故按照本发明中d-h坐标的定义方式，当四足机器人单腿采用肘式配置(前腿)时，θ3角度为正值；当采用膝式配置(后腿)时，θ3角度应为负值，故取θ3＝-θ3。
[0164]
s24、以杆件长度a、足端坐标{x,y,z}为变量，利用函数关系，在matlab simmechanics工具箱中完成运动学模块搭建，如图2所示，得到液压四足机器人d-h坐标角{ct1,ct2,ct3}，为下一步计算液压驱动单元行程做准备。
[0165]
s3、以液压四足机器人d-h坐标角{ct1,ct2,ct3}作为驱动输入量，通过关节空间与液压驱动单元行程映射模块，得到液压驱动单元期望行程{ct1,l2,l3}。
[0166]
s31、根据运动学逆解得到的液压四足机器人d-h坐标角，得到液压驱动单元的行程；
[0167]
本发明中液压四足机器人液压驱动单元行程与关节角度映射关系基于图5中腿2(即右前腿)推导，其余各个单腿的映射关系可根据以下推导内容类似得出。
[0168]
由于液压四足机器人单腿髋关节纵摆与膝关节纵摆均由液压驱动单元驱动，故需要将运动学逆解得出的液压四足机器人腿部各关节角度得到液压驱动单元的行程，进而通过控制液压驱动单元的行程实现液压四足机器人髋关节与膝关节的角度变化，并结合髋关节横摆的角度控制，最终实现液压四足机器人腿部位姿控制。
[0169]
液压四足机器人腿部结构示意图及对应的简图分别如图6a和图6b所示。其中ac、de分别表示驱动液压四足机器人髋关节纵摆与膝关节纵摆运动的液压驱动单元。本节主要讨论的内容即是通过液压四足机器人单腿位姿角度θ2与θ3求解ac与de的长度。
[0170]
s311、求解驱动液压四足机器人髋关节纵摆运动的液压驱动单元ac的长度，由于：
[0171]
∠abc＝90
°
+θ
2-∠cbg+∠abm(14)
[0172]
其中，∠abc表示ab与bc的夹角；ab表示髋结合两个铰点之间的距离；bc表示髋结合和膝缸体之间的铰点到髋缸和膝缸体之间的铰点的距离；∠cbg表示bc与bg的夹角；bg表示髋结合与膝缸体之间的铰点到膝缸体与小腿之间的铰点的距离；∠abm表示ab与bm的夹角；bm表示过髋结合与膝缸体之间的铰点的水平参考线。
[0173]
在δabc中，由对ac边使用余弦定理得：
[0174][0175]
s312、求解驱动液压四足机器人膝关节纵摆运动的液压驱动单元de的长度，由于：
[0176]
∠fgd＝180
°‑
θ
3-∠fgh-∠bgd(16)
[0177]
其中，∠fgd表示fg与gd的夹角；fg表示膝缸体与小腿之间的铰点到连杆和小腿之间的铰点的距离；gd表示膝缸体与小腿之间的铰点到膝缸体内部活塞缸无杆腔底部中心的距离；∠fgh表示fg与gh的夹角；gh表示膝缸体与小腿之间的铰点到小腿足端的距离；∠bgd表示bg与gd的夹角。
[0178]
且满足几何关系：
[0179]
de
2-2df*cos∠fde*de+df
2-ef2＝0(17)
[0180]
其中，df表示膝缸体无杆腔底部到连杆与小腿之间的铰点的距离；ef表示连杆的长度，即连杆与膝缸体活塞杆之间的铰点到连杆与小腿之间的铰点的距离；∠fde表示fd与de的夹角。
[0181]
从而在式(17)中，除de长度外，其他参数均已知或已求出，则式(17)可视为以de长度为未知量的一元二次方程，考虑液压四足机器人单腿几何关系的限制，在δdef中，∠efd始终为锐角，由一元二次方程的求根公式得到de较小的一个解作为所求de的值：
[0182][0183]
故液压四足机器人前腿关节角度与液压驱动单元ac、de长度关系如下：
[0184][0185]
s32、以θ1、θ2和θ3为变量，利用函数关系，在matlab simmechanics工具箱中完成对关节空间与液压驱动单元行程映射模块的搭建，得到液压驱动单元的期望行程{ct1,l2,l3}。在一个具体实施例中，为表达简洁，将液压四足机器人的四个髋关节纵摆液压驱动单元期望行程和四个膝关节纵摆液压驱动单元期望行程用集合信号线表示，如图2所示。
[0186]
s4、以关节空间与液压驱动单元行程映射模块得到液压驱动单元的期望行程{ct1,l2,l3}、动力学模型采集回的力矩torque与力force作为负载力为实际驱动输入量，输入液压驱动单元位置控制模块，并得到液压驱动单元实际旋转角度xc和实际行程{xp2,xp3}。在一个具体实施例中，为表达简洁，将液压四足机器人的四个髋关节横摆液压驱动单元实际旋转角度、四个髋关节纵摆液压驱动单元实际行程和四个膝关节纵摆液压驱动单元实际行程用集合信号线表示，如图2所示。
[0187]
分析液压驱动单元的结构，采用机理建模的方式建立液压驱动单元的位置控制模型，并在simmechanics环境中完成对液压驱动单元位置控制模块的搭建，如图7所示。
[0188]
s41、通过对液压驱动单元的机理分析，选取6个线性无关的系统变量作为控制系统的状态变量，其分别为：x1＝x
p
,x3＝xv,x5＝p1,x6＝p2，得到方程：
[0189][0190]
其中，分别表示x1,x2,x3,x4,x5,x6的一阶导数；x
p
表示伺服缸缸杆位移；xv表示伺服阀阀芯位移；分别表示x
p
,xv的一阶导数；p1表示无杆腔压力；p2表示有杆腔压力；ζ表示伺服阀阻尼比；ω表示伺服阀固有频率，单位为rad/s；k表示负载刚度，单位为n/m；k1表示与伺服缸无杆腔对应的伺服阀流量系数；k2表示与伺服缸有杆腔对应的流量系数；k
x
表示位移传感器增益；k
axv
表示伺服阀增益；βe表示有效体积弹性模量，单位为pa；v1表示无杆腔容积；v2表示有杆腔容积；c
ip
表示伺服缸内泄漏系数，单位为m3/(s
·
pa)；c
ep
表示伺服缸外泄漏系数，单位为m3/(s
·
pa)；a1表示非对称伺服缸无杆腔面积，单位为m2；a2表示非对称伺服缸有杆腔面积，单位为m2；m
t
表示折算到伺服缸活塞上总质量，单位为kg；b
p
表示负载及阀控非对称缸的黏性阻尼系数，单位为n
·
s/m；u1,u2分别表示第一输入量和第二输入量，且有u1＝l，u2＝f
p
，l表示伺服缸活塞总行程，f
p
表示动力机构的驱动力。
[0191]
s42、由式(20)在matlab simmechanics工具箱中完成对液压驱动单元位置控制模块的搭建。
[0192]
s5、根据液压四足机器人的机械结构，构建液压四足机器人的动力学模型模块，绘制液压四足机器人的三维模型，并设置材料和质量的相关属性，在matlab simmechanics工具箱中建立液压四足机器人的动力学模型模块。
[0193]
s51、根据设计需求在solidworks三维设计软件中绘制液压四足机器人的三维模型，并添加各零件的相关属性。
[0194]
s52、在solidworks软件装配体环境中，根据液压四足机器人机械结构各部件的相互运动关系施加构件间的约束。
[0195]
s53、利用solidworks软件的simscape multibody link工具，将液压四足机器人机械结构的装配体文件导出为一个.xml文件和多个.stl文件。
[0196]
s54、在matlab命令窗口中运行“smimport”命令，matlab自动读取相对应的.xml文件和.stl文件，完成液压四足机器人的动力学模型模块的搭建。
[0197]
s6、根据液压四足机器人的所述足端轨迹规划模块、运动学模块、关节空间与液压驱动单元行程映射模块、液压驱动单元位置控制模块和动力学模型模块间信号流转与各关节驱动单元受力与负载力之间的关系，共同构建得到液压四足机器人机电液控感仿真平台。
[0198]
s61、根据仿真平台中的坐标系及实际需要，完成对重力在内基本参数的设置。
[0199]
s62、在动力学模型模块中，添加接触力模块，并通过第一坐标变换模块transform1与第二坐标变换模块transform2使得液压四足机器人的足端与地面相接触，获得液压四足机器人的足端和地面的接触点fc。
[0200]
s63、将xc、xp2、xp3输入动力学模型，测量得到torque和force；并将torque和force作为负载力输入液压驱动单元位置控制模块，完成液压四足机器人机电液控感仿真平台搭建。
[0201]
在一个具体实施例中，机器人四条腿的拓扑结构均相同，四条腿与机身和世界坐标的关系均相同，故以1号腿为例进行具体介绍：
[0202]
如图8所示，机身_01_1_rigid与01腿_1通过转动副模块revolute3相连，以液压系统输出的实际关节角度xc
11
作为输入量输入该转动副模块revolute3，torque
11
作为输出量采集回液压系统，作为其负载力矩。xp
12
与xp
13
作为输入量输入x01腿_1，force
12
、force
13
作为输出量采集回液压系统模块。其中，腿部结构均被封装在x01腿_1中。
[0203]
s64、设置f模块作为液压四足机器人机身与腿部结构的结合模块，将腿部髋缸结构与髋活塞杆结构通过第一移动副模块相连接，第一移动副模块以第一条腿的第二个关节的输入信号xp
12
信号为输入量，以第一位置信号p1、第一速度信号v1、第一力信号f1为输出量，完成对髋关节液压驱动单元的实时控制与监测，并将第一力信号f1作为负载力输入液压系统，完成髋关节液压驱动单元的闭环控制；腿部膝缸体结构与膝活塞杆结构通过第二移动副模块相连接，第二移动副模块以第一条腿的第三个关节的输入信号xp
13
信号为输入量，以第二位置信号p2、第二速度信号v2、第二力信号f2为输出量，完成膝关节液压驱动单元的实时控制与监测，并将第二力信号f2作为负载力输入液压系统，完成膝关节液压驱动单元的闭环控制；完成对液压四足机器人机电液控感仿真平台的搭建。
[0204]
在一个具体实施例中，如图9所示，设置f模块作为液压四足机器人机身_01_1_rigid与腿部结构x01kuanjiehe_2_rigid的结合模块，将腿部髋缸结构与髋活塞杆结构通过第一移动副模块prismatic1相连接，第一移动副模块prismatic1以图8中第一条腿的第二个关节的输入信号xp
12
信号为输入量，以第一位置信号p1、第一速度信号v1、第一力信号f1为输出量，完成对髋关节液压驱动单元的实时控制与监测，并将第一力信号f1作为负载力输入液压系统，完成髋关节液压驱动单元的闭环控制；腿部膝缸体结构与膝活塞杆结构通过第二移动副模块prismatic相连接，第二移动副模块prismatic以图8中第一条腿的第三个关节的输入信号xp
13
信号为输入量，以第二位置信号p2、第二速度信号v2、第二力信号f2为输出量，完成膝关节液压驱动单元的实时控制与监测，并将第二力信号f2作为负载力输入液压系统，完成膝关节液压驱动单元的闭环控制；其他腿部结构构建方式均与步骤s63腿部结构构建方式相同，至此，完成对液压四足机器人机电液控感仿真平台的搭建。
[0205]
s65、运行液压四足机器人机电液控感仿真平台，运行效果如图10所示。
[0206]
s7、验证所建立的液压四足机器人机电液控感仿真平台的行程和速度结果的准确性。
[0207]
主要验证以下内容：理论计算得到的液压驱动单元期望行程、液压驱动单元位置控制模块得到的液压驱动单元实际行程和机电液控感仿真平台测量得到的仿真行程；液压四足机器人理论计算速度和仿真测量速度。
[0208]
s71、对理论计算得到的液压驱动单元期望行程、液压驱动单元位置控制模块得到的液压驱动单元实际行程和机电液控感仿真平台测量得到的仿真行程进行验证。
[0209]
在一个具体实施例中，液压四足机器人四个腿部结构相同，所以选取01号腿进行仿真结果验证。对髋关节液压驱动单元进行验证，验证结果如图11所示，仿真验证结果表明控制效果良好，误差在可接受范围内；对膝关节液压驱动单元进行验证，验证结果如图12所示，仿真验证结果表明控制效果良好，误差在可接受范围内。经验证，其他液压驱动单元仿真结果同样满足要求。
[0210]
s72、对液压四足机器人理论计算速度和仿真测量速度结果进行对比验证。
[0211]
在一个具体实施例中，对液压四足机器人理论计算速度和仿真测量速度结果的验证结果如图13所示。结果表明：稳定状态下，仿真得到的液压四足机器人速度约为7km/h，与初始设置基本相同，达到预期效果。
[0212]
本发明设计的一种液压四足机器人机电液控感仿真平台搭建方法，借助matlab中的simmechanics工具箱，对液压四足机器人的足端轨迹规划、运动学、关节空间和液压驱动单元行程映射、液压驱动单元位置控制和动力学模型进行了模块化仿真设计，能够验证四足机器人控制理论，简化四足机器人设计和优化过程，缩短设计和优化周期；相比于传统的设计方法，所提方法能够更好地规划机器人的运动状态，避免了大量样机试错的周期，大大降低了研发成本；此外，可以提前在仿真平台上验证程序设计的正确性，防止技术不成熟时直接应用在样机上，导致发生意外事故。
[0213]
最后所应说明的是：以上实施例仅以说明而非限制本发明的技术方案，尽管参照上述实施例对本发明进行了详细说明，本领域的普通技术人员应当理解：依然可以对本发明进行修改或者等同替换，而不脱离本发明的精神和范围的任何修改或局部替换，其均应涵盖在本发明的权利要求范围当中。