一种复杂地形下足式机器人的主动柔顺控制方法与流程

本发明涉及足式机器人主动柔顺控制的技术领域，尤其涉及一种复杂地形下足式机器人的主动柔顺控制方法。

背景技术：

足式机器人是目前特种机器人领域研究的热点，按照腿足结构的数量划分足式机器人可分为双足机器人，四足机器人和多足机器人；四足机器人由于其仿生性能好，控制稳定性和控制复杂度比较均衡，目前其受关注度较其他类型的足式机器人更多一些。

目前常见的串联腿四足机器人结构较为简单、运动灵活性较高，这种构型的四足机器人在平坦的硬质地面行走控制算法目前已经较为成熟，且四足机器人在合适的步态和运动速度下均可连续保持较好的运动稳定性。但是在复杂的地形环境下，例如起伏高度差较大的沙土坑道或者坡角大于50°的坡度上，四足机器人四条腿和躯干运动姿态的互相耦合运动是高度非线性的，难以建立精确的动力学模型。因此如何保证四足机器人在较为复杂的地形条件下的运动稳定性，涉及相适宜的柔顺性运动补偿控制算法，是目前还没有解决的问题之一。目前已有的电机驱动的串联腿四足机器人平台在复杂非结构化环境下行走的步态不够稳定，容易出现打滑或者摔倒的问题。

技术实现要素：

本发明的目的在于克服现有技术的不足，本发明提供了一种复杂地形下足式机器人的主动柔顺控制方法，通过阻抗滤波器同时修正调整电机的关节角度和关节角速度，同时通过计算躯干姿态角度误差实时计算支撑腿长度的调整量，通过阻抗滤波器得到修正后的腿长指令，主动控制并减小躯干姿态角度的变化幅度，减小四足机器人失稳摔倒的可能性。

为了解决上述技术问题，本发明实施例提供了一种复杂地形下足式机器人的主动柔顺控制方法，所述方法包括：

通过阻抗滤波器同时生成关节角度修正量和关节角速度修正量；

基于所述关节角度修正量和关节角速度修正量，将所述关节角度的指令和所述关节角速度的指令输出至关节pd控制器；

在所述输出之后，通过机身姿态角误差计算得出支撑腿长度的调整量；

基于所述支撑腿长度的调整量，通过所述阻抗滤波器生成腿长修正指令，并将所述腿长修正指令输出至关节pd控制器；

通过所述关节pd控制器，主动柔顺控制在复杂地形下的足式机器人。

可选的，所述通过阻抗滤波器同时生成关节角度修正量和关节角速度修正量包括：

外环控制器将足式机器人足端的接触力误差映射到关节空间内；

通过阻抗滤波器和积分器生成关节角度修正量和关节角速度修正量；

根据所述关节角度修正量和所述关节角速度修正量，修正步态规划器所给出的关节角度和关节角速度的参考值。

可选的，所述基于所述关节角度修正量和关节角速度修正量，将所述关节角度的指令和所述关节角速度的指令输出至关节pd控制器包括：

基于所述关节角度修正量和关节角速度修正量，将所述关节角度的指令和所述关节角速度的指令输出至关节pd控制器，即内环位置控制器；

所述内环位置控制器通过反馈调整阻断位置，控制足端接触力。

可选的，所述足式机器人足端的接触力误差映射到关节空间内，具体的计算公式如下：

terr＝j^t(qref)fref-j^t(q)f；

其中，terr为力矩误差向量，fref为参考足地接触力向量，f为实际的足地接触力向量，j^t为雅可比矩阵转置，qref为参考关节角向量，q为实际关节角向量。

可选的，所述阻抗滤波器的传递函数形式具体如下：

其中，

其中，gc(s)为阻抗滤波器的传递函数，(k0，b0)，(k1，b1)，(k2，b2)分别为根关节，髋关节，膝关节的阻抗滤波器参数，γ为自适应学习率，eα和分别为俯仰角和滚转角误差，s为拉普拉斯变换，ks为传递函数的增益积分，为传递函数的增益积分的一阶导数。

可选的，所述在所述输出之后，通过机身姿态角误差计算得出支撑腿长度的调整量包括：

