CN107045552B - 一种基于正弦对角步态与快速查表法的四足机器人运动控制方法及控制装置 - Google Patents

Abstract

本发明涉及一种基于正弦对角步态与快速查表法的四足机器人运动控制方法及控制装置，包括：(1)结合对角步态，分析四足机器人腿部结构，建立四足机器人腿部运动数学模型；采用新型正弦步态；(2)通过求解非线性方程组得到四肢各个关节的控制率；建立一种专门面向机器人运动控制的数据库，通过快速查表法得到精确的关节开合角度，快速准确地实现足端轨迹控制。(3)设计足端轨迹控制单元与转向控制单元，以便接入其他控制系统。在通过性上，设计并采用了一种新型正弦步态，使机器人在面对复杂地形时拥有高通过性。在运行稳定性上，迈步周期截止时，摆动相水平速度为零，因此具有较高的稳定性。采用查表法进行足端轨迹控制快速、准确、稳定。

Description

一种基于正弦对角步态与快速查表法的四足机器人运动控制 方法及控制装置

技术领域

本发明涉及一种机器人运动控制方法，具体涉及一种基于正弦对角步态与快速查表法的四足机器人运动控制方法及控制装置。

背景技术

随着科学技术的飞速发展，机器人技术得到了突飞猛进的发展。机器人可以在远离人类的情况下，快速精准地完成高精度的工作。可广泛应用于抢险救灾、星球探测、军事排雷等不适宜人类生存的环境当中。而足式机器人相对于轮式或履带式机器人在非结构环境中运动具有一定的优越性。在多足步行机器人中，四足机器人结构简单，且运动稳定，在足式机器人中有着最广泛的研究与应用。但四足机器人的运动过程是一个多连杆、强耦合、时变的非线性系统，因此四足机器人的运动控制十分复杂。现有方法多为基于转矩控制的四足机器人步态控制方法，只能对机器人进行固定步长，固定转弯半径的运动控制。无法根据要求随意改变步长与转弯半径。

中国专利文献CN104267720A公开了一种四足仿生机器人的自由步态生成方法，是将四足机器人的整个自由步态按运动周期进行规划，每个运动周期分为四足支撑阶段和迈步阶段；在四足支撑阶段，机器人的四只足均处于支撑相，机器人的重心沿前进方向和前进方向的侧方向移动，在前进方向上，机器人移动当前状态允许的最大前进距离，在侧方向上，同时兼顾机器人的稳定性以及能量消耗确定移动距离；在迈步阶段，机器人的一只足处于摆动相，其余三只足处于支撑相；当某个运动周期中的摆动足接触地面时，机器人将进行下一个运动周期的运动规划。但是，该专利存在以下缺陷：计算成本较高，足端滑动易造成扰动，抗干扰能力差。

发明内容

针对现有技术的不足，本发明提供了一种基于正弦对角步态与快速查表法的四足机器人运动控制方法。

本发明还提供了上述基于正弦对角步态与快速查表法的四足机器人运动控制装置；

本发明通过分析机器人运动机理，得到四足机器人腿部运动数学模型。并在此基础上，设计并采用了一种新型正弦对角步态。与此同时，设计了一种专门面向四足机器人运动控制的数据库，能够通过快速查表法得到四足机器人的控制率。最终，设计出足端轨迹控制单元与转向控制单元以便接入其他系统。总之，本发明能够较好地实现机器人在迈步周期内的运动控制，具有广泛的应用价值与研究意义。

本发明的技术方案为：

一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，四足机器人包括躯干以及与所述躯干连接的四条腿，在四足机器人的一个迈步周期中，在同一对角线上的两条腿以相同运动方式摆动，支撑躯干并推动躯干前进的两条腿称为支撑相，而按照事先设定轨迹进行摆动的另一对角线上的两条腿称为摆动相，包括步骤如下：

(1)通过分析四足机器人腿部结构，构建四足机器人腿部运动数学模型，包括四足机器人足端位置模型：

每条腿包括大腿、小腿；四足机器人的一个迈步周期包括一个支撑相、一个摆动相，躯干坐标系中，x轴表示足端在水平方向上的位移，y轴代表足端高度，原点o为腿髋关节在地面上的投影点，躯干坐标系下足端位置模型如式(Ⅰ)所示：

