CN112847354B - 变电站足式机器人姿态调整方法、控制器、***及机器人 - Google Patents

Abstract

本发明属于机器人领域，提供了变电站足式机器人姿态调整方法、控制器、***及机器人。其中该方法包括在足式机器人在前进时，采用同速后撤法以使摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，保证摆动腿竖直抬起；在足式机器人站立平稳后，基于站立平稳后的姿态信息和几何信息，计算足端在肩关节坐标系中的初始坐标，再结合躯干姿态调整量和左右脚分开的距离，得到足端在肩关节坐标系中新坐标并进行站立位姿调整；在足式机器人前进及站立位姿调整的过程中，基于当前运动工况，在满足位姿约束下，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制，以使足式运动平台保持受力及转矩的平衡状态。

Description

变电站足式机器人姿态调整方法、控制器、***及机器人

技术领域

本发明属于机器人领域，尤其涉及一种变电站足式机器人姿态调整方法、控制器、***及机器人。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

足式巡检运动平台比传统轮式机器人具有更强的环境适应性和运动灵活性，既可以在站内草地、砂石路面等复杂路面环境运行，也可以跨越障碍、上下楼梯、翻越路边石等站内典型障碍，对站内复杂路面环境的适应能力得到了极大提高。足式巡检运动平台基于“腿-足”式的运动结构，另一方面对以及平台整体姿态的稳定控制，能够保障足式机器人稳定运行。目前足式机器人常见的步态规划方法有基于中央模式发生器的规划方法、基于弹簧负载倒立摆模型的规划方法、基于预设足端运动轨迹的规划方法，以及基于这些方法的混合规划方法等。

发明人发现，现有的足式机器人在姿态调整的过程中存在以下问题：1)现有的足式机器人步态规划方法在足式机器人抬脚时可能会发生踢到障碍物影响平衡甚至被障碍物绊倒的情况。2)由于在变电站内实际应用时，足式运功平台上通常需要搭载巡检作业设备，这样会影响平台重心位置，从而影响运动平台的姿态稳定控制。3)足式机器人在行走时会产生质心高度波动和躯干姿态抖动，导致传感器定位精度不高，足式机器人的巡检步骤是逐个遍历预置的巡检点位，若到达目标点后立即减速并停止，往往存在较大的误差，即使踏步一段时间进行位置微调，也会由于躯干抖动导致停止位置不精确。

发明内容

为了解决上述背景技术中存在的至少一项技术问题，本发明提供一种变电站足式机器人姿态调整方法、控制器及***，其能够保障变电站足式机器人在前进以及站立平稳后快速调整姿态，同时保障足式运动平台始终保持受力及转矩的平衡状态。

为了实现上述目的，本发明采用如下技术方案：

本发明的第一个方面提供一种变电站足式机器人姿态调整方法。

一种变电站足式机器人姿态调整方法，包括：

在足式机器人在前进时，采用同速后撤法以使摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，以保证摆动腿竖直抬起；

在足式机器人站立平稳后，基于站立平稳后的姿态信息和几何信息，计算足端在肩关节坐标系中的初始坐标，再结合躯干姿态调整量和左右脚分开的距离，得到足端在肩关节坐标系中新坐标并进行站立位姿调整；

在足式机器人前进及站立位姿调整的过程中，基于当前运动工况，在满足位姿约束下，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制，以使足式运动平台保持受力及转矩的平衡状态。

作为一种可选实施方式，在足式机器人在前进时，还采用超速后撤法，使摆动腿的后撤速度超过支撑腿的蹬地速度，从而避免抬脚时被地面障碍立面的凸出部分压住而破坏平衡。

作为一种可选实施方式，超速后撤法用速度描述：摆动腿的后撤速度＝k*支撑腿的蹬地速度；其中，k大于1。

作为一种可选实施方式，超速后撤法用位置描述：摆动腿的后撤位置＝支撑腿的位置-抬脚结束后多后撤的距离与摆动相的预计时长的比值*进入摆动相后的计时时间。

作为一种可选实施方式，摆动腿抬脚时的运动轨迹分为X轴轨迹和Z轴轨迹，X轴轨迹向前为正，Z轴轨迹为摆动腿在Z轴的位置＝迈步与高度与摆动相的预计时长的比值*进入摆动相后的计时时间。

