在降维解耦框架中可知采用VMC虚拟模型方法可以将四足机器人本体控制简化为对虚拟刚体高度、速度和姿态的独立控制,通过构建PD控制器模拟虚拟弹簧刚度产生期望的伺服力与力矩最终通过力分配的方式将控制量映射到支撑腿上,并进一步使用虚功原理采用雅克比矩阵将力与关节力矩进行映射。该方法在顶层控制中仅关心对刚体的控制不明确具体的机器人构型因此可以扩展到车、无人机、双足等不同的机器人中,而面向最终机器人构型的主要就是力分配环节。

如前文所述解耦步态控制框架中位姿控制力矩主要依靠支撑腿足地作用产生,通过静力学分配可将该虚拟扭矩向物理腿分解,假设控制周期很高则可以将整个机器人看做一个刚体,分析力平衡是仅需要考虑支撑力臂与力矢量朝向即可因此可构建如下的静力学平衡等式:

上式可以在很多足式机器人运动控制论文中看到如MIT猎豹那篇经典的论文中。上式实际就是一个典型的求解线性方程组问题或者最小二乘问题,仅对求取Ax=b来说如果矩阵A满秩则上述可直接求逆得到对应的解析解x=inv(A)b,,而还可以转化为求取最小二乘问题,当然A满秩同样可以得到最优解,但在四足机器人静力学支撑公式中矩阵A不满秩,即给定期望3维力和3维力矩结合当前支撑腿位置仅能构建6个等式方程,而由于足式机器人每个支撑腿末端力矢量都有3个自由度变量需要求解,上式中已知条件不足。要求解上述的超定方程除了简化支撑情况之前文章中提到在Trot步态两腿支撑下假设前向力已知的方式实现,还可以使用二次线QP优化的方法得到,这也是在MIT 使用MPC之前所采用的Balance控制器的方法,该方法除了能不抛弃某自由度分解外还可以增加力幅值、摩擦圆锥等约束条件。综上传统解耦控制方法即构建PD反馈控制率求取机体速度、机体姿态所需的虚拟力与虚拟力矩:

则进一步采用QP优化的方法来对上述控制量进行优化分解,我们首先仅考虑二维矢量平面上的力分配问题,当前该例子同样可以应用于无人机某一轴的姿态控制中,假设我们考虑机器人前后两虚拟腿着地,希望给定机体力矩与力向支撑腿进行分解,已知机器的姿态Pitch角,质心在全局坐标系下的位置以及各虚拟腿在机体坐标系下的位置,则该问题的示意图如下:

如上图所示的二维矢量平面力分配问题,我们仅在全局坐标系{n}下进行分配,最终使用姿态旋转矩阵将其转换至跨关节{h}坐标系下再使用雅克比矩阵映射关节力矩,则我们给定在机体质心上的虚拟力Fd和虚拟力矩Td,希望求取能产生与他们最接近结果的足底前腿力Ff={Fx,Fz}和后退力Fh={Fx,Fz},则依据静力学平衡可以得到如下力平衡公式:

和如下力矩平衡公式:

则可以构建如下的方程:

则要求解x可以构建如下的最优化问题 即最小化等式两边的误差,那下面只需要将其转换为QP优化所需要的形式:

则其可以化简为:

即:

其中最后一项为常数项不影响最小化结果可以忽略,则与QP优化公式对应可以得到所需矩阵:

进一步可以增加力幅值与摩擦圆锥的约束,首先限制最大最小力,对X方向来说设定其正负限幅值有:

对Z方向力来说足底只能提供正的支撑力因此:

而对于摩擦圆锥来说即给定地面摩擦因子,可以构建

的约束,通过相关论文可知可以通过计算法向量与圆锥三角向量的角度建立简单的线性约束:

则重新整理上述约束可以构建QP优化中的不等式约束:

重写为:

则可以得到所需的不等式约束矩阵为:

则对于X方向和Z方向最大值限制可以使用如matlab优化函数中quadprog的lb和ub来进行限制,则运行QP优化后既可以得到所需的足底力。下面我们使用matlab的优化库来进行数值仿真验证,验证方法为给定期望虚拟伺服期望力和力矩通过上述的QP优化过程求解足底力之后在使用足底力重新计算在质心的力和力矩进行对比,同时可以使用之前提到的静力学分配增加假设的方式与QP优化结果进行对比,仿真代码如下:

function qptest1
clc;
clear all;
close all;
axix_range=0.2;
f_draw_scale=0.03;
t_draw_scale=0.3;
length=0.1;

pit=5;%姿态角

fd_n=[3.5,3];%期望力
td_n=-0.1;%期望扭矩

pc_n=[0,0.06];%全局质心位置
pf_h=[0.0225,0];%机体下足位置
ph_h=[-0.015,0];

fd_n1=[5,11];
td_n1=-0.5;
fd_n2=[5,11];
td_n2=-0.5;
ff_n1=[0.1,2];
hf_n1=[0.1,2];
ff_n2=[0.1,2];
hf_n2=[0.1,2];

ankle_fn(1)=pc_n(1)+cosd(pit)*length;%跨关节全局位置
ankle_fn(2)=pc_n(2)+sind(pit)*length;

ankle_hn(1)=pc_n(1)-cosd(pit)*length;
ankle_hn(2)=pc_n(2)-sind(pit)*length;

pf_n(1)=ankle_fn(1)+pf_h(1);%全局足位置
pf_n(2)=0;

ph_n(1)=ankle_hn(1)+ph_h(1);
ph_n(2)=0;
%欧拉力分配
%设计前向力
ff_n1(1)=fd_n(1)*0.5;
hf_n1(1)=fd_n(1)*0.5;
A1=[1,                         1;
pf_n(1)-pc_n(1),        ph_n(1)-pc_n(1)];
B1=[fd_n(2);
    td_n-ankle_fn(2)*ff_n1(1)-ankle_hn(2)*hf_n1(1)
];
Fz=inv(A1)*B1;
ff_n1(2)=Fz(1);
hf_n1(2)=Fz(2);


%QP优化
A2=[1,              0,                   1,            0;
    0,              1,                   0,            1;
   ankle_fn(2),   (pf_n(1)-pc_n(1)),   ankle_hn(2), (ph_n(1)-pc_n(1)) ];
B2=[fd_n(1);
    fd_n(2);
    td_n];

%min (Af-B)'S(Af-b)  -->  1/2f'Hf+F'f
%无约束 
w_x=0.1;
w_z=0.1;
w_att= 0.05;
S= [w_x,  0,  0;
    0,  w_z,  0;
    0,  0,  w_att];%控制权重矩阵
if 0%无权重
    H = 2*A2'*A2;
    f = -2*A2'*B2;