式(Ⅰ)中，p_x(θ₁,θ₂)为足端水平位移与θ₁、θ₂的函数，p_z(θ₁,θ₂)为足端高度与θ₁、θ₂的函数，L₁为机器人大腿的长度，L₂为机器人小腿的长度，θ₁为机器人髋关节纵向开合角度，取值范围为0°-180°；θ₂为膝关节开合角度，取值范围为0°-180°；H为机器人躯干底端到地面的距离；

(2)采用一种新型正弦对角步态，求取支撑相足端在躯干坐标系下的运动轨迹、摆动相足端在躯干坐标系下的运动轨迹，所述新型正弦对角步态是指：四足机器人的四条腿以对角分为两组，前左腿和右后腿为一组，前右腿和后左腿为一组，两组交替作为支撑相与摆动相；支撑相推动躯干前行，摆动相向前迈步跨越障碍，这些过程均可通过足端轨迹进行解释。尤其是摆动相足端轨迹，直接决定了摆动相以何种方式进行迈步。因此，足端轨迹的选择对机器人运动控制有着十分重要的意义。

支撑相足端在躯干坐标系下的运动轨迹如式(Ⅱ)所示：

式(Ⅱ)中，p1_x(t)为支撑相足端水平位移关于时间t的函数，p1_z(t)支撑相足端高度关于时间t的函数，S为当前迈步周期步长，S_1为上一迈步周期实际步长，t为迈步周期内的时刻，T为迈步周期；

支撑相支撑躯干并推动其前行，因此，为使机器人平稳运行，支撑相足端在躯干坐标系下运动应尽量保持匀速，且在z轴方向上保持不变。在x0y平面上，支撑相足端进行匀速运动运动距离为d＝0.5*(S+S_1)。

摆动相足端在躯干坐标系下的运动轨迹如式(Ⅲ)所示：

式(Ⅲ)中，p2_x(t)为摆动相足端水平位移关于时间t的函数，p2_z(t)摆动相足端高度关于时间t的函数，h为步高，为摆动相上升过程中达到的最高点；

(3)控制率的求解

设定步长的取值范围是0.38m～0.42m，精度为1mm，则摆动相足端在躯干坐标系下的运动轨迹有41*41＝1681种可能性；

将1681种可能性对应的S_1、S带入公式(Ⅲ)，求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)；