作为一种可选实施方式，从地面坐标系来看，摆动腿足端无水平初速度。

作为一种可选实施方式，对肩关节坐标系进行旋转得到第二坐标系，进一步得到第二坐标系中髋关节相对于原点的位置，结合腿左右张开调整量、躯干调整量和初始坐标，得到新坐标。

作为一种可选实施方式，躯干调整量包括躯干扭转角调整量、躯干俯仰角调整量、躯干横滚角调整量、躯干左右平移调整量、躯干前后平移调整量和躯干上下平移调整量；

作为一种可选实施方式，旋转采用Z-Y-X欧拉角表示，根据躯干的横滚角、俯仰角和扭转角调整量得到旋转矩阵。

作为一种可选实施方式，新坐标为：

^BHIPp_TOE＝R_X(-ψ_ref)R_Y(-θ_ref)R_Z(-φ_ref)(^PHIPp_TOE+^Pp_HIP+^Pp_B+δw)-^Bp_HIP；

其中，^Pp_B代表躯干在各个方向的平移量，^Pp_HIP代表第二坐标系中髋关节相对于原点的位置，δ代表左腿或者右腿，w代表腿左右张开调整量，ψ_ref、θ_ref、φ_ref分别代表躯干的横滚角、俯仰角和扭转角调整量。

作为一种可选实施方式，足式机器人站立时的四只脚围成的支撑多边形大于肩关节和髋关节围成的四边形；

或几何信息包括躯干到大腿根部的长度、大腿长度和小腿长度，姿态信息包括躯干、大腿和小腿间夹角。

作为一种可选实施方式，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制的过程为：

在同一坐标下，基于力和转矩空间矢量的平衡关系，计算当机械臂不动作时的运动平台重心空间位姿及当运动平台不动作时的机械臂重心空间位姿；

在满足位姿约束下，以当前机器人及机械臂位姿为起始值，通过能耗最小准则及位姿迭代反馈，获取优化后的运动平台重心空间位姿和机械臂重心空间位姿，并分别对应发送至足式运动平台控制***和机械臂控制***中执行。

作为一种可选实施方式，所述能耗最小准则为：优化后的运动平台重心空间位姿和机械臂重心空间位姿与期望的位姿的偏差最小。

作为一种可选实施方式，所述位姿约束包括腿部各关节约束和机械臂约束；所述腿部各关节约束包括腿部关节存在运动范围及极限转矩约束；所述机械臂约束为机械臂操作空间约束。

本发明的第二个方面提供一种控制器。

一种控制器，其包括计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，该程序被处理器执行时实现如上述所述的变电站巡检机器人的增稳控制方法中的步骤。

本发明的第三个方面提供一种变电站足式机器人姿态调整***。

一种变电站足式机器人姿态调整***，其包括上述所述的控制器。

本发明的第四个方面提供一种变电站足式机器人。

一种变电站足式机器人，其包括上述所述的变电站足式机器人姿态调整***。

本发明的有益效果是：

创新性提出一种足式机器人抬脚步态规划方法，研制了足式机器人抬脚步态规划***，采用同速后撤法使得摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，保证足式机器人在前进时摆动腿竖直抬起，避免踢到地面；采用超速后撤法，使摆动腿的后撤速度超过支撑腿的蹬地速度，解决了足式机器人在爬越楼梯时摆动腿容易踢到楼梯立面的问题，提高了足式机器人运动的稳定性。

创新性提出了一种足式巡检机器人站立位姿调整方法，构建了笛卡尔坐标系下腿部运动学模型，研制了足式机器人站立位姿控制方法，避免了足式机器人在行走时产生质心位置波动和躯干姿态抖动导致的定位精度低的问题，提升机器人站立的位置和角度精度。

创新性提出了一种变电站足式巡检机器人增稳方法，构建了足式作业平台多自由度主动调整运动模型，研制了足式巡检机器人增稳控制***，利用足式运动平台上安装的机械臂姿态的主动调整，辅助足式平台控制***对平台姿态的控制，解决了足式巡检机器人作业时平台重心位置不稳定的问题，提升了足式巡检机器人在不同路面环境下的稳定性，增强了足式平台对站内不同路面的适应性。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明实施例的同速后撤法示意图；

图2是本发明实施例的机器人处于坡面时的同速后撤法示意图；

