一种轮足式机器人的柔顺控制方法
【专利摘要】本发明公开了一种轮足式机器人的柔顺控制方法，包括：外环控制，为机器人本体位姿控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；中环控制，为足端接触力控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；内环控制，为主动关节驱动力矩控制，控制目标是实现支撑腿足端接触力跟踪外环计算得到的期望力；通过上述三个控制，在准确控制机器人本体位姿的同时，对支撑腿足端接触力进行控制。本发明具有原理简单、控制精度高、能够提高机器人适应能力等优点。
【专利说明】
【技术领域】
[0001] 本发明主要涉及到机器人的控制【技术领域】，特指一种适用于轮足式机器人的柔顺 控制方法。 一种轮足式机器人的柔顺控制方法
【背景技术】
[0002] 轮式机器人是以轮式机构为支撑和驱动的移动机器人，其主要优点是速度快、能 耗低，缺点是越障能力、非结构化地形适应能力差、转弯半径大。足式机器人以步行、奔跑和 跳跃为移动方式，其主要优点是重量轻、转弯半径小和地形适应性强，缺点是相对于轮式机 器人速度较慢，结构复杂。轮足式机器人综合了轮式机器人和足式机器人的构型优点，既继 承了足式机器人对非结构化地形的适应能力又避免了其结构复杂的缺点。
[0003] 轮足式机器人一般分为三种：1、轮式机构和足式机构在安装方式上相互独立，轮 式机构带有驱动装置，即轮子为主动轮；2、轮式机构和足式机构在安装方式上相互独立，轮 式机构不带有驱动装置，即轮子为从动轮，机器人依靠腿部的主动关节运动；3、轮式机构安 装在腿部或者足端。
[0004] 现有对于轮足式机器人的行走控制，基本的方法是通过计算腿部各个主动关节的 驱动力矩来实现对机器人本体位置和姿态的控制。轮足式机器人在非结构化地形中的稳定 行走，要求腿部对环境有柔顺适应性。目前，机器人控制中普遍采用以下方法：
[0005] 1、被动柔顺；即借助柔性机构或者柔性关节（例如弹簧）使机器人对环境作用力 产生自然顺从，但是被动柔顺方法能够提供的机器人对非结构化环境的适应能力有限，而 且容易使机器人足端在行走过程中产生滑动。
[0006] 2、腿部关节期望运动轨迹的在线修正；利用接触开关检测信息判断足端与地面的 接触状态，根据接触状态对支撑腿部关节的期望位置轨迹进行在线修正。这种方法能够为 机器人提供一定的柔顺性，但是由于落足瞬间地面与足端为刚性接触，所以会产生较大的 冲击力，容易影响机器人本体的稳定性。
[0007] 3、足端接触力控制；通过力传感器反馈足端在摆动期和支撑期转换阶段与地面的 接触力信息，设计足端接触力控制算法，实现机器人足端接触地面的柔顺性。足端接触力控 制策略可以实现机器人足端与地面的柔顺接触，但是会导致控制目标向量维数扩大，影响 机器人本体位姿的可控性。
【发明内容】
[0008] 本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一 种原理简单、控制精度高、能够提高机器人适应能力的轮足式机器人的柔顺控制方法。
[0009] 为解决上述技术问题，本发明采用以下技术方案：
[0010] 一种轮足式机器人的柔顺控制方法，包括：
[0011] 外环控制，为机器人本体位姿控制，控制目标是使机器人本体的位姿准确跟踪期 望的位姿轨迹；
[0012] 中环控制，为足端接触力控制，控制目标是使机器人本体的位姿准确跟踪期望的 位姿轨迹；
[0013] 内环控制，为主动关节驱动力矩控制，控制目标是实现支撑腿足端接触力跟踪外 环计算得到的期望力；
[0014] 通过上述三个控制，在准确控制机器人本体位姿的同时，对支撑腿足端接触力进 行控制。
[0015] 作为本发明的进一步改进：所述外环控制中，首先建立基于牛顿-欧拉方程的包 含非完整约束条件的机器人本体动力学模型，将通过惯性传感器得到的机器人本体位姿数 据作为反馈量，与期望位姿轨迹比较；然后，基于动力学模型设计PID控制器，计算控制量， 即支撑腿足端期望接触力；最后，对接触面切向力进行限幅，使之满足摩擦锥约束。
[0016] 作为本发明的进一步改进：所述基于牛顿-欧拉方程的机器人本体动力学模型方 程如下：
[0017] Μχ β Fey ⑴
[0018] y = R+L〇sin β (2)
[0019] y = 0 (3)
[0020] xsina + zcosa = 0 (4)
[0021] 其中，Mi为惯性矩阵，A为力臂矩阵，x，y，z为机器人本体位置，x轴方向为机器人 本体前进方向，y轴方向与重力方向相反，z轴方向由右手法则确定；F ex，Fey，Fez为支撑腿足 端接触力，R为后轮半径，U为机器人本体质心与轮轴之间的距离，α，β，γ分别为机器人 本体的方位角、俯仰角和滚动角；公式（1)表示机器人本体的动力学方程，公式（2)和公式 (3)表示完整约束条件，公式（4)表示非完整约束条件。
[0022] 作为本发明的进一步改进：所述外环控制采用PID控制器作为外环控制器，具体 形式如下： 「1 Kpx (?-x) + Kdx (xd-x) + kix^(xJ-x)dr
[0023] Feyd = A lMl Κρβ {β?~β) + Κ?β {^β? -β) + kip^{β?-β)?τ L Κρα (atl -a) + Kda (?, -?) + Μα ￡ (a, -a)dT
[0024] 其中，F' exd，F' eyd，F' ezd为期望的足端接触力，xd为期望的χ方向位置，β d为期望 的俯仰角，a d为期望的方位角；加入摩擦锥约束条件：F' exd〈 μ F' eyd、F' ezd〈 μ F' eyd，μ为摩 擦系数，控制器变为：
[0025] Feyd = F' eyd π \ F\,d F - -順，-順-
[ ] exd _ I p' f > p' p' -〇 SjuF L exd_max exd - exd_max exd_max * eyd 「?r-, _ I ^ ezd F e=d < F m-輝，!7e:d max=Q^pF ey(i ezd ~\Ρ' F > F F = 〇 %uF L ezd_maK ezd - ezd msy.9 ezd υω?χ
[0028] 通过调节参数Kpx、Kdx、kix、Kp e、Kde、kie、Κρα、Kda、ki a，保证在控制本体位姿的 同时，避免机器人在行走和转弯的过程中出现支撑腿足端滑动的现象。
[0029] 作为本发明的进一步改进：所述中环控制中，首先基于支撑腿和摆动腿解耦控制 的思想建立支撑腿的运动学模型和基于拉格朗日方程的动力学模型，通过足端力传感器反 馈足端接触力信息，计算支撑腿的各个主动关节的输出力矩；中环控制采用基于ro算法的 力/位混合控制器。
[0030] 作为本发明的进一步改进：所述支撑腿的运动学模型如下：
[0031 ] 在支撑腿髋部建立坐标系，各坐标轴方向与机器人本体坐标系方向相同，在髋部 坐标系下，足端点的坐标为：
[0032] xe = LjCOS ( θ θ 2~*~ ^ 3_ ) +L3C0S ( θ 3_ ) -L2C0S ( θ 2_ ? 3~*~ )
[0033] ye = -cos θ 4 (Ljsin ( θ θ 2+θ 3-β )+L3sin ( θ 3-β )+L2sin ( θ 2-θ 3+β )) (5)
[0034] ze = -sin θ 4 (I^sin ( θ 厂 θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β ))
[0035] 其中，（xe，ye，ze)为足端点坐标，Lp L2, L3分别为脚掌、小腿、大腿的长度， θ2, θ3, 04分别为踝关节角、膝关节角、髋关节角、髋部侧向关节角；
[0036] 令 θ = [θ" θ2, θ3, 0jT，q = [xe，ye，zJT，夺= [υ.，ζ』-(?]7'，则 $ = .%，其中J为支撑腿雅克比矩阵，J为增广雅克比矩阵： dv./o^ cxjd62 cxjde, c\xr/d04
[0037] J= dyJd〇l dye[d02 dyjd03 dye/d04 (6) czJdO^ dzjc02 czJdO. DzJceA_ &',/叫 碑（3λ:/坤（1\',/('沒4 romsi J= dy^d0x ?γ^?θι dy^d°l dy^d0A (7) cz,/c^ czjde, czjde, czJc6a 1 -1 0 0
[0039] 基于拉格朗日方程的支撑腿动力学方程为：
[0040] /W (<9)Θ f €(θ,θ)Θ f Ν[θ,θ) = τ + JrFe (8)
[0041] 其中，Μ(θ)为惯性矩阵，C|>,句为哥式力矩阵，?为重力项，Fe为足端接触 力向量，将$ = 代入公式（8)，得到：
[0042] M (cj)cj + C{q,c/)c/ Ν= v + JlFe
[0043] 其中，々⑷=Μ⑷⑷-\ (Τ'(以）=C(以)(J)'+从⑷。 V J
[0044] 作为本发明的进一步改进：所述中环控制的控制器如下：
[0045] ^ 卜札-匕)+人艰夂)_ τ=Μ 〇 +Q + iV -，仰2(匕-/g + A?/2(Z：,.(/-心)。 ^4 (0 - ^： + )+Kih (〇 - 4 + 4) Kp'A'h - D+κ(!人匕厂匕)_
[0046] 作为本发明的进一步改进：所述内环控制是利用关节位移传感器和力传感器实时 检测作动器长度信息和关节力信息，根据运动学模型计算关节力臂，解算出关节期望力，与 实际的关节力进行比较；再利用PI控制器，生成关节作动器的控制电流，实现关节力准确 跟踪期望力，进而实现关节驱动力矩对期望力矩的跟踪。
[0047] 作为本发明的进一步改进：所述内环控制的PI控制器如下：
[0048] i - KPi (./；, -./) + Κι) ￡ (./；, -f)dr
[0049] 生成关节作动器的控制电流i，实现关节力f准确跟踪期望力fd，进而实现关节驱 动力矩对期望力矩的跟踪。
[0050] 与现有技术相比，本发明的优点在于：
[0051] 1、在轮足式机器人运动过程中，采用本发明设计的三环控制策略可以实现本体的 位姿准确跟踪由步态规划生成的期望位姿轨迹。
[0052] 2、本发明提出的控制策略能够有效避免机器人在行走、转弯过程中支撑腿足端滑 动的现象。
[0053] 3、本发明能够提高轮足式机器人对非结构化环境的适应能力，实现机器人在不平 整地面上的行走、转弯。
[0054] 4、本发明具有较强的通用性。控制策略不依赖具体的轮足式机器人系统，只要构 建合适的传感器体系，推导机器人运动学、动力学模型，适当调整控制器参数，即可实现本 发明所提出的方法。
[0055] 5、本发明结构清晰、层次分明，具有较好的理论价值和工程意义
【专利附图】
【附图说明】
[0056] 图1是轮足式机器人系统平面示意图。
[0057] 图2是本发明的控制结构框图。
[0058] 图3是单个步行周期内控制流程图。
【具体实施方式】
[0059] 以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
[0060] 本发明主要应用于轮足式机器人系统，主要针对的对象为轮式机构和足式机构在 安装方式上相互独立，轮式机构不带有驱动装置的轮足式机器人，即轮子为从动轮，机器人 依靠腿部的主动关节运动。如图1所示，该轮足式机器人系统由两条腿和两个从动轮轮子 组成，每条腿包含四个主动关节（踝关节、膝关节、髋关节、髋部侧向关节），各主动关节均 采用液压作动器驱动，足端与地面为点接触。每个主动关节装有位移传感器和力传感器，用 于检测作动器长度信息和关节力信息，在足端点通过三维力传感器反馈足端与地面的接触 力信息，机器人本体在惯性空间中的位置、姿态可通过MU实时检测。作为本发明的控制对 象，上述轮足式机器人具有如下机械结构特性：
[0061] a、机器人重心靠近后轮，即在单腿支撑状态下机器人满足ZMP稳定性条件，可以 稳定站立。
[0062] b、基于特性a，轮足式机器人在行走过程中，后轮不会离开地面。
[0063] c、机器人本体的质量远大于腿部的质量。
[0064] 如图2所示，发明轮足式机器人的柔顺控制方法，基于支撑腿和摆动腿解耦控制 的思想，采用三环控制策略，即：外环为机器人本体位姿控制，中环为足端接触力控制，内环 为主动关节驱动力矩控制。在准确控制机器人本体位姿的同时，将支撑腿足端接触力控制 在一个较小的范围内，避免支撑腿足端滑动，提高机器人对非结构化环境的适应能力。图 rA Γ 0 ~r3 r2 中，，，= r3 0 -71 ; Fd为足端与地面期望接触力；F为足端与地面实际接触 _r3」 卜2 η 〇 _ 力；fd为关节期望力；f为关节实际力；τ d为期望输出力矩。
[0065] 外环控制，其控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹。由于轮 子受到非完整约束（轮子在轴向满足速度约束，但是无位置约束），所以首先建立基于牛 顿-欧拉方程的包含非完整约束条件的机器人本体动力学模型，将通过惯性传感器得到的 机器人本体位姿数据（惯性坐标系下的三维位置信息、方位角、俯仰角、滚动角）作为反馈 量，与期望位姿轨迹比较；然后，基于动力学模型设计PID控制器，计算控制量一-支撑腿足 端期望接触力。最后，对接触面切向力进行限幅，使之满足摩擦锥约束，这就可以在实现轮 足式机器人本体位姿控制的同时，有效避免行走过程中支撑腿足端滑动。
[0066] 中环控制，其控制目标是实现支撑腿足端接触力跟踪外环计算得到的期望力，从 而提高机器人对非结构化环境的适应能力。首先，基于支撑腿和摆动腿解耦控制的思想，建 立支撑腿的运动学模型和基于拉格朗日方程的动力学模型。通过足端力传感器反馈足端接 触力信息，设计控制算法，计算支撑腿的各个主动关节的输出力矩。由于单腿有四个主动关 节（三个前向关节，一个侧向关节，见附图1)，而足端接触力信息的维数为三维，所以加入 一个运动学约束条件一踝关节角与膝关节角相等，保证支撑腿雅克比矩阵为方阵。中环控 制器为基于ro算法的力/位混合控制器。经过实验验证，支撑腿足端接触力可以准确跟踪 期望力。
[0067]内环控制，其作用是实现对关节期望力矩的跟踪。借助关节位移传感器和力传感 器实时检测作动器长度信息和关节力信息，根据运动学模型计算关节力臂，解算出关节期 望力，与实际的关节力进行比较；进一步设计PI控制器，生成关节作动器的控制电流，实现 关节力准确跟踪期望力，进而实现关节驱动力矩对期望力矩的跟踪。
[0068] 在上述控制过程中，建模的过程为：
[0069] 首先建立机器人本体坐标系，X轴方向为机器人本体前进方向，y轴方向与重力方 向相反，z轴方向由右手法则确定。由于行走过程中两个后轮不会离开地面，机器人本体受 到三个运动学约束，其中两个为完整约束，一个为非完整约束。基于牛顿-欧拉方程的单腿 支撑期机器人本体动力学方程（本发明以左前腿支撑为例）如下： '!] P;
[0070] ΜΛ β =A Fey (1) β\ \_Fe：_
[0071] y = R+L〇sin β (2)
[0072] y = 0 (3)
[0073] xsinor + zcosa = 0 (4)
[0074] 其中，Mi为惯性矩阵，A为力臂矩阵，x，y，z为机器人本体位置，Fex，F ey，Fez为支撑 腿足端接触力，R为后轮半径，U为机器人本体质心与轮轴之间的距离，α，β，γ分别为机 器人本体的方位角、俯仰角和滚动角。公式（1)表示机器人本体的动力学方程，公式（2)和 公式（3)表示完整约束条件，公式（4)表示非完整约束条件。
[0075] 在支撑腿髋部建立坐标系（以左腿为例），各坐标轴方向与机器人本体坐标系方 向相同，在髋部坐标系下，足端点的坐标为：
[0076] xe = LjCOS ( θ θ 2~*~ ^ 3_ ) +L3COS ( θ 3_ ) -L2COS ( θ 2_ ? 3~*~ )
[0077] ye = -cos θ 4 (I^sin ( θ「θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β )) (5)
[0078] ze = -sin θ 4 (I^sin ( θ 厂 θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β ))
[0079] 其中，（xe，ye，ζε)为足端点坐标，Lp L2, L3分别为脚掌、小腿、大腿的长度， θ2, θ3, 04分别为踝关节角、膝关节角、髋关节角、髋部侧向关节角。
[0080] 令 θ = [Θ" θ2, θ3, 04]T，q = [xe，ye，zJT，$ = 沒:]'，则 4 = | = %,其中J为支撑腿雅克比矩阵，J为增广雅克比矩阵： dv,/e<9, dv./oft clv,/e<9, dv./e^4
[0081] J= dyjd0l dyjd02 dyjd03 cyJcO^ (6) czjce, dzjc02 czjc0, οζ^οθΑ_ cxJdO^ dxjd02 cxjde, cxjde4 Γ00821 J= δ)；，/δθ' Cy<,Cai d)；，/d^ 〇}：，^04 (7) νζ,,/νθ^ czJ〇e2 czjde.^ czjde4 1 -1 0 0
[0083] 基于拉格朗日方程的支撑腿动力学方程为：
[0084] Μ (/}θ + C (θ,--θ + N 0,0) = τ + JT Fe (8)
[0085] 其中，Μ( θ )为惯性矩阵，为哥式力矩阵，为重力项，Fe为足端接触 力向量，将| = 代入公式（8)，得到：
[0086] M {q)ij + C(ci，q)q + N(ci，q) = T + JTFe (9)
[0087] 其中，A ⑷=M ⑷(J·) e(iM〇 = C(以)(J)丨+从⑷ 。 V J
[0088] 在上述控制过程中，控制器的设计过程为：
[0089] 基于公式（1)，设计经典PID控制器作为外环控制器，具体形式如下： 「 ? Κρλ (.Vf/ -x) + Kdx (ir/ -Λ-) + ki^ (.V,, -χ'γ?τ
[0090] F e'.d 二 A ' M、Kp - β) + Kd - 0、+ ki."、、[pd - β、1τ (10) L 以」 卜" (α(/ - a) + - ?) + 人乂』(a" - a )i/r
[0091] 其中，匕!￡(^'_^'^为期望的足端接触力，&为期望的1方向位置，0 (1为期望 的俯仰角，〇(1为期望的方位角。加入摩擦锥约束条件：？'^〈以？'_、？'^〈以？'_，以为摩 擦系数，控制器变为：
[0092] Feyd = F' eyd 「〇1 P j F，eu, F W 咖應=〇.8"广川、
[0093] 巧w= ， F' >F' ^ F' ^ =0SuF' (H) L extf_niax exd - exd _max9 exd max ?。户i .Λ1 π _ \ F，e,d F，e:d <F，e"l ^F，e,d max L0094J tezd - /7' >F' F' =()8〃F' L ezd _max ezd - ezd _max9 ezii_max · A*J eyd
[0095] 通过调节参数Kpx、Kdx、kix、Kp e、Kde、kie、Kpa、Kda、ki a，可以保证在控制本体位 姿的同时，避免机器人在行走和转弯的过程中出现支撑腿足端滑动的现象。
[0096] 基于公式（9)，设计中环控制器为：
[0097] r = M 〇 +Cq + N-Jr Kp2 (Feyd - Fey) + Kd2 (Feyd - Fev) Kp4 (0 - 6?, + ) + KdA (〇 - ^ + <92) [ KP3 -K,) + Kd, {F", - K )_ (12)
[0098] 经过实验验证，控制器公式（12)可以使支撑腿足端接触力准确跟踪期望力。
[0099] 内环控制回路的作用是实现对关节期望力矩的跟踪。借助关节位移传感器实时检 测作动器长度信息，根据支撑腿运动学模型，计算关节力臂，解算出关节期望力fd，设计PI 控制器如下：
[0100] ? = ΚΡχ---?) + Κ?.Ι^\-Γ)?τ (13)
[0101] 生成关节作动器的控制电流i，实现关节力f准确跟踪期望力fd，进而实现关节驱 动力矩对期望力矩的跟踪。
[0102] 如图3所示，本发明中轮足式机器人前进的动力主要来自支撑腿与地面之间的摩 擦力，在前进过程中两条前腿以固定周期T交替作为支撑腿，在具体实施例中，其具体实施 过程如下：
[0103] (1)轮足式机器人的初始状态为双足双轮支撑状态。
[0104] (2)在0到T/4时间内，左腿由支撑腿变为摆动腿，按照足端点规划轨迹运动至最 高点（本发明主要的对象为支撑腿的柔顺控制方法，摆动腿的规划不在本发明的研究范围 内），右腿作为支撑腿，根据公式（10)、公式（11)计算右腿足端期望接触力 计算频率为400Hz。
[0105] (3)在0到T/4时间内，根据步骤⑵中得到的右腿足端期望接触力 通过公式（12)计算得到右腿各个主动关节的输出力矩计算频率为 4KHz。
[0106] (4)在0到T/4时间内，根据步骤（3)中得到的主动关节输出力矩通过公 式（13)计算得到右腿关节作动器的控制电流U通过驱动电路输出控制电流，控制频率为 ΙΟΚΗζο
[0107] (5)在T/4到T/2时间内，右腿作为支撑腿重复步骤⑵?（4)，左腿按照 规划轨迹从最高点落足，同时以ΙΚΗζ的频率实时检测左腿足端接触力是否满足条件 |匕| = C > HW，如果不满足，返回步骤(5)，如果满足，进入步骤（6)。
[0108] (6)在T4到T2时间内，满足条件||。| = yjK+K,+C之1祕的情况下，右腿作为 支撑腿重复步骤（2)?（4)，左腿由摆动腿变为支撑腿，根据公式（10)、公式（11)计算左腿 足端期望接触力Fexdl，Feydl，Fezdl，计算频率为400Hz。
[0109] (7)在T/4到T/2时间内，满足条件||心|| =」F?F上,>1(W的情况下，右腿作 为支撑腿重复步骤（2)?（4)。根据步骤（6)中得到的左腿足端期望接触力Fexdl，Feydl，F ezdl， 通过公式（12)计算得到左腿各个主动关节的输出力矩τι，计算频率为4KHz。
[0110] (8)在T/4到T/2时间内，满足条件||心I = 〇 〇 107V的情况下，右腿作 为支撑腿重复步骤（2)?（4)。根据步骤（7)中得到的主动关节输出力矩τ i，通过公式（13) 计算得到左腿关节作动器的控制电流h，通过驱动电路输出控制电流，控制频率为ΙΟΚΗζ。
[0111] (9)在T/2到3T/4时间内，右腿由支撑腿变为摆动腿，按照足端点规划轨迹运动至 最高点，左腿作为支撑腿，重复步骤￠)?（8)中左腿的计算过程。
[0112] (10)在3T/4到T时间内，左腿作为支撑腿重复步骤（6)?（8)，右腿按照 规划轨迹从最高点落足，同时以ΙΚΗζ的频率实时检测右腿足端接触力是否满足条件 ΙΙ^,,Ι-?s/OOd > 10iV，如果不满足，返回步骤(10)，如果满足，进入步骤（11)。
[0113] (11)在3T4到T时间内，满足条件|',,|| = 4F^+Kr+Fl 况的情况下，左腿作 为支撑腿重复步骤￠)?（8)，右腿由摆动腿变为支撑腿，重复步骤（2)?（4)。
[0114] (12)返回步骤（1)，进入下一个步行周期。
[0115] 以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例， 凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本【技术领域】的 普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护 范围。
【权利要求】
1. 一种轮足式机器人的柔顺控制方法，其特征在于，包括： 外环控制，为机器人本体位姿控制，控制目标是使机器人本体的位姿准确跟踪期望的 位姿轨迹； 中环控制，为足端接触力控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿 轨迹； 内环控制，为主动关节驱动力矩控制，控制目标是实现支撑腿足端接触力跟踪外环计 算得到的期望力； 通过上述三个控制，在准确控制机器人本体位姿的同时，对支撑腿足端接触力进行控 制。
2. 根据权利要求1所述的轮足式机器人的柔顺控制方法，其特征在于，所述外环控制 中，首先建立基于牛顿-欧拉方程的包含非完整约束条件的机器人本体动力学模型，将通 过惯性传感器得到的机器人本体位姿数据作为反馈量，与期望位姿轨迹比较；然后，基于动 力学模型设计PID控制器，计算控制量，即支撑腿足端期望接触力；最后，对接触面切向力 进行限幅，使之满足摩擦锥约束。
3. 根据权利要求2所述的轮足式机器人的柔顺控制方法，其特征在于，所述基于牛 顿-欧拉方程的机器人本体动力学模型方程如下： Μχ β =A Fer (1) >」_ y = R+L〇sin β (2) Y = 0 (3) ,vsina + icos? =0 (4) 其中，Mi为惯性矩阵，A为力臂矩阵，x，y，z为机器人本体位置，x轴方向为机器人本体 前进方向，y轴方向与重力方向相反，z轴方向由右手法则确定；Fex，Fey，Fez为支撑腿足端接 触力，R为后轮半径，U为机器人本体质心与轮轴之间的距离，α，β，γ分别为机器人本体 的方位角、俯仰角和滚动角；公式（1)表示机器人本体的动力学方程，公式（2)和公式（3) 表示完整约束条件，公式（4)表示非完整约束条件。
4. 根据权利要求3所述的轮足式机器人的柔顺控制方法，其特征在于，所述外环控制 采用PID控制器作为外环控制器，具体形式如下： 「 ? Kpx {x(l-x) + Kdx (i(/ - i：) + AV, ￡ (.v(/ - x)dr ^ ,,./ = A KP/I {β,ι ~ β) + {β,? ~ β) + ^7/; Jn (A ~βΥτ p' L '」'」 卜凡(?-a) + /? (?" - dr) + 人7: ￡ (a" - ? r 其中，F'《d，F' _，F' ezd为期望的足端接触力，xd为期望的x方向位置，β d为期望的俯 仰角，a d为期望的方位角；加入摩擦锥约束条件：F' exd〈 μ F' eyd、F' ezd〈 μ F' _，μ为摩擦系 数，控制器变为： F = F， 丄eyd 丄 eyd P _ f exc^ F ex(i〈 F exd-υο?χ,F w―瓜怒-0.8"/^咖 exd ~ \ p' pr' :> /7. p - 〇 R//F y exd_max exd _ ext/_max > exr^_max ,V^A* X eyd pi _ I ^ ezd F ezd〈 F ezcf -max，F ezd - max - eyif (" I [7 Γ7 > [7 [7 _ Λ Qi?f7 L ezd_ max ezd _ ezd max5 ezd_ max _ eyd 通过调节参数Kpx、Kdx、kix、Kpe、Kd e、kie、Kpa、Kda、kia，保证在控制本体位姿的同 时，避免机器人在行走和转弯的过程中出现支撑腿足端滑动的现象。
5. 根据权利要求1?4中任意一项所述的轮足式机器人的柔顺控制方法，其特征在于， 所述中环控制中，首先基于支撑腿和摆动腿解耦控制的思想建立支撑腿的运动学模型和基 于拉格朗日方程的动力学模型，通过足端力传感器反馈足端接触力信息，计算支撑腿的各 个主动关节的输出力矩；中环控制采用基于ro算法的力/位混合控制器。
6. 根据权利要求5所述的轮足式机器人的柔顺控制方法，其特征在于，所述支撑腿的 运动学模型如下： 在支撑腿髋部建立坐标系，各坐标轴方向与机器人本体坐标系方向相同，在髋部坐标 系下，足端点的坐标为： Xe = LjCOS ( θ θ 2~*~ ^ 3_ ) +L3C0S ( θ 3_ ) -L2C0S ( θ 2_ ? 3~*~ ^ ) ye = -cos θ 4 (Lpin ( θ「θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β )) (5) ze = -sin θ 4 (Lpin ( θ「θ 2+ θ 3- β ) +L3sin ( θ 3- β ) +L2sin ( θ 2- θ 3+ β )) 其中，（Xe，L %)为足端点坐标，Lp L2, L3分别为脚掌、小腿、大腿的长度， θ2, θ3, 04分别为踝关节角、膝关节角、髋关节角、髋部侧向关节角； 令 0 = [% θ2, Θ 3，04]T，q= [xe，ye，zJT，泛= f'，则々 = /6，| = J6， 其中J为支撑腿雅克比矩阵，J为增广雅克比矩阵： cxjde^ Dxjde, cxjc6. cxjde^ ?ν,,/?^ 〇ν:,/δθ2 cyjde, cyJcOA (6) czjde, czJ〇e2 czjde, 6zjd04_ cx JcG^ d.\\ /c0·, cx JcG^ av/c6*4 j= 〇yJ〇^ cyJc〇2 cyjee, czJcO^ dzjc02 czjde, czJoO, 1-10 0 基于拉格朗日方程的支撑腿动力学方程为： Μ (θ)θ + C(θ,θ)θ + Ν(θ,θ) = τ + J' F, (8) 其中，Μ(θ)为惯性矩阵，为哥式力矩阵，久巧为重力项，匕为足端接触力向 量，将| = ?76代入公式（8)，得到： M {q)q + C(q,q)q + N(q,q) - τ + JTFt 其中，Λ；/⑷=M ⑷(J)-、。 v )
7. 根据权利要求6所述的轮足式机器人的柔顺控制方法，其特征在于，所述中环控制 的控制器如下： ^ [[A(4-6'.) +叫之)_ τ = Μ 〇 +0j + N-JTKP2(Feyd-Fey) + Kd1(Peytl-Pe})。 Kp, (0-^+0,) + Κ?Λ [?-θ,+θ2) [_ ΚΡ.^ (f，j ~FJ + Kd, (t.,,· ~t)_
8. 根据权利要求5所述的轮足式机器人的柔顺控制方法，其特征在于，所述内环控制 是利用关节位移传感器和力传感器实时检测作动器长度信息和关节力信息，根据运动学模 型计算关节力臂，解算出关节期望力，与实际的关节力进行比较；再利用PI控制器，生成关 节作动器的控制电流，实现关节力准确跟踪期望力，进而实现关节驱动力矩对期望力矩的 跟踪。
9. 根据权利要求8所述的轮足式机器人的柔顺控制方法，其特征在于，所述内环控制 的PI控制器如下： i = KP, (./；/ -/) + Κ?, ￡ (.A/ - f ￥^ 生成关节作动器的控制电流i，实现关节力f准确跟踪期望力fd，进而实现关节驱动力 矩对期望力矩的跟踪。
【文档编号】B62D57/028GK104108433SQ201410306901
【公开日】2014年10月22日 申请日期:2014年6月30日 优先权日:2014年6月30日
【发明者】王剑, 马宏绪, 郎琳, 韦庆, 王建文, 陈阳祯, 安宏雷, 侯文琦, 朱开盈, 饶锦辉 申请人:中国人民解放军国防科学技术大学