CN110202584B - 带半圆柱形足端的足式机器人腿部运动学修正方法及系统 - Google Patents

Abstract

本发明公开了一种带半圆柱形足端的足式机器人腿部运动学修正方法及系统。本发明首先推导将足端视为点足时的运动学正反解，并对带有半圆柱形足端引起机器人腿部根关节轨迹产生偏移的原因进行分析；考虑到半圆柱足端与地面接触过程中恒相切，本发明将半圆柱足端虚拟为一条恒垂直于地面的杆件，提出带有半圆柱形足端的足式机器人单腿运动学模型。采用本发明修正方法，在机器人机身与接触面成不同角度时均能有效减少根关节轨迹偏移的现象，针对装配有半圆柱形足端的具有任何自由度的腿部结构，都可利用本发明方法设计运动学修正策略，提高机器人的运动控制精度，并使修正过程更加简便。

Description

带半圆柱形足端的足式机器人腿部运动学修正方法及系统

技术领域

本发明涉及足式机器人运动学控制技术领域，特别是涉及一种带半圆柱形足端的足式机器人腿部运动学修正方法及系统。

背景技术

机器人通常用于危险的环境中代替人类执行任务，足式机器人相对于轮式和履带式机器人，在沙滩、丛林、雪地、草地等复杂非结构环境中具有更好的适应性，液压驱动相对于电机驱动和气压驱动具有功重比高、承载能力大、响应快等优点，特别适合足式机器人高性能需求，近年来已成为各国学者的研究热点。为使机器人对不同环境有足够的适应能力，机器人腿部足端的结构设计尤为重要，半圆柱形足端相对于平足、圆形平足、带踝关节的足部结构，不仅设计和制造简单、成本低且在适应复杂地形方面也有很大的优势，因此在许多足式机器人中得到了应用。

当机器人采用半圆柱形足端腿部结构时，较小的半径易陷入柔软地形，增大足端半径能够有效地缓解这一问题，但在坚硬地面，较大半径的足端在腿部作为支撑相时会发生滚动改变足端与地面的接触点，造成根关节轨迹发生偏移，从而使机器人产生错误的位姿。针对这一现象，研究人员已经实现了机器人轨迹的闭环修正，但机器人足端的实际位置和期望位置的偏移问题没有得到很好的解决。

针对这一问题，有关文献已经解决了足端的实际位置和期望位置的偏移，但机器人在实际运动过程中机身与接触面成不同角度时足端的实际位置和期望位置的偏移没有得到很好的解决。但机器人在实际运动过程中，机身与接触面不平行的情况时有出现，因此有必要提出一种在机器人机身与接触面成不同角度时仍然适用的腿部运动学修正策略，解决半圆柱形足端引起的根关节轨迹偏移现象，提高机器人的运动控制精度。

发明内容

本发明的目的是提供一种带半圆柱形足端的足式机器人腿部运动学修正方法及系统，以解决机器人在实际运动过程中机身与接触面成不同角度时足端的实际位置和期望位置的偏移没有得到很好的解决，从而导致控制精度低的问题。

为实现上述目的，本发明提供了如下方案：

一种带半圆柱形足端的足式机器人腿部运动学修正方法，所述方法包括：

将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度和踝关节旋转角度；建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解；对所述腿部液压驱动系统运动学正解进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解；根据所述腿部液压驱动系统运动学反解对所述足式机器人腿部运动进行修正。

一种带半圆柱形足端的足式机器人腿部运动学修正系统，所述系统包括：单腿坐标系建立模块，用于将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；旋转角度计算模块，用于根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度和踝关节旋转角度；单腿运动学模型建立模块，用于建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；运动学正解求解模块，用于根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解；运动学反解求解模块，用于对所述腿部液压驱动系统运动学正解进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解；运动学修正模块，用于根据所述腿部液压驱动系统运动学反解对所述足式机器人腿部运动进行修正。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供一种带半圆柱形足端的足式机器人腿部运动学修正方法及系统，本发明首先推导将足端视为点足时的运动学正反解，对带有半圆柱形足端引起机器人腿部根关节轨迹产生偏移的原因进行分析，得到机器人机身与接触面成不同角度时，足端在工作空间内偏移量的大小；考虑到半圆柱足端与地面接触过程中恒相切，本发明将半圆柱足端虚拟为一条恒垂直于地面的杆件，提出一种带有半圆柱形足端的足式机器人单腿运动学模型。采用本发明修正方法，在机器人机身与接触面成不同角度时都能有效的减少根关节轨迹偏移的现象，针对装配有半圆柱形足端的具有任何自由度的腿部结构，都可利用本发明方法设计运动学修正策略，提高机器人的运动控制精度，并且修正过程更加简便。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的带半圆柱形足端的足式机器人腿部运动学修正方法的流程图；

图2为本发明提供的足式机器人的腿部液压驱动系统实物图；