图3是本发明实施例的超速后撤法示意图；

图4是本发明实施例提供的足式巡检机器人站立位姿调整方法的示意图；

图5是本发明实施例提供的足端坐标在肩关节坐标系中的坐标计算方法示意图；

图6是本发明实施例提供的坐标转换示意图；

图7是本发明实施例的运动平台受力情况；

图8是本发明实施例的一种变电站巡检机器人的增稳控制方法流程图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在本发明中，术语如“上”、“下”、“左”、“右”、“前”、“后”、“竖直”、“水平”、“侧”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，只是为了便于叙述本发明各部件或元件结构关系而确定的关系词，并非特指本发明中任一部件或元件，不能理解为对本发明的限制。

本发明中，术语如“固接”、“相连”、“连接”等应做广义理解，表示可以是固定连接，也可以是一体地连接或可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的相关科研或技术人员，可以根据具体情况确定上述术语在本发明中的具体含义，不能理解为对本发明的限制。

实施例一

本实施例提供了一种变电站足式机器人姿态调整方法，包括：

在足式机器人在前进时，采用同速后撤法以使摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，以保证摆动腿竖直抬起；

在足式机器人站立平稳后，基于站立平稳后的姿态信息和几何信息，计算足端在肩关节坐标系中的初始坐标，再结合躯干姿态调整量和左右脚分开的距离，得到足端在肩关节坐标系中新坐标并进行站立位姿调整；

在足式机器人前进及站立位姿调整的过程中，基于当前运动工况，在满足位姿约束下，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制，以使足式运动平台保持受力及转矩的平衡状态。

具体地，足式机器人在行走时，腿根据支撑状态分为支撑腿和摆动腿，支撑腿蹬地，足端后移推动躯干前进；摆动腿腾空前伸，为触地做准备。其中，摆动腿的摆动阶段按照足端在Z轴(竖直向上为正)的运动方向分为上升阶段和下降阶段，上升阶段即为抬脚阶段。

在足式机器人抬脚步态规划过程中，采用同速后撤法，使摆动腿与支撑腿在前后方向的运动相关联，保证摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，从而保证足式机器人在前进时摆动腿竖直抬起，避免踢到地面立面而影响平衡。

如图1所示，摆动腿抬脚时的运动轨迹可分为X轴(向前为正)轨迹和Z轴轨迹，同速后撤法的核心是保证摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，即：

于是从地面坐标系来看，摆动腿足端无水平初速度。

为摆动腿的后撤速度；

为支撑腿的蹬地速度。

对于Z轴轨迹，采用以下方程中的任意一种均可：

其中，Z_max为迈步高度，t_Swing为摆动相的预计时长，t为进入摆动相后的计时时间，0≤t≤t_Swing。p_{Z_Swing}为摆动腿在Z轴的位置。

得到足端轨迹后，使用足端逆运动学方程，将X轴和Z轴坐标换算为关节转角，再通过关节伺服实现运动。逆运动学方程由腿部机械结构决定，因腿而异。具体地，该运动在XOZ平面内通过规划X轴和Z轴的足端轨迹，然后经过逆运动学方程换算至关节角度，然后通过伺服关节角度实现。

在躯干坐标系中，摆动腿和支撑腿后撤速度相同，支撑腿后撤推动躯干前进，摆动腿后撤避免碰撞障碍。从地面坐标系来看，摆动腿末端是竖直抬起的。

当足式机器人处于斜坡上时，X轴和Z轴轨迹规划方式与上述相同，但此时的X轴不再与前进方向相同，而是旋转至水平，Z轴竖直向上。该旋转角度为机器人的俯仰角，可由安装于机器人本体的惯性测量单元或其他传感器测出。此时摆动腿表现为竖直抬脚，如图2所示。

在一些实施例中，当地面障碍立面存在凸起时，采用超速后撤法。该方法可用速度描述或位置描述，如图3所示。在躯干坐标系中，摆动腿的后撤速度大于支撑腿的后撤速度。从地面坐标系来看，摆动腿末端是边抬起边后撤的。

在具体实施中，摆动腿的后撤值可通过增加速度或增加位移量实现。

速度描述方程为：

其中，k>1，该参数可根据实际情况进行调整。

为摆动腿的后撤速度；

为支撑腿的蹬地速度。

