双足行走机器人的步态分析

分析两轮小车的控制设计，做出如下双足机器人控制方案设计。

1两轮小车的运动控制设计

小车两轮之间的距离为2R，车轮半径为r,小车可围绕中心旋转。

小车的速度为V，方向为φ，目标点对小车产生一个力的作用(力的综合效应),经过时间dt，产生一个速度dV，小车的速度为V'，方向为φ',则小车的角速度为ω=(φ‘-φ)/dt

小车的左右轮目标速度为：

VR=V'+Rω

VL=V'-Rω

小车的左右轮的目标位移为：

SR=(V'+Rω)dt

SL=(V'-Rω)dt

两轮不打滑的情况下，其目标角位移为：

θR=(V'+Rω)dt/r

θL=(V'-Rω)dt/r

2双足机器人的运动控制

双足机器人的两足可以将其简化为两个轮子，不过与小车不同的是小车的两个轮子一直连续运动，而双足机器人是两足交替运动，一足运动，一足静止，一组支撑，一组向前运动。

3多足机器人的运动控制:

每一组足简化为一个轮子，如四足机器人也是交替的，一般是采用对角线换步法，对角线的方向脚同时离地，同时落地

六足的蚂蚁一侧的前足、后足和另一侧的中足为一组，交替运动；

八足的蜘蛛也有类似的规律，一组支持，一组向前运动，运动的一组左右两边看成是轮子向前运动

分好组以后，按照关键帧法，设定腿部的运动，采用路径跟踪法（steering behavior）

从上述分析来看，所有的关节运动机器人的行走控制都可以简化为两轮小车的运动控制

上述内容属于2层分析决策的内容，2层小车（蚂蚁，蜘蛛等，以下简称智能体）简化为一个质点，分析了环境，经过分析后决定智能体采用seek的方式运动至目标点，设定了控制目标(左右轮的目标速度等);3层根据控制目标分析，将目标转化为动作。

在2层智能体分析运动时，可能要考虑障碍等情况，在3层智能体也要考虑障碍等情况，不过是有所不同。一般要用到所谓的包围体。

智能体一般用一个包围体来粗略描述，通常采用比较简单的几何形状，如球体或盒体。在简化智能体的时候一般选取其包围体中的某个点来简化智能体，如采用球体(圆)的圆心等。2层分析环境时通常用包围体代替智能体来分析，并通过地图"膨胀"的方式来避免智能体与障碍物碰撞。

理想状态下，包围体应该紧密拟合其所包围的对象，但是在2层这是没有必要的，计算的工作量也比较大。

在2层，当前智能体不会与其包围体外界的物体发生碰撞，预测的下一步动作，智能体也不会与其包围体外界物体发生碰撞，也就是说当前位置的包围体所在空间S1与目标包围体所在空间S2的并集构成了一个空间S，在这个空间中智能体可以任意动作，都是不会与为外界发生碰撞的。

S空间为智能体的动作空间，为简化计算，一般可认为S1空间为智能体的动作空间(应该适当放大，否则有可能发生碰撞)。

在3层，智能体用另一个相对紧密拟合的包围体，其动作限定在动作空间中，分析要完成当前的动作。比如2层决策向前方某点以某个速度移动，但是在当前的动作空间内，通过分析，智能体发现当前是斜坡，要考虑是否有关节发生碰撞等，这都需要一个相对紧密拟合的包围体。

除了包围体的不同，从上述分析中也可以看出2层和3层所需要处理的场景也是不同的，2层是全局的，粗略的，3层是局部的，精细的。

前文中分析双足机器人或多足机器人的行走都可以简化为两轮小车，通过控制小车的左右轮速度，可以控制小车的直线运动，或曲线运动，知道轨迹 或运动要求，小车的行走是相对容易控制的，那么下面一个问题就是如何把左右轮的运动控制转换为腿部的运动呢？另外我们发现当小车静止时，其左右轮的速度为0，如果让双足机器人也是静止的话，其假想的左右轮转速也应该为0，其腿部的是静止时，其不运动；但是双足机器人是可以在原地踏步的，在原地踏步时双足并非静止，所以是否可以假设双足机器人的腿部运动是有两部分构成的，一部分是由双足机器人的整体速度，角速度决定（假想的左右轮），而另一部分是相当于其在原地踏步？双足机器人的腿部运动是这两种效应的综合结果？

ps：两轮小车就是平衡车的问题了，那么在运动中还有平衡的问题.一般的双驱小车可能有一个导向轮，这样一来的话就不需要考虑车体平衡的问题，是否要加一个“虚拟导向轮”以便简化控制？---三点为什么是自平衡的？四个点为什么会破坏平衡？踩高跷只是两点为什么可以（动态）平衡？自行车为什么可以保持平衡？不倒翁是怎么回事（一点自平衡）？有新的元素参与了平衡？

假设这两种运动都可以用圆周运动等效（小车的左右轮就是圆周运动）

1轮运动：根据智能体的目标速度就可以设定其速度，除此之外，这个假设的轮的半径会影响轮的转速，可以这样认为，轮子半径小，转速快，这就是“小步快跑”；轮子半径大就是“大步流星”

小车的轮子在转动时，其轴心（或回转中心）速度就是设定的速度。是否能这样考虑：当这个轴心的速度为0时，对于双足机器人而言，双足机器人不运动，但是腿部可能运动；原地踏步运动在一个周期内对轴心的影响为0。

双足机器人的两脚分别为支撑脚和游脚，假设将游脚放在这个假想的轮子上，轮子带动游脚运动，这是一个合理的假设，那么这时候游脚的运动轨迹是什么？

如果没有滑动的话，游脚的轨迹是摆线：

z=r*(1-cosθ)

x=r*(θ-sinθ)

其中r是假想的轮子的半径，θ是轮子滚动的角度

但是考虑双足机器人的原地踏步时，在一个周期内,x要从0变为0，所以又做如下变换：令θ=πsinΦ

之所以幅度为π是考虑到要让Φ能在一个周期内变化,采用正弦的原因是让Φ和θ同步变化，则有：

z=r*(1-cosπsinΦ)

x=r*(πsinΦ-sinπsinΦ)

当Φ匀速变化时，z和x的变换规律如下：

其中绿色代表x轴方向，蓝色代表z轴方向

在奔跑时，该规律似乎可用，在原地踏步时，x的位置应该没有负数，所以该变换继续修改为：θ=-π/2*cosΦ+π/2

则有:

z=r*(1-sin(π/2*cosΦ))

x=r*(π/2-π/2*cosΦ-cos(π/2*cosΦ))

当Φ匀速变化时，z和x的变换规律如下：

此时能够保证原地踏步时虚拟圆做匀速圆周运动，游脚轨迹为摆线

但是采用这种曲线是没有办法统一跑和原地踏步的，角度的变化只在0~π之间

那么是否可以说：双足机器人的运动就是上述两种运动的叠加？

定义一个综合角：

θ=k1*πsinΦ+k2(-π/2*cosΦ+π/2)

其中k1,k2为0~1

当k1=0,k2=1时机器人静止

当k1=0,k2=0时机器人原地踏步

当k1=0,k2=1时机器人奔跑

当k1=k2=1时机器人发疯

而这样一来，由设定的虚拟轮的转速，需要设定的参数有：r，Φ，k1,k2

r体现了步幅，Φ体现了步频，k1,k2体现了步态

仅仅从虚拟轮的转速是无法k1,k2的

能够想到的是人在运动时，大腿小腿，及虚拟圆构成一个四连杆机构

 图中有3个圆：

大圆代表运动的速度，当其转速为0时，表示机器人不前进或转动，其半径为大腿和小腿的长度之和

蓝色小圆代表游脚移动时做摆线运动时的滚动圆，其半径与步幅有关，当原地踏步时，其退化为上下活动的滑块

红色虚拟圆：控制腿部的运动，其与大腿构成一个曲柄摇杆机构

从图中可以看出1)系统一共有两个曲柄摇杆机构;2)大圆的线速度与小圆的线速相同

另外可以看出一旦设定好红色圆的半径及位置，设定好小圆的半径（或限定小圆为滑块），那么代表腿部的两个两杆的运动规律就确定了，也就是说其设计腿部运动主要就是设计红色控制圆的大小及位置。

这里将红色圆设定为驱动圆，半径记为R;小圆为游动圆，半径记为r。

根据机械学的知识

1曲柄存在的条件：

a最短杆与最长杆之和不大于其余两杆之和

b曲杆是最短杆

2对于铰链四连杆机构,当存在曲轴时，只有在曲柄为连架杆时,系统才为曲柄摇杆机构。

可以看出，当代表大腿的杆运动范围确定时，驱动圆的解是有无数个的，而游动圆也有无数个，这和我们的直觉是一致的：同样的速度，要不就是步频快一些，要不就是步幅大一些（是不是在这里选择解的时候用到模糊算法？）但是这里也是有些限制的，主要是各个关节的弯曲程度是其约束。

我们从前面的分析中看到，2层给出的是左右两轮的目标速度，那么最后这个速度最好设计成与驱动轮的位置和半径大小有非常简单的关系，如果驱动圆的半径是不变的，或只有位置变化，那就太好了。我们在控制时可以将其想象为一个电机，并且最好是驱动圆匀速运动时，游动圆也能匀速运动。

进一步分析发现驱动轮最好是位于胯关节的正前方，因为大腿必然有一个位置，即直立于地面的，所以图形改变如下：

进一步的分析有发现，这种结构大腿是无法抬高到与地面平行，甚至是腿向上倾斜的，要实现向上倾斜，驱动圆位置还要向上移动，其位置应该在大腿向上倾斜时的极限位置的那条线上，在目前的情况下，机器人大腿抬起的角度不大，就不考虑了，从这里也可以看出，驱动圆的位置决定了大腿抬起的极限位置。（并且也可以猜想出大腿向上倾斜时，可以假想为地面进行了旋转，下面的结论也是可用的，后记）

 另外这其中是两组四连杆机构，通过大腿这一个中间环节，且都是曲柄摇杆，末端都为曲杆，很自然的有一种想法：这两组两杆能设计为相似的吗？如果相似，则驱动圆与游动圆运动规律相似，那么设计计算的工作量要小很多，这要做进一步的分析。

首先假设两者是一样的，如下图所示，并将各个杆用颜色标记。

此图为一个正方形，在图中补充了两个四边形，相对的四边形是全等的，左下角的蓝色代表的是游动圆所代表的曲杆，右上角的蓝色代表驱动圆所代表的曲杆，很容易证明出这两条线是平行的，它们与垂直方向的夹角相同，也就是说驱动圆与游动圆同相。在进一步的，可以做一个相似四边形，将驱动圆标准化，其运动规律与游动圆是相同的，其角速度与游动圆相同。

好像是有问题：分析以后又画了上图， 作图过程如下：首先最左边四连杆，然后复制旋转画最优边四连杆，接着连接上下两根紫色线条，发现上下两根紫色线段有可能是不等于左右两根紫色线段的，那么上述结论就有可能不成立。后来发现这种担心是不成立的，因为上述作图过程并没有考虑四连杆机构为曲柄摇杆机构的条件，下面做分析：

上图中左边的四连杆机构中红色代表大腿，长度记为L，紫色代表小腿，长度记为l,蓝色代表游动圆半径，其长度为r，而黑色代表的是"机架",因为要能够直立在地面所以其长度为Len=L+l-r，在做示意图时显然没有做到。

另外当l≥r时，最长杆就为机架，最短杆为游动圆，且有最短杆与最长杆之和等于其余两杆之和，即结构中存在曲柄；当l<r时，游动圆不再是曲柄了。

上述是非常重要的结论，也是设计时的重要前提。

按照大小腿的比例1.62:1，以游脚与垂直方向的角度为参考了，游脚长度从0-1变化（小腿规范化为1），应该得出一个曲面，下面考察一下，各个角度定义如下：

下图是当r=0.6时，α、β分别随θ的变化规律：

 

 
 当r为其他值时，均有类似的变化规律。下图为r，α、β三者之间的规律视图：

图中X轴代表r，Y轴代表β，Z轴代表α（r在0-1之间，范围太小，图中r放大了360倍）上述数据采用下列代码计算：

#include<iostream>
#include<cmath>
#include<stdio.h>
#include<stdlib.h>
const double PI=3.1415926;
using namespace std;
int main(int argc,char *argv[])
{
    double r=1;
    if(argc>1) r=atof(argv[1]);
    double r_2=r*r;
    double upperLeg=1.62,lowerLeg=1,upperLeg_2=upperLeg*upperLeg,lowerLeg_2=lowerLeg*lowerLeg;
    double lg=0,lg_2;
    double theta=0,beta=0,alpha=0;
    double Len=upperLeg+lowerLeg-r,Len_2=Len*Len;
    cout<<"r*360,theta,beta,alpha\n";
    double i=0;
    for (i=1.0;i>0.01;i-=0.01){
       r=i;r_2=i*i;Len=upperLeg+lowerLeg-r,Len_2=Len*Len;
       for(theta=0;theta<=360;theta+=5) {
       lg_2=Len_2+r_2-2*Len*r*cos(PI/180*theta);
       lg=sqrt(lg_2);
 
      beta=acos((upperLeg_2+lowerLeg_2-lg_2)/(2*upperLeg*lowerLeg));
      alpha=acos((upperLeg_2+lg_2-lowerLeg_2)/(2*upperLeg*lg))+acos((lg_2+Len_2-r_2)/(2*lg*Len));
      if(theta>180){
        beta=2*PI-beta;
        alpha=-alpha;
     }
     cout<<r*360<<"  ,  ";//<<theta<<" , ";
     cout<<beta*180/PI<<" , "<<alpha*180/PI<<endl;       
  }   
 }

图形分别有wps和meshlab绘制。

从图中也可以看出r越大，α的极限也越大，β的极限也越大，这也就是步子要拉大，两腿要尽量分开也是一致的。

且当 θ=180度时，α=0度，β=180度，这也是符合直觉的。

β可能计算有问题，不应该有大于180度的，是不是下面的关系：

后面再研究吧。

现在控制规律基本理清了：

根据目标到智能体的距离等设定一个速度V(steering behavior方法），然后根据V由模糊算法确定一个r，这个r也是虚拟圆的半径，这个虚拟圆也代表游动圆和驱动圆，驱动圆当前的角位移就是β，根据驱动圆（虚拟圆）的半径r和目标速度V，可以算出其角速度，然后可以算出更新时间内，驱动圆的目标角位移，进而算出α、β的目标角位移，根据目标角位移然后采用steering behavior方法将其等效为一个力矩，外界的障碍、关节的传递等也等效为一个力矩，实现避障等功能，比较复杂！
 

比如说根据原先设定的目标，足部将与一个斜坡碰撞，穿过斜坡了，按照steering beha vior控制的思想，这应该产生一个作用力，力的方向垂直于作用面，大小与预计穿越的大小成正比，这个力有一个力矩作用，将使足部旋转避免碰撞，真实杆的运动就是在目标力矩和阻力矩的共同作用下的结果。

理清楚了下面就是要干了。

------------------------------------------------------------------后记---------------------------------------------------------------------------------

我好像还是把双足机器人的腿部动作想简单了： β不可能大于180度。前半周期和后半周期也不是对称的。

四连杆机构中一个是主动杆，它的运动带动其他四个杆运动，而在腿部运动的这个虚拟四连杆机构中，主动杆是有两个的(大腿和小腿),其中的曲杆还是虚拟的。

现在的分析有些像是在模拟人骑独轮车。

下面的程序代码是新的：

#include<iostream>
#include<cmath>
#include<stdio.h>
#include<stdlib.h>
const double PI=3.1415926;
using namespace std;
int main(int argc,char *argv[])
{
   double r=1;
    if(argc>1) r=atof(argv[1]);
    double r_2=r*r;
    double upperLeg=1.62,lowerLeg=1,upperLeg_2=upperLeg*upperLeg,lowerLeg_2=lowerLeg*lowerLeg;
    double lg=0,lg_2;
    double theta=0,beta=0,alpha=0;
   double Len=upperLeg+lowerLeg-r,Len_2=Len*Len;
    cout<<"theta,beta,alpha\n";
    for(theta=0;theta<=360;theta+=5){
       lg_2=Len_2+r_2-2*Len*r*cos(PI/180*theta);
       lg=sqrt(lg_2);
       beta=acos((upperLeg_2+lowerLeg_2-lg_2)/(2*upperLeg*lowerLeg)); 
       if(theta<180){
          alpha=acos((upperLeg_2+lg_2-lowerLeg_2)/(2*upperLeg*lg))+acos((lg_2+Len_2-r_2)/(2*lg*Len)); 
        }
       else{
          alpha=acos((upperLeg_2+lg_2-lowerLeg_2)/(2*upperLeg*lg))-acos((lg_2+Len_2-r_2)/(2*lg*Len)); 
}
       cout<<theta<<"  ,  ";
       cout<<beta*180/PI<<" , "<<alpha*180/PI<<endl;       
    }
}

主要的改动是当θ>180度时虚拟四连杆机构变成反向四连杆机构，保证膝关节不大于180度。

下面是新的是当r=0.6时，α、β分别随θ的变化规律：

 

儿子说第三张图像鲨鱼的鱼鳍，很形象。

但是还有一个问题就是 α 有负值的，我认为要重新考虑其意义，该参数表示与地面垂直方向的夹角，在现实生活中出现负值时，一般这时人的身体会前倾，我认为不如该参数的意义改为与上身之间的夹角，当身体前倾时相当于虚拟圆下移，在人体行走时人的重心是有起伏的，不如用此来模拟。

 

 

2018.8.19

理想很美好，现实很骨感，做动作仿真实验，结果动作做得一塌糊涂！

NAO的大腿是100，小腿102.9，脚45.19，按上面所做，动作像是抽风！

2018.8.25

实验是首先实验一条腿，然后是两条腿，进一步是两条腿的协调运动。

按照上一篇进行实验，效果奇差无比，实验的过程是首先确定脚的位置，然后根据本篇的方法，确定各个关节的角度。

改进的是设想的是机器人在骑自行车，然后方法相同（脚不是摆线运动了，是圆周运动），效果改善。

进一步的改进是左脚先完成动作，等待，然后右脚动作，等待；效果好多了，但还是骑自行车的动作，效果进一步改善。

而且在加大r时会出现错误，同时膝关节理想的状况是r越大，角度越大，实验结果并非如此。

试验了原地踏步，效果还行；很好玩！

有一个猜想：出左腿，右腿支撑，则右臂向前，左臂向后，胯部向右；反之，出右腿，左腿支撑，则左臂向前，右臂向后，胯部向左；抬腿胯部后仰，压腿胯部前倾。

2021.3.2

看了波士顿动力的双足机器人控制，真是大开眼界！很多东西关键是观念和思维误区的突破！很多东西就在眼前，就看你是不是视而不见了！复杂的总归要回归简单！

2022.3.19

下面是两段机器人的仿真视频，机器人的模型是从网络上找到的，仿真平台是自搭的

机器人仿真实验

机器人仿真