0 前言

在波士顿动力机器人中，最大的困难是控制腿式机器人的稳定性和移动能力，而计算机控制的单腿弹跳杆将是研究控制和平衡的很好的项目。在《解构波士顿动力机器人（四）》一文的腿式机器人历史研究中提到，腿式机器人的主动平衡阶段是以“单阶倒立摆”的研究为发展起点的。为系统学习，本文将从单阶倒立摆的数学模型、系统控制器出发解析倒立摆的平衡控制问题，用于后续对“单腿弹跳杆”的分析基础。

图1 单腿弹跳杆实物图

1 数学模型

单级倒立摆系统（Simple Inverted Pendulum System）是由倒立摆和小车两部分组成，如图2所示。小车依靠直流电动机施加控制力，可以在导轨上左右移动，其控制目标是在有限长导轨上使倒立摆能够稳定竖立在小车上而不倒，达到动态平衡^[1]。图中F为施加于小车的水平方向的作用力，φ 是摆杆的倾斜角。若不给小车施加控制力，摆杆会左右倾斜，控制的过程即当摆杆出现偏角，在水平方向给小车以作用力，通过小车的水平运动，使摆杆保持在垂直位置，意即控制系统的状态参数，以保持倒立摆的倒立稳定。

图2 倒立摆模型

倒立摆系统由6大部分组成，并构成一个闭环结构，包括计算机、数据采集卡、电源及功率放大器、直流伺服电机、倒立摆本体和两个光电编码器等模块，为了建立倒立摆系统的数学模型，先作出如下假设：

• 倒立摆与摆杆均为匀质刚体；
• 各部分的摩擦力（力矩）与相对速度（角速度）成正比；
• 施加在小车上的驱动力与加在功率放大器上的输入电压成正比；
• 皮带轮与传送带之间无滑动，传送带无伸长状态；
• 信号与力的传递无延时；
• 忽略空气阻力。

1.1 Newton-Euler法

牛顿-欧拉法是指牛顿的平移运动受力分析，与欧拉方程的描述刚体旋转运动时刚体所受外力矩与角加速度的关系式，一起写出作为牛顿-欧拉方程，表1给出来下述分析所用符号的含义及拟定数值。

表1 牛顿力学法符号含义

对系统中的摆杆和小车分别进行受力分析，如图3。其中，N和P为小车与摆杆相互作用力的水平和垂直方向的分量。 以顺时针方向为矢量方向，且角速度、角加速度都按同一方向为矢量方向。

图3 倒立摆模型受力分析

　　分析小车水平方向所受的力，可以得到以下方程：

　　由摆杆水平方向的受力进行分析，摆杆水平方向的合外力为N，则可以得到运动微分方程：

    合并二者，得系统的第一个运动方程：

　　对摆杆垂直方向上的受力进行分析，可以得到下面方程：

 　　以摆杆质心为基点，得摆杆的力矩平衡方程如下：

     系统的第二个运动方程为：

　　线性化后两个运动方程如下：

　　因为摆体绕支点的转动惯量I与摆体绕质心转动惯量Ic关系为

       则运动方程可写为：

1.2 Newton力学法

　　对系统中的摆杆和小车分别进行受力分析，如图4。其中，N和P为小车与摆杆相互作用力的水平和垂直方向的分量。

图4 倒立摆模型受力分析

　　分析小车水平方向所受的力，可以得到以下方程：

　　由摆杆水平方向的受力进行分析，摆杆水平方向需要保持平衡，故合外力为0，则可以得到平衡方程：

　　对摆杆垂直方向上的受力进行分析，可以得到下面方程：

 　　以摆杆质心为基点，得摆杆的力矩平衡方程如下：

　　则可以进行近似处理，线性化后两个运动方程如下：

1.3 Lagrange方程法

　　Lagrange 方程是以能量观点建立的运动方程式，需要从两个方面分析，一个是表征系统运动的动力学量——系统的动能和势能，另一个是表征主动力作用的动力学量——广义力。表2给出来下述分析所用符号的含义及拟定数值。

表2 拉格朗日方程法符号含义

　　由n个关节部件组成的机械系统，其Lagrange方程应为：

　　其中，q为系统的广义坐标，表示系统中线位移和角度的变量；T为倒立摆系统的动能，V为倒立摆系统的势能，D为倒立摆系统中的耗散能。

　　由图5分析，可以得出，小车和摆杆的各部分能量表达式为：

图5 单级倒立摆分析图

　　对于小车：

　　对于摆杆：

　　对于倒立摆系统，由式（13）和式（14），得：

　　对小车而言，由式（15），得：

　　对摆杆而言，由式（15），得：

　　根据上面各式综合，可以得到单级倒立摆系统动力学方程为：

2 系统控制方程

2.1 微分方程

2.2 传递函数

令初始条件为零，对于输入为作用力F，输出为摆杆角度时，对微分方程进行拉普拉斯变换，得到：

由于力F为输入，角度 φ为输出，求解以上方程组，得传递函数：

2.3 状态方程

由现代控制理论，控制系统的状态空间方程

令

则

式中 ，

故状态空间方程为：

整理如下：

3 控制器设计

