基于反步法的云台非线性控制器

Introduction

云台的pitch、yaw两轴并不独立:pitch抬起后yaw轴的等效转动惯量会改变,yaw高速转动时pitch会受到离心力矩,两轴同时转动时yaw还会受到科里奥利耦合力矩,再加上pitch轴始终存在的重力矩。
传统的串级PID把这些都当成扰动,只能被动抑制。本文在云台动力学模型的基础上,用反步法(Backstepping)设计非线性控制器,把重力项、离心力项和耦合项作为前馈直接抵消,使两轴的误差动态解耦且可证明稳定。

理论推导

动力学模型

由云台动力学解算可得两轴的力矩方程:

重力项惯性项离心力项
惯性项科里奥利耦合项

其中运动学关系为


为pitch角,为yaw角,、分别为pitch、yaw电机的输出力矩

两个方程中的方括号是同一个关于的函数,为书写方便,记


其中pitch部分绕yaw轴的转动惯量随pitch角变化:


离心力项和科里奥利耦合项都来源于此。对求导可得


这正是拉格朗日方程给出的结构:pitch方程中的系数为,yaw方程中的系数为

控制律中需要每个周期按当前的pitch角重新计算

状态空间方程

设


则


即


控制目标是让角度跟踪目标值: ,

反步法设计

系统是两条”角度→角速度→力矩”的积分链,控制量只出现在角速度的方程中。反步法的思路是从离控制量最远的状态开始,逐级把下一级状态当作虚拟控制量来设计

第一步:角度误差

设角度误差


取李雅普诺夫函数


显然正定(PD),求导


令


则负定(ND),为系统的渐近稳定点

注:、是状态而不是控制量,不能直接”令”它们取某个值。这里实际是在给出虚拟控制量的期望值、,上面的结论只在、时成立,第二步正是要处理的情况

记虚拟控制量


问题变为,

第二步:角速度误差

设角速度误差


取李雅普诺夫函数


显然正定。注意此时,第一步中的要重新计算。由、得


本身没有变化,仍然正定;失去负定性的只是,多出的交叉项留到这一步处理

交叉项不能省略:若第一步的精确成立,则,第二步就没有必要了

于是


令


其中、用于抵消交叉项,则


对任意正的增益都负定,为系统的渐近稳定点

若省去、,则中残留,只有在时才负定。例如,在处

控制律

虚拟控制量的导数为


代入:


整理得


同理,代入:


整理得


最终得到控制律:



方括号内是期望的角加速度,乘以转动惯量得到产生该角加速度所需的力矩;方括号外的重力项、离心力项和耦合项则是前馈,用来抵消模型中已知的非线性

可改进点

以下内容尚未并入正文,正文中与之冲突的说法以本节为准

控制律的等价形式与调参

记角度误差,则,。正文的可以改写为


闭环误差动态为


可见这个反步控制器等价于计算力矩法:前馈抵消非线性后,剩下的就是一个带加速度前馈的PD控制器,等效,。这给出了调参的物理意义:

  • 特征方程的判别式为,因此时为过阻尼或临界阻尼,否则为欠阻尼。特别地,时极点为,总带有一点振荡
  • 极点不能超过电机电流环带宽和控制频率允许的范围,否则离散化与电流环延迟会让实际系统振荡

    多出的”“来自中角度误差和角速度误差的权重都取了,二者单位不同。可以取(),此时令即可消去交叉项,特征方程变为。越大,极点越接近、

目标轨迹的导数

控制律需要、、、。目标角来自遥控器或自瞄,直接差分两次会把噪声放大到不可用:

  • 自瞄: 目标观测器(如EKF)的状态中通常已经包含目标的速度和加速度,可以由预测模型直接解析得到目标角的一二阶导数
  • 遥控器: 用跟踪微分器(TD)或轨迹规划对目标角做平滑,同时输出其一二阶导数
  • 实在拿不到时取,只保留速度前馈,代价是跟踪加速运动的目标时有滞后

yaw角误差过零处理

yaw轴可以无限旋转,在处跳变。必须归一化到,否则目标从变到时云台会反向转一整圈。pitch轴有机械限位,不存在这个问题

角速度的坐标系