位置描述方程为：

其中，d为抬脚结束后多后撤的距离，可根据地形预估，也可使用激光雷达或立体相机等传感器扫描地形后动态设定。p_{X_Swing}为摆动腿的后撤位置；p_{X_Support}为支撑腿的位置。t_Swing为摆动相的预计时长，t为进入摆动相后的计时时间，0≤t≤t_Swing。

上述的足式机器人抬脚步态规划方式也适用于砾石地形、草地等其他可能在抬脚时绊倒腿足式机器人的情况。

如图4所示，在足式巡检机器人站立位姿调整的过程具体包括如下内容：

足式机器人的巡检步骤是逐个遍历预置的巡检点位，每当到达目标点附近时，逐渐减速直至踏步，随后切换至站立状态。

站立时，左右腿分开一定间距，使四只脚围成的支撑多边形大于肩关节和髋关节围成的四边形，增强机器人的站立稳定性，该间距需要按实际情况进行调整，太小会导致后续躯干偏移时失稳倾倒，太大会导致腿部侧摆角度达到机械极限。

站稳后，足式机器人根据定位程序的位置和姿态反馈，进行躯干的位姿调整。其中，位置调整包含前后调整(沿X轴平移)和左右调整(沿Y轴平移)，姿态调整为机器人左右扭转(绕Z轴扭转)。此外，足式机器人也可按照巡检需要进行躯干倾斜度的调整，即横滚角调整(绕X轴旋转)和俯仰角调整(绕Y轴旋转)。

下面介绍具体的姿态调整流程：

常规步态规划方式为控制四条腿末端在躯干坐标系下的位置，即^BHIPp_TOE来实现，本实施例将站立控制与姿态控制、腿张开的距离解耦，通过Σ_P和Σ_B两个坐标系来实现运动。

不改变原先的步态生成方式，站立步态输入^PHIPp_TOE，经变换后融入躯干位移、姿态调整量和左右脚分开的距离，生成新的^BHIPp_TOE，用于足端控制。

具体旋转方式和平移方式推导过程如下，如图5所示，单腿足端坐标在肩关节坐标系中的坐标为：

由图5可知：

^Pp_TOE＝^PR_B ^Bp_TOE+^Pp_B (8)

^PR_B＝R_Z(φ_ref)R_Y(θ_ref)R_X(ψ_ref) (9)

^Pp_TOE＝^PHIPp_TOE+^Pp_HIP+δw (10)

进一步的，得到：

^BHIPp_TOE＝R_X(-ψ_ref)R_Y(-θ_ref)R_Z(-φ_ref)(^PHIPp_TOE+^Pp_HIP+^Pp_B+δw)-^Bp_HIP (11)

其中，

坐标系Σ_P由Σ_B旋转而成，旋转采用Z-Y-X欧拉角表示，由Σ_P到Σ_B的旋转矩阵为式(9)；其中，φ_ref表示绕动坐标系Z轴的扭转角，θ_ref表示绕动坐标系Y轴的俯仰角，ψ_ref表示绕动坐标系X轴的横滚角，^PHIPp_TOE为坐标系Σ_P中足尖相对于髋关节的位置，^Pp_HIP为坐标系Σ_P中髋关节相对于原点的位置。

由此即可通过在^PHIPp_TOE中加入姿态和位置信息，得到^BHIPp_TOE。

控制式(11)中的变量即可控制足式机器人进行调整运动，变量列表如下：

如图8所示，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制的过程为：

在同一坐标下，基于力和转矩空间矢量的平衡关系，计算当机械臂不动作时的运动平台重心空间位姿及当运动平台不动作时的机械臂重心空间位姿；

在满足位姿约束下，以当前机器人及机械臂位姿为起始值，通过能耗最小准则及位姿迭代反馈，获取优化后的运动平台重心空间位姿和机械臂重心空间位姿，并分别对应发送至足式运动平台控制***和机械臂控制***中执行。

此处所述能耗最小准则为：优化后的运动平台重心空间位姿和机械臂重心空间位姿与期望的位姿的偏差最小。

在具体实施中，所述位姿约束包括腿部各关节约束和机械臂约束。所述腿部各关节约束包括腿部关节存在运动范围及极限转矩约束。所述机械臂约束为机械臂操作空间约束。这样更加贴合实际机器人的运动情况，提高了足式机器人的运动平台的稳定性。

