技术领域
[0001] 本
发明涉及机器人控制领域，具体是一种四足机器人对角步态控制方法。
背景技术
[0002] 四足机器人因其机构简单灵活，承载能
力强、
稳定性好，在抢险救灾、军事等许多方面有很好的应用前景，受到各国四足机器人研究人员的重视。四足机器人是一个多变量、强耦合的动力学模型，其变步态下非结构化地形的自适应以及变步态的稳定性调节问题，对控制理论的研究及实时控制系统的设计都具有很大的挑战性。
[0003] 研究人员首先对静态步行展开了研究，取得一定的成果。但是从提高步行速度来说，动态步行有一定的优越性，然而动态步行的技术难度远大于静态步行。
[0004] 本发明是针对四足机器人动态步行中的对角步态，即处于对角线上的两条腿动作完全一致，均处于摆动相或均处于
支撑相。该步态是研究机器人动态步行的
基础，同时也是最容易与静态步态进行切换的步态，因此我们对此种步态进行研究。
[0005] 搜索相关文献，以往对于对角步态的研究主要是两个方面：(1)对于对角步态的稳定性研究，例如在“四足机器人对角小跑步态的研究”文中步态采用离线规划方法预定义，根据计算机仿真及三维图像仿真进行优化设计。然后由计算机协调控制JTUWM-III型四足机器人的关节，完成对角动态步行实验。在”一种四足步行仿生机器人设计及步态研究”文中按照多足步行机器人行走稳定性原则，设计出了慢走和对角小跑两种步态的具体过程，实验证明，所设计的步态具有良好的稳定性。(2)基于避障的路径规划。在
申请号为201010273466的发明
专利申请中，公开了一种基于卡尔曼的
滤波器预测的机器人避障方法。该方法提出一种当
传感器系统探测到有新障碍物出现，根据观测数据建立卡尔曼滤波器模型，利用观测数据和经典的线性动态系统期望最大化模型辨识
算法对参数进行辨识和修正，更新数字地图，供路径进行新一轮的局部重规划的方法。此方法属于避障的路径规划策略。上述两个研究方向并没有考虑四足机器人以对角步态行走时，在复杂地形环境中的步态规划问题。
发明内容
[0006] 为克服
现有技术中存在的没有考虑四足机器人以对角步态行走时，在复杂地形环境中步态规划问题的不足，本发明提出了一种四足机器人对角步态的自适应控制方法。
[0007] 本发明包括以下步骤：
[0008] 步骤1：参数初始化。初始化四足机器人迈步长度执行矩阵QL和迈步高度执行矩阵QH。
[0009] 步骤2：通过传感器采集外部环境信息。通过红外传感器和
超声波传感器测得的四足机器人前足到障碍物前端的距离L和视觉传感器测得的障碍物高度H。
[0010] 步骤3：通过模糊推理算法获得每条迈步长度的激活约束条件对应的隶属度函数和真值αlk(t)。根据四组
机器人本体的
感知状态，依照模糊推理得到相应的激活约束条件，通过公式(1)和公式(2)，分别计算隶属度函数 和以及真值αlk(t)：
[0011] 公式(1)
[0012] 公式(2)
[0013] 步骤4：采用波尔滋曼分布搜索算法获得迈步长度执行动作。对于第j条激活约束条件通过搜索算法选择局部动作 通过公式(3)和公式(4)对所有的局部动作按真值加权得到全局动作yl(t)及相应的ql(t)值。
[0014] 公式(3)
[0015] 公式(4)
[0016] 步骤5：通过模糊推理算法获得每条迈步高度的激活约束条件对应的隶属度函数和真值αhk(t)。将迈步长度执行动作yl(t)和高度信息H进行模糊推理计算，可以得到相应的激活约束条件，通过公式(5)和公式(6)，分别计算隶属度函数 以及真值αhk(t)：
[0017] 公式(5)
[0018] 公式(6)
[0019] 步骤6：采用波尔滋曼分布搜索算法获得迈步高度执行动作。对于第j条激活约束条件通过搜索算法选择局部动作 通过公式(7)和公式(8)对所有的局部动作按真值加权得到全局动作yh(t)及相应的qh(t)值：
[0020] 公式(7)
[0021] 公式(8)
[0022] 步骤7：推算执行矩阵的全局动作Y(t)及相应的Q(t)值。全局动作Y(t)由两个特征量表示：四足机器人对角两足的迈步长度和迈步高度，由于对角步态下对角足是同步运动的，通过公式(9)和公式(10)，规划其中一个对角足的足部运动信息即可规划出全部对角足的足部运动信息。
[0023] Y(t)＝[yl(t)yh(t)] 公式(9)
[0024] Q(t)＝[ql(t)qh(t)] 公式(10)
[0025] 步骤8：确定总体误差。四足机器人执行全局动作Y(t)以后，通过公式(11)和公式(12)确定总体函数误差ΔQ(t)＝|ΔQL(t)ΔQH(t)]，包括迈步长度函数误差ΔQL(t)和迈步高度函数误差ΔQH(t)：
[0026] 公式(11)
[0027] 公式(12)
[0028] 其中式中：β为学习率，γ为折扣系数，r为回报
信号。
[0029] 步骤9：更新迈步长度和迈步高度执行矩阵。对于每一条激活约束条件，通过公式(13)和公式(14)修正相应执行矩阵的评估值，包括迈步长度和迈步高度执行矩阵的评估值 和
[0030] 公式(13)
[0031] 公式(14)
[0032] 然后重复步骤(3)，直到迈步长度执行矩阵QL和迈步高度执行矩阵QH收敛为止。
[0033] 步骤10：对角足部运动轨迹规划。由全局动作Y(t)可得出四足机器人对角足部落点
位置信息，其主要包括迈步起始位置、终止位置、最高点位置信息。对角足部落点位置信息经过足轨迹拟合得出对角足部运动规划信息，也即把离散的对角足部落点位置信息通过拟合算法推算出连续的对角足部运动规划信息。
[0034] 步骤11：对步骤3～10规划的四足机器人对角步态的稳定性判定及调整。如图3所示。通过四足机器人以规划步态行走时，其
重心位置是否在对角支撑足的连线上，判定四足机器人以规划步态行走时的稳定性。如果是稳定的，则通过对角足部规划信息确定四足机器人各个关节角，然后送入四足
机器人关节控制系统。反之，通过公式(15)调整对角支撑足的支撑时间ΔT，改变迈步周期，调整四足机器人以规划步态行走时的稳定性。
[0035] 公式(15)
[0036] 其中xi、和 pi是经迈步长度和迈步高度规划的四足机器人重心的位移和速度瞬时值以及ZMP的位移和速度瞬时值，xf、 和pf、 是修正后的四足机器人重心的位移和速度瞬时值以及ZMP的位移和速度瞬时值，ω是常数。将调整后的对角足支撑时间ΔT和对角足部运动规划信息送入四足机器人
控制器，确定各个关节角和迈步周期，然后通过四足机器人关节控制系统，以达到非结构化地形的自适应控制。
[0037] 本发明是一种步态规划方法，用于四足机器人非结构环境下对角步态的规划和调整，分工明确，逻辑性强。本发明的核心是一种在线模糊推理学习方法，根据外部环境信息提前规划好未来几步的迈步信息，具有预测性，这种预测的方法是通过一些模糊推理学习加上回馈信息的方法建立起来的，与离线学习不能适应预测环境之外的情况相比，该方法具有合理性、有效性。其次采用两套模糊推理学习方法进行实时规划步态。如果采用一套学习方法把迈步高度和迈步长度信息一次性规划模式，高维
知识库将导致程序计算量大，计算效率低，从而使得机器人控制实时性差。采用两套学习系统同时规划，则可把高维知识库变成两个二维知识库，计算量就会大大减小，也就保证规划的实时性。经过验证，此方法是有效可行的，并且明显的提高了四足机器人的执行效率。除此采用多种传感器进行外部环境的信息采集，测量范围广，精确度高。该系统可实现自主感知，自主调整的调节
姿态以适应非结构环境。
附图说明
[0038] 图1是四足机器人对角步态控制的总体示意方
框图；
[0039] 图2是四足机器人对角足轨迹规划示意方框图；
[0040] 图3是四足机器人对角足轨迹实时修正示意方框图。
具体实施方式
[0041] 本
实施例是一种四足机器人在非结构环境下以对角步态为行走方式的自适应控制方法。本实施例以模糊推理学习算法和足轨迹实时修正算法为基础，以对角步态为运动方式，综合控制四足机器人本体适应各种复杂地形。
[0042] 如图1所示。首先通过组合传感器从环境中实时获得有用信息，包括四足机器人前足到障碍物前端的距离信息和障碍物高度信息，接着根据足轨迹模糊推理学习算法获得对角足部落点位置信息，然后拟合成对角足轨迹步态。然而四足机器人适应非结构环境下变步态行走时稳定性较差，需要进行实时调整，图中的对角足轨迹实时修正算法则具有这种功能。我们将所规划信息进行稳定性判定，如果输出不在稳定裕度内，则对此规划信息进行实时修正。最后将调整后的对角足轨迹步态转换成四足机器人各个关节角并下达给四足机器人执行，达到四足机器人在非结构环境下自适应的能力。
[0043] 如图2所示，本实施例包括以下步骤：
[0044] 步骤1：参数初始化。初始化四足机器人迈步长度执行矩阵QL和迈步高度执行矩阵QH。一般初始化两矩阵为全0矩阵。
[0045] 步骤2：通过传感器采集外部环境信息。通过红外传感器和
超声波传感器测得的四足机器人前足到障碍物前端的距离L和视觉传感器测得的障碍物高度H。
[0046] 步骤3：确定隶属度函数 和真值αlk(t)。四足机器人的控制器接收步骤2中获得的距离L。通过模糊推理算法获得每条迈步长度的激活约束条件所对应的隶属度函数和真值αlk(t)。每条迈步长度的模糊约束条件表示为：
[0047] if
[0048] ……
[0049] then
[0050] 其中xi为模糊推理的系统的输入，i＝(1…Ni)，Ni为输入变量所激活的约束条件j个数。 为输入语言变量。 为模糊推理的系统的输出，C 为常数。j＝(1…Nj)，Nj为模糊约束条件数。利用模糊推理算法，对测得的障碍物距离信息L进行模糊推理，得到相应的激活约束条件；通过公式(1)和公式(2)，分别获得隶属度函数 和真值αlk(t)：
[0051] 公式(1)
[0052] 公式(2)
[0053] 其中x是四足机器人前足到障碍物前端的距离，为隶属度函数中的输入变量；li＝(1…Nli)；Nli是输入变量的激活约束条件数。
[0054] 步骤4：确定四足机器人的迈步长度。采用波尔滋曼分布搜索算法获得迈步长度执行动作。对于第j条激活约束条件通过搜索算法选择局部动作 通过公式(3)和公式(4)对所有的局部动作按真值加权，得到全局动作yl(t)及相应的ql(t)值。
[0055] 公式(3)
[0056] 公式(4)
[0057] 步骤5：确定隶属度函数 和以及真值αhk(t)。四足机器人的控制器接收步骤2中获得的高度信息以及规划的迈步长度yl(t)，通过模糊推理算法获得每条迈步高度的激活约束条件对应的隶属度函数和真值。每条迈步高度模糊约束条件表示为：
[0058] if and
[0059] ……
[0060] and then
[0061] 其中 为模糊推理的系统的两个输入，i＝(1…Ni)，Ni为输入变量激活的约束条件个数。 为输入语言变量。 为模糊推理的系统的输出，fj(x1，…，xi)为x1，…，xi的线性组合。j＝(1…Nj)，Nj为模糊约束条件数。将迈步长度执行动作yl(t)和高度信息H进行模糊推理，可以得到相应的激活约束条件；通过公式(5)和公式(6)，分别获得隶属度函数 和真值αhk(t)：
[0062] 公式(5)
[0063] 公式(6)
[0064] 其中xi是隶属度函数的输入。hi＝(1…Nhi)，Nhi是输入变量激活约束条件个数。
[0065] 步骤6：确定四足机器人的迈步高度。采用波尔滋曼分布搜索算法获得迈步高度执行动作。对于第j条激活约束条件通过搜索算法选择局部动作 通过公式(7)和公式(8)对所有的局部动作按真值加权，得到全局动作yh(t)及相应的qh(t)值：
[0066] 公式(7)
[0067] 公式(8)
[0068] 步骤7：推算执行矩阵的全局动作Y(t)及相应的Q(t)值。全局动作Y(t)由两个特征量表示：四足机器人对角两足的迈步长度yl(t)和迈步高度yh(t)；由于对角步态下对角足是同步运动的，通过公式(9)和公式(10)，规划其中一个对角足的足部运动信息即可规划出全部对角足的足部运动信息。
[0069] Y(t)＝[yl(t)yh(t)] 公式(9)
[0070] Q(t)＝[ql(t)qh(t)] 公式(10)
[0071] 步骤8：确定总体误差。四足机器人执行全局动作Y(t)以后，通过公式(11)和公式(12)计算总体函数误差ΔQ(t)＝[ΔQL(t)ΔQH(t)]，包括迈步长度函数误差ΔQL(t)和迈步高度函数误差ΔQH(t)：
[0072] 公式(11)
[0073] 公式(12)
[0074] 式中： 为下一时间点激活约束条件下的迈步长度执行矩阵的最大值。 为下一时间点激活约束条件下的迈步高度执行
矩阵的最大值。β为学习率，取值范围为[0，1]，本实施例中，学习率β取0.8。γ为折扣系数，取值范围为[0，1]，本实施例中，折扣系数γ取0.1。r为强化信号，根据跨越障碍物成功与否来选择r的取值，当跨越障碍物成功时为1，失败时为0。
[0075] 步骤9：更新迈步长度和迈步高度执行矩阵。对于每一条激活的约束条件，通过公式(13)和公式(14)修正迈步长度和迈步高度的执行矩阵的评估值 包括迈步长度的评估值 和迈步高度执行矩阵的评估值
[0076] 公式(13)
[0077] 公式(14)
[0078] 将迈步长度评估值 添加至迈步长度执行矩阵中；将迈步高度执行矩阵的评估值 添加至迈步高度执行矩阵中。得到一次更新后的迈步长度和迈步高度执行矩阵。
[0079] 重复步骤3～9，反复更新迈步长度和迈步高度执行矩阵，直到迈步长度执行矩阵QL和迈步高度执行矩阵QH收敛为止。
[0080] 步骤10：对角足部运动轨迹规划。根据当前的执行动作和所测得的障碍物距离信息L的激活约束条件，查询迈步长度执行矩阵QL中对应的元素值，其中最大值对应执行动作即为下一步的迈步长度yl(t)；根据当前的执行动作、所规划的迈步长度yl(t)和障碍物高度信息H的激活约束条件，查询迈步高度执行矩阵QH中对应的元素值，其中最大值所对应的执行动作即为下一步的迈步高度yh(t)；最终获得全局动作Y(t)＝[yl(t)yh(t)]。由所述的全局动作Y(t)＝[yl(t)yh(t)]得到四足机器人对角足部落点位置信息，包括迈步起始位置、终止位置和最高点位置信息。对角足部落点位置信息经过足轨迹拟合得出对角足部运动规划信息，即把离散的对角足部落点位置信息通过拟合方法得出连续的对角足部运动规划信息。
[0081] 步骤11：对步骤3～10规划的四足机器人对角步态的稳定性判定及调整。如图3所示。通过判定四足机器人以规划步态行走时的重心位置是否在对角支撑足的连线上，判定四足机器人以规划步态行走时的稳定性。如果是稳定的，则通过对角足部规划信息确定四足机器人各个关节角，然后送入四足机器人关节控制系统。反之，通过公式(15)调整对角支撑足的支撑时间ΔT，改变迈步周期，调整四足机器人以规划步态行走时的稳定性。
[0082] 公式(15)
[0083] 其中xi、和 pi是经迈步长度和迈步高度规划的四足机器人重心的位移和速度瞬时值以及ZMP的位移和速度瞬时值，xf、 和pf、 是修正后的四足机器人重心的位移和速度瞬时值以及ZMP的位移和速度瞬时值，ω是常数。将调整后的对角足支撑时间ΔT和对角足部运动规划信息送入四足机器人控制器，确定各个关节角和迈步周期，然后通过四足机器人关节控制系统，以达到非结构化地形的自适应控制。