在所述输出之后，通过机身姿态角误差计算得出支撑腿长度的调整量；

基于所述支撑腿长度的调整量，使得机身实际姿态与期望姿态重合；

通过调整左右腿长度差来改变机身滚转角，以及通过调整前后腿长度差来改变机身俯仰角。

可选的，所述机身姿态角误差中，姿态转换矩阵的具体计算公式如下：

其中，s1＝sin(α)；c1＝cos(α)；

s3＝sin(ψ)；c3＝cos(ψ)；

其中，为姿态转换矩阵，α为俯仰角，为滚转角，ψ为偏航角。

可选的，所述支撑腿长度的调整量的具体计算公式如下：

其中，plx为左支撑腿足端x轴坐标，prx为右支撑腿足端x轴坐标，pfy为前支撑腿足端z轴坐标，pby为后支撑腿足端z轴坐标，zep和zer分别为对应调整俯仰角和滚转角的腿长修正量，和α0为滚转角和俯仰角阈值。

可选的，所述基于所述支撑腿长度的调整量，通过所述阻抗滤波器生成腿长修正指令中，所述阻抗滤波器的传递函数形式具体如下：

其中，gp(s)和gr(s)为阻抗滤波器的传递函数，(kp，bp)和(kr，br)分别为对应俯仰角和滚转角的阻抗滤波参数，s为拉普拉斯变换。

可选的，所述方法还包括复合摆线形式足端运动轨迹，其具体计算公式如下：

其中，s为跨距，h为抬腿高度，tw为摆动相持续时间，t为足端画一个摆线轨迹的运动时间，y为y方向的位移量，z为足端离地面高度的变化量。

在本发明实施中，所述方法包括复合摆线形式足端运动轨迹、基于关节空间的阻抗控制算法和机器人躯干姿态柔顺控制算法理论；其中，所述复合摆线形式足端运动轨迹满足位置、速度和加速度连续可导、无突变，摆动相与支撑相切换平滑等优点；所述基于关节空间的阻抗控制算法可以调节得机器人腿部关节直接响应不平整地形造成的接触力突变，通过足端接触力矩的主动控制从而实现机器人的主动柔顺性，防止机器人足端打滑，或者冲击力矩过大而失稳跌倒；所述机器人躯干姿态柔顺控制算法理论利用支撑腿长度的调整使机身实际姿态与期望姿态重合，通过调整左右腿长度差改变机身滚转角，通过调整前后腿长度差改变机身俯仰角，从而保证机器人躯干姿态的运动稳定性。

附图说明

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

图1是本发明实施中的复杂地形下足式机器人的主动柔顺控制方法的流程示意图；

图2是本发明实施例中的基于关节空间的阻抗控制框图；

图3是本发明实施例中的复杂路面下姿态调整图；

图4是本发明实施例中的整体步态规划与控制框架图；

图5是本发明实施中的四足机器人结构示意图。

具体实施方式

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

实施例

请参阅图1，图1是本发明实施中的复杂地形下足式机器人的主动柔顺控制方法的流程示意图。

如图1所示，一种复杂地形下足式机器人的主动柔顺控制方法，其特征在于，所述方法包括：

s11：通过阻抗滤波器同时生成关节角度修正量和关节角速度修正量；

在本发明具体实施过程中，所述通过阻抗滤波器同时生成关节角度修正量和关节角速度修正量包括：外环控制器将足式机器人足端的接触力误差映射到关节空间内；通过阻抗滤波器和积分器生成关节角度修正量和关节角速度修正量；根据所述关节角度修正量和所述关节角速度修正量，修正步态规划器所给出的关节角度和关节角速度的参考值。

s12：基于所述关节角度修正量和关节角速度修正量，将所述关节角度的指令和所述关节角速度的指令输出至关节pd控制器；

在本发明具体实施过程中，所述基于所述关节角度修正量和关节角速度修正量，将所述关节角度的指令和所述关节角速度的指令输出至关节pd控制器包括：基于所述关节角度修正量和关节角速度修正量，将所述关节角度的指令和所述关节角速度的指令输出至关节pd控制器，即内环位置控制器；所述内环位置控制器通过反馈调整阻断位置，控制足端接触力。

