MIT Cheetah3 虚拟支撑多边形

 如图所示，中心的蓝色*号是虚拟支撑多边形计算出的期望质心位置；根据仿真，此方法的效果不明显；参考文献为【MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot】；

部分代码如下，提供给算法足端位置，各个脚的接触状态和摆动状态的相位，即可得出虚拟支撑多边形

    %虚拟支持多边形会偏离了接近接触阶段结束的腿朝向即将着陆的腿。
        sigma=ones(4,1)*0.2;
    KcC=zeros(4,1);
    KcS=zeros(4,1);
    Fai=zeros(4,1);
    legnum=[2,3;4,1;1,4;3,2];
    for leg=1:4
        sq=2^0.5;
%         if abs(contactstate(leg))<0.001
        KcS(leg)=0.5*(2+erf(-swingstate(leg)/(sigma(3)*sq) ) + erf( ( swingstate(leg)-1 ) /( sigma(4)*sq ) ) );
%         else
        KcC(leg)=0.5*(  erf(  contactstate(leg) / (sigma(1)*sq) ) + erf(  ( 1-contactstate(leg) )/(sigma(2)*sq )) );
%         end
        Fai(leg)=ceil(contactstate(leg))*KcC(leg)+ceil(swingstate(leg))*KcS(leg);
    end
    E=zeros(6,4);
    Ee=zeros(3,4);
    for leg=1:4
        p=[foot_pos(:,leg),foot_pos(:,legnum(leg,1));foot_pos(:,leg),foot_pos(:,legnum(leg,2))];
        E(:,leg)=p*[Fai(leg);1-Fai(leg)];
        Ee(:,leg)= foot_pos(:,leg)*Fai(leg)+ Fai(legnum(leg,1))*E(1:3,leg)+Fai(legnum(leg,2))*E(4:6,leg);
        Ee(:,leg)=Ee(:,leg)/(Fai(leg)+Fai(legnum(leg,1))+Fai(legnum(leg,2)));
    end
    off=sum(Ee')/4;
clf;
center=sum(foot_pos')/4;
plotx=[E(1:2,1),E(4:5,2),E(1:2,2),E(4:5,4),E(1:2,4),E(4:5,3),E(1:2,3),E(4:5,1),E(1:2,1)];
plot(plotx(1,:),plotx(2,:))
hold on;
plot(Ee(1,:),Ee(2,:),'o'); %虚拟接触多边形顶点
plot(off(1),off(2),'*'); %期望质心位置
plot(foot_pos(1,:),foot_pos(2,:),'.'); %足点
plot(center(1),center(2),'o'); %足点的中心