CN104108433B - 一种轮足式机器人的柔顺控制方法 - Google Patents

Abstract

本发明公开了一种轮足式机器人的柔顺控制方法，包括：外环控制，为机器人本体位姿控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；中环控制，为足端接触力控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；内环控制，为主动关节驱动力矩控制，控制目标是实现支撑腿足端接触力跟踪外环计算得到的期望力；通过上述三个控制，在准确控制机器人本体位姿的同时，对支撑腿足端接触力进行控制。本发明具有原理简单、控制精度高、能够提高机器人适应能力等优点。

Description

一种轮足式机器人的柔顺控制方法

技术领域

本发明主要涉及到机器人的控制技术领域，特指一种适用于轮足式机器人的柔顺控制方法。

背景技术

轮式机器人是以轮式机构为支撑和驱动的移动机器人，其主要优点是速度快、能耗低，缺点是越障能力、非结构化地形适应能力差、转弯半径大。足式机器人以步行、奔跑和跳跃为移动方式，其主要优点是重量轻、转弯半径小和地形适应性强，缺点是相对于轮式机器人速度较慢，结构复杂。轮足式机器人综合了轮式机器人和足式机器人的构型优点，既继承了足式机器人对非结构化地形的适应能力又避免了其结构复杂的缺点。

轮足式机器人一般分为三种：1、轮式机构和足式机构在安装方式上相互独立，轮式机构带有驱动装置，即轮子为主动轮；2、轮式机构和足式机构在安装方式上相互独立，轮式机构不带有驱动装置，即轮子为从动轮，机器人依靠腿部的主动关节运动；3、轮式机构安装在腿部或者足端。

现有对于轮足式机器人的行走控制，基本的方法是通过计算腿部各个主动关节的驱动力矩来实现对机器人本体位置和姿态的控制。轮足式机器人在非结构化地形中的稳定行走，要求腿部对环境有柔顺适应性。目前，机器人控制中普遍采用以下方法：

1、被动柔顺；即借助柔性机构或者柔性关节(例如弹簧)使机器人对环境作用力产生自然顺从，但是被动柔顺方法能够提供的机器人对非结构化环境的适应能力有限，而且容易使机器人足端在行走过程中产生滑动。

2、腿部关节期望运动轨迹的在线修正；利用接触开关检测信息判断足端与地面的接触状态，根据接触状态对支撑腿部关节的期望位置轨迹进行在线修正。这种方法能够为机器人提供一定的柔顺性，但是由于落足瞬间地面与足端为刚性接触，所以会产生较大的冲击力，容易影响机器人本体的稳定性。

3、足端接触力控制；通过力传感器反馈足端在摆动期和支撑期转换阶段与地面的接触力信息，设计足端接触力控制算法，实现机器人足端接触地面的柔顺性。足端接触力控制策略可以实现机器人足端与地面的柔顺接触，但是会导致控制目标向量维数扩大，影响机器人本体位姿的可控性。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、控制精度高、能够提高机器人适应能力的轮足式机器人的柔顺控制方法。

为解决上述技术问题，本发明采用以下技术方案：

一种轮足式机器人的柔顺控制方法，包括：

外环控制，为机器人本体位姿控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；

中环控制，为足端接触力控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；

内环控制，为主动关节驱动力矩控制，控制目标是实现支撑腿足端接触力跟踪外环计算得到的期望力；

通过上述三个控制，在准确控制机器人本体位姿的同时，对支撑腿足端接触力进行控制。

作为本发明的进一步改进：所述外环控制中，首先建立基于牛顿-欧拉方程的包含非完整约束条件的机器人本体动力学模型，将通过惯性传感器得到的机器人本体位姿数据作为反馈量，与期望位姿轨迹比较；然后，基于动力学模型设计PID控制器，计算控制量，即支撑腿足端期望接触力；最后，对接触面切向力进行限幅，使之满足摩擦锥约束。

作为本发明的进一步改进：所述基于牛顿-欧拉方程的机器人本体动力学模型方程如下：

$M_1\left[\begin{array}{c}\stackrel{..}{x}\\ \stackrel{..}{\mathrm{\β}}\\ \stackrel{..}{\mathrm{\α}}\end{array}\right]=A\left[\begin{array}{c}{F}_{\mathrm{ex}}\\ F_{\mathrm{ey}}\\ F_{\mathrm{ez}}\end{array}\right]---\left(1\right)$

y＝R+L₀sinβ (2)

γ＝0 (3)

$\stackrel{.}{x}\sin \mathrm{\α}+\stackrel{.}{z}\cos \mathrm{\α}=0---\left(4\right)$

其中，M₁为惯性矩阵，A为力臂矩阵，x,y,z为机器人本体位置，x轴方向为机器人本体前进方向，y轴方向与重力方向相反，z轴方向由右手法则确定；F_ex,F_ey,F_ez为支撑腿足端接触力，R为后轮半径，L₀为机器人本体质心与轮轴之间的距离，α,β,γ分别为机器人本体的方位角、俯仰角和滚动角；公式(1)表示机器人本体的动力学方程，公式(2)和公式(3)表示完整约束条件，公式(4)表示非完整约束条件。

作为本发明的进一步改进：所述外环控制采用PID控制器作为外环控制器，具体形式如下：

