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; 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; }
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; }
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; }
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);
float d1 = th_t - th; float d2 = dth_t + p->k1 * d1 - w_y; 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;
float d3 = wrap_pi(psi_t - psi); float d4 = dpsi_t + p->k3 * d3 - w_z; 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, 手稿转录、推导校核与可改进点