else
    H = 2*A2'*S*A2;
    f = -2*A2'*S'*B2;
    f = (-2*B2'*S*A2)';
end
u=0.35;
uf=2^0.5*u/2;
uh=2^0.5*u/2 ;
%计算摩擦圆锥 fx^2+ fy^2《 (ufz)^2    => fx<=u'fz fy<=u'fz u'=sqrt(2)u/2
%使用正方形近似约束圆锥
%
%  fx<=u'fz  转换为不等式约束
%  fx-u'fz<=0 -> Ax<=b
%圆锥约束
% x1<u'x2   
% x3<u'x4
% x2>0 =>  -x2<0
if(1)
A=[1, -uf,  0,  0; %1构建fz fx不等式关系
   0,  0,    1, -uh;
   0,  -1,   0   ,0;
   0,  0,    0   ,-1];
B=[0;0;0;0];
else
A=[1, -uf,  0,  0; %1构建fz fx不等式关系
   0,  0,    1, -uh;];
B=[0;0];    
end
%力幅值约束 
ub = [4;  8; 4; 8];%上约束  F  H
lb = [-4; 0.1; -4; 0.1];%下约束
[x,fval,exitflag,output,lambda] = quadprog(H,f,A,B,[],[],lb,ub)    %[],[]为Aeq beq为等式约束
%[x,fval,exitflag,output,lambda] = quadprog(H,f,A,B,[],[],[],[]);    %[],[]为Aeq beq为等式约束
%[x,fval,exitflag,output,lambda] = quadprog(H,f,[],[],[],[],[],[]); 
ff_n2(1)=x(1);%/X
ff_n2(2)=x(2);%Z
hf_n2(1)=x(3);
hf_n2(2)=x(4);

ff_n2
hf_n2
%反向解算  使用足底力重新合成机体力与扭矩
%力分配
ffu=ff_n1;
hfu=hf_n1;
if(ffu(2)<0)
    ffu(2)=0;
end
if(hfu(2)<0)
    hfu(2)=0;
end
A=[1,              0,                   1,            0;
    0,              1,                   0,            1;
   ankle_fn(1),   pf_n(1)-pc_n(1),   ankle_hn(1), ph_n(1)-pc_n(1) ];
X1=[ffu(1);
    ffu(2);
    hfu(1);
    hfu(2)];
B1=A*X1;
fd_n1=[B1(1),B1(2)];
td_n1=B1(3);

%QP
ffu=ff_n2;
hfu=hf_n2;
if(ffu(2)<0)
    ffu(2)=0;
end
if(hfu(2)<0)
    hfu(2)=0;
end
X2=[ffu(1);
    ffu(2);
    hfu(1);
    hfu(2)];
B2=A*X2;
fd_n2=[B2(1),B2(2)];
td_n2=B2(3);
%
figure
%绘制地形
hold on;
line([-axix_range,axix_range],[0,0],'LineWidth',2);
grid on;
axis equal;
%绘制机体
line([ankle_fn(1),ankle_hn(1)],[ankle_fn(2),ankle_hn(2)],'LineWidth',1.5);
hold on;
plot(ankle_fn(1),ankle_fn(2),'o','MarkerSize',4,'LineWidth',4);
hold on;
plot(ankle_hn(1),ankle_hn(2),'o','MarkerSize',4,'LineWidth',4);
hold on;
plot(pc_n(1),pc_n(2),'o','MarkerSize',6,'LineWidth',4);
hold on;
draw_force(pc_n+[0,0],fd_n,'blue',f_draw_scale);
draw_force(pc_n+[0.01,0],fd_n1,'red',f_draw_scale);
draw_force(pc_n+[-0.01,0],fd_n2,'green',f_draw_scale);
%绘制腿
line([ankle_fn(1),pf_n(1)],[ankle_fn(2),pf_n(2)],'LineWidth',1.5);
hold on;
line([ankle_hn(1),ph_n(1)],[ankle_hn(2),ph_n(2)],'LineWidth',1.5);
hold on;
draw_force(ph_n,hf_n1,'red',f_draw_scale);
draw_force(pf_n,ff_n1,'red',f_draw_scale);

draw_force(ph_n,hf_n2,'green',f_draw_scale);
draw_force(pf_n,ff_n2,'green',f_draw_scale);

draw_torque(pc_n+[0.21,0],td_n,'blue',t_draw_scale);
draw_torque(pc_n+[0.24,0],td_n1,'red',t_draw_scale);
draw_torque(pc_n+[0.26,0],td_n2,'green',t_draw_scale);
%绘制摩擦锥
end

function draw_force(pn,fn,color,f_draw_scale)
line([pn(1),pn(1)+fn(1)*f_draw_scale],[pn(2),pn(2)+fn(2)*f_draw_scale],'Color',color,'LineWidth',1.5);
hold on;
end

function draw_torque(pn,tn,color,t_draw_scale)
line([pn(1),pn(1)],[pn(2),pn(2)+tn*t_draw_scale],'Color',color,'LineWidth',1.5);
hold on;
end

function A=fan_matrix2X2(pi,pc)


end

下面给出两个仿真例子一个是Z方向不会产生负向力的例子:

可以看到图中红色线为假设法产生的足底力,绿色为QP优化的结果,在机体质心蓝色为实际设定的力红绿为足底力重新解算的结果,图右边为力矩分配结果,可以看到两方法均能产生较合理的分配结果,最终反向计算结果基本与设定值相符,当但期望力矩会产生负向Z方力或者需要约束摩擦圆锥和力幅值时QP优化方法有更合理的结果,如下图所示:

从图中可以看到为产生期望力和力矩,假设法在前腿产生了负Z力这在实际情况中是无法实现的而QP优化的方法能产生更合理的结果,下面再提供一个X方向不在摩擦圆锥的例子:

上图中可以看到为分配力,直接法在前腿产生了不满足摩擦圆锥的力,这在实际情况中会出现打滑难以保证负载输出,而QP优化保证了摩擦圆锥虽然最终逆解算结果与设定值有所偏差,但在实际反馈控制中只要逆解算结果符号不反向就能保证反馈控制的执行,相关参考文献下载地址为(提取码:q6xt):