图3为本发明提供的足式机器人腿部的单腿坐标系示意图；

图4为本发明提供的足式机器人腿部足端空间运动位置范围示意图；

图5为本发明提供的LHDS半圆形足端结构实物图；

图6为本发明提供的理想足端的LHDS支撑阶段运动轨迹示意图；

图7为本发明提供的带半圆柱形足端的LHDS支撑阶段运动轨迹示意图；

图8为应用本发明修正方法后带有半圆柱形足端的LHDS支撑阶段运动轨迹示意图；

图9为本发明提供的半圆柱形足端滚动引起根关节偏移现象的示意图；

图10为平行移动后的半圆柱形足端滚动引起根关节偏移现象的示意图；

图11为本发明提供的腿部工作空间内半圆柱形足端引起根关节轨迹x₀轴正方向的偏移量Δx示意图；

图12为本发明提供的腿部工作空间内半圆柱形足端引起根关节轨迹y₀轴正方向的偏移量Δy示意图；

图13为本发明提供的带半圆柱形足端的足式机器人腿部的单腿运动学模型示意图；

图14为本发明提供的构造的直角三角形的示意图；

图15为本发明提供的负载模拟实验平台的三维模型示意图；

图16为本发明提供的腿部液压驱动系统加载实验图；

图17为本发明提供的腿部液压驱动系统加载实验原理图；

图18为本发明提供的方案1仿真结果示意图；

图19为本发明提供的方案2仿真结果示意图；

图20为本发明提供的方案3仿真结果示意图；

图21为本发明提供的方案4仿真结果示意图；

图22为本发明提供的带半圆柱形足端的足式机器人腿部运动学修正系统的结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明针对带有半圆柱形足端的足式机器人腿部作为支撑相时，由于半圆柱形足端的滚动效应，引起足端轨迹偏移产生错误位姿的现象，提出一种具有更简便计算过程的、并在机身与接触面成不同角度时都适用的新型运动学修正方法及系统，以解决机器人在实际运动过程中控制精度低的问题。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明提供的带半圆柱形足端的足式机器人腿部运动学修正方法的流程图。参见图1，本发明足式机器人腿部运动学修正方法具体包括：

步骤101：将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系。

足式机器人的腿部液压驱动系统(LHDS，Leg Hydraulic Drive System)实物图如图2所示，机器人腿部原由踝关节、膝关节、髋部横摆关节、髋部纵摆关节4个关节组成，共有4个自由度，本发明将髋关节纵摆自由度和横摆自由度固定，最终的LHDS为两自由度，分别为膝关节(根关节)和踝关节转动副，通过改变膝关节和踝关节液压驱动单元伸出长度，实现两个关节的转动控制，进而实现机器人足端的运动。其中膝关节和踝关节液压驱动单元主要由伺服缸、伺服阀、位移传感器、力传感器和阀块等几部分组成，位移传感器安装于伺服缸侧面与伺服缸缸杆相连，可检测液压驱动单元伸出长度变化，用于实现位置闭环控制。

为了分析和建模方便，根据LHDS本身机械结构和自由度情况，本发明将图2所示机械结构简化至图3，图3为将所述足式机器人的足端视为点足，基于D-H(Denavit-Hartenberg)方法建立的足式机器人腿部的单腿坐标系，图3中，定义根关节为坐标原点O，水平向右为x₀轴正方向，竖直向上为y₀轴正方向；OD为大腿构件，长度设为l₁；DF为小腿构件，长度设为l₂；大腿OD与x₀轴正向的旋转角度为膝关节旋转角度θ₁，小腿DF与大腿OD延长线方向的旋转角度为踝关节旋转角度θ₂，OD与OB所成的夹角为α，OA与x₀轴负方向所成的夹角为β，规定关节旋转角度逆时针方向旋转为正方向，AB和CE分别为膝关节和踝关节驱动器液压驱动单元伸出总长度，膝关节液压驱动单元伸出长度为Δx_p1，踝关节HDU(Hydraulicdriveunit，液压驱动单元)伸出长度为Δx_p2，本发明通过动态改变膝关节和踝关节液压驱动单元伸出长度进而控制足端F的位置。

本发明实施例中，LHDS主要结构尺寸参数如表1所示：

表1单腿机械尺寸结构参数表

名称

OA

OB

OD

DF

CD

DE

α

β

∠ODC

∠EDF

长度(mm)/角度(°)

249

44

310

359

248

46

1°

10°

13°

37.35°

LHDS机械结构的D-H参数如表2所示：

表2 LHDS机械结构D-H参数表

连杆序号	a<sub>i-1</sub>	α<sub>i-1</sub>	d<sub>i</sub>	θ<sub>i</sub>
					1	0	0	0	θ<sub>1</sub>
2	l<sub>1</sub>	0	0	θ<sub>2</sub>
					3	l<sub>2</sub>	0	0	—