本实施例基于机器人当前运动工况，在满足腿部各关节约束条件及机械臂操作空间约束下，利用足式运动平台上安装的机械臂姿态的主动调整，辅助足式运动平台控制***对平台姿态的控制，使足式运动平台保持受力及转矩的平衡状态。

具体地，参照图7，以足式运动平台整体重心位置为原点O，以运动平台前向运动方向为X轴，依据右手法则建立坐标系OXYZ，在该坐标系下，运动平台足式足端在该坐标系下形成四边形ABCD，运动平台所受重力G_r，其方向射线与ABCD相交于O_g点；运动平台足式端受力的合力为F。另外，运动平台上安装多自由度机械臂的整体重心位于O_a处，所受重力为G_a。

本实施例的机械臂和足式运动平台的坐标***一。保障了机器人的控制精度，增加了加装巡检作业设备后平台巡检运行的稳定性。

假设此时在机器人以加速度a运动，则为保证运动平台稳定则在坐标系OXYZ应有以下力和转矩空间矢量的平衡关系：

F+G_r+G_a＝(m_r+m_a)×a

M＝M_a+M_ra+M_aa

其中：m_r和m_a分别是运动平台和机械臂整体质量，M是力F在O处的力矩，M_a是机械臂重力在O处产生的转矩，M_ra和M_aa则是加速度下的惯性力在O处产生的转矩。

依据上述方程，可以分别求得当机械臂不动作时的运动平台足端力矩对应的空间位姿Z_r以及当运动平台不动作时的机械臂力矩向量对应的空间位姿Z_a，也就是运动平台重心空间位姿Z_r和机械臂重心空间位姿Z_a。

之后，基于机器人腿部关节存在运动范围及极限转矩约束以及机械臂操作空间约束，以当前机器人及机械臂位姿为起始值，通过能耗最小准则，通过对上述求解过程迭代运算，获取优化后的运动平台重心空间位姿和机械臂重心空间位姿。

最后，将上述位姿分别发送至运动平台和机械臂控制***执行，如图8所示。

本实施例的足式平台姿态稳定控制可简化为在当前运动工况(速度、加速度、足端受力)下，在满足腿部各关节约束条件下，使平台保持受力及转矩的平衡状态。

实施例二

本实施例提供了一种控制器，其包括计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，该程序被处理器执行时实现如上述实施例一所述的变电站巡检机器人的增稳控制方法中的步骤。

实施例三

本实施例提供了一种变电站足式机器人姿态调整***，其包括上述实施例二所述的控制器。

此处需要说明的是，变电站足式机器人姿态调整***的其他结构均可采用现有的结构来实现，此处不再详述。

实施例四

本实施例提供了一种变电站足式机器人，其包括上述实施例三所述的变电站足式机器人姿态调整***。

在具体实施中，所述变电站足式机器人包括足式运动平台，所述足式运动平台上安装有机械臂。

具体地，所述足式运动平台与足式运动平台控制***相连，机械臂与机械臂控制***相连。

此处需要说明的是，足式运动平台(比如：二足、三足或四足等运动平台)、足式运动平台控制***(比如：PLC控制器等)、机械臂(比如：多关节机械臂)与机械臂控制***(比如：PLC控制器等)均为现有结构，此处不再详述。

另外，足式机器人可为二足、三足或四足等机器人，机器人的其他结构均为现有结构，此处不再详述。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (15)

1.一种变电站足式机器人姿态调整方法，其特征在于，包括：

在足式机器人在前进时，采用同速后撤法以使摆动腿相对于躯干的移动速度与支撑腿相对于躯干的移动速度相同，以保证摆动腿竖直抬起；

在足式机器人站立平稳后，基于站立平稳后的姿态信息和几何信息，计算足端在肩关节坐标系中的初始坐标，对肩关节坐标系进行旋转得到第二坐标系，进一步得到第二坐标系中髋关节相对于原点的位置，再结合躯干姿态调整量和左右脚分开的距离，得到足端在肩关节坐标系中新坐标并进行站立位姿调整；

在足式机器人前进及站立位姿调整的过程中，基于当前运动工况，在满足位姿约束下，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制，以使足式运动平台保持受力及转矩的平衡状态。