将求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)代入公式(Ⅰ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制摆动相运动的控制率；

将1681种可能性对应的S_1、S带入公式(Ⅱ)，求得支撑相足端不同时刻的位置p1_x(t)、p1_z(t)；

将求得的支撑相足端不同时刻的位置p1_x(t)、p1_z(t)代入公式(Ⅰ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制支撑相运动的控制率；

(4)根据求取的摆动相运动的控制率、支撑相运动的控制率，建立数据库

数据库采用二维地址指针，第一维为S_1，第二维为S，每个地址存有2个2*200的控制矩阵，分别为支撑相关节控制率与摆动相关节控制率；

(5)通过快速查表法，实现对四足机器人的足端轨迹控制

A、求取S与S_1；

B、根据S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

C、将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动；

(6)对四足机器人进行转向控制

求取支撑相髋关节横向开合角的控制率、摆动相髋关节横向开合角的控制率，并传递给相应腿组，令其依照控制率变化，实现四足机器人的转向控制。

根据本发明优选的，所述步骤(6)，对四足机器人进行转向控制，包括步骤如下：

a、获取四足机器人前一迈步周期结束时刻的躯干朝向y_θ_1、理想情况下迈步周期结束时躯干的朝向θ；

b、通过式(Ⅳ)求得躯干的转向角度Δθ，式(Ⅳ)如下所示：

θ＝y_θ_1+Δθ (Ⅳ)

c、在机器人运动过程中，躯干的转向是由支撑相在躯干坐标系下向相反的方向转动相同的角度而实现的。因此，在已知目标转向角度的前提下，就可得到支撑相髋关节横向开合角的控制率，通过式(Ⅴ)求得支撑相髋关节横向开合角的控制率θ₄：

d、摆动相的胯关节横向开合角要回正，准备状态时，y_θ_1＝y_θ_2＝0，对式(Ⅵ)进行迭代运算求得摆动相的回正角度为Δθ_1，大小为当前躯干朝向y_θ_1与前一周期初始时刻躯干朝向y_θ_2的差：

Δθ_1＝y_θ_1-y_θ_2 (Ⅵ)

e、通过式(Ⅶ)求得摆动相髋关节横向开合角的控制率θ1₄：

f、通过控制率分配，将信号传递给相应的腿组，令其依照控制率变化，实现四足机器人的转向控制。

相较于足端轨迹控制，转向控制较为简单，控制率可直接通过方程求得，仅需将控制率分配给相应的腿组即可精确地实现转向控制与摆动相的回正控制。

根据本发明优选的，所述四足机器人运动控制方法，还包括：

当机器人在复杂地面运动时，在迈步结束时，摆动相足端与地面可能会随机发生相对滑动。这会影响机器人运动的稳定性。在迈步周期结束后T/10内，足端轨迹控制单元与转向控制单元协同工作调整机器人姿态，假设随机发生的扰动大小为(Δp_x,Δp_y)，(Δp_x,Δp_y)是指相较于目标落点的偏差量，包括：

作为摆动相的腿贴着地面向前挪动，在调整姿态的同时，把能耗降到最低，步长为Δr，通过式(Ⅷ)求取：

与此同时，支撑相将机器人躯干向前推进

同时髋关节横向开合角改变Δθ₂，通过式(Ⅸ)求取：

经过上述调整，虽然机器人躯干的朝向与重心位置都可能发生变化。但左右腿髋关节连线得以保持在支撑相足端落点与摆动相足端落点的正中间，且同一腿组内的腿姿态相同。这样可使运动过程拥有极高的稳定性。

根据本发明优选的，所述式(Ⅰ)的推导过程如下所示：

机器人在一个迈步周期内的运动是由机器人的躯干朝向θ、本周期步长S与前一周期实际步长S_1共同决定的，在水平方向运动的数学模型如式(Ⅹ)所示：

机器人的躯干朝向与步长需要通过控制四肢的各个关节才能控制，因此需要进一步对四足机器人腿部结构与其运动方式进行分析。

假设，机器人足端与地面不发生相对滑动，且机器人躯干不发生形变，则机器人躯干的运动则可由足端位置的变化与躯干角度共同进行解释，通过式(Ⅺ)求取机器人足端位置与L₁、L₂、θ₁、θ₂、小腿与铅垂线所成的角度θ₃的关系：

通过分析几何关系，θ₃由θ₁、θ₂表示，如式(Ⅻ)所示：

θ₃＝π-θ₁-θ₂(Ⅻ)

将式(Ⅻ)带入式(Ⅺ)化简得到足端位置的数学模型，即(Ⅰ)。

根据本发明优选的，所述式(Ⅲ)的推导过程如下所示：

支撑相推动躯干前行，摆动相向前迈步跨越障碍，这些过程均可通过足端轨迹进行解释。尤其是摆动相足端轨迹，直接决定了摆动相以何种方式进行迈步。因此，足端轨迹的选择对机器人运动控制有着十分重要的意义。

摆动相依据规划好的足端轨迹进行摆动。为使摆动相运动过程平顺、节能，步态切换过程连贯、稳定，并且具有较好的通过性，摆动相足端在躯干坐标系下的运动轨迹需满足以下具体约束条件，具体约束条件如下：

①躯干坐标系下，在x方向上，周期开始时，即t＝0时，p_x为上一周期步长的一半，方向为负；周期结束时，即t＝T时，p_y为该周期步长的一半，方向为正，转化为数学约束条件，如式(XIII)所示：

②为使得机器人在面对复杂地形时，有较好的通过性，摆动相的足端在摆动过程中先升高再降低，开始与结束时足端都应与地面接触，高度为0；而上升过程中达到的最高点高度为h，称为步高，转化为数学约束条件如式(XIV)所示：

③由于摆动相在摆动结束时会再次与地面接触，如果能够消除足端与地面水平方向上相互作用所产生的力，便可大大提高运动的连贯性与稳定性。本发明提出，摆动相结束时刻，水平方向位置的一阶导数为0，即消除了足端与地面水平方向上相互作用所产生的力；与此同时，为满足步态切换过程中的连贯性与稳定性要求，摆动相在结束时刻需满足水平方向位置的二阶导数小于0；将上述要求转化为数学约束条件如式(XV)所示：

结合式(XIII)、(XIV)、(XV)所述的约束条件，推导出一种满足上述要求的摆动相足端轨迹，如式(Ⅲ)所示。

一种基于正弦对角步态与快速查表法的四足机器人运动控制装置，包括足端轨迹控制单元、转向控制单元；

所述足端轨迹控制单元用于完成以下操作：

所述足端轨迹控制单元接收机器人上一周期的运动距离U_r_1与本周期的指导运动距离dr，运动开始时，四足机器人会将姿态调整为准备姿态，此时对应的S_1为0.4m，对式(XVI)进行迭代运算可求得S与S_1：

式(XVI)中，S_2为倒数第二个周期的步长；

根据求取的S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动；

所述转向控制单元用于求取支撑相髋关节横向开合角的控制率、摆动相髋关节横向开合角的控制率，并传递给相应腿组，令其依照控制率变化，实现四足机器人的转向控制。

本发明的有益效果为：

1、在通过性方面，由于设计并采用了一种新型正弦步态，使机器人在面对复杂地形时拥有高通过性。

2、在运行稳定性方面，迈步周期截止时，摆动相水平速度为零，因此具有较高的稳定性。

3、本发明采用查表法进行足端轨迹控制快速准确，高速、精确、稳定，计算成本更低。

4、正常的步态周期结束后，本发明在可通过足端轨迹控制单元与转向控制单元协同工作，调整机器人姿态，以应对足端滑动所造成的扰动，大大增强了四足机器人步态控制系统的抗干扰能力。

附图说明

图1是四足机器人的躯干与腿部结构示意图；

图2是理想情况下摆动相足端轨迹示意图；

图3是摆动相可能出现的足端轨迹；

图4是足端轨迹控制单元控制示意图；

图5是足端轨迹控制单元控制效果图；

图6是转向控制单元控制示意图。

具体实施方式

下面结合说明书附图和实施例对本发明作进一步限定，但不限于此。

实施例1

一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，四足机器人包括躯干以及与所述躯干连接的四条腿，在四足机器人的一个迈步周期中，在同一对角线上的两条腿以相同运动方式摆动，支撑躯干并推动躯干前进的两条腿称为支撑相，而按照事先设定轨迹进行摆动的另一对角线上的两条腿称为摆动相，包括步骤如下：

(1)通过分析四足机器人腿部结构，构建四足机器人腿部运动数学模型，包括四足机器人足端位置模型：

每条腿包括大腿、小腿；四足机器人的一个迈步周期包括一个支撑相、一个摆动相，躯干坐标系中，x轴表示足端在水平方向上的位移，y轴代表足端高度，原点o为腿髋关节在地面上的投影点，躯干坐标系下足端位置模型如式(Ⅰ)所示：

式(Ⅰ)中，p_x(θ₁,θ₂)为足端水平位移与θ₁、θ₂的函数，p_z(θ₁,θ₂)为足端高度与θ₁、θ₂的函数，L₁为机器人大腿的长度，L₂为机器人小腿的长度，θ₁为机器人髋关节纵向开合角度，取值范围为0°-180°；θ₂为膝关节开合角度，取值范围为0°-180°；H为机器人躯干底端到地面的距离；图1是四足机器人的躯干与腿部结构示意图，四足机器人主要由躯干与四条可以灵活摆动的腿组成，L₁＝L₂＝300mm；H由静止状态下的髋关节纵向开合角度与膝关节开合角度决定，H＝519.6mm。

(2)采用一种新型正弦对角步态，求取支撑相足端在躯干坐标系下的运动轨迹、摆动相足端在躯干坐标系下的运动轨迹，所述新型正弦对角步态是指：四足机器人的四条腿以对角分为两组，前左腿和右后腿为一组，前右腿和后左腿为一组，同一组采用相同方式运动，两组交替作为支撑相与摆动相；支撑相推动躯干前行，摆动相向前迈步跨越障碍，这些过程均可通过足端轨迹进行解释。尤其是摆动相足端轨迹，直接决定了摆动相以何种方式进行迈步。因此，足端轨迹的选择对机器人运动控制有着十分重要的意义。

支撑相足端在躯干坐标系下的运动轨迹如式(Ⅱ)所示：

式(Ⅱ)中，p1_x(t)为支撑相足端水平位移关于时间t的函数，p1_z(t)支撑相足端高度关于时间t的函数，S为当前迈步周期步长，S_1为上一迈步周期实际步长，t为迈步周期内的时刻，T为迈步周期；

支撑相支撑躯干并推动其前行，因此，为使机器人平稳运行，支撑相足端在躯干坐标系下运动应尽量保持匀速，且在z轴方向上保持不变。在x0y平面上，支撑相足端进行匀速运动运动距离为d＝0.5*(S+S_1)。

摆动相足端在躯干坐标系下的运动轨迹如式(Ⅲ)所示：

式(Ⅲ)中，p2_x(t)为摆动相足端水平位移关于时间t的函数，p2_z(t)摆动相足端高度关于时间t的函数，h为步高，为摆动相上升过程中达到的最高点；S＝S_1＝0.4m，其足端轨迹如图2所示；

(3)控制率的求解

设定步长的取值范围是0.38m～0.42m，精度为1mm，则摆动相足端在躯干坐标系下的运动轨迹有41*41＝1681种可能性；如图3所示。

将1681种可能性对应的S_1、S带入公式(Ⅲ)，求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)；

将求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)代入公式(Ⅰ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制摆动相运动的控制率；

将1681种可能性对应的S_1、S带入公式(Ⅱ)，求得支撑相足端不同时刻的位置p1_x(t)、p1_z(t)；

将求得的支撑相足端不同时刻的位置p1_x(t)、p1_z(t)代入公式(Ⅰ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制支撑相运动的控制率；

(4)根据求取的摆动相运动的控制率、支撑相运动的控制率，建立数据库

数据库采用二维地址指针，第一维为S_1，第二维为S，每个地址存有2个2*200的控制矩阵，分别为支撑相关节控制率与摆动相关节控制率；

(5)通过快速查表法，实现对四足机器人的足端轨迹控制

A、求取S与S_1；

B、根据S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

C、将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动；

(6)对四足机器人进行转向控制

求取支撑相髋关节横向开合角的控制率、摆动相髋关节横向开合角的控制率，并传递给相应腿组，令其依照控制率变化，实现四足机器人的转向控制。

步骤(6)，对四足机器人进行转向控制，包括步骤如下：

a、获取四足机器人前一迈步周期结束时刻的躯干朝向y_θ_1、理想情况下迈步周期结束时躯干的朝向θ；

b、通过式(Ⅳ)求得躯干的转向角度Δθ，式(Ⅳ)如下所示：

θ＝y_θ_1+Δθ (Ⅳ)

c、在机器人运动过程中，躯干的转向是由支撑相在躯干坐标系下向相反的方向转动相同的角度而实现的。因此，在已知目标转向角度的前提下，就可得到支撑相髋关节横向开合角的控制率，通过式(Ⅴ)求得支撑相髋关节横向开合角的控制率θ₄：

d、摆动相的胯关节横向开合角要回正，准备状态时，y_θ_1＝y_θ_2＝0，对式(Ⅵ)进行迭代运算求得摆动相的回正角度为Δθ_1，大小为当前躯干朝向y_θ_1与前一周期初始时刻躯干朝向y_θ_2的差：

Δθ_1＝y_θ_1-y_θ_2 (Ⅵ)

e、通过式(Ⅶ)求得摆动相髋关节横向开合角的控制率θ1₄：

f、通过控制率分配，将信号传递给相应的腿组，令其依照控制率变化，实现四足机器人的转向控制。

相较于足端轨迹控制，转向控制较为简单，控制率可直接通过方程求得，仅需将控制率分配给相应的腿组即可精确地实现转向控制与摆动相的回正控制。

四足机器人运动控制方法，还包括：

当机器人在复杂地面运动时，在迈步结束时，摆动相足端与地面可能会随机发生相对滑动。这会影响机器人运动的稳定性。在迈步周期结束后T/10内，足端轨迹控制单元与转向控制单元协同工作调整机器人姿态，假设随机发生的扰动大小为(Δp_x,Δp_y)，(Δp_x,Δp_y)是指相较于目标落点的偏差量，包括：

作为摆动相的腿贴着地面向前挪动，在调整姿态的同时，把能耗降到最低，步长为Δr，通过式(Ⅷ)求取：

与此同时，支撑相将机器人躯干向前推进

同时髋关节横向开合角改变Δθ₂，通过式(Ⅸ)求取：

经过上述调整，虽然机器人躯干的朝向与重心位置都可能发生变化。但左右腿髋关节连线得以保持在支撑相足端落点与摆动相足端落点的正中间，且同一腿组内的腿姿态相同。这样可使运动过程拥有极高的稳定性。

式(Ⅰ)的推导过程如下所示：

机器人在一个迈步周期内的运动是由机器人的躯干朝向θ、本周期步长S与前一周期实际步长S_1共同决定的，在水平方向运动的数学模型如式(Ⅹ)所示：

机器人的躯干朝向与步长需要通过控制四肢的各个关节才能控制，因此需要进一步对四足机器人腿部结构与其运动方式进行分析。

假设，机器人足端与地面不发生相对滑动，且机器人躯干不发生形变，则机器人躯干的运动则可由足端位置的变化与躯干角度共同进行解释，通过式(Ⅺ)求取机器人足端位置与L₁、L₂、θ₁、θ₂、小腿与铅垂线所成的角度θ₃的关系：

通过分析几何关系，θ₃由θ₁、θ₂表示，如式(Ⅻ)所示：

θ₃＝π-θ₁-θ₂(Ⅻ)

将式(Ⅻ)带入式(Ⅺ)化简得到足端位置的数学模型，即(Ⅰ)。

式(Ⅲ)的推导过程如下所示：

支撑相推动躯干前行，摆动相向前迈步跨越障碍，这些过程均可通过足端轨迹进行解释。尤其是摆动相足端轨迹，直接决定了摆动相以何种方式进行迈步。因此，足端轨迹的选择对机器人运动控制有着十分重要的意义。

摆动相依据规划好的足端轨迹进行摆动。为使摆动相运动过程平顺、节能，步态切换过程连贯、稳定，并且具有较好的通过性，摆动相足端在躯干坐标系下的运动轨迹需满足以下具体约束条件，

具体约束条件如下：

①躯干坐标系下，在x方向上，周期开始时，即t＝0时，p_x为上一周期步长的一半，方向为负；周期结束时，即t＝T时，p_y为该周期步长的一半，方向为正，转化为数学约束条件，如式(XIII)所示：

②为使得机器人在面对复杂地形时，有较好的通过性，摆动相的足端在摆动过程中先升高再降低，开始与结束时足端都应与地面接触，高度为0；而上升过程中达到的最高点高度为h，称为步高，转化为数学约束条件如式(XIV)所示：

③由于摆动相在摆动结束时会再次与地面接触，如果能够消除足端与地面水平方向上相互作用所产生的力，便可大大提高运动的连贯性与稳定性。本发明提出，摆动相结束时刻，水平方向位置的一阶导数为0，即消除了足端与地面水平方向上相互作用所产生的力；与此同时，为满足步态切换过程中的连贯性与稳定性要求，摆动相在结束时刻需满足水平方向位置的二阶导数小于0；将上述要求转化为数学约束条件如式(XV)所示：

结合式(XIII)、(XIV)、(XV)所述的约束条件，推导出一种满足上述要求的摆动相足端轨迹，如式(Ⅲ)所示。

传统方法是采用正逆运动学推导，需要复杂的公式推导才能得到各个关节的开合角度与足端位置的关系。本发明查表法通过软件计算得到每个足端位置对应的关节角度并建立数据库，使用时，运算复杂度低，实时性好。

实施例2

一种基于正弦对角步态与快速查表法的四足机器人运动控制装置，包括足端轨迹控制单元、转向控制单元；图4是足端轨迹控制单元控制示意图；

所述足端轨迹控制单元用于完成以下操作：

所述足端轨迹控制单元接收机器人上一周期的运动距离U_r_1与本周期的指导运动距离dr，运动开始时，四足机器人会将姿态调整为准备姿态，此时对应的S_1为0.4m，对式(XVI)进行迭代运算可求得S与S_1：

式(XVI)中，S_2为倒数第二个周期的步长；

根据求取的S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动；图5是足端轨迹控制单元控制效果图；由图5可知，控制效果十分理想，足端轨迹控制单元能够精确地实现支撑相控制与摆动相控制。

所述转向控制单元用于求取支撑相髋关节横向开合角的控制率、摆动相髋关节横向开合角的控制率，并传递给相应腿组，令其依照控制率变化，实现四足机器人的转向控制。相较于足端轨迹控制，转向控制较为简单，控制率可直接通过方程求得，仅需将控制率分配给相应的腿组即可精确地实现转向控制与摆动相的回正控制。图6是转向控制单元控制示意图。

Claims (6)

1.一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，其特征在于，四足机器人包括躯干以及与所述躯干连接的四条腿，在四足机器人的一个迈步周期中，在同一对角线上的两条腿以相同运动方式摆动，支撑躯干并推动躯干前进的两条腿称为支撑相，而按照事先设定轨迹进行摆动的另一对角线上的两条腿称为摆动相，包括步骤如下：

(1)通过分析四足机器人腿部结构，构建四足机器人腿部运动数学模型，包括四足机器人足端位置模型：

每条腿包括大腿、小腿；四足机器人的一个迈步周期包括一个支撑相、一个摆动相，躯干坐标系中，x轴表示足端在水平方向上的位移，y轴代表足端高度，原点o为腿髋关节在地面上的投影点，躯干坐标系下足端位置模型如式(Ⅰ)所示：

式(Ⅰ)中，p_x(θ₁,θ₂)为足端水平位移与θ₁、θ₂的函数，p_z(θ₁,θ₂)为足端高度与θ₁、θ₂的函数，L₁为机器人大腿的长度，L₂为机器人小腿的长度，θ₁为机器人髋关节纵向开合角度，取值范围为0°-180°；θ₂为膝关节开合角度，取值范围为0°-180°；H为机器人躯干底端到地面的距离；

(2)采用一种正弦对角步态，求取支撑相足端在躯干坐标系下的运动轨迹、摆动相足端在躯干坐标系下的运动轨迹，所述正弦对角步态是指：四足机器人的四条腿以对角分为两组，前左腿和右后腿为一组，前右腿和后左腿为一组，两组交替作为支撑相与摆动相；支撑相足端在躯干坐标系下的运动轨迹如式(Ⅱ)所示：

式(Ⅱ)中，p1_x(t)为支撑相足端水平位移关于时间t的函数，p1_z(t)为支撑相足端高度关于时间t的函数，S为当前迈步周期步长，S_1为上一迈步周期实际步长，t为迈步周期内的时刻，T为迈步周期；

摆动相足端在躯干坐标系下的运动轨迹如式(Ⅲ)所示：

式(Ⅲ)中，p2_x(t)为摆动相足端水平位移关于时间t的函数，p2_z(t)为摆动相足端高度关于时间t的函数，h为步高，为摆动相上升过程中达到的最高点；

(3)控制率的求解

设定步长的取值范围是0.38m～0.42m，精度为1mm，则摆动相足端在躯干坐标系下的运动轨迹有41*41＝1681种可能性；

将1681种可能性对应的S_1、S带入公式(Ⅲ)，求得摆动相足端不同时刻的位置p2_x(t)、p2_z(t)；

将求得摆动相足端不同时刻的位置p2_x(t)、p2₂(t)代入公式(Ⅰ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制摆动相运动的控制率；

将1681种可能性对应的S_1、S带入公式(Ⅱ)，求得支撑相足端不同时刻的位置p1_x(t)、p1_z(t)；

将求得的支撑相足端不同时刻的位置p1_x(t)、p1_z(t)代入公式(Ⅰ)中的p_x(θ₁,θ₂)、p_z(θ₁,θ₂)，得到一非线性方程组，通过MATLAB求解该非线性方程组，并依据关节约束条件筛选得到符合关节约束条件的髋关节纵向开合角度与膝关节开合角度，作为控制支撑相运动的控制率；

(4)根据求取的摆动相运动的控制率、支撑相运动的控制率，建立数据库

数据库采用二维地址指针，第一维为S_1，第二维为S，每个地址存有2个2*200的控制矩阵，分别为支撑相关节控制率与摆动相关节控制率；

(5)通过快速查表法，实现对四足机器人的足端轨迹控制

A、求取S与S_1；

B、根据S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

C、将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动；

(6)对四足机器人进行转向控制

求取支撑相髋关节横向开合角的控制率、摆动相髋关节横向开合角的控制率，并传递给相应腿组，令其依照控制率变化，实现四足机器人的转向控制。

2.根据权利要求1所述的一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，其特征在于，所述步骤(6)，对四足机器人进行转向控制，包括步骤如下：

a、获取四足机器人前一迈步周期结束时刻的躯干朝向y_θ_1、理想情况下迈步周期结束时躯干的朝向θ；

b、通过式(IV)求得躯干的转向角度Δθ，式(IV)如下所示：

θ＝y_θ_1+Δθ (IV)

c、在机器人运动过程中，躯干的转向是由支撑相在躯干坐标系下向相反的方向转动相同的角度而实现的；因此，在已知目标转向角度的前提下，就得到支撑相髋关节横向开合角的控制率，通过式(Ⅴ)求得支撑相髋关节横向开合角的控制率θ₄：

d、摆动相的胯关节横向开合角要回正，准备状态时，y_θ_1＝y_θ_2＝0，对式(Ⅵ)进行迭代运算求得摆动相的回正角度为Δθ_1，大小为当前躯干朝向y_θ_1与前一周期初始时刻躯干朝向y_θ_2的差：

Δθ_1＝y_θ_1-y_θ_2 (Ⅵ)

e、通过式(Ⅶ)求得摆动相髋关节横向开合角的控制率θ1₄：

f、通过控制率分配，将信号传递给相应的腿组，令其依照控制率变化，实现四足机器人的转向控制。

3.根据权利要求1所述的一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，其特征在于，所述四足机器人运动控制方法，还包括：

当机器人在复杂地面运动时，在迈步周期结束后T/10内，假设随机发生的扰动大小为(Δp₄,Δp_y)，(Δp₄,Δp_y)是指相较于目标落点的偏差量，包括：

作为摆动相的腿贴着地面向前挪动，步长为Δr，通过式(Ⅷ)求取：

与此同时，支撑相将机器人躯干向前推进

同时髋关节横向开合角改变Δθ₂，通过式(Ⅸ)求取：

4.根据权利要求1所述的一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，其特征在于，所述式(Ⅰ)的推导过程如下所示：

机器人在一个迈步周期内的运动是由机器人的躯干朝向θ、本周期步长S与前一周期实际步长S_1共同决定的，在水平方向运动的数学模型如式(Ⅹ)所示：

通过式(XI)求取机器人足端位置与L₁、L₂、θ₁、θ₂、小腿与铅垂线所成的角度θ₃的关系：

通过分析几何关系，θ₃由θ₁、θ₂表示，如式(Ⅻ)所示：

θ₃＝π-θ₁-θ₂ (Ⅻ)

将式(Ⅻ)带入式(XI)化简得到足端位置的数学模型，即(Ⅰ)。

5.根据权利要求1所述的一种基于正弦对角步态与快速查表法的四足机器人运动控制方法，其特征在于，所述式(Ⅲ)的推导过程如下所示：摆动相足端在躯干坐标系下的运动轨迹需满足以下具体约束条件，具体约束条件如下：

①躯干坐标系下，在x方向上，周期开始时，即t＝0时，p_x为上一周期步长的一半，方向为负；周期结束时，即t＝T时，p_x为该周期步长的一半，方向为正，转化为数学约束条件，如式(XIII)所示：

②摆动相的足端在摆动过程中先升高再降低，开始与结束时足端都应与地面接触，高度为0；而上升过程中达到的最高点高度为h，称为步高，转化为数学约束条件如式(XIV)所示：

③摆动相结束时刻，水平方向位置的一阶导数为0，即消除了足端与地面水平方向上相互作用所产生的力；与此同时，摆动相在结束时刻需满足水平方向位置的二阶导数小于0；将上述要求转化为数学约束条件如式(XV)所示：

结合式(XIII)、(XIV)、(XV)所述的约束条件，推导出一种满足上述要求的摆动相足端轨迹，如式(Ⅲ)所示。

6.一种实现权利要求1所述的一种基于正弦对角步态与快速查表法的四足机器人运动控制方法的控制装置，其特征在于，包括足端轨迹控制单元、转向控制单元；

所述足端轨迹控制单元用于完成以下操作：

所述足端轨迹控制单元接收机器人上一周期的运动距离U_r_1与本周期的指导运动距离dr，运动开始时，四足机器人将姿态调整为准备姿态，此时对应的S_1为0.4m，对式(XVI)进行迭代运算可求得S与S_1：

式(XVI)中，S_2为倒数第二个周期的步长；

根据求取的S_1与S，进行快速查表，从数据库中得到存有支撑相关节控制率的支撑相控制矩阵与存有摆动相关节控制率的摆动相控制矩阵；

将支撑相控制矩阵传递给支撑相，将摆动相控制矩阵传递给摆动相，依照相应的控制率运动；

所述转向控制单元用于求取支撑相髋关节横向开合角的控制率、摆动相髋关节横向开合角的控制率，并传递给相应腿组，令其依照控制率变化，实现四足机器人的转向控制。

Images