具体的，基于关节空间的阻抗控制算法主要是调节机器人腿部关节直接响应不平整地形造成的接触力突变，通过足端接触力矩的主动控制从而实现机器人的主动柔顺性，防止机器人足端打滑，或者冲击力矩过大而失稳跌倒。结合附图2所示，附图2示出本发明实施例中的基于关节空间的阻抗控制框图，由位置控制内环和关节力控制外环组成，其算法的核心控制策略是外环控制器将足端的接触力误差映射到关节空间内，再通过阻抗滤波器和积分器生成关节角度和关节角速度修正指令，用于修正步态规划器给出的关节角度和角速度参考值，然后将生成的关节角度和角速度指令输出至内环位置控制器，通过反馈调整足端位置达到控制接触力的目的。

其阻抗控制算法如下，根据步态规划中给出的参考轨迹，每条腿关节空间内的力矩误差为如下所示的雅克比矩阵表达式：

terr＝j^t(qref)fref-j^t(q)f；

其中，terr为力矩误差向量，fref为参考足地接触力向量，f为实际的足地接触力向量，j^t为雅可比矩阵转置，qref为参考关节角向量，q为实际关节角向量；

将此力矩误差通过一阶阻抗滤波器得到关节角修正量阻抗滤波器的传递函数形式具体如下：

其中，

其中，gc(s)为阻抗滤波器的传递函数，(k0，b0)，(k1，b1)，(k2，b2)分别为根关节，髋关节，膝关节的阻抗滤波器参数，γ为自适应学习率，eα和分别为俯仰角和滚转角误差，s为拉普拉斯变换，ks为传递函数的增益积分，为传递函数的增益积分的一阶导数。

进而将关节角速度修正量通过积分器生成关节角修正量；由上述算法可知，当机器人足端不与地面接触时，参考接触力和实际接触力都为0，此时关节角度修正量也趋于0，可保证位置跟踪精度。

s13：在所述输出之后，通过机身姿态角误差计算得出支撑腿长度的调整量；

在本发明具体实施过程中，所述在所述输出之后，通过机身姿态角误差计算得出支撑腿长度的调整量包括：在所述输出之后，通过机身姿态角误差计算得出支撑腿长度的调整量；基于所述支撑腿长度的调整量，使得机身实际姿态与期望姿态重合；通过调整左右腿长度差来改变机身滚转角，以及通过调整前后腿长度差来改变机身俯仰角。

s14：基于所述支撑腿长度的调整量，通过所述阻抗滤波器生成腿长修正指令，并将所述腿长修正指令输出至关节pd控制器；

具体的，结合附图3所示，附图3示出本发明实施例中的复杂路面下姿态调整图，机器人在通过复杂不平整地形时，四条腿的落足高度不一致，会导致机身姿态发生偏转，从而使机器人失稳摔倒，因此需要加入机身姿态反馈控制算法保证机器人身体姿态保证稳定。机身姿态反馈控制利用支撑腿长度的调整使机身实际姿态与期望姿态重合，通过调整左右腿长度差改变机身滚转角，通过调整前后腿长度差改变机身俯仰角，从而使机器人躯干保持平稳运动。

具体实施中，定义机身水平坐标系{l}，原点与机体坐标系{b}相同，x轴水平向右，y轴水平向前，z轴垂直向上；设机体坐标系{b}相对于机身水平坐标系{l}的欧拉角按照1-2-3转序定义：俯仰角α，滚转角和偏航角ψ，则姿态转换矩阵可表示为：

其中，s1＝sin(α)；c1＝cos(α)；

s3＝sin(ψ)；c3＝cos(ψ)；

其中，为姿态转换矩阵，α为俯仰角，为滚转角，ψ为偏航角；

则根据测得的关节角度和身体姿态，有

其中，^bpi为支撑腿i的足端在坐标系{b}中的坐标，^lpi＝[pixpiypiz]^t为支撑腿i的足端在坐标系{l}中的坐标，则可以估计出调整俯仰角和滚转所需的腿长修正量：