$\left[\begin{array}{c}{F^{\′}}_{\mathrm{exd}}\\ {F^{\′}}_{\mathrm{eyd}}\\ {F^{\′}}_{\mathrm{ezd}}\end{array}\right]=A^{-1}{M}_1\left[\begin{array}{c}{\mathrm{Kp}}_x(x_d-x)+{\mathrm{Kd}}_x({\stackrel{.}{x}}_d-\stackrel{.}{x})+{\mathrm{ki}}_x{\∫}_0^t(x_d-x)\mathrm{d\τ}\\ {\mathrm{Kp}}_{\mathrm{\β}}({\mathrm{\β}}_d-\mathrm{\β})+{\mathrm{Kd}}_{\mathrm{\β}}({\stackrel{.}{\mathrm{\β}}}_d-\stackrel{.}{\mathrm{\β}})+{\mathrm{ki}}_{\mathrm{\β}}{\∫}_0^t({\mathrm{\β}}_d-\mathrm{\β})\mathrm{d\τ}\\ {\mathrm{Kp}}_{\mathrm{\α}}({\mathrm{\α}}_d-\mathrm{\α})+{\mathrm{Kd}}_{\mathrm{\α}}({\stackrel{.}{\mathrm{\α}}}_d-\stackrel{.}{\mathrm{\α}})+{\mathrm{ki}}_{\mathrm{\α}}{\∫}_0^t({\mathrm{\α}}_d-\mathrm{\α})\mathrm{d\τ}\end{array}\right]$

其中，F'_exd,F'_eyd,F'_ezd为期望的足端接触力，x_d为期望的x方向位置，β_d为期望的俯仰角，α_d为期望的方位角；加入摩擦锥约束条件：F'_exd<μF'_eyd、F'_ezd<μF'_eyd，μ为摩擦系数，控制器变为：

F_eyd＝F'_eyd