表2中，a_i-1表示图2中z_i-1到z_i沿x_i-1测量的距离；α_i-1表示z_i-1到z_i绕x_i-1旋转的角度；d_i表示x_i-1到x_i沿z_i测量的距离；θ_i表示x_i-1到x_i绕z_i旋转的角度。

在图3所示的单腿D-H坐标系中，位姿关系可以用连杆变换通式

来表示：

根据表1单腿机械结构D-H参数表再结合连杆变换通式(2-1)可以得到相邻连杆之间的变换矩阵：

将式(2-2)、式(2-3)和式(2-4)依次相乘可得

于是由式(2-5)可以得到运动学正解为：

根据式(2-6)，如果膝关节和踝关节角度(θ₁、θ₂)和单腿各连杆长度(l₁、l₂)已知，则可以求得此时的足端F相对于根关节O的位置关系(x，y)，但实际运动过程中可以直接采集的量为膝关节和踝关节液压驱动单元伸出长度(Δx_p1和Δx_p2)，因此有必要求出各关节旋转角度与各关节液压驱动单元的伸出长度变化量之间的关系。

步骤102：根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度和踝关节旋转角度。

在图3的ΔAOB中，利用余弦定理可求得∠AOB：

设膝关节液压驱动单元的初始长度为l₀₁，则实际运动过程中液压驱动单元总长度AB设为：

AB＝l₀₁+Δx_p1 (2-8)

由根关节O处的角度关系，可得∠AOB为∠AOB＝β+π+α+θ₁ (2-9)

由式(2-7)和式(2-8)代入式(2-9)联立可以得到膝关节旋转角度θ₁为：

在ΔCDE中，利用余弦定理可求得∠CDE：

设踝关节液压驱动单元的初始长度为l₀₂，则实际运动过程中液压驱动单元总长度CE设为

CE＝l₀₂+Δx_p2 (2-12)

由踝关节D处的角度关系可得

θ₂＝π-∠ODC-∠EDF-∠CDE (2-13)

由式(2-11)和式(2-12)代入式(2-13)联立可以得到膝关节旋转角度θ₂为：

运动学反解是指已知足端相对根关节O的位置关系(x，y)和单腿各连杆的长度参数(l₁、l₂)可以求得膝关节和踝关节角度，由式(2-6)可以求得单腿膝关节角度θ₁和踝关节的角度θ₂为：

但实际运动过程中可以直接控制的量为膝关节和踝关节HDU伸出长度Δx_p1和Δx_p2，由式(2-10)、式(2-14)带入式(2-15)整理可得最终运动学反解为：

为了便于理论分析和仿真实验方便，本发明设定膝关节角度θ₁＝-120°和踝关节的角度θ₂＝60°时对应的足端位置为初始位置，定义此时足端坐标为(a,b)，此时运动学位置正解变为：

此时运动学位置反解变为：

要求得腿部足端的空间运动位置范围，首先要得到膝关节旋转角度和踝关节旋转角度运动范围，而膝关节旋转角度和踝关节旋转角度由两液压驱动单元伸出长度范围决定，膝关节液压驱动单元总长度AB＝l₀₁+Δx_p1和踝关节液压驱动单元总长度CE＝l₀₂+Δx_p2范围为：

212mm≤l₀₁+Δx_p1＝l₀₂+Δx_p2≤282mm (2-19)

结合式(2-10)、式(2-14)和式(2-19)可以求得膝关节运动角度θ₁和踝关节运动角度θ₂范围为：

由式(2-17)和各关节角度运动范围利用MATLAB进行编程，可得腿部足端的空间运动位置范围如图4所示。

步骤103：建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型。

LHDS半圆形足端结构实物图如图5所示，其中，半圆柱形足端通过螺栓连接到小腿的刚性筒上，足端外侧包裹着一层橡胶垫，橡胶垫表面带有均匀排布的颗粒状突起，起防滑和被动缓冲的作用。

图6所示为足端为点足时单腿在支撑阶段的根关节轨迹，从图6中可以看出LHDS在支撑阶段时，根据式(2-16)运动学反解得到t₁→t₃时刻膝关节转角θ₁和踝关节转角θ₂所生成的根关节实际轨迹与期望轨迹重合；图7所示为带有半圆柱形足端与地面不存在相对滑动时根关节实际轨迹与期望轨迹，从图7中可以看出，由于半圆柱形足端的存在，LHDS在向前移动的同时半圆柱形足端与地面的接触点不断变化，使得LHDS根关节实际轨迹偏移了根关节期望轨迹，这些偏移将会时机器人产生错误的位姿，导致机器人整机运动性能变差，因此有必要对t₁→t₃时刻膝关节转角θ₁和踝关节转角θ₂进行修正，如图8所示。

从图7中可以看出t₁→t₃时刻，根关节轨迹偏移同时发生在垂直于接触面方向和水平于接触面方向，根关节的偏移将导致机器人躯体理想的位姿和实际位姿之间的偏移，对足式机器人运动性能有着重要的影响，因此针对半圆柱形足端设计一种运动学修正方法对于提升机器人的运动控制性能有着重要的意义。