2.如权利要求1所述的变电站足式机器人姿态调整方法，其特征在于，在足式机器人在前进时，还采用超速后撤法，使摆动腿的后撤速度超过支撑腿的蹬地速度，从而避免抬脚时被地面障碍立面的凸出部分压住而破坏平衡。

3.如权利要求2所述的变电站足式机器人姿态调整方法，其特征在于，超速后撤法用速度描述：摆动腿的后撤速度＝k*支撑腿的蹬地速度；其中，k大于1。

4.如权利要求2所述的变电站足式机器人姿态调整方法，其特征在于，超速后撤法用位置描述：摆动腿的后撤位置＝支撑腿的位置-抬脚结束后多后撤的距离与摆动相的预计时长的比值*进入摆动相后的计时时间。

5.如权利要求4所述的变电站足式机器人姿态调整方法，其特征在于，摆动腿抬脚时的运动轨迹分为X轴轨迹和Z轴轨迹，X轴轨迹向前为正，Z轴轨迹为摆动腿在Z轴的位置＝迈步高度与摆动相的预计时长的比值*进入摆动相后的计时时间。

6.如权利要求1所述的变电站足式机器人姿态调整方法，其特征在于，躯干调整量包括躯干扭转角调整量、躯干俯仰角调整量、躯干横滚角调整量、躯干左右平移调整量、躯干前后平移调整量和躯干上下平移调整量。

7.如权利要求1所述的变电站足式机器人姿态调整方法，其特征在于，旋转采用Z-Y-X欧拉角表示，根据躯干的横滚角、俯仰角和扭转角调整量得到旋转矩阵。

8.如权利要求7所述的变电站足式机器人姿态调整方法，其特征在于，新坐标为：^BHIPp_TOE＝R_X(-ψ_ref)R_Y(-θ_ref)R_Z(-φ_ref)(^PHIPp_TOE+^Pp_HIP+^Pp_B+δw)-^Bp_HIP；

其中，^Pp_B代表躯干在各个方向的平移量，^Pp_HIP代表第二坐标系中髋关节相对于原点的位置，δ代表左腿或者右腿，w代表腿左右张开调整量，ψ_ref、θ_ref、φ_ref分别代表躯干的横滚角、俯仰角和扭转角调整量，^PHIPp_TOE为第二坐标系中足尖相对于髋关节的位置，^Bp_HIP为肩关节坐标系中髋关节相对于原点的位置，R_X,、R_Y,、R_Z,分别为第二坐标系的X轴Y轴Z轴。

9.如权利要求1所述的变电站足式机器人姿态调整方法，其特征在于，足式机器人站立时的四只脚围成的支撑多边形大于肩关节和髋关节围成的四边形；

或几何信息包括躯干到大腿根部的长度、大腿长度和小腿长度，姿态信息包括躯干、大腿和小腿间夹角。

10.如权利要求1所述的变电站足式机器人姿态调整方法，其特征在于，利用足式运动平台上机械臂的姿态主动调整，辅助足式运动平台控制***对平台姿态的控制的过程为：

在同一坐标下，基于力和转矩空间矢量的平衡关系，计算当机械臂不动作时的运动平台重心空间位姿及当运动平台不动作时的机械臂重心空间位姿；

在满足位姿约束下，以当前机器人及机械臂位姿为起始值，通过能耗最小准则及位姿迭代反馈，获取优化后的运动平台重心空间位姿和机械臂重心空间位姿，并分别对应发送至足式运动平台控制***和机械臂控制***中执行。

11.如权利要求10所述的变电站足式机器人姿态调整方法，其特征在于，所述能耗最小准则为：优化后的运动平台重心空间位姿和机械臂重心空间位姿与期望的位姿的偏差最小。

12.如权利要求10所述的变电站足式机器人姿态调整方法，其特征在于，所述位姿约束包括腿部各关节约束和机械臂约束；所述腿部各关节约束包括腿部关节存在运动范围及极限转矩约束；所述机械臂约束为机械臂操作空间约束。

13.一种控制器，其包括计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-12中任一项所述的变电站足式机器人姿态调整方法。

14.一种变电站足式机器人姿态调整***，其特征在于，包括如权利要求13所述的控制器。

15.一种变电站足式机器人，其特征在于，包括如权利要求14所述的变电站足式机器人姿态调整***。

Images