$F_{\mathrm{exd}}=\left\{\begin{array}{cc}{F^{\&prime;}}_{\mathrm{exd}}& {F^{\&prime;}}_{\mathrm{exd}}<{F^{\&prime;}}_{\mathrm{exd}\_\max },{F^{\&prime;}}_{\mathrm{exd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\\ {F^{\&prime;}}_{\mathrm{exd}\_\max }& {F^{\&prime;}}_{\mathrm{exd}}\&GreaterEqual;{F^{\&prime;}}_{\mathrm{exd}\_\max },{F^{\&prime;}}_{\mathrm{exd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\end{array}\right.$

$F_{\mathrm{ezd}}=\left\{\begin{array}{cc}{F^{\&prime;}}_{\mathrm{ezd}}& {F^{\&prime;}}_{\mathrm{ezd}}<{F^{\&prime;}}_{\mathrm{ezd}\_\max },{F^{\&prime;}}_{\mathrm{ezd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\\ {F^{\&prime;}}_{\mathrm{ezd}\_\max }& {F^{\&prime;}}_{\mathrm{ezd}}\&GreaterEqual;{F^{\&prime;}}_{\mathrm{ezd}\_\max },{F^{\&prime;}}_{\mathrm{ezd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\end{array}\right.$

通过调节参数Kp_x、Kd_x、ki_x、Kp_β、Kd_β、ki_β、Kp_α、Kd_α、ki_α，保证在控制本体位姿的同时，避免机器人在行走和转弯的过程中出现支撑腿足端滑动的现象。

作为本发明的进一步改进：所述中环控制中，首先基于支撑腿和摆动腿解耦控制的思想建立支撑腿的运动学模型和基于拉格朗日方程的动力学模型，通过足端力传感器反馈足端接触力信息，计算支撑腿的各个主动关节的输出力矩；中环控制采用基于PD算法的力/位混合控制器。

作为本发明的进一步改进：所述支撑腿的运动学模型如下：

在支撑腿髋部建立坐标系，各坐标轴方向与机器人本体坐标系方向相同，在髋部坐标系下，足端点的坐标为：

x_e＝L₁cos(θ₁-θ₂+θ₃-β)+L₃cos(θ₃-β)-L₂cos(θ₂-θ₃+β)

y_e＝-cosθ₄(L₁sin(θ₁-θ₂+θ₃-β)+L₃sin(θ₃-β)+L₂sin(θ₂-θ₃+β)) (5)

z_e＝-sinθ₄(L₁sin(θ₁-θ₂+θ₃-β)+L₃sin(θ₃-β)+L₂sin(θ₂-θ₃+β))

其中，(x_e,y_e,z_e)为足端点坐标，L₁,L₂,L₃分别为脚掌、小腿、大腿的长度，θ₁,θ₂,θ₃,θ₄分别为踝关节角、膝关节角、髋关节角、髋部侧向关节角；

令θ＝[θ₁,θ₂,θ₃,θ₄]^T，q＝[x_e,y_e,z_e]^T，则其中J为支撑腿雅克比矩阵，为增广雅克比矩阵：

$J=\left[\begin{array}{cccc}{\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;x}_e/\&PartialD;{\mathrm{\&theta;}}_2& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_4\end{array}\right]---\left(6\right)$

$\tilde{J}=\left[\begin{array}{cccc}{\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ 1& -1& 0& 0\end{array}\right]---\left(7\right)$

基于拉格朗日方程的支撑腿动力学方程为：

$M\left(\mathrm{\&theta;}\right)\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+C(\mathrm{\&theta;},\stackrel{\&CenterDot;}{\mathrm{\&theta;}})\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+N(\mathrm{\&theta;},\stackrel{\&CenterDot;}{\mathrm{\&theta;}})=\mathrm{\&tau;}+J^T{F}_e---\left(8\right)$

其中，M(θ)为惯性矩阵，为哥式力矩阵，为重力项，F_e为足端接触力向量，将代入公式(8)，得到：

$\tilde{M}\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+\tilde{C}(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+N(q,\stackrel{\&CenterDot;}{q})=\mathrm{\&tau;}+J^T{F}_e$

其中， $\tilde{M}\left(q\right)=M\left(q\right){\left(\tilde{J}\right)}^{-1},\tilde{C}(q,\stackrel{\&CenterDot;}{q})=C(q,\stackrel{\&CenterDot;}{q}){\left(\tilde{J}\right)}^{-1}+M\left(q\right)(d{\left(\tilde{J}\right)}^{-1}/\mathrm{dt}).$

作为本发明的进一步改进：所述中环控制的控制器如下：

$\mathrm{\&tau;}=\tilde{M}\left[\begin{array}{c}0\\ 0\\ 0\\ K{p}_4(0-{\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2)+K{d}_4(0-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1+{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2)\end{array}\right]\widetilde{+C}\stackrel{\&CenterDot;}{q}+N-J^T\left[\begin{array}{c}K{p}_1(F_{\mathrm{exd}}-F_{\mathrm{ex}})+K{d}_1({\stackrel{\&CenterDot;}{F}}_{\mathrm{exd}}-{\stackrel{\&CenterDot;}{F}}_{\mathrm{ex}})\\ K{p}_2(F_{\mathrm{eyd}}-F_{\mathrm{ey}})+K{d}_2({\stackrel{\&CenterDot;}{F}}_{\mathrm{eyd}}-{\stackrel{\&CenterDot;}{F}}_{\mathrm{ey}})\\ K{p}_3(F_{\mathrm{ezd}}-F_{\mathrm{ez}})+K{d}_3({\stackrel{\&CenterDot;}{F}}_{\mathrm{ezd}}-{\stackrel{\&CenterDot;}{F}}_{\mathrm{ez}})\end{array}\right].$

作为本发明的进一步改进：所述内环控制是利用关节位移传感器和力传感器实时检测作动器长度信息和关节力信息，根据运动学模型计算关节力臂，解算出关节期望力，与实际的关节力进行比较；再利用PI控制器，生成关节作动器的控制电流，实现关节力准确跟踪期望力，进而实现关节驱动力矩对期望力矩的跟踪。

作为本发明的进一步改进：所述内环控制的PI控制器如下：

$i=K{p}_i(f_d-f)+K{i}_i{\&Integral;}_0^t(f_d-f)\mathrm{d\&tau;}$

生成关节作动器的控制电流i，实现关节力f准确跟踪期望力f_d，进而实现关节驱动力矩对期望力矩的跟踪。

与现有技术相比，本发明的优点在于：

1、在轮足式机器人运动过程中，采用本发明设计的三环控制策略可以实现本体的位姿准确跟踪由步态规划生成的期望位姿轨迹。

2、本发明提出的控制策略能够有效避免机器人在行走、转弯过程中支撑腿足端滑动的现象。

3、本发明能够提高轮足式机器人对非结构化环境的适应能力，实现机器人在不平整地面上的行走、转弯。

4、本发明具有较强的通用性。控制策略不依赖具体的轮足式机器人系统，只要构建合适的传感器体系，推导机器人运动学、动力学模型，适当调整控制器参数，即可实现本发明所提出的方法。

5、本发明结构清晰、层次分明，具有较好的理论价值和工程意义

附图说明

图1是轮足式机器人系统平面示意图。

图2是本发明的控制结构框图。

图3是单个步行周期内控制流程图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

本发明主要应用于轮足式机器人系统，主要针对的对象为轮式机构和足式机构在安装方式上相互独立，轮式机构不带有驱动装置的轮足式机器人，即轮子为从动轮，机器人依靠腿部的主动关节运动。如图1所示，该轮足式机器人系统由两条腿和两个从动轮轮子组成，每条腿包含四个主动关节(踝关节、膝关节、髋关节、髋部侧向关节)，各主动关节均采用液压作动器驱动，足端与地面为点接触。每个主动关节装有位移传感器和力传感器，用于检测作动器长度信息和关节力信息，在足端点通过三维力传感器反馈足端与地面的接触力信息，机器人本体在惯性空间中的位置、姿态可通过IMU实时检测。作为本发明的控制对象，上述轮足式机器人具有如下机械结构特性：

a、机器人重心靠近后轮，即在单腿支撑状态下机器人满足ZMP稳定性条件，可以稳定站立。

b、基于特性a，轮足式机器人在行走过程中，后轮不会离开地面。

c、机器人本体的质量远大于腿部的质量。

如图2所示，发明轮足式机器人的柔顺控制方法，基于支撑腿和摆动腿解耦控制的思想，采用三环控制策略，即：外环为机器人本体位姿控制，中环为足端接触力控制，内环为主动关节驱动力矩控制。在准确控制机器人本体位姿的同时，将支撑腿足端接触力控制在一个较小的范围内，避免支撑腿足端滑动，提高机器人对非结构化环境的适应能力。图中， $r=\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right],$ $r^{^}=\left[\begin{array}{ccc}0& -r_3& r_2\\ r_3& 0& -r_1\\ -r_2& r_1& 0\end{array}\right];$ F_d为足端与地面期望接触力；F为足端与地面实际接触力；f_d为关节期望力；f为关节实际力；τ_d为期望输出力矩。

外环控制，其控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹。由于轮子受到非完整约束(轮子在轴向满足速度约束，但是无位置约束)，所以首先建立基于牛顿-欧拉方程的包含非完整约束条件的机器人本体动力学模型，将通过惯性传感器得到的机器人本体位姿数据(惯性坐标系下的三维位置信息、方位角、俯仰角、滚动角)作为反馈量，与期望位姿轨迹比较；然后，基于动力学模型设计PID控制器，计算控制量—-支撑腿足端期望接触力。最后，对接触面切向力进行限幅，使之满足摩擦锥约束，这就可以在实现轮足式机器人本体位姿控制的同时，有效避免行走过程中支撑腿足端滑动。

中环控制，其控制目标是实现支撑腿足端接触力跟踪外环计算得到的期望力，从而提高机器人对非结构化环境的适应能力。首先，基于支撑腿和摆动腿解耦控制的思想，建立支撑腿的运动学模型和基于拉格朗日方程的动力学模型。通过足端力传感器反馈足端接触力信息，设计控制算法，计算支撑腿的各个主动关节的输出力矩。由于单腿有四个主动关节(三个前向关节，一个侧向关节，见附图1)，而足端接触力信息的维数为三维，所以加入一个运动学约束条件—踝关节角与膝关节角相等，保证支撑腿雅克比矩阵为方阵。中环控制器为基于PD算法的力/位混合控制器。经过实验验证，支撑腿足端接触力可以准确跟踪期望力。

内环控制，其作用是实现对关节期望力矩的跟踪。借助关节位移传感器和力传感器实时检测作动器长度信息和关节力信息，根据运动学模型计算关节力臂，解算出关节期望力，与实际的关节力进行比较；进一步设计PI控制器，生成关节作动器的控制电流，实现关节力准确跟踪期望力，进而实现关节驱动力矩对期望力矩的跟踪。

在上述控制过程中，建模的过程为：

首先建立机器人本体坐标系，x轴方向为机器人本体前进方向，y轴方向与重力方向相反，z轴方向由右手法则确定。由于行走过程中两个后轮不会离开地面，机器人本体受到三个运动学约束，其中两个为完整约束，一个为非完整约束。基于牛顿-欧拉方程的单腿支撑期机器人本体动力学方程(本发明以左前腿支撑为例)如下：

$M_1\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{x}\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&beta;}}\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&alpha;}}\end{array}\right]A\left[\begin{array}{c}{F}_{\mathrm{ex}}\\ F_{\mathrm{ey}}\\ F_{\mathrm{ez}}\end{array}\right]---\left(1\right)$

y＝R+L₀sinβ (2)

γ＝0 (3)

$\stackrel{\&CenterDot;}{x}\sin \mathrm{\&alpha;}+\stackrel{\&CenterDot;}{z}\cos \mathrm{\&alpha;}=0---\left(4\right)$

其中，M₁为惯性矩阵，A为力臂矩阵，x,y,z为机器人本体位置，F_ex,F_ey,F_ez为支撑腿足端接触力，R为后轮半径，L₀为机器人本体质心与轮轴之间的距离，α,β,γ分别为机器人本体的方位角、俯仰角和滚动角。公式(1)表示机器人本体的动力学方程，公式(2)和公式(3)表示完整约束条件，公式(4)表示非完整约束条件。

在支撑腿髋部建立坐标系(以左腿为例)，各坐标轴方向与机器人本体坐标系方向相同，在髋部坐标系下，足端点的坐标为：

x_e＝L₁cos(θ₁-θ₂+θ₃-β)+L₃cos(θ₃-β)-L₂cos(θ₂-θ₃+β)

y_e＝-cosθ₄(L₁sin(θ₁-θ₂+θ₃-β)+L₃sin(θ₃-β)+L₂sin(θ₂-θ₃+β)) (5)

z_e＝-sinθ₄(L₁sin(θ₁-θ₂+θ₃-β)+L₃sin(θ₃-β)+L₂sin(θ₂-θ₃+β))

其中，(x_e,y_e,z_e)为足端点坐标，L₁,L₂,L₃分别为脚掌、小腿、大腿的长度，θ₁,θ₂,θ₃,θ₄分别为踝关节角、膝关节角、髋关节角、髋部侧向关节角。

令θ＝[θ₁,θ₂,θ₃,θ₄]^T，q＝[x_e,y_e,z_e]^T，则其中J为支撑腿雅克比矩阵，为增广雅克比矩阵：

$J=\left[\begin{array}{cccc}{\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;x}_e/\&PartialD;{\mathrm{\&theta;}}_2& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_4\end{array}\right]---\left(6\right)$

$\tilde{J}=\left[\begin{array}{cccc}{\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;x}_e/\&PartialD;{\mathrm{\&theta;}}_3& {\&PartialD;x}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;y}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_1& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_2& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_3& {\&PartialD;z}_e/{\&PartialD;\mathrm{\&theta;}}_4\\ 1& -1& 0& 0\end{array}\right]---\left(7\right)$

基于拉格朗日方程的支撑腿动力学方程为：

$M\left(\mathrm{\&theta;}\right)\stackrel{..}{\mathrm{\&theta;}}+C(\mathrm{\&theta;},\stackrel{.}{\mathrm{\&theta;}})\stackrel{.}{\mathrm{\&theta;}}+N(\mathrm{\&theta;},\stackrel{.}{\mathrm{\&theta;}})=\mathrm{\&tau;}+J^T{F}_e---\left(8\right)$

其中，M(θ)为惯性矩阵，为哥式力矩阵，为重力项，F_e为足端接触力向量，将代入公式(8)，得到：

$\tilde{M}\left(q\right)\stackrel{..}{q}+\tilde{C}(q,\stackrel{.}{q})\stackrel{.}{q}+N(q,\stackrel{.}{q})=\mathrm{\&tau;}+J^T{F}_e---\left(9\right)$

其中， $\tilde{M}\left(q\right)=M\left(q\right){\left(\tilde{J}\right)}^{-1},\tilde{C}(q,\stackrel{.}{q})=C(q,\stackrel{.}{q}){\left(\tilde{J}\right)}^{-1}+M\left(q\right)(d{\left(\tilde{J}\right)}^{-1}/\mathrm{dt}).$

在上述控制过程中，控制器的设计过程为：

基于公式(1)，设计经典PID控制器作为外环控制器，具体形式如下：

$\left[\begin{array}{c}{F^{\&prime;}}_{\mathrm{exd}}\\ {F^{\&prime;}}_{\mathrm{eyd}}\\ {F^{\&prime;}}_{\mathrm{ezd}}\end{array}\right]=A^{-1}{M}_1\left[\begin{array}{c}{\mathrm{Kp}}_x(x_d-x)+{\mathrm{Kd}}_x({\stackrel{.}{x}}_d-\stackrel{.}{x})+{\mathrm{ki}}_x{\&Integral;}_0^t(x_d-x)\mathrm{d\&tau;}\\ {\mathrm{Kp}}_{\mathrm{\&beta;}}({\mathrm{\&beta;}}_d-\mathrm{\&beta;})+{\mathrm{Kd}}_{\mathrm{\&beta;}}({\stackrel{.}{\mathrm{\&beta;}}}_d-\stackrel{.}{\mathrm{\&beta;}})+{\mathrm{ki}}_{\mathrm{\&beta;}}{\&Integral;}_0^t({\mathrm{\&beta;}}_d-\mathrm{\&beta;})\mathrm{d\&tau;}\\ {\mathrm{Kp}}_{\mathrm{\&alpha;}}({\mathrm{\&alpha;}}_d-\mathrm{\&alpha;})+{\mathrm{Kd}}_{\mathrm{\&alpha;}}({\stackrel{.}{\mathrm{\&alpha;}}}_d-\stackrel{.}{\mathrm{\&alpha;}})+{\mathrm{ki}}_{\mathrm{\&alpha;}}{\&Integral;}_0^t({\mathrm{\&alpha;}}_d-\mathrm{\&alpha;})\mathrm{d\&tau;}\end{array}\right]---\left(10\right)$

其中，F'_exd,F'_eyd,F'_ezd为期望的足端接触力，x_d为期望的x方向位置，β_d为期望的俯仰角，α_d为期望的方位角。加入摩擦锥约束条件：F'_exd<μF'_eyd、F'_ezd<μF'_eyd，μ为摩擦系数，控制器变为：

F_eyd＝F'_eyd

$F_{\mathrm{exd}}=\left\{\begin{array}{cc}{F^{\&prime;}}_{\mathrm{exd}}& {F^{\&prime;}}_{\mathrm{exd}}<{F^{\&prime;}}_{\mathrm{exd}\_\max },{F^{\&prime;}}_{\mathrm{exd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\\ {F^{\&prime;}}_{\mathrm{exd}\_\max }& {F^{\&prime;}}_{\mathrm{exd}}\&GreaterEqual;{F^{\&prime;}}_{\mathrm{exd}\_\max },{F^{\&prime;}}_{\mathrm{exd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\end{array}\right.---\left(11\right)$

$F_{\mathrm{ezd}}=\left\{\begin{array}{cc}{F^{\&prime;}}_{\mathrm{ezd}}& {F^{\&prime;}}_{\mathrm{ezd}}<{F^{\&prime;}}_{\mathrm{ezd}\_\max },{F^{\&prime;}}_{\mathrm{ezd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\\ {F^{\&prime;}}_{\mathrm{ezd}\_\max }& {F^{\&prime;}}_{\mathrm{ezd}}\&GreaterEqual;{F^{\&prime;}}_{\mathrm{ezd}\_\max },{F^{\&prime;}}_{\mathrm{ezd}\_\max }=0.8\mathrm{\&mu;}{F^{\&prime;}}_{\mathrm{eyd}}\end{array}\right.$

通过调节参数Kp_x、Kd_x、ki_x、Kp_β、Kd_β、ki_β、Kp_α、Kd_α、ki_α，可以保证在控制本体位姿的同时，避免机器人在行走和转弯的过程中出现支撑腿足端滑动的现象。

基于公式(9)，设计中环控制器为：

$\mathrm{\&tau;}=\tilde{M}\left[\begin{array}{c}0\\ 0\\ 0\\ {\mathrm{Kp}}_4(0-{\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2)+{\mathrm{Kd}}_4(0-{\stackrel{.}{\mathrm{\&theta;}}}_1+{\stackrel{.}{\mathrm{\&theta;}}}_2)\end{array}\right]+\tilde{C}\stackrel{.}{q}+N-J^T\left[\begin{array}{c}{\mathrm{Kp}}_1(F_{\mathrm{exd}}-F_{\mathrm{ex}})+{\mathrm{Kd}}_1({\stackrel{.}{F}}_{\mathrm{exd}}-{\stackrel{.}{F}}_{\mathrm{ex}})\\ {\mathrm{Kp}}_2(F_{\mathrm{eyd}}-F_{\mathrm{ey}})+{\mathrm{Kd}}_2({\stackrel{.}{F}}_{\mathrm{eyd}}-{\stackrel{.}{F}}_{\mathrm{ey}})\\ {\mathrm{Kp}}_3(F_{\mathrm{ezd}}-F_{\mathrm{ez}})+{\mathrm{Kd}}_3({\stackrel{.}{F}}_{\mathrm{ezd}}-{\stackrel{.}{F}}_{\mathrm{ez}})\end{array}\right]---\left(12\right)$

经过实验验证，控制器公式(12)可以使支撑腿足端接触力准确跟踪期望力。

内环控制回路的作用是实现对关节期望力矩的跟踪。借助关节位移传感器实时检测作动器长度信息，根据支撑腿运动学模型，计算关节力臂，解算出关节期望力f_d，设计PI控制器如下：

$i={\mathrm{Kp}}_i(f_d-f)+{\mathrm{Ki}}_i{\&Integral;}_0^t(f_d-f)\mathrm{d\&tau;}---\left(13\right)$

生成关节作动器的控制电流i，实现关节力f准确跟踪期望力f_d，进而实现关节驱动力矩对期望力矩的跟踪。

如图3所示，本发明中轮足式机器人前进的动力主要来自支撑腿与地面之间的摩擦力，在前进过程中两条前腿以固定周期T交替作为支撑腿，在具体实施例中，其具体实施过程如下：

(1)轮足式机器人的初始状态为双足双轮支撑状态。

(2)在0到T/4时间内，左腿由支撑腿变为摆动腿，按照足端点规划轨迹运动至最高点(本发明主要的对象为支撑腿的柔顺控制方法，摆动腿的规划不在本发明的研究范围内)，右腿作为支撑腿，根据公式(10)、公式(11)计算右腿足端期望接触力F_exdr,F_eydr,F_ezdr，计算频率为400Hz。

(3)在0到T/4时间内，根据步骤(2)中得到的右腿足端期望接触力F_exdr,F_eydr,F_ezdr，通过公式(12)计算得到右腿各个主动关节的输出力矩τ_r，计算频率为4KHz。

(4)在0到T/4时间内，根据步骤(3)中得到的主动关节输出力矩τ_r，通过公式(13)计算得到右腿关节作动器的控制电流i_r，通过驱动电路输出控制电流，控制频率为10KHz。

(5)在T/4到T/2时间内，右腿作为支撑腿重复步骤(2)～(4)，左腿按照规划轨迹从最高点落足，同时以1KHz的频率实时检测左腿足端接触力是否满足条件如果不满足，返回步骤(5)，如果满足，进入步骤(6)。

(6)在T4到T2时间内，满足条件的情况下，右腿作为支撑腿重复步骤(2)～(4)，左腿由摆动腿变为支撑腿，根据公式(10)、公式(11)计算左腿足端期望接触力F_exdl,F_eydl,F_ezdl，计算频率为400Hz。

(7)在T/4到T/2时间内，满足条件的情况下，右腿作为支撑腿重复步骤(2)～(4)。根据步骤(6)中得到的左腿足端期望接触力F_exdl,F_eydl,F_ezdl，通过公式(12)计算得到左腿各个主动关节的输出力矩τ_l，计算频率为4KHz。

(8)在T/4到T/2时间内，满足条件的情况下，右腿作为支撑腿重复步骤(2)～(4)。根据步骤(7)中得到的主动关节输出力矩τ_l，通过公式(13)计算得到左腿关节作动器的控制电流i_l，通过驱动电路输出控制电流，控制频率为10KHz。

(9)在T/2到3T/4时间内，右腿由支撑腿变为摆动腿，按照足端点规划轨迹运动至最高点，左腿作为支撑腿，重复步骤(6)～(8)中左腿的计算过程。

(10)在3T/4到T时间内，左腿作为支撑腿重复步骤(6)～(8)，右腿按照规划轨迹从最高点落足，同时以1KHz的频率实时检测右腿足端接触力是否满足条件如果不满足，返回步骤(10)，如果满足，进入步骤(11)。

(11)在3T4到T时间内，满足条件的情况下，左腿作为支撑腿重复步骤(6)～(8)，右腿由摆动腿变为支撑腿，重复步骤(2)～(4)。

(12)返回步骤(1)，进入下一个步行周期。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (8)

1.一种轮足式机器人的柔顺控制方法，其特征在于，包括：

外环控制，为机器人本体位姿控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；

中环控制，为足端接触力控制，控制目标是使机器人本体的位姿准确跟踪期望的位姿轨迹；

内环控制，为主动关节驱动力矩控制，控制目标是实现支撑腿足端接触力跟踪外环计算得到的期望力；

通过上述三个控制，在准确控制机器人本体位姿的同时，对支撑腿足端接触力进行控制；

所述外环控制中，首先建立基于牛顿-欧拉方程的包含非完整约束条件的机器人本体动力学模型，将通过惯性传感器得到的机器人本体位姿数据作为反馈量，与期望位姿轨迹比较；然后，基于动力学模型设计PID控制器，计算控制量，即支撑腿足端期望接触力；最后，对接触面切向力进行限幅，使之满足摩擦锥约束。

2.根据权利要求1所述的轮足式机器人的柔顺控制方法，其特征在于，所述基于牛顿-欧拉方程的机器人本体动力学模型方程如下：

$M_1\left[\begin{array}{c}\stackrel{\&CenterDot;\&CenterDot;}{x}\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&beta;}}\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&alpha;}}\end{array}\right]=A\left[\begin{array}{c}{F}_{ex}\\ F_{ey}\\ F_{ez}\end{array}\right]---\left(1\right)$

y＝R+L₀sinβ (2)

γ＝0 (3)

$\stackrel{\&CenterDot;}{x}sin\mathrm{\&alpha;}+\stackrel{\&CenterDot;}{z}cos\mathrm{\&alpha;}=0---\left(4\right)$

其中，M₁为惯性矩阵，A为力臂矩阵，x,y,z为机器人本体位置，x轴方向为机器人本体前进方向，y轴方向与重力方向相反，z轴方向由右手法则确定；F_ex,F_ey,F_ez为支撑腿足端接触力，R为后轮半径，L₀为机器人本体质心与轮轴之间的距离，α,β,γ分别为机器人本体的方位角、俯仰角和滚动角；公式(1)表示机器人本体的动力学方程，公式(2)和公式(3)表示完整约束条件，公式(4)表示非完整约束条件。

3.根据权利要求2所述的轮足式机器人的柔顺控制方法，其特征在于，所述外环控制采用PID控制器作为外环控制器，具体形式如下：

$\left[\begin{array}{c}{F^{\&prime;}}_{exd}\\ {F^{\&prime;}}_{eyd}\\ {F^{\&prime;}}_{ezd}\end{array}\right]=A^{-1}{M}_1\left[\begin{array}{c}K{p}_x(x_d-x)+K{d}_x({\stackrel{\&CenterDot;}{x}}_d-\stackrel{\&CenterDot;}{x})+k{i}_x{\&Integral;}_0^t(x_d-x)d\mathrm{\&tau;}\\ K{p}_{\mathrm{\&beta;}}({\mathrm{\&beta;}}_d-\mathrm{\&beta;})+K{d}_{\mathrm{\&beta;}}({\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_d-\stackrel{\&CenterDot;}{\mathrm{\&beta;}})+k{i}_{\mathrm{\&beta;}}{\&Integral;}_0^t({\mathrm{\&beta;}}_d-\mathrm{\&beta;})d\mathrm{\&tau;}\\ K{p}_{\mathrm{\&alpha;}}({\mathrm{\&alpha;}}_d-\mathrm{\&alpha;})+K{d}_{\mathrm{\&alpha;}}({\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_d-\stackrel{\&CenterDot;}{\mathrm{\&alpha;}})+k{i}_{\mathrm{\&alpha;}}{\&Integral;}_0^t({\mathrm{\&alpha;}}_d-\mathrm{\&alpha;})d\mathrm{\&tau;}\end{array}\right]$

其中，F'_exd,F'_eyd,F'_ezd为期望的足端接触力，x_d为期望的x方向位置，β_d为期望的俯仰角，α_d为期望的方位角；加入摩擦锥约束条件：F'_exd<μF'_eyd、F'_ezd<μF'_eyd，μ为摩擦系数，控制器变为：

F_eyd＝F'_eyd

$F_{exd}=\left\{\begin{array}{cc}{F^{\&prime;}}_{exd}& {F^{\&prime;}}_{exd}<{F^{\&prime;}}_{exd\_max},{F^{\&prime;}}_{exd\_max}=0.8{{\mathrm{\&mu;F}}^{\&prime;}}_{eyd}\\ {F^{\&prime;}}_{exd\_max}& {F^{\&prime;}}_{exd}\&GreaterEqual;{F^{\&prime;}}_{exd\_max},{F^{\&prime;}}_{exd\_max}=0.8{{\mathrm{\&mu;F}}^{\&prime;}}_{eyd}\end{array}\right.$

$F_{ezd}=\left\{\begin{array}{cc}{F^{\&prime;}}_{ezd}& {F^{\&prime;}}_{ezd}<{F^{\&prime;}}_{ezd\_max},{F^{\&prime;}}_{ezd\_max}=0.8{{\mathrm{\&mu;F}}^{\&prime;}}_{eyd}\\ {F^{\&prime;}}_{ezd\_max}& {F^{\&prime;}}_{ezd}\&GreaterEqual;{F^{\&prime;}}_{ezd\_max},{F^{\&prime;}}_{ezd\_max}=0.8{{\mathrm{\&mu;F}}^{\&prime;}}_{eyd}\end{array}\right.$

通过调节参数Kp_x、Kd_x、ki_x、Kp_β、Kd_β、ki_β、Kp_α、Kd_α、ki_α，保证在控制本体位姿的同时，避免机器人在行走和转弯的过程中出现支撑腿足端滑动的现象。

4.根据权利要求1～3中任意一项所述的轮足式机器人的柔顺控制方法，其特征在于，所述中环控制中，首先基于支撑腿和摆动腿解耦控制的思想建立支撑腿的运动学模型和基于拉格朗日方程的动力学模型，通过足端力传感器反馈足端接触力信息，计算支撑腿的各个主动关节的输出力矩；中环控制采用基于PD算法的力/位混合控制器。

5.根据权利要求4所述的轮足式机器人的柔顺控制方法，其特征在于，所述支撑腿的运动学模型如下：

在支撑腿髋部建立坐标系，各坐标轴方向与机器人本体坐标系方向相同，在髋部坐标系下，足端点的坐标为：

$\begin{array}{c}{x}_e=L_{1}cos({\mathrm{\&theta;}}_1-{\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3-\mathrm{\&beta;})+L_3\cos ({\mathrm{\&theta;}}_3-\mathrm{\&beta;})-L_2\cos ({\mathrm{\&theta;}}_2-{\mathrm{\&theta;}}_3+\mathrm{\&beta;})\\ y_e=-{\mathrm{cos\&theta;}}_4\left(L_1\sin \right({\mathrm{\&theta;}}_1-{\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3-\mathrm{\&beta;})+L_3\sin ({\mathrm{\&theta;}}_3-\mathrm{\&beta;})+L_2\sin ({\mathrm{\&theta;}}_2-{\mathrm{\&theta;}}_3+\mathrm{\&beta;}))\\ z_e=-{\mathrm{sin\&theta;}}_4\left(L_1\sin \right({\mathrm{\&theta;}}_1-{\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3-\mathrm{\&beta;})+L_3\sin ({\mathrm{\&theta;}}_3-\mathrm{\&beta;})+L_2\sin ({\mathrm{\&theta;}}_2-{\mathrm{\&theta;}}_3+\mathrm{\&beta;}))\end{array}---\left(5\right)$

其中，(x_e,y_e,z_e)为足端点坐标，L₁,L₂,L₃分别为脚掌、小腿、大腿的长度，θ₁,θ₂,θ₃,θ₄分别为踝关节角、膝关节角、髋关节角、髋部侧向关节角；

令θ＝[θ₁,θ₂,θ₃,θ₄]^T，q＝[x_e,y_e,z_e]^T，则其中J为支撑腿雅克比矩阵，为增广雅克比矩阵：

$J=\left[\begin{array}{cccc}\&part;x_e/\&part;{\mathrm{\&theta;}}_1& \&part;x_e/\&part;{\mathrm{\&theta;}}_2& \&part;x_e/\&part;{\mathrm{\&theta;}}_3& \&part;x_e/\&part;{\mathrm{\&theta;}}_4\\ \&part;y_e/\&part;{\mathrm{\&theta;}}_1& \&part;y_e/\&part;{\mathrm{\&theta;}}_2& \&part;y_e/\&part;{\mathrm{\&theta;}}_3& \&part;y_e/\&part;{\mathrm{\&theta;}}_4\\ \&part;z_e/\&part;{\mathrm{\&theta;}}_1& \&part;z_e/\&part;{\mathrm{\&theta;}}_2& \&part;z_e/\&part;{\mathrm{\&theta;}}_3& \&part;z_e/\&part;{\mathrm{\&theta;}}_4\end{array}\right]---\left(6\right)$

$\tilde{J}=\left[\begin{array}{cccc}\&part;x_e/\&part;{\mathrm{\&theta;}}_1& \&part;x_e/\&part;{\mathrm{\&theta;}}_2& \&part;x_e/\&part;{\mathrm{\&theta;}}_3& \&part;x_e/\&part;{\mathrm{\&theta;}}_4\\ \&part;y_e/\&part;{\mathrm{\&theta;}}_1& \&part;y_e/\&part;{\mathrm{\&theta;}}_2& \&part;y_e/\&part;{\mathrm{\&theta;}}_3& \&part;y_e/\&part;{\mathrm{\&theta;}}_4\\ \&part;z_e/\&part;{\mathrm{\&theta;}}_1& \&part;z_e/\&part;{\mathrm{\&theta;}}_2& \&part;z_e/\&part;{\mathrm{\&theta;}}_3& \&part;z_e/\&part;{\mathrm{\&theta;}}_4\\ 1& -1& 0& 0\end{array}\right]---\left(7\right)$

基于拉格朗日方程的支撑腿动力学方程为：

$M\left(\mathrm{\&theta;}\right)\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}+C(\mathrm{\&theta;},\stackrel{\&CenterDot;}{\mathrm{\&theta;}})\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+N(\mathrm{\&theta;},\stackrel{\&CenterDot;}{\mathrm{\&theta;}})=\mathrm{\&tau;}+J^T{F}_e---\left(8\right)$

其中，M(θ)为惯性矩阵，为哥式力矩阵，为重力项，F_e为足端接触力向量，将代入公式(8)，得到：

$\tilde{M}\left(q\right)\stackrel{\&CenterDot;\&CenterDot;}{q}+\tilde{C}(q,\stackrel{\&CenterDot;}{q})\stackrel{\&CenterDot;}{q}+N(q,\stackrel{\&CenterDot;}{q})=\mathrm{\&tau;}+J^T{F}_e$

其中，

6.根据权利要求5所述的轮足式机器人的柔顺控制方法，其特征在于，所述中环控制的控制器如下：

$\mathrm{\&tau;}=\tilde{M}\left[\begin{array}{c}0\\ 0\\ 0\\ {\mathrm{Kp}}_4(0-{\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2)+{\mathrm{Kd}}_4(0-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_1+{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_2)\end{array}\right]+\tilde{C}\stackrel{\&CenterDot;}{q}+N-J^T\left[\begin{array}{c}{\mathrm{Kp}}_1(F_{exd}-F_{ex})+{\mathrm{Kd}}_1({\stackrel{\&CenterDot;}{F}}_{exd}-{\stackrel{\&CenterDot;}{F}}_{ex})\\ {\mathrm{Kp}}_2(F_{eyd}-F_{ey})+{\mathrm{Kd}}_2({\stackrel{\&CenterDot;}{F}}_{eyd}-{\stackrel{\&CenterDot;}{F}}_{ey})\\ {\mathrm{Kp}}_3(F_{ezd}-F_{ez})+{\mathrm{Kd}}_3({\stackrel{\&CenterDot;}{F}}_{ezd}-{\stackrel{\&CenterDot;}{F}}_{ez})\end{array}\right].$

7.根据权利要求4所述的轮足式机器人的柔顺控制方法，其特征在于，所述内环控制是利用关节位移传感器和力传感器实时检测作动器长度信息和关节力信息，根据运动学模型计算关节力臂，解算出关节期望力，与实际的关节力进行比较；再利用PI控制器，生成关节作动器的控制电流，实现关节力准确跟踪期望力，进而实现关节驱动力矩对期望力矩的跟踪。

8.根据权利要求7所述的轮足式机器人的柔顺控制方法，其特征在于，所述内环控制的PI控制器如下：

$i={\mathrm{Kp}}_i(f_d-f)+{\mathrm{Ki}}_i{\&Integral;}_0^t(f_d-f)d\mathrm{\&tau;}$

生成关节作动器的控制电流i，实现关节力f准确跟踪期望力f_d，进而实现关节驱动力矩对期望力矩的跟踪。