机器人实际运动过程中，机器人机身与接触面不平行的情况时有出现，图9为机器人机身与接触面所成的夹角为

时，腿部由于半圆柱形足端滚动使根关节偏移现象的示意图，采用现有技术可估计出机器人机身与接触面所成的夹角

为了分析方便，本发明图9将半圆柱形足端放大，以根关节为原点O建立坐标系，其中平行于接触面向右为x₀轴正方向，垂直于接触面向上为y₀轴正方向，图9中虚线为将足端视为点足时的单腿位置，实线为由于半圆柱形足端滚动引起的单腿实际位置，其中O_r为半圆柱形足端的圆心，O_d为刚性筒中轴线与半圆柱形足端的矩形平面的交点，F′为理想足落点，F为实际参考点，O’、x₀’、y₀’分别为理想原点和坐标轴，D’为理想踝关节位置，P为半圆柱形足端与地面实际接触点，

为

与

夹角，γ为踝关节与足端顶点形成的连线与刚性筒中轴线夹角，由于半圆柱形足端与地面相切，所以

垂直于接触面，

为由于半圆柱形足端滚动根关节发生的位置偏移。

由图9可得：

假设足端与地面之间始终没有相对滑动，可得：

其中

表示半圆柱形足端中P点到F点之间的圆弧长度。

为了求得

与

的夹角

本发明将膝关节旋转角度θ₁、踝关节旋转角度θ₂以及γ、

和

平行移动至踝关节D点，平行移动后得到带有半圆柱形足端的足式机器人腿部结构模型如图10所示。

由图10中的结构关系可得

与

的夹角

为：

由图9可得根关节的位置偏移

为

将式(2-10)、式(2-14)、式(2-23)带入式(2-24)可得根关节位置偏移

与膝关节液压驱动单元伸出长度Δx_p1、踝关节液压驱动单元伸出长度Δx_p2、机身与接触面夹角

的关系为：

由式(2-25)可得，当机器人机身与斜面成不同角度时，根关节位置偏移

与斜面的坡

关，但当坡度较大时，前后腿的工作空间都极度减小，限制了行进步幅，如果坡度更大，可能超出单腿工作空间，因此本发明结合式(2-6)、式(2-20)、式(2-25)和图4所示的机器人腿部工作空间，给出机器人机身与接触面所成的夹角

为0°、10°和20°时，腿部工作空间内由于半圆柱形足端引起的根关节偏移量

如图11和图12所示。

从图11和图12中可以看出，半圆柱形足端引起的根关节轨迹偏移量不仅发生在垂直于接触面方向上，同时也会引起平行于接触面方向上；其中，如图11所示，机器人机身与接触面所成的夹角

为0°、10°和20°时，x₀轴正方向的最大的偏移量绝对值分别为45.25mm、55.85mm和67.43mm，如图12所示，y₀轴正方向的最大的偏移量绝对值分别为57.67mm、63.64mm和68.88mm。对于多条腿同时着地的并联结构，这些偏移将会引起机器人躯体理想位姿和实际位姿之前的偏差，引起机器人运动性能变差，而且对于多条支撑腿同时运动时，其各条腿偏差各不相同，这将会导致机器人运动控制性能进一步变差，因此针对半圆柱形足端的特殊结构提出运动学修正方法是十分必要的。

从上文对带有半圆柱形足端支撑腿根关节轨迹偏移现象的分析中可以看出，足式机器人在行走过程中，由于半圆柱形足端的存在引起根关节轨迹偏移，进而造成机器人躯体轨迹发生偏移，对机器人整体控制精度、性能产生不利影响，因此发明针对带有半圆柱形足端LHDS根轨迹偏移问题提出了一种机器人机身与接触面成任意角度均适用的运动学修正方法。

本发明所提出的运动学修正方法基于的单腿运动学模型如图13所示，如图13所示的单腿运动学模型以所述足式机器人腿部的根关节为原点O，以平行于接触面向右为x₀轴正方向，垂直于接触面向上为y₀轴正方向；将大腿构件表示为线段OD，OD与x₀轴正向的旋转角度为膝关节旋转角度θ₁；O_r为所述半圆柱形足端的圆心，O_d为刚性筒中轴线与半圆柱形足端的矩形平面的交点，F为足落点的实际参考点，P为半圆柱形足端与地面实际接触点，

为

与

夹角，DO_d与OD延长线方向的旋转角度为踝关节旋转角度θ₂；所述足式机器人机身与接触面所成的夹角为

γ为O_r F与DF的夹角。R为腿部液压驱动系统半圆柱形足端的半径。

图13所示的单腿运动学模型中，半圆柱形足端与地面接触点为P，考虑到半圆柱形足端与地面恒相切，因此本发明采用假设自由度的方法，将半圆柱足端虚拟为一条恒垂直于地面的杆件