3.1 稳定性分析

利用MATLAB工具表达传递函数

clc
%闭环系统传递函数建立
%输入倒立摆传递函数 G（e）=num/den
M=0.5;
m=0.2;
b=0.1;
c=0.1;
I=0.006;
g=9.8;
l=0.3;
%传递函数分子分母系数分母用F，分子用Z表示
F1=(I+m*l^2)*(M+m)+m^2*l^2;
F2=(I+m*l^2)*b+(M+m)*c;
F3=b*c-(M+m)*m*g*l;
F4=m*g*l*b;
Z1=m*l;
%传递函数
num=[Z1 0 0];
den=[F1 F2 F3 F4 0];
G=tf(num,den)

对其进行分析系统的极点以判断稳定性

clc
%知传递函数分析脉冲响应
num=[0.06 0 0];
den=[0.0204 0.0724 -0.4016 0.0588 0];
G=tf(num,den)
%稳定性分析
p=eig(G);%求系统的特征根
p1=pole(G);%求系统的极点
r=roots(den);%求系统的特征方程的根
disp('系统极点')
disp(r)
a=find(real(r)>0);%查找特征方程跟实部大于0的值
b=length(a);
if b>0
    disp('系统不稳定');
else
    disp('系统稳定');
end

运行结果为：

系统极点：

        0

   -6.5986

    2.8989

    0.1507

有两个极点位于右半平面：系统不稳定

对系统进行开环脉冲响应分析（给系统一个脉冲推力），

clc
%知传递函数分析脉冲响应
num=[0.06 0 0];
den=[0.0204 0.0724 -0.4016 0.0588 0];
G=tf(num,den)
%脉冲响应绘制图线
t=0:0.005:5;
impulse(num,den,t)
axis([0 2 0 60]);

得到结果如图6所示

图6 系统开环脉冲响应

3.2 PID控制器设计

PID控制器的传递函数为：

构成闭环系统的传递函数为：

筛选合适的PID参数，利用MATLAB中的PID Tuner仿真，当：参数为表4时达到较好的性能，相应的性能指标见表5

表4 较佳参数值

表5 性能指标

用MATLAB求得最后传递函数

clc
%知传递函数分析脉冲响应
num=[0.06 0 0];
den=[0.0204 0.0724 -0.4016 0.0588 0];
G=tf(num,den)
%PID控制器参数
Kp=23.161;
Ki=36.4217;
Kd=3.0075;
%PID控制器传递函数
pnum=[Kd Kp Ki];
pden=[1 0];
KD=tf(pnum,pden)
%系统传递函数
xnum=conv(num,pnum);
xden=polyadd(conv(pden,den),conv(pnum,num));
Gx=tf(xnum,xden)

3.3 闭环系统稳定性分析

（1）极点判断稳定性

clc
%传递函数
num=[0.1804 1.39 2.185 0 0];
den=[0.0204 0.2528 0.9881 2.244 0 0];
G=tf(num,den);
%稳定性分析
p=eig(G);%求系统的特征根
p1=pole(G);%求系统的极点
r=roots(den);%求系统的特征方程的根
disp('系统极点')
disp(r)
a=find(real(r)>0);%查找特征方程跟实部大于0的值
b=length(a);
if b>0
    disp('系统不稳定');
else
    disp('系统稳定');
end

（2）时域分析

clc
%传递函数
num=[0.1804 1.39 2.185 0 0];
den=[0.0204 0.2528 0.9881 2.244 0 0];
G0=tf(num,den)
[y,t]=step(G0);%返回系统时域响应曲线
C=dcgain(G0) %系统终值
%峰值计算时间
[max_y,k]=max(y);
peak_time=t(k)
%超调量计算
max_overshoot=100*(max_y-C)/C
%上升时间
r1=1;
while(y(r1)<0.1*C)
    r1=r1+1;
end
r2=1;
while(y(r2)<0.9*C)
    r2=r2+1;
end
rise_time=t(r2)-t(r1)
%调整时间计算
s=length(t);
while (y(s)>0.98*C&&y(s)<1.02*C)
    s=s-1;
end
settling_time=t(s)

step(G0)
    

（3）频域分析

clc
%传递函数
num=[0.1804 1.39 2.185 0 0];
den=[0.0204 0.2528 0.9881 2.244 0 0];
G=tf(num,den);
%绘制伯德图
w=logspace(-2,3,100);%选取10^-2到10^3的频率点
[mag,phase,w]=bode(num,den,w)%获取幅值、相位参数
magdB=20*log10(mag);
subplot(211);
semilogx(w,magdB);
title('幅频曲线')
xlabel('频率（HZ）')
ylabel('幅值')
grid
subplot(212)
semilogx(w,phase);
title('相频曲线')
xlabel('频率（HZ）')
ylabel('相位')
grid

4 参考文献

[1] Raibert M H ,  Tello E R . Legged Robots That Balance[J]. IEEE Expert, 1986, 1(4):89-89.

[2] Rygg L A . Mechanical horse: US, US491927 A[P].1893-2-14.

[3]郝丽娜,刘兴刚.计算机仿真技术及CAD.[M].高等教育出版社.2009