其中，plx为左支撑腿足端x轴坐标，prx为右支撑腿足端x轴坐标，pfy为前支撑腿足端z轴坐标，pby为后支撑腿足端z轴坐标，zep和zer分别为对应调整俯仰角和滚转角的腿长修正量，和α0为滚转角和俯仰角阈值，可根据平地行走身体波动状况设定，当姿态角超过一定阈值时，姿态控制器开始发挥作用；

将上述计算得到的支撑腿长度调节量通过一阶阻抗滤波器后生成支撑腿长度修正指令，滤波器传递函数如下：

其中，gp(s)和gr(s)为阻抗滤波器的传递函数，(kp，bp)和(kr，br)分别为对应俯仰角和滚转角的阻抗滤波参数，s为拉普拉斯变换；

令通过滤波器后的俯仰角和滚转角的支撑腿长度修正指令分别为zep和zer，再将该腿长度调节量分别分配至四条腿：

其中，δz＝[δz1δz2δz3δz4]^t为腿长修正指令，该指令反馈至步态规划器与主对角步态的足端轨迹相加，之后输出至单腿逆运动学模块求解关节角度和关节角速度参考量qref，结合附图4所示，附图4示出本发明实施例中的整体步态规划与控制框架图。

通过四足机器人在不平整地面行走的动力学仿真，可以对比得出本发明提出的算法相较于普通对角步态控制算法其步态稳定性、躯干姿态变化角、足端冲击力均得到了不同程度的优化。

s15：通过所述关节pd控制器，主动柔顺控制在复杂地形下的足式机器人。

另外，一种复杂地形下足式机器人的主动柔顺控制方法，所述方法还包括复合摆线形式足端运动轨迹，所述复合摆线形式足端运动轨迹满足位置、速度和加速度连续可导、无突变，摆动相与支撑相切换平滑等优点，其具体计算公式如下：

其中，s为跨距，h为抬腿高度，tw为摆动相持续时间，t为足端画一个摆线轨迹的运动时间，y为y方向的位移量，z为足端离地面高度的变化量。

需要说明的是，本发明实施例中的四足机器人由电机驱动，每条腿有三个自由度，分别是髋关节侧摆、髋关节正摆、膝关节正摆自由度。结合附图5所示，附图5示出本发明实施中的四足机器人结构示意图，定义固连于机器人身体的机体坐标系{b}，原点位于机器人身体中心，x轴指向机器人右方，y轴指向机器人正前方，z轴垂直于身体向上。

为了实现四足机器人在复杂、不平坦路面上能够控制其稳定的行走，本发明实施例中将四足机器人运动控制分为主步态规划模块与主动柔顺控制模块两部分，结合附图4所示，附图4示出本发明实施例中的整体步态规划与控制框架图，图中qref和代表主步态规划模块输出的关节角和角速度参考量，qc和代表关节阻抗控制模块输出的关节角和角速度修正量，qd和代表输出到关节pd控制器的关节角和角速度指令，q和代表机器人实际的关节角和角速度，f代表力传感器测得的足地接触力，α，ψ代表机器人机身姿态欧拉角。

在本发明实施中，所述方法包括复合摆线形式足端运动轨迹、基于关节空间的阻抗控制算法和机器人躯干姿态柔顺控制算法理论；其中，所述复合摆线形式足端运动轨迹满足位置、速度和加速度连续可导、无突变，摆动相与支撑相切换平滑等优点；所述基于关节空间的阻抗控制算法可以调节得机器人腿部关节直接响应不平整地形造成的接触力突变，通过足端接触力矩的主动控制从而实现机器人的主动柔顺性，防止机器人足端打滑，或者冲击力矩过大而失稳跌倒；所述机器人躯干姿态柔顺控制算法理论利用支撑腿长度的调整使机身实际姿态与期望姿态重合，通过调整左右腿长度差改变机身滚转角，通过调整前后腿长度差改变机身俯仰角，从而保证机器人躯干姿态的运动稳定性。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(rom，readonlymemory)、随机存取存储器(ram，randomaccessmemory)、磁盘或光盘等。

另外，以上对本发明实施例所提供的一种复杂地形下足式机器人的主动柔顺控制方法进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。