恒垂直于接触面且可绕半圆柱形足端圆心O_r不断旋转，此时带有半圆柱形足端的两连杆腿部机械结构简化为三连杆腿部机械结构，即腿部机械结构由OD杆、DO_dO_r杆和一条恒垂直于接触面的O_rP杆组成，在根关节坐标系下求得接触点P与根关节的相对位置关系即为本发明得到LHDS的运动学正反解。

步骤104：根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解。

由图13所示的所述单腿运动学模型可得，实际参考位置

和足端与地面实际接触点

的关系为：

式(3-1)中

为：

式(3-1)中

为：

联合式(2-23)、式(3-1)、式(3-2)和式(3-3)可得

为：

由式(3-4)可以得到带有半圆柱形足端的第一腿部液压驱动系统运动学正解为：

其中x、y分别为点F的横、纵坐标；

以长度和长度

构造直角三角形，如图14所示，ε为直角三角形斜边和长度为

的边的夹角，由图14可得：

将式(3-6)和式(3-7)带入式(3-5)，所述带有半圆柱形足端的第一腿部液压驱动系统运动学正解可转化为：

利用式(2-10)和式(2-14)将式(3-8)整理可得，最终带有半圆柱形足端的腿部液压驱动系统运动学正解如下：

步骤105：对所述腿部液压驱动系统运动学正解进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解。

对所述腿部液压驱动系统运动学正解(3-9)进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解为：

步骤106：根据所述腿部液压驱动系统运动学反解对所述足式机器人腿部运动进行修正。

本发明所述腿部液压驱动系统运动学反解(3-10)表示的就是根关节和踝关节液压驱动单元伸出长度(Δx_p1、Δx_p2)与足端位置F(x，y)的关系，因此可以采用所述腿部液压驱动系统运动学反解(3-10)对所述足式机器人腿部运动进行修正，控制其足端实际位置F(x，y)与期望位置F’重合。

下面将对本发明提出的足式机器人腿部运动学修正方法的有效性进行验证。

由于根关节水平方向偏移量难以直接测量，从式(2-24)中可得，如果根关节竖直方向偏移得到改善，则水平方向的偏移也一定得到改善，因此，本发明将根关节轨迹竖直方向的偏移量作为评价指标，进行测试，由于在腿部液压驱动系统性能测试实验台中直接检测支撑阶段根关节轨迹偏移量较为困难，本发明利用负载模拟实验平台进行根关节轨迹竖直方向偏移量的检测，其中负载模拟实验平台主要由两套高集成性阀控缸系统、可拆卸固连结构、竖直移动平台、水平移动平台及固定底板组成，其三维模型如图15所示，图15中1为基座、2为水平移动平台、3为高集成阀控非对称缸系统、4为竖直移动平台、5为足端连接结构。

在图15中，水平移动平台2与基座1通过直线导轨实现水平方向的移动。足端连接结构5固接于竖直移动平台4，通过光轴实现竖直方向的移动。连接结构5用于负载模拟实验台与机器人单腿足端的连接，可实现足端位置和力的加载，当拆除连接结构5时，用于模拟不同的路面环境。

腿部液压驱动系统支撑阶段根关节轨迹偏移实验测试现场如图16所示，其中腿部液压驱动系统的基座被紧固在支架上，通过改变膝关节和踝关节液压驱动单元的伸出长度实现足端的位置控制。为实现负载模拟实验平台能够实时检测足端竖直方向上偏移量的变化，负载模拟实验平台需在足端不断移动的过程中与足端紧密接触且随着足端竖直方向的偏移不断移动，因此，本发明设计负载模拟实验平台竖直方向做阻抗控制，具体工作原理如图17所示。

图17中，当足式机器人足端竖直方向发生偏移时会对负载模拟平台竖直方向产生干扰力，干扰力作用到负载模拟平台时，竖直方向力传感器检测到干扰力通过阻抗特性求解器使干扰力信号转化为竖直方向液压驱动单元的位置变化量，从而实现竖直移动的平面随着足端竖直方向的偏移产生竖直方向上的移动，通过上述过程保证在支撑阶段负载模拟实验台与足端紧密接触且随着足端竖直方向的偏移不断移动，从而间接的实现根关节轨迹竖直方向偏移量的检测。

根关节轨迹偏移量曲线由根关节到负载模拟实验平台缩到最短长度减去竖直方向液压驱动单元阻抗实际位置减去腿部运动学竖直方向期望位置得到，仿真测试中给出了理论根关节偏移量曲线、未修正偏移量曲线和修正后偏移量曲线。本发明通过垫高负载模拟实验平台，实现机身与接触面成不同角度的模拟，具体测试方案如表3所示：

表3测试方案