模型中、是关节角速度,而陀螺仪测得的是其安装刚体坐标系下的角速度。若IMU装在pitch上,按的转动顺序有


因此应取,,而不是直接用(具体符号取决于各轴正方向的约定)。pitch角较大时,二者的差别不可忽略

模型误差与扰动

控制律完全依赖模型抵消非线性,而模型参数(、、各转动惯量)只能近似得到,且没有考虑摩擦:

  • 重力补偿有误差时,pitch会出现稳态误差,而纯PD结构无法消除。可以在上加积分项(积分反步法),或用扩张状态观测器(ESO)估计总扰动后补偿
  • 重力矩系数是主要的不确定参数,也可以用自适应反步法在线估计
  • 小陀螺时底盘相对云台高速旋转,yaw轴的轴承和电机摩擦取决于相对转速,会持续拖拽云台。可以在中加入摩擦前馈,参数通过实验辨识

执行器饱和

李雅普诺夫证明默认、能被完全执行。电机电流有上限,大误差时控制量饱和,抵消不再精确,稳定性证明也不再成立。实际使用时:

  • 目标角经过轨迹规划后再送入控制器,避免大阶跃
  • 饱和期间冻结积分项(若加了积分)
  • 更系统的做法是指令滤波反步法(command filtered backstepping),把饱和与虚拟控制量求导一并处理

改进后的实现

综合正文控制律与以上几点(yaw角过零处理、输出限幅)的控制器:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
typedef struct {
float J_Ay, J_z0, J_Qperp, J_Qpar, m, l_A, l_y, g; // J_z0 = J_Cz + J_Bz
float k1, k2, k3, k4;
float tau_y_max, tau_z_max;
} GimbalParam;

static float wrap_pi(float a)
{
while (a > PI) a -= 2.0f * PI;
while (a <= -PI) a += 2.0f * PI;
return a;
}

// 耦合项 Gamma(theta)
static float coupling(const GimbalParam *p, float th)
{
float s = sinf(th), c = cosf(th);
return (p->J_Qperp - p->J_Qpar) * s * c + p->m * p->l_A * (p->l_y + p->l_A * c) * s;
}

// yaw轴转动惯量 J_z(theta) = J_Cz + J_Bz + J_Az(theta)
static float yaw_inertia(const GimbalParam *p, float th)
{
float s = sinf(th), c = cosf(th), r = p->l_y + p->l_A * c;
return p->J_z0 + p->J_Qperp * c * c + p->J_Qpar * s * s + p->m * r * r;
}

// th_t, dth_t, ddth_t: pitch目标角及其一二阶导数; psi_t, dpsi_t, ddpsi_t 同理
// th, w_y, psi, w_z: 反馈的关节角与关节角速度(见「角速度的坐标系」)
void gimbal_backstepping(const GimbalParam *p,
float th_t, float dth_t, float ddth_t,
float psi_t, float dpsi_t, float ddpsi_t,
float th, float w_y, float psi, float w_z,
float *tau_y, float *tau_z)
{
float G = coupling(p, th);

// pitch
float d1 = th_t - th; // delta_1
float d2 = dth_t + p->k1 * d1 - w_y; // delta_2 = x2t - x2
float a_y = ddth_t + p->k1 * (dth_t - w_y) + p->k2 * d2 + d1;
*tau_y = p->J_Ay * a_y + p->m * p->g * p->l_A * cosf(th) + G * w_z * w_z;

// yaw
float d3 = wrap_pi(psi_t - psi); // delta_3, 过零处理
float d4 = dpsi_t + p->k3 * d3 - w_z; // delta_4 = x4t - x4
float a_z = ddpsi_t + p->k3 * (dpsi_t - w_z) + p->k4 * d4 + d3;
*tau_z = yaw_inertia(p, th) * a_z - 2.0f * G * w_y * w_z;

*tau_y = fminf(fmaxf(*tau_y, -p->tau_y_max), p->tau_y_max);
*tau_z = fminf(fmaxf(*tau_z, -p->tau_z_max), p->tau_z_max);
}

AIGC:
Claude Opus 5.5, 手稿转录、推导校核与可改进点


基于反步法的云台非线性控制器
http://chose-b-log.netlify.app/gimbal-controller/
作者
B
发布于
2026年11月18日
更新于
2026年11月21日
许可协议