表3中，方案1和方案2目的是测试腿部液压驱动系统支撑阶段机身与接触面成相同角度，足端位于不同位置时，本发明运动学修正方法的修正效果；方案1、方案3和方案4目的是测试腿部液压驱动系统支撑阶段机身与接触面成不同角度时，本发明运动学修正方法的修正效果。

仿真测得方案1、方案2、方案3和方案4下根关节轨迹理论偏移量、未修正偏移量和本发明修正后偏移量与足端平行于接触面的实际位置关系分别如图18-21所示。在图18-21中，理论偏移量曲线181为各液压驱动单元实时伸出长度带入式(2-25)得到的根关节y₀轴正方向偏移量；未修正偏移量曲线182为应用将足端视为点足时运动学正反解下的根关节y₀轴正方向偏移量；修正后偏移量曲线183为应用本发明所提出的运动学修正方法时的根关节y₀轴正方向偏移量。通过理论偏移量181和未修正仿真偏移量182基本重合可以看出，本发明关于根关节轨迹偏移量的理论计算是正确的；在方案1至方案4中，加入本发明所提出的修正方法后，根关节轨迹偏移量几乎为零，表明本发明所提出的运动学修正方法在机身与接触面成不同角度时都适用。

本发明考虑到半圆柱足端与地面接触过程中恒相切，采用将半圆柱足端虚拟为一条恒垂直于地面的杆件，结合将足端视为理想足时的运动学策略，提出了一种机器人机身与接触面成不同角度时仍然适用的足式机器人腿部运动学修正方法。仿真结果证明了本发明关于根关节轨迹偏移量理论计算的正确性和本发明提出的运动学修正方法的有效性。采用理想运动学时，虽然能够实现机器人的移动，但由于半圆柱形足端的存在，会引起根关节轨迹发生偏移，从而使机器人产生错误的位姿，影响机器人的运动控制性能，在应用本发明所提出的运动学修正方法对机器人腿部运动过程进行修正后，根关节轨迹偏移的现象得到了消除，本发明方法有效提升了LHDS的运动控制精度。

基于本发明提供的运动学修正方法，本发明还提供一种带半圆柱形足端的足式机器人腿部运动学修正系统，参见图22，所述系统包括：

单腿坐标系建立模块221，用于将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；

旋转角度计算模块222，用于根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度和踝关节旋转角度；

单腿运动学模型建立模块223，用于建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；

运动学正解求解模块224，用于根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解；

运动学反解求解模块225，用于对所述腿部液压驱动系统运动学正解进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解；

运动学修正模块226，用于根据所述腿部液压驱动系统运动学反解对所述足式机器人腿部运动进行修正。

其中，所述单腿坐标系建立模块221具体包括：

单腿坐标系建立单元，用于将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；所述单腿坐标系以所述足式机器人腿部的根关节为坐标原点O，水平向右为x₀轴正方向，竖直向上为y₀轴正方向，将大腿构件表示为线段OD，小腿构件表示为线段DF，将膝关节液压驱动单元伸出总长度表示为线段AB，踝关节液压驱动单元伸出总长度表示为线段CE；OD与x₀轴正向的旋转角度为膝关节旋转角度θ₁，DF与OD延长线方向的旋转角度为踝关节旋转角度θ₂；OD与OB所成的夹角为α；OA与x₀轴负方向所成的夹角为β；所述单腿坐标系中关节旋转角度逆时针方向旋转为正方向；所述膝关节液压驱动单元伸出长度为Δx_p1，所述踝关节液压驱动单元伸出长度为Δx_p2。

所述旋转角度计算模块222具体包括：

第一角度计算单元，用于根据所述单腿坐标系，利用余弦定理求得∠AOB：

膝关节初始长度获取单元，用于获取所述膝关节液压驱动单元的初始长度l₀₁；

膝关节总长度确定单元，用于根据所述初始长度l₀₁确定实际运动过程中所述膝关节液压驱动单元总长度AB为：AB＝l₀₁+Δx_p1 (2-8)

第二角度计算单元，用于由所述单腿坐标系中根关节O处的角度关系，得到∠AOB为：∠AOB＝β+π+α+θ₁ (2-9)

膝关节旋转角度计算单元，用于由式(2-7)和式(2-8)代入式(2-9)联立得到所述膝关节旋转角度θ₁为：

第三角度计算单元，用于在所述单腿坐标系的ΔCDE中，利用余弦定理求得∠CDE：

踝关节初始长度获取单元，用于获取踝关节液压驱动单元的初始长度l₀₂；

踝关节总长度确定单元，用于根据所述初始长度l₀₂确定实际运动过程中踝关节液压驱动单元总长度CE为：CE＝l₀₂+Δx_p2 (2-12)

角度关系获取单元，用于由所述单腿坐标系中踝关节D处的角度关系得：

θ₂＝π-∠ODC-∠EDF-∠CDE (2-13)

由式(2-11)和式(2-12)代入式(2-13)联立可得所述膝关节旋转角度θ₂为：

所述单腿运动学模型建立模块223具体包括：

单腿运动学模型建立单元，用于以所述足式机器人腿部的根关节为原点O建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型。

所述运动学正解求解模块224，具体包括：

夹角计算单元，用于由所述单腿运动学模型中的结构关系可得

与

的夹角

为：

线段关系获取单元，用于由所述单腿运动学模型得到实际参考位置

和足端与地面实际接触点

的关系为：

式(3-1)中

为：

式(3-1)中

为：

线段计算单元，用于联合式(2-23)、式(3-1)、式(3-2)和式(3-3)可得

为：

第一腿部液压驱动系统运动学正解计算单元，用于由式(3-4)得到带有半圆柱形足端的第一腿部液压驱动系统运动学正解(3-5)。

直角三角形构建单元，用于以长度

和长度

构造直角三角形，ε为直角三角形斜边和长度为

的边的夹角，得到：

运动学正解可转化单元，用于将式(3-6)和式(3-7)带入式(3-5)，将所述带有半圆柱形足端的第一腿部液压驱动系统运动学正解转化为式(3-8)。

腿部液压驱动系统运动学正解求解单元，用于利用式(2-10)和式(2-14)将式(3-8)整理得到最终带有半圆柱形足端的腿部液压驱动系统运动学正解(3-9)。

所述运动学反解求解模块225具体包括：

运动学反解求解单元，用于对所述腿部液压驱动系统运动学正解(3-9)进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解(3-10)。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种带半圆柱形足端的足式机器人腿部运动学修正方法，其特征在于，所述方法包括：

将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；

根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度和踝关节旋转角度；

建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；

根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解；

对所述腿部液压驱动系统运动学正解进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解；

根据所述腿部液压驱动系统运动学反解对所述足式机器人腿部运动进行修正；

所述将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系，具体包括：

将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；所述单腿坐标系以所述足式机器人腿部的根关节为坐标原点O，将大腿构件表示为线段OD，小腿构件表示为线段DF，将膝关节液压驱动单元伸出总长度表示为线段AB，踝关节液压驱动单元伸出总长度表示为线段CE；OD与OB所成的夹角为α；OA与x₀轴负方向所成的夹角为β；所述膝关节液压驱动单元伸出长度为Δx_p1，所述踝关节液压驱动单元伸出长度为Δx_p2；

所述建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型，具体包括：

以所述足式机器人腿部的根关节为原点O建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；O_r为所述半圆柱形足端的圆心，F为足落点的实际参考点，x、y分别为点F的横、纵坐标；所述足式机器人机身与接触面所成的夹角为

γ为O_r F与DF的夹角；

所述带有半圆柱形足端的腿部液压驱动系统运动学反解为：

其中，l₀₁为所述膝关节液压驱动单元的初始长度；l₀₂为踝关节液压驱动单元的初始长度；l₁为大腿构件OD的长度；l₂为小腿构件DF的长度；R为所述半圆柱形足端的半径。

2.根据权利要求1所述的足式机器人腿部运动学修正方法，其特征在于，所述将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系，具体包括：

水平向右为x₀轴正方向，竖直向上为y₀轴正方向，OD与x₀轴正向的旋转角度为膝关节旋转角度θ₁，DF与OD延长线方向的旋转角度为踝关节旋转角度θ₂；所述单腿坐标系中关节旋转角度逆时针方向旋转为正方向。

3.根据权利要求2所述的足式机器人腿部运动学修正方法，其特征在于，所述根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度，具体包括：

根据所述单腿坐标系，利用余弦定理求得∠AOB：

获取所述膝关节液压驱动单元的初始长度l₀₁；

根据所述初始长度l₀₁确定实际运动过程中所述膝关节液压驱动单元总长度AB为：

AB＝l₀₁+Δx_p1 (2-8)

由所述单腿坐标系中根关节O处的角度关系，可得∠AOB为：

∠AOB＝β+π+α+θ₁(2-9)

由式(2-7)和式(2-8)代入式(2-9)联立可得所述膝关节旋转角度θ₁为：

4.根据权利要求3所述的足式机器人腿部运动学修正方法，其特征在于，所述根据所述单腿坐标系确定所述足式机器人腿部的踝关节旋转角度，具体包括：

在所述单腿坐标系的ΔCDE中，利用余弦定理可求得∠CDE：

获取踝关节液压驱动单元的初始长度l₀₂；

根据所述初始长度l₀₂确定实际运动过程中踝关节液压驱动单元总长度CE为：

CE＝l₀₂+Δx_p2 (2-12)

由所述单腿坐标系中踝关节D处的角度关系可得：

θ₂＝π-∠ODC-∠EDF-∠CDE (2-13)

由式(2-11)和式(2-12)代入式(2-13)联立可得所述膝关节旋转角度θ₂为：

5.根据权利要求4所述的足式机器人腿部运动学修正方法，其特征在于，所述建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型，具体包括：

所述单腿运动学模型中平行于接触面向右为x₀轴正方向，垂直于接触面向上为y₀轴正方向；所述单腿运动学模型中，将大腿构件表示为线段OD，OD与x₀轴正向的旋转角度为膝关节旋转角度θ₁；所述单腿运动学模型中，O_d为刚性筒中轴线与半圆柱形足端的矩形平面的交点，P为半圆柱形足端与地面实际接触点，

为

与

夹角，DO_d与OD延长线方向的旋转角度为踝关节旋转角度θ₂。

6.根据权利要求5所述的足式机器人腿部运动学修正方法，其特征在于，所述根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解，具体包括：

由所述单腿运动学模型中的结构关系可得

与

的夹角

为：

由所述单腿运动学模型可得实际参考位置

和足端与地面实际接触点

的关系为：

式(3-1)中

为：

式(3-1)中

为：

联合式(2-23)、式(3-1)、式(3-2)和式(3-3)可得

为：

由式(3-4)得到带有半圆柱形足端的第一腿部液压驱动系统运动学正解为：

以长度

和长度

构造直角三角形，ε为直角三角形斜边和长度为

的边的夹角，可得：

将式(3-6)和式(3-7)带入式(3-5)，所述带有半圆柱形足端的第一腿部液压驱动系统运动学正解可转化为：

利用式(2-10)和式(2-14)将式(3-8)整理可得，最终带有半圆柱形足端的腿部液压驱动系统运动学正解如下：

7.一种带半圆柱形足端的足式机器人腿部运动学修正系统，其特征在于，所述系统包括：

单腿坐标系建立模块，用于将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；

旋转角度计算模块，用于根据所述单腿坐标系确定所述足式机器人腿部的膝关节旋转角度和踝关节旋转角度；

单腿运动学模型建立模块，用于建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；

运动学正解求解模块，用于根据所述单腿运动学模型确定带有半圆柱形足端的腿部液压驱动系统运动学正解；

运动学反解求解模块，用于对所述腿部液压驱动系统运动学正解进行逆运动学计算，生成带有半圆柱形足端的腿部液压驱动系统运动学反解；

运动学修正模块，用于根据所述腿部液压驱动系统运动学反解对所述足式机器人腿部运动进行修正；

所述将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系，具体包括：

将所述足式机器人的足端视为点足，基于D-H方法建立所述足式机器人腿部的单腿坐标系；所述单腿坐标系以所述足式机器人腿部的根关节为坐标原点O，将大腿构件表示为线段OD，小腿构件表示为线段DF，将膝关节液压驱动单元伸出总长度表示为线段AB，踝关节液压驱动单元伸出总长度表示为线段CE；OD与OB所成的夹角为α；OA与x₀轴负方向所成的夹角为β；所述膝关节液压驱动单元伸出长度为Δx_p1，所述踝关节液压驱动单元伸出长度为Δx_p2；

所述建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型，具体包括：

以所述足式机器人腿部的根关节为原点O建立带有半圆柱形足端的足式机器人腿部液压驱动系统的单腿运动学模型；O_r为所述半圆柱形足端的圆心，F为足落点的实际参考点，x、y分别为点F的横、纵坐标；所述足式机器人机身与接触面所成的夹角为

γ为O_r F与DF的夹角；

所述带有半圆柱形足端的腿部液压驱动系统运动学反解为：

其中，l₀₁为所述膝关节液压驱动单元的初始长度；l₀₂为踝关节液压驱动单元的初始长度；l₁为大腿构件OD的长度；l₂为小腿构件DF的长度；R为所述半圆柱形足端的半径。

8.根据权利要求7所述的足式机器人腿部运动学修正系统，其特征在于，所述单腿坐标系建立模块具体包括：

水平向右为x₀轴正方向，竖直向上为y₀轴正方向，OD与x₀轴正向的旋转角度为膝关节旋转角度θ₁，DF与OD延长线方向的旋转角度为踝关节旋转角度θ₂；所述单腿坐标系中关节旋转角度逆时针方向旋转为正方向。

9.根据权利要求8所述的足式机器人腿部运动学修正系统，其特征在于，所述旋转角度计算模块具体包括：

第一角度计算单元，用于根据所述单腿坐标系，利用余弦定理求得∠AOB：

膝关节初始长度获取单元，用于获取所述膝关节液压驱动单元的初始长度l₀₁；

膝关节总长度确定单元，用于根据所述初始长度l₀₁确定实际运动过程中所述膝关节液压驱动单元总长度AB为：

AB＝l₀₁+Δx_p1 (2-8)

第二角度计算单元，用于由所述单腿坐标系中根关节O处的角度关系，得到∠AOB为：

∠AOB＝β+π+α+θ₁ (2-9)

膝关节旋转角度计算单元，用于由式(2-7)和式(2-8)代入式(2-9)联立得到所述膝关节旋转角度θ₁为：

Images