倒立摆串级 PID 复习博客完整版覆盖课件 7.1 – 7.7共 62 张幻灯片。所有插图取自原始课件已裁去空白边距所有数值用 Python 复算过。考前速览请用 → 精简版一页纸原理总图① 三层串级结构 ② 把非线性搬进单片机的实现链 ③ 参数与公式速查。建议先把这张图看熟。〇、课程全景七份课件是一条链不是七个知识点课件主题在链条中的位置本节要回答的问题7.1串级 PID① 控制方案为什么用三个环三个环分别控什么7.2电机闭环传递函数② 内环模型把 PID 电机打包后等效成什么7.3倒立摆建模③ 被控对象θ̈ 与 ẍ 的关系式怎么来的7.4绘制系统结构框图④ 拼装两块模型如何串成一张完整框图7.5倒立摆逆解算⑤ 消掉非线性非线性求不出传函怎么办7.6参数整定⑥ 定参数三个环的 Kp / Ki 各是多少怎么定的7.7编写代码⑦ 落地框图上的每个方块写在代码哪一行一条主线控制方案 建立模型 拼成框图 消除非线性 整定与实现 7.1 三级串级PID 7.2 电机闭环1/(s/401) 7.4 完整框图 7.5 逆解算ẍf(θ̈,θ) 7.6 Kp / Ki 7.3 倒立摆 θ̈f(θ,ẍ) 把非线性搬进单片机 7.7 C 代码符号约定全文统一符号含义单位θ / θ̇ / θ̈姿态角 / 角速度 / 角加速度相对竖直方向rad, rad/s, rad/s²ω轮胎转速电机环被控量rad/sx / ẋ / ẍ底盘的位移 / 线速度 / 线加速度m, m/s, m/s²mp / lp / Jp摆的质量 / 摆长 / 摆的转动惯量kg, m, kg·m²Rw车轮半径mKp / Ki / KdPID 的比例 / 积分 / 微分系数—排版提示θ̇ 用的是「θ U0307 组合上点」。Windows 上的微软雅黑、Calibri、Arial、Times 都会把这个点丢掉显示成 θ只有 Consolas 和宋体/仿宋/楷体等少数字体支持。浏览器通常会自动回退若看到「θ」请换字体。一、串级 PID 控制方案7.11.1 被控对象车轮上的倒立摆把车身的上半部分近似成一个质量集中于一点的摆摆质量 mp、摆长 lp下面是一只能前后驱动的车轮。控制目标是让摆保持在竖直位置即 θ → 0。课件 7.1 p2左边是正常的摆稳定平衡点在下右边是倒立摆稳定平衡点在上。控制直觉非常关键车体前倾 → 车轮向前加速车体后倾 → 车轮向后加速。就像用手托住一根正在倒下的扫帚底盘要往它倒下去的方向「追」过去把角追回竖直。这也解释了为什么唯一可控输入是加速度 ẍ —— 控制摆角靠的就是底盘的加速度。课件 7.1 p3单级 PID —— 只能控制轮胎转速 ω不能保证车身平衡。1.2 传感器与执行器部件角色测量 / 输出用在哪一环电机 车轮执行器产生水平加速度 ẍ整条通路的最末端编码器内环传感器轮胎转速 ω电机环最内环MPU6050外环传感器姿态角 θpitch以及姿态角速度 θ̇gyro角度环 角速度环关键点MPU6050 能同时给出 θ 和 θ̇这正是能再加一层环的硬件前提。如果传感器只给角度就只能做两级串级正因为能直接测到 θ̇才做成了三级串级。1.3 从单级到三级串级 PID 的产生过程第 1 步 · 单级 PIDp3ωref 与 ω 相减 → PID → 电机模型 → ω反馈来自编码器。只能控转速。第 2 步 · 两级串级p4外环 PID 吃 θref 与 θ 的误差输出 ωref 交给内环形成「姿态环 转速环」。第 3 步 · 三级串级p5既然 θ̇ 可以直接测量就在角度环与电机环之间插入角速度环。课件 7.1 p4两级串级 —— 内环控制轮胎转速 ω外环控制车身姿态 θ。课件 7.1 p5三级串级 PID —— 最终方案。橙色框是角速度环蓝色框是电机环。最终三级方案的信号流θref →(Σ)→ PID_角度 → θ̇ref →(Σ)→ PID_角速度 → ωref →(Σ)→ PID_电机 → 电机模型 → ω → 车身模型 → θ̇ →(1/s)→ θ ↑ -θ ↑ -θ̇ ↑ -ω └──── 角度环 ──────────────┴──── 角速度环 ───────────┴──── 电机环 ────┘环被控量给定信号反馈信号传感器角度环最外姿态角 θθrefθMPU6050 融合角角速度环中间姿态角速度 θ̇θ̇refθ̇MPU6050 陀螺电机环最内轮胎转速 ωωrefω编码器1.4 整定总策略从内向外逐级整定串级结构的整定不能一次把三个环的参数一起调。7.1 p6 给出统一方法通过波特图整定该级 PID 参数计算该级的闭环响应函数把它当作一个已知方块交给外环换到下一级更外的一环重复。顺序电机环 → 角速度环 → 角度环。7.1 的 p7 / p8 / p9 正是这三步的示意每一张图里被绿色框高亮的部分就是当前正在整定的那一级。课件 7.1 p7第 1 步 —— 整定电机环参数并把它化简成「电机闭环」方块。课件 7.1 p8第 2 步 —— 用电机闭环整定角速度环。为什么必须从内向外外环的「被控对象」包含内环。只有内环的闭环行为先确定下来成为已知传函外环才有确定的模型可以画波特图。反过来先调外环内环一动外环的设计就全部作废。二、电机闭环传递函数7.22.1 目标把「PID 电机」打包成一个方块课件 7.2 p3把整个闭环替换成一个「电机闭环传递函数」方块。2.2 结构框图化简规则课件 7.2 p4串联、并联、反馈、分离点前后移、比较点前后移。规则化简结果记忆要点串联G1(s)·G2(s)方块前后相连传函相乘并联G1(s) G2(s)同一输入分两路再相加传函相加反馈负反馈 HG(s) / [1 G(s)H(s)]分母为 1 加上环内传函之积分离点前后移跨过方块时乘 / 除该方块引出点搬家要配平比较点前后移跨过方块时乘 / 除该方块相减点搬家要配平2.3 已知量速度环 PID 参数已经由波特图整定好p2电机模型由辨识得到p51 5.591 G_PID(s) Kp Ki·─ Kd·s , Gm(s) ────────────────────────────── s (s/14.2 1)(s/1962.2 1)课件 7.2 p5Kp 0.5、Ki 7、Kd 0。先把参数钉死复算过Kp 0.5、Ki 7、Kd 0。于是 PID 零点在 Ki/Kp 14 rad/s而电机模型恰好有一个 14.2 rad/s 的极点 ——两者几乎重合这就是整节的题眼。低频增益 Kp·Gm(0) 0.5 × 5.591 2.7955。2.4 化简步骤(1) 把 PID 写成零极点形式 G_PID(s) Kp Ki/s (0.5s 7) / s 0.5(s 14) / s 7(s/14 1) / s ← 零点在 14 rad/s (2) 串联相乘得开环传递函数 G(s) G_PID(s) · Gm(s) 7(s/14 1) 5.591 ───────────────────────── · ────────────────────────────── s (s/14.2 1)(s/1962.2 1) 39.14 (s/14 1) ─────────────────────────────────────── s (s/14.2 1)(s/1962.2 1)课件 7.2 p7G(s) G_pid(s)·G_m(s)。2.5 两条近似为什么可以只留一个积分环节零极点靠得很近p8零点 14 与极点 14.2 几乎重合对波特图几乎没有影响可以相消很大的极点可以忽略p8极点 1962.2 对应很高的频率远在穿越频率之外可以忽略。课件 7.2 p8零极点相消 忽略大极点结果 39.4/s。⚠️39.4 的正确含义它是「穿越点附近的等效增益」不是低频渐近线。两个数要分清楚低频渐近线Kp·Gm(0) 0.5 × 5.591 2.80/s第一个转折约在 1 rad/s实际穿越频率38.78 rad/s相位裕度 89.1° —— 这才是课件 39.4 的出处。如果把 39.4/s 当成低频渐近线去起手画幅频图会得到错误的高度但用它估穿越频率、再写出闭环 1/(s/401)结论是对的。考试安全写法「穿越频率 ≈ 39 rad/s闭环主导极点 ≈ −40 rad/s」。2.6 闭环一阶惯性环节G(s) 39.4/s 39.4 1 1 Φ(s) ─────────────── ─────────── ─────────── ──────────── ≈ ──────────── 1 G(s)H(s) 1 39.4/s s 39.4 s/39.4 1 s/40 1课件 7.2 p9电机环闭环 —— 等效一阶惯性环节 1/(s/40 1)。结论后面每一节都会用到整个「PID 电机 编码器反馈」打包之后等效为一个时间常数约 1/40 s 的一阶惯性环节 Φ(s) 1/(s/40 1)带宽约 39 rad/s≈ 6.2 Hz。从此刻起电机环在框图里就是这一个方块不再拆开。2.7 闭环极点的精确校验不做任何近似直接解闭环特征方程 s(s/14.21)(s/1962.21) (0.5s 7)×5.591 0极点数值 (rad/s)作用p1−13.89与 PID 零点 −14 近乎对消可以删掉p2−40.84主导极点 → 时间常数 1/40.84 s即课件的 1/40p3−1921.66远在带宽之外可以删掉对消误差只有 0.107 rad/s相对 14 约 0.8%所以课件说「零极点靠得很近」是有依据的。2.8 两条近似的适用条件零极点相消14 与 14.2与忽略大极点1962.2前提都是穿越频率远低于这些频率。本例穿越在 38.8 rad/s只有 14.2 的 2.7 倍所以严格说第一条近似是「刚好够用」的程度不是非常宽松若把带宽再抬高就会失效。三、倒立摆建模7.33.1 物理图景课件 7.3 p2输入是轴做直线运动的加速度 ẍ输出是倒立摆的角加速度 θ̈。3.2 符号与方向约定符号含义符号含义x轴的位移θ摆的角度相对竖直方向ẋ轴运动的线速度θ̇摆旋转的角速度ẍ轴运动的线加速度θ̈摆旋转的角加速度3.3 受力分析把摆球当作质点它只受两个力重力 mp·g和底盘加速带来的惯性力 mp·ẍ。两个力都分解到「垂直于摆杆」和「沿摆杆」两个方向上其中垂直于摆杆的分量之差就是让摆产生角加速度的切向合力。课件 7.3 p3切向合力 FT → 扭矩 T → 转动定律 T Jp·θ̈ 的完整推导。① 切向合力垂直于摆杆的两个分量相减 FT mp·g·sinθ − mp·ẍ·cosθ ② 力矩力臂 摆长 lp T FT · lp ③ 摆的转动惯量质点绕端点 Jp mp·lp² ④ 转动定律对照平动的 F m·a T Jp · θ̈3.4 合成倒立摆的数学模型Jp·θ̈ FT·lp ⇒ mp·lp²·θ̈ (mp·g·sinθ − mp·ẍ·cosθ)·lp 消去 mp·lp 后得到 g·sinθ − ẍ·cosθ θ̈ ──────────────────── lp 写成方块 ẍ ──▶【 倒立摆 】──▶ θ̈课件 7.3 p4非线性不能写成传递函数的形式。关键结论式中含 sinθ 与 cosθ是非线性的因此不能直接写成传递函数。这正是 7.5 要做「逆解算」的原因 —— 不是数学上变线性了而是把非线性搬到了控制器内部。3.5 符号勘误很重要7.3 p3 现场推出的 (1) 式写成了FT mp·ẍ·cosθ − mp·g·sinθ与最终方块中的(g·sinθ − ẍ·cosθ)/lp符号相反。7.7 p3 上把这一项的负号划掉、改成了绿色修订版FT mp·g·sinθ − mp·ẍ·cosθ7.7 p2 的绿字也写成θ̈ (g·sinθ − ẍ·cosθ)/lp。以绿色修订版为准。物理校验θ 0往 x 侧倒时加大 ẍ往 x 加速会让 θ̈ 变小角被追回来说明控制方向正确。四、绘制系统结构框图7.4把已经拿到的两块模型拼起来并补上物理世界里剩下的两个运动学关系。4.1 先看角速度环课件 7.4 p5上半张是展开形式下半张是「电机闭环 倒立摆数学模型」的化简形式。下半张图里出现两个新方块绿色Rw·s与含 sinθ / cosθ 的非线性方块。4.2 补上运动学环节车轮的线位移与转角之间是纯几何关系求导两次即可得到 ẍ 与 ω 的关系x Rw·θ ← 几何关系Rw 为轮半径 ↓ 求导 ẋ Rw·θ̇ Rw·ω ← θ̇ 就是轮胎转速 ω ↓ 求导 ẍ Rw·dω/dt ↓ 拉普拉斯变换 ẍ(s) Rw·s·ω(s) ẍ(s) ⇒ ───────── Rw·s ← 图中绿色方块 ω(s)课件 7.4 p6由 x Rw·θ 一路求导到 ẍ(s)/ω(s) Rw·s。4.3 补上积分环节角加速度积分一次得到角速度在框图里就是乘一个 1/sθ̇ ∫ θ̈ dt ──拉普拉斯变换──▶ θ̇(s) θ̈(s)·(1/s) ⇒ θ̇(s)/θ̈(s) 1/s课件 7.4 p8框图里补上的第二个方块 1/s。4.4 完整框图课件 7.4 p10上三个环的层次下化简后可直接对照代码的形式。方块传递函数 / 表达式来源电机闭环传函1/(s/40 1)7.2 化简结果车轮运动学Rw·s7.4 p6倒立摆数学模型θ̈ (g·sinθ − ẍ·cosθ)/lp7.3 p4非线性积分角加速度 → 角速度1/s7.4 p8积分角速度 → 角度1/s框图末端五、倒立摆逆解算7.55.1 问题非线性卡在中间课件 7.5 p2非线性求不出传递函数想办法把它消掉。正着看框图前向通路是ẍ → 【非线性 θ̈ f(θ, ẍ)】 → θ̈。中间这个方块含 sinθ、cosθ没有传递函数后面的 PID 也就无从整定。5.2 关键洞察θ 是已知的θ 由 MPU6050实时测得。既然 θ 已知它就不再是「未知变量」而只是公式里的一个已知系数。于是可以把整条式子反过来解出 ẍg·sinθ − ẍ·cosθ θ̈ ───────────────────── ⇒ lp·θ̈ g·sinθ − ẍ·cosθ lp ⇒ ẍ·cosθ g·sinθ − lp·θ̈ ⇒ ẍ (g·sinθ − lp·θ̈) / cosθ ← 逆解算课件 7.5 p5两个式子并列 —— 上为正解算分析用下为逆解算控制器用。5.3 结构上的意义非线性被搬进了单片机课件 7.5 p4虚线左侧是「单片机里的程序」右侧是「物理实在」。反解之后非线性模块从「物理世界」被搬到了控制器内部单片机里。整条前向通路上只剩线性环节 —— 电机闭环 1/(s/401) 与运动学 Rw·s —— PID 的整定就可以照常进行。课件 7.5 p6逆解算方块移入单片机内部。5.4 正问题与逆问题对照用途公式出现在特点正问题分析、建模θ̈ (g·sinθ − ẍ·cosθ)/lp7.3 p4、7.4 p5、7.5 p5非线性含 sinθ、cosθ逆问题控制器里算 ẍẍ (g·sinθ − lp·θ̈)/cosθ7.5 p6–p9、7.7 p2–p6θ 视为已知量可实时计算5.5 逆解算里用哪个 θ̈逆变里用的 θ̈ 并不是从物理世界测来的而是从位置 / 角度环 PID 得到的期望值 θ̈ref。实际系统中 θ 与 θ̇ 由 MPU6050 直接测量它们构成所有反馈量。5.6 车轮半径环节的位置易错注意框图里是 ẍ(s)/ω(s) Rw·sRw 是物理世界的几何常数。当这个环节被搬进单片机、用于由 ẍref 反算 ωref 时要除以 Rw而不是乘。代码里写作1.0f / rw见第七节。六、参数整定7.6整定顺序依然是从内向外电机环7.2 已完成→ 角速度环 → 角度环。6.1 第 1 级电机环回顾 7.2项目结果PID 参数Kp 0.5Ki 7Kd 0PID 零极点形式0.5(s14)/s 7(s/141)/s零点在 14 rad/s开环穿越频率≈ 38.8 rad/s课件写 39.4闭环响应函数Φ(s) 1/(s/40 1)主导极点 −40.86.2 第 2 级角速度环先把被控对象整理清楚角速度环的内环被控对象 电机闭环 × 车身模型含积分。合并积分环节后开环对象是1 1 1 G_θ̇(s) ─────────── · ─── ──────────────────── s/40 1 s s(s/40 1)课件 7.6 p3把角速度环的被控对象合并为 1/[s(s/401)]。用G_PID(s) 10 10/s做波特图整定p6。图中黑线是原对象幅频、红线是 PID 的幅频、绿线是补偿后的开环幅频。复算结果开环穿越9.05 rad/s相位裕度70.9°。课件 7.6 p6角速度环波特图整定 —— G_PID(s) 10 10/s。整定完成后求闭环响应函数p9。课件写成两个实数极点 14.8 与 20.4 的形式1 Φ_θ̇(s) ────────────────────────────────────────────── (s/14.8 1)(s/20.4 1)课件 7.6 p9确定了角速度环 PID 参数Kp 10, Ki 10并算出闭环响应函数。这处极点值对不上已复算。按图面参数直接算内环对象 1/(s/401)PID 10 10/s闭环分母恒为s² 440s 400两个根是−439.09 与 −0.911。课件写的 14.8 与 20.4对应分母 s² 35.2s 301.92与它不符极点之和差 12.5 倍、之积差 1.3 倍。我也试过把 1/s 放回环内按 7.6 p3 图面写法分母变成 s³/40 s² 10s 10根为 −1.12、−14.80、−24.08 —— 其中 −14.80 与课件的 14.8 巧合但另一个是 24.08 而不是 20.4而且该配置含右半平面极点、本身不稳定所以不是课件的本意。考试安全写法按图面参数老老实实算 s² 440s 400答「−439.1 与 −0.9」若题目沿用课件的 14.8/20.4则照抄这两个数继续往下做不要自行改动。6.3 第 3 级角度环把角速度环整体当成一个方块前面再串一个积分 1/s得到角度环的开环对象p10–p11。注意这里用的是课件给的极点 14.8 / 20.41 G_θ(s) ────────────────────────────────────────────────────── s (s/14.8 1)(s/20.4 1) 展开分母 s³ 35.2 s² 301.92 s课件 7.6 p11开环传函积分 两个闭环极点。角度环这一级只用比例控制p14G_PID(s) 4即4·G_θ(s) Φ_θ(s) ───────────────────────── 1 4·G_θ(s)课件 7.6 p14G_PID(s) 4纯比例。用 Kp 4 复算这级的裕度取课件的 14.8 / 20.4 时穿越频率2.92 rad/s、相位裕度70.7°若改用内环的精确极点−439.1 / −0.911则穿越1.50 rad/s、相位裕度31.0°。两种取法都稳定但裕度差别不小 —— 这也是上面那处极点存疑会连带影响的地方。6.4 最终 PID 参数表环被控量KpKiG_PID闭环 / 整定备注角度环最外θ404纯比例开环 4·G_θ(s)穿越 ≈ 2.9 rad/s角速度环中间θ̇101010 10/s开环穿越 ≈ 9.05 rad/s裕度 70.9°电机环最内ω0.570.5 7/s 7(s/141)/s闭环 ≈ 1/(s/401)主极点 −40.8把三个环的 Kp 放在一起看0.5 → 10 → 4。内环增益最小、角速度环最大这是「内环快、外环慢」的典型分布角速度环的开环穿越9.05 rad/s明显高于角度环2.9 rad/s。6.5 整定步骤总结(1) 电机环 波特图定 Kp0.5, Ki7 → 求闭环 Φ 1/(s/401)主导极点 −40.8 (2) 角速度环 把 (1) 当已知方块 → 波特图定 Kp10, Ki10 → 闭环极点 −439.1 / −0.91 课件写 14.8 / 20.4与图面参数不符 (3) 角度环 把 (2) 当已知方块串 1/s → 波特图定 Kp4七、编写代码7.77.1 最终框图带参数标注课件 7.7 p2单片机内部两个 PIDKp4Kp10, Ki10 逆解算 1/(Rw·s)。7.2 从框图到代码的对应关系框图位置代码里做什么对应参数角度环 PIDθref − θ → 得到 θ̇refKp 4角速度环 PIDθ̇ref − θ̇ → 得到 θ̈refKp 10, Ki 10逆解算方块由 θ̈ref 和实测 θ 算出 ẍrefẍ (g·sinθ − lp·θ̈)/cosθ单片机 / 物理世界分界除以轮半径 Rw 并积分得到 ωref 发给电机调速系统1/Rw输出限幅限制 θ̇ 与 θ̈ 的幅度防积分饱和与飞车±4π, ±40π7.3 输出限幅课件 7.7 p5θ̇ ∈ [−4π, 4π] rad/sθ̈ ∈ [−40π, 40π] rad/s²。限幅放在 PID 输出之后、进入积分之前的位置。它有两个作用不让期望角速度 / 角加速度大到物理上做不到避免控制器提出无法执行的要求一旦传感器异常或状态发散输出不会无界增长保护电机与机械结构。7.4 逆解算与积分的实现框图上的 1/(Rw·s) 由一个积分环节1/s和一个常数 1/Rw 组成。在代码里积分就是「每个控制周期累加一次」课件 7.7 p6ωref (1/Rw)·∫ ẍref dt代码即一行累加。/* 轮半径换算 积分一步完成 */omega_ref1.0f/rw*x_dot_dot_ref*deltaT;1 ωref ───────── · ∫ ẍref dt Rw7.5 一帧控制流程把上面的内容串起来每个控制周期 deltaT 执行一次 1. 读取传感器 θ ← MPU6050pitch θ̇ ← MPU6050gyro ω ← 编码器 2. 角度环 PID θ̇ref PID_angle(θref − θ) Kp 4 3. 角速度环 PID θ̈ref PID_rate(θ̇ref − θ̇) Kp 10, Ki 10 4. 输出限幅 θ̇ref, θ̈ref 限幅到 ±4π, ±40π 5. 逆解算 ẍref (g·sinθ − lp·θ̈ref) / cosθ 6. 换算 积分 omega_ref (1.0f / rw) * ẍref * deltaT ← 1/(Rw·s) 7. 电机调速 Motor_SetSpeed(omega_ref) ← 电机环闭环已在底层实现7.6 代码文件组织文件内容建议包含app_control.h参数宏、结构体、函数声明Kp / Ki / Kd、轮半径 Rw、限幅值、PID 结构体app_control.c实现串级 PID 计算、逆解算、积分、限幅、调用电机调速接口附录 A公式速查表#公式含义出处1G(s) G1·G2串联化简7.2 p42G(s) G1 G2并联化简7.2 p43Φ(s) G/(1 GH)负反馈闭环7.2 p44G_PID(s) Kp Ki/s Kd·s标准 PID7.2 p55G_PID(s) 0.5(s14)/s 7(s/141)/s速度环 PID零点在 14 rad/s7.2 p56Gm(s) 5.591/[(s/14.21)(s/1962.21)]电机模型7.2 p57穿越 ≈ 38.8 rad/s课件写 39.4速度环开环穿越频率7.2 p88Φ(s) 1/(s/40 1)主极点 −40.8电机环闭环传递函数7.2 p99FT mp·g·sinθ − mp·ẍ·cosθ切向合力7.3 p3 / 7.7 p310T FT·lpJp mp·lp²T Jp·θ̈扭矩与转动定律7.3 p311θ̈ (g·sinθ − ẍ·cosθ)/lp倒立摆数学模型非线性7.3 p412x Rw·θ ⇒ ẍ(s)/ω(s) Rw·s车轮运动学7.4 p613θ̇(s)/θ̈(s) 1/s积分关系7.4 p814ẍ (g·sinθ − lp·θ̈)/cosθ逆解算7.5 p6 / 7.7 p215Φ_θ̇(s) 1/[(s/14.81)(s/20.41)]角速度环闭环课件值存疑7.6 p916G_θ(s) 1/[s(s/14.81)(s/20.41)]角度环开环对象7.6 p1117ωref (1/Rw)·∫ ẍref dt代码中的换算与积分7.7 p6附录 B数值验算表所有关键数字用 Python 复算精确值与课件值对照。项目复算值课件值是否一致电机 PID 零点 Ki/Kp14.000 rad/s14✔电机 PID 零极点形式0.5(s14)/s 7(s/141)/s7(s/141)/s✔开环低频增益 Kp·Gm(0)2.7955低频渐近线 2.80/s写 2.8✔开环穿越频率38.78 rad/s39.4✔39.4 是穿越点等效相位裕度89.1°未给—闭环极点精确−13.89 / −40.84 / −1921.66零极点对消 忽略✔主导极点 / 时间常数−40.84 → 1/40.84 s1/40 s✔角速度环开环穿越9.05 rad/s未给—角速度环相位裕度70.9°未给—角速度环闭环极点−439.09 / −0.91114.8 / 20.4✘ 不符角速度环闭环直流增益1.0001.0✔角度环穿越用课件极点2.92 rad/s裕度 70.7°未给—角度环穿越用精确极点1.50 rad/s裕度 31.0°未给—正/逆解互逆校验误差 1e−12—✔ 自洽1 rad/s ? rpm9.549 rpm未给换算用限幅 4π / 40π12.566 rad/s / 125.66 rad/s²4π / 40π✔附录 C课件勘误汇总#位置问题处理17.3 p3 式(1)FT mp·ẍ·cosθ − mp·g·sinθ与最终方块符号相反 以 7.7 p3 绿色修订版为准FT mp·g·sinθ − mp·ẍ·cosθ27.6 p9角速度环闭环极点写 14.8 / 20.4按图面参数应为 −439.09 / −0.911 已在正文标注考试若沿用课件数值则照抄37.2 p8把 39.4/s 当作化简终式易被误读为低频渐近线实际是 2.80/s⚠️ 已在正文说明39.4 是穿越点等效增益47.6 p3图面把对象画成 1/[s(s/401)]但若 1/s 真在环内则系统不稳定⚠️ 说明 1/s 应在环外角度环那一级才引入附录 D自测清单为什么要在角度环和电机环之间再加一层角速度环提示MPU6050 能直接测 θ̇写出从 G(s) 39.14(s/141)/[s(s/14.21)(s/1962.21)] 化简到只剩积分环节的两条近似理由并说明 39.4 是穿越点等效增益而不是低频渐近线。倒立摆模型为什么不能直接写成传递函数逆解算是怎么绕过这个问题的框图里 Rw·s 在物理世界一侧代码里为什么变成1.0f / rw三个环的 Kp / Ki 分别是多少整定顺序为什么必须「从内向外」输出限幅限的是哪两个量限幅值是多少不加限幅会怎样如果发现摆往 x 侧倒下时小车也跟着往 x 走却越倒越快最可能是哪个符号写错了电机闭环的主极点是 −40.8角速度环按图面参数算出的闭环极点是 −439.1 与 −0.91而课件写 14.8/20.4。哪个对为什么「内环必须比外环快」用 9.05 rad/s角速度环与 2.92 rad/s角度环两个穿越频率说明。
【STM32踩坑日记】倒立摆串级 PID
倒立摆串级 PID 复习博客完整版覆盖课件 7.1 – 7.7共 62 张幻灯片。所有插图取自原始课件已裁去空白边距所有数值用 Python 复算过。考前速览请用 → 精简版一页纸原理总图① 三层串级结构 ② 把非线性搬进单片机的实现链 ③ 参数与公式速查。建议先把这张图看熟。〇、课程全景七份课件是一条链不是七个知识点课件主题在链条中的位置本节要回答的问题7.1串级 PID① 控制方案为什么用三个环三个环分别控什么7.2电机闭环传递函数② 内环模型把 PID 电机打包后等效成什么7.3倒立摆建模③ 被控对象θ̈ 与 ẍ 的关系式怎么来的7.4绘制系统结构框图④ 拼装两块模型如何串成一张完整框图7.5倒立摆逆解算⑤ 消掉非线性非线性求不出传函怎么办7.6参数整定⑥ 定参数三个环的 Kp / Ki 各是多少怎么定的7.7编写代码⑦ 落地框图上的每个方块写在代码哪一行一条主线控制方案 建立模型 拼成框图 消除非线性 整定与实现 7.1 三级串级PID 7.2 电机闭环1/(s/401) 7.4 完整框图 7.5 逆解算ẍf(θ̈,θ) 7.6 Kp / Ki 7.3 倒立摆 θ̈f(θ,ẍ) 把非线性搬进单片机 7.7 C 代码符号约定全文统一符号含义单位θ / θ̇ / θ̈姿态角 / 角速度 / 角加速度相对竖直方向rad, rad/s, rad/s²ω轮胎转速电机环被控量rad/sx / ẋ / ẍ底盘的位移 / 线速度 / 线加速度m, m/s, m/s²mp / lp / Jp摆的质量 / 摆长 / 摆的转动惯量kg, m, kg·m²Rw车轮半径mKp / Ki / KdPID 的比例 / 积分 / 微分系数—排版提示θ̇ 用的是「θ U0307 组合上点」。Windows 上的微软雅黑、Calibri、Arial、Times 都会把这个点丢掉显示成 θ只有 Consolas 和宋体/仿宋/楷体等少数字体支持。浏览器通常会自动回退若看到「θ」请换字体。一、串级 PID 控制方案7.11.1 被控对象车轮上的倒立摆把车身的上半部分近似成一个质量集中于一点的摆摆质量 mp、摆长 lp下面是一只能前后驱动的车轮。控制目标是让摆保持在竖直位置即 θ → 0。课件 7.1 p2左边是正常的摆稳定平衡点在下右边是倒立摆稳定平衡点在上。控制直觉非常关键车体前倾 → 车轮向前加速车体后倾 → 车轮向后加速。就像用手托住一根正在倒下的扫帚底盘要往它倒下去的方向「追」过去把角追回竖直。这也解释了为什么唯一可控输入是加速度 ẍ —— 控制摆角靠的就是底盘的加速度。课件 7.1 p3单级 PID —— 只能控制轮胎转速 ω不能保证车身平衡。1.2 传感器与执行器部件角色测量 / 输出用在哪一环电机 车轮执行器产生水平加速度 ẍ整条通路的最末端编码器内环传感器轮胎转速 ω电机环最内环MPU6050外环传感器姿态角 θpitch以及姿态角速度 θ̇gyro角度环 角速度环关键点MPU6050 能同时给出 θ 和 θ̇这正是能再加一层环的硬件前提。如果传感器只给角度就只能做两级串级正因为能直接测到 θ̇才做成了三级串级。1.3 从单级到三级串级 PID 的产生过程第 1 步 · 单级 PIDp3ωref 与 ω 相减 → PID → 电机模型 → ω反馈来自编码器。只能控转速。第 2 步 · 两级串级p4外环 PID 吃 θref 与 θ 的误差输出 ωref 交给内环形成「姿态环 转速环」。第 3 步 · 三级串级p5既然 θ̇ 可以直接测量就在角度环与电机环之间插入角速度环。课件 7.1 p4两级串级 —— 内环控制轮胎转速 ω外环控制车身姿态 θ。课件 7.1 p5三级串级 PID —— 最终方案。橙色框是角速度环蓝色框是电机环。最终三级方案的信号流θref →(Σ)→ PID_角度 → θ̇ref →(Σ)→ PID_角速度 → ωref →(Σ)→ PID_电机 → 电机模型 → ω → 车身模型 → θ̇ →(1/s)→ θ ↑ -θ ↑ -θ̇ ↑ -ω └──── 角度环 ──────────────┴──── 角速度环 ───────────┴──── 电机环 ────┘环被控量给定信号反馈信号传感器角度环最外姿态角 θθrefθMPU6050 融合角角速度环中间姿态角速度 θ̇θ̇refθ̇MPU6050 陀螺电机环最内轮胎转速 ωωrefω编码器1.4 整定总策略从内向外逐级整定串级结构的整定不能一次把三个环的参数一起调。7.1 p6 给出统一方法通过波特图整定该级 PID 参数计算该级的闭环响应函数把它当作一个已知方块交给外环换到下一级更外的一环重复。顺序电机环 → 角速度环 → 角度环。7.1 的 p7 / p8 / p9 正是这三步的示意每一张图里被绿色框高亮的部分就是当前正在整定的那一级。课件 7.1 p7第 1 步 —— 整定电机环参数并把它化简成「电机闭环」方块。课件 7.1 p8第 2 步 —— 用电机闭环整定角速度环。为什么必须从内向外外环的「被控对象」包含内环。只有内环的闭环行为先确定下来成为已知传函外环才有确定的模型可以画波特图。反过来先调外环内环一动外环的设计就全部作废。二、电机闭环传递函数7.22.1 目标把「PID 电机」打包成一个方块课件 7.2 p3把整个闭环替换成一个「电机闭环传递函数」方块。2.2 结构框图化简规则课件 7.2 p4串联、并联、反馈、分离点前后移、比较点前后移。规则化简结果记忆要点串联G1(s)·G2(s)方块前后相连传函相乘并联G1(s) G2(s)同一输入分两路再相加传函相加反馈负反馈 HG(s) / [1 G(s)H(s)]分母为 1 加上环内传函之积分离点前后移跨过方块时乘 / 除该方块引出点搬家要配平比较点前后移跨过方块时乘 / 除该方块相减点搬家要配平2.3 已知量速度环 PID 参数已经由波特图整定好p2电机模型由辨识得到p51 5.591 G_PID(s) Kp Ki·─ Kd·s , Gm(s) ────────────────────────────── s (s/14.2 1)(s/1962.2 1)课件 7.2 p5Kp 0.5、Ki 7、Kd 0。先把参数钉死复算过Kp 0.5、Ki 7、Kd 0。于是 PID 零点在 Ki/Kp 14 rad/s而电机模型恰好有一个 14.2 rad/s 的极点 ——两者几乎重合这就是整节的题眼。低频增益 Kp·Gm(0) 0.5 × 5.591 2.7955。2.4 化简步骤(1) 把 PID 写成零极点形式 G_PID(s) Kp Ki/s (0.5s 7) / s 0.5(s 14) / s 7(s/14 1) / s ← 零点在 14 rad/s (2) 串联相乘得开环传递函数 G(s) G_PID(s) · Gm(s) 7(s/14 1) 5.591 ───────────────────────── · ────────────────────────────── s (s/14.2 1)(s/1962.2 1) 39.14 (s/14 1) ─────────────────────────────────────── s (s/14.2 1)(s/1962.2 1)课件 7.2 p7G(s) G_pid(s)·G_m(s)。2.5 两条近似为什么可以只留一个积分环节零极点靠得很近p8零点 14 与极点 14.2 几乎重合对波特图几乎没有影响可以相消很大的极点可以忽略p8极点 1962.2 对应很高的频率远在穿越频率之外可以忽略。课件 7.2 p8零极点相消 忽略大极点结果 39.4/s。⚠️39.4 的正确含义它是「穿越点附近的等效增益」不是低频渐近线。两个数要分清楚低频渐近线Kp·Gm(0) 0.5 × 5.591 2.80/s第一个转折约在 1 rad/s实际穿越频率38.78 rad/s相位裕度 89.1° —— 这才是课件 39.4 的出处。如果把 39.4/s 当成低频渐近线去起手画幅频图会得到错误的高度但用它估穿越频率、再写出闭环 1/(s/401)结论是对的。考试安全写法「穿越频率 ≈ 39 rad/s闭环主导极点 ≈ −40 rad/s」。2.6 闭环一阶惯性环节G(s) 39.4/s 39.4 1 1 Φ(s) ─────────────── ─────────── ─────────── ──────────── ≈ ──────────── 1 G(s)H(s) 1 39.4/s s 39.4 s/39.4 1 s/40 1课件 7.2 p9电机环闭环 —— 等效一阶惯性环节 1/(s/40 1)。结论后面每一节都会用到整个「PID 电机 编码器反馈」打包之后等效为一个时间常数约 1/40 s 的一阶惯性环节 Φ(s) 1/(s/40 1)带宽约 39 rad/s≈ 6.2 Hz。从此刻起电机环在框图里就是这一个方块不再拆开。2.7 闭环极点的精确校验不做任何近似直接解闭环特征方程 s(s/14.21)(s/1962.21) (0.5s 7)×5.591 0极点数值 (rad/s)作用p1−13.89与 PID 零点 −14 近乎对消可以删掉p2−40.84主导极点 → 时间常数 1/40.84 s即课件的 1/40p3−1921.66远在带宽之外可以删掉对消误差只有 0.107 rad/s相对 14 约 0.8%所以课件说「零极点靠得很近」是有依据的。2.8 两条近似的适用条件零极点相消14 与 14.2与忽略大极点1962.2前提都是穿越频率远低于这些频率。本例穿越在 38.8 rad/s只有 14.2 的 2.7 倍所以严格说第一条近似是「刚好够用」的程度不是非常宽松若把带宽再抬高就会失效。三、倒立摆建模7.33.1 物理图景课件 7.3 p2输入是轴做直线运动的加速度 ẍ输出是倒立摆的角加速度 θ̈。3.2 符号与方向约定符号含义符号含义x轴的位移θ摆的角度相对竖直方向ẋ轴运动的线速度θ̇摆旋转的角速度ẍ轴运动的线加速度θ̈摆旋转的角加速度3.3 受力分析把摆球当作质点它只受两个力重力 mp·g和底盘加速带来的惯性力 mp·ẍ。两个力都分解到「垂直于摆杆」和「沿摆杆」两个方向上其中垂直于摆杆的分量之差就是让摆产生角加速度的切向合力。课件 7.3 p3切向合力 FT → 扭矩 T → 转动定律 T Jp·θ̈ 的完整推导。① 切向合力垂直于摆杆的两个分量相减 FT mp·g·sinθ − mp·ẍ·cosθ ② 力矩力臂 摆长 lp T FT · lp ③ 摆的转动惯量质点绕端点 Jp mp·lp² ④ 转动定律对照平动的 F m·a T Jp · θ̈3.4 合成倒立摆的数学模型Jp·θ̈ FT·lp ⇒ mp·lp²·θ̈ (mp·g·sinθ − mp·ẍ·cosθ)·lp 消去 mp·lp 后得到 g·sinθ − ẍ·cosθ θ̈ ──────────────────── lp 写成方块 ẍ ──▶【 倒立摆 】──▶ θ̈课件 7.3 p4非线性不能写成传递函数的形式。关键结论式中含 sinθ 与 cosθ是非线性的因此不能直接写成传递函数。这正是 7.5 要做「逆解算」的原因 —— 不是数学上变线性了而是把非线性搬到了控制器内部。3.5 符号勘误很重要7.3 p3 现场推出的 (1) 式写成了FT mp·ẍ·cosθ − mp·g·sinθ与最终方块中的(g·sinθ − ẍ·cosθ)/lp符号相反。7.7 p3 上把这一项的负号划掉、改成了绿色修订版FT mp·g·sinθ − mp·ẍ·cosθ7.7 p2 的绿字也写成θ̈ (g·sinθ − ẍ·cosθ)/lp。以绿色修订版为准。物理校验θ 0往 x 侧倒时加大 ẍ往 x 加速会让 θ̈ 变小角被追回来说明控制方向正确。四、绘制系统结构框图7.4把已经拿到的两块模型拼起来并补上物理世界里剩下的两个运动学关系。4.1 先看角速度环课件 7.4 p5上半张是展开形式下半张是「电机闭环 倒立摆数学模型」的化简形式。下半张图里出现两个新方块绿色Rw·s与含 sinθ / cosθ 的非线性方块。4.2 补上运动学环节车轮的线位移与转角之间是纯几何关系求导两次即可得到 ẍ 与 ω 的关系x Rw·θ ← 几何关系Rw 为轮半径 ↓ 求导 ẋ Rw·θ̇ Rw·ω ← θ̇ 就是轮胎转速 ω ↓ 求导 ẍ Rw·dω/dt ↓ 拉普拉斯变换 ẍ(s) Rw·s·ω(s) ẍ(s) ⇒ ───────── Rw·s ← 图中绿色方块 ω(s)课件 7.4 p6由 x Rw·θ 一路求导到 ẍ(s)/ω(s) Rw·s。4.3 补上积分环节角加速度积分一次得到角速度在框图里就是乘一个 1/sθ̇ ∫ θ̈ dt ──拉普拉斯变换──▶ θ̇(s) θ̈(s)·(1/s) ⇒ θ̇(s)/θ̈(s) 1/s课件 7.4 p8框图里补上的第二个方块 1/s。4.4 完整框图课件 7.4 p10上三个环的层次下化简后可直接对照代码的形式。方块传递函数 / 表达式来源电机闭环传函1/(s/40 1)7.2 化简结果车轮运动学Rw·s7.4 p6倒立摆数学模型θ̈ (g·sinθ − ẍ·cosθ)/lp7.3 p4非线性积分角加速度 → 角速度1/s7.4 p8积分角速度 → 角度1/s框图末端五、倒立摆逆解算7.55.1 问题非线性卡在中间课件 7.5 p2非线性求不出传递函数想办法把它消掉。正着看框图前向通路是ẍ → 【非线性 θ̈ f(θ, ẍ)】 → θ̈。中间这个方块含 sinθ、cosθ没有传递函数后面的 PID 也就无从整定。5.2 关键洞察θ 是已知的θ 由 MPU6050实时测得。既然 θ 已知它就不再是「未知变量」而只是公式里的一个已知系数。于是可以把整条式子反过来解出 ẍg·sinθ − ẍ·cosθ θ̈ ───────────────────── ⇒ lp·θ̈ g·sinθ − ẍ·cosθ lp ⇒ ẍ·cosθ g·sinθ − lp·θ̈ ⇒ ẍ (g·sinθ − lp·θ̈) / cosθ ← 逆解算课件 7.5 p5两个式子并列 —— 上为正解算分析用下为逆解算控制器用。5.3 结构上的意义非线性被搬进了单片机课件 7.5 p4虚线左侧是「单片机里的程序」右侧是「物理实在」。反解之后非线性模块从「物理世界」被搬到了控制器内部单片机里。整条前向通路上只剩线性环节 —— 电机闭环 1/(s/401) 与运动学 Rw·s —— PID 的整定就可以照常进行。课件 7.5 p6逆解算方块移入单片机内部。5.4 正问题与逆问题对照用途公式出现在特点正问题分析、建模θ̈ (g·sinθ − ẍ·cosθ)/lp7.3 p4、7.4 p5、7.5 p5非线性含 sinθ、cosθ逆问题控制器里算 ẍẍ (g·sinθ − lp·θ̈)/cosθ7.5 p6–p9、7.7 p2–p6θ 视为已知量可实时计算5.5 逆解算里用哪个 θ̈逆变里用的 θ̈ 并不是从物理世界测来的而是从位置 / 角度环 PID 得到的期望值 θ̈ref。实际系统中 θ 与 θ̇ 由 MPU6050 直接测量它们构成所有反馈量。5.6 车轮半径环节的位置易错注意框图里是 ẍ(s)/ω(s) Rw·sRw 是物理世界的几何常数。当这个环节被搬进单片机、用于由 ẍref 反算 ωref 时要除以 Rw而不是乘。代码里写作1.0f / rw见第七节。六、参数整定7.6整定顺序依然是从内向外电机环7.2 已完成→ 角速度环 → 角度环。6.1 第 1 级电机环回顾 7.2项目结果PID 参数Kp 0.5Ki 7Kd 0PID 零极点形式0.5(s14)/s 7(s/141)/s零点在 14 rad/s开环穿越频率≈ 38.8 rad/s课件写 39.4闭环响应函数Φ(s) 1/(s/40 1)主导极点 −40.86.2 第 2 级角速度环先把被控对象整理清楚角速度环的内环被控对象 电机闭环 × 车身模型含积分。合并积分环节后开环对象是1 1 1 G_θ̇(s) ─────────── · ─── ──────────────────── s/40 1 s s(s/40 1)课件 7.6 p3把角速度环的被控对象合并为 1/[s(s/401)]。用G_PID(s) 10 10/s做波特图整定p6。图中黑线是原对象幅频、红线是 PID 的幅频、绿线是补偿后的开环幅频。复算结果开环穿越9.05 rad/s相位裕度70.9°。课件 7.6 p6角速度环波特图整定 —— G_PID(s) 10 10/s。整定完成后求闭环响应函数p9。课件写成两个实数极点 14.8 与 20.4 的形式1 Φ_θ̇(s) ────────────────────────────────────────────── (s/14.8 1)(s/20.4 1)课件 7.6 p9确定了角速度环 PID 参数Kp 10, Ki 10并算出闭环响应函数。这处极点值对不上已复算。按图面参数直接算内环对象 1/(s/401)PID 10 10/s闭环分母恒为s² 440s 400两个根是−439.09 与 −0.911。课件写的 14.8 与 20.4对应分母 s² 35.2s 301.92与它不符极点之和差 12.5 倍、之积差 1.3 倍。我也试过把 1/s 放回环内按 7.6 p3 图面写法分母变成 s³/40 s² 10s 10根为 −1.12、−14.80、−24.08 —— 其中 −14.80 与课件的 14.8 巧合但另一个是 24.08 而不是 20.4而且该配置含右半平面极点、本身不稳定所以不是课件的本意。考试安全写法按图面参数老老实实算 s² 440s 400答「−439.1 与 −0.9」若题目沿用课件的 14.8/20.4则照抄这两个数继续往下做不要自行改动。6.3 第 3 级角度环把角速度环整体当成一个方块前面再串一个积分 1/s得到角度环的开环对象p10–p11。注意这里用的是课件给的极点 14.8 / 20.41 G_θ(s) ────────────────────────────────────────────────────── s (s/14.8 1)(s/20.4 1) 展开分母 s³ 35.2 s² 301.92 s课件 7.6 p11开环传函积分 两个闭环极点。角度环这一级只用比例控制p14G_PID(s) 4即4·G_θ(s) Φ_θ(s) ───────────────────────── 1 4·G_θ(s)课件 7.6 p14G_PID(s) 4纯比例。用 Kp 4 复算这级的裕度取课件的 14.8 / 20.4 时穿越频率2.92 rad/s、相位裕度70.7°若改用内环的精确极点−439.1 / −0.911则穿越1.50 rad/s、相位裕度31.0°。两种取法都稳定但裕度差别不小 —— 这也是上面那处极点存疑会连带影响的地方。6.4 最终 PID 参数表环被控量KpKiG_PID闭环 / 整定备注角度环最外θ404纯比例开环 4·G_θ(s)穿越 ≈ 2.9 rad/s角速度环中间θ̇101010 10/s开环穿越 ≈ 9.05 rad/s裕度 70.9°电机环最内ω0.570.5 7/s 7(s/141)/s闭环 ≈ 1/(s/401)主极点 −40.8把三个环的 Kp 放在一起看0.5 → 10 → 4。内环增益最小、角速度环最大这是「内环快、外环慢」的典型分布角速度环的开环穿越9.05 rad/s明显高于角度环2.9 rad/s。6.5 整定步骤总结(1) 电机环 波特图定 Kp0.5, Ki7 → 求闭环 Φ 1/(s/401)主导极点 −40.8 (2) 角速度环 把 (1) 当已知方块 → 波特图定 Kp10, Ki10 → 闭环极点 −439.1 / −0.91 课件写 14.8 / 20.4与图面参数不符 (3) 角度环 把 (2) 当已知方块串 1/s → 波特图定 Kp4七、编写代码7.77.1 最终框图带参数标注课件 7.7 p2单片机内部两个 PIDKp4Kp10, Ki10 逆解算 1/(Rw·s)。7.2 从框图到代码的对应关系框图位置代码里做什么对应参数角度环 PIDθref − θ → 得到 θ̇refKp 4角速度环 PIDθ̇ref − θ̇ → 得到 θ̈refKp 10, Ki 10逆解算方块由 θ̈ref 和实测 θ 算出 ẍrefẍ (g·sinθ − lp·θ̈)/cosθ单片机 / 物理世界分界除以轮半径 Rw 并积分得到 ωref 发给电机调速系统1/Rw输出限幅限制 θ̇ 与 θ̈ 的幅度防积分饱和与飞车±4π, ±40π7.3 输出限幅课件 7.7 p5θ̇ ∈ [−4π, 4π] rad/sθ̈ ∈ [−40π, 40π] rad/s²。限幅放在 PID 输出之后、进入积分之前的位置。它有两个作用不让期望角速度 / 角加速度大到物理上做不到避免控制器提出无法执行的要求一旦传感器异常或状态发散输出不会无界增长保护电机与机械结构。7.4 逆解算与积分的实现框图上的 1/(Rw·s) 由一个积分环节1/s和一个常数 1/Rw 组成。在代码里积分就是「每个控制周期累加一次」课件 7.7 p6ωref (1/Rw)·∫ ẍref dt代码即一行累加。/* 轮半径换算 积分一步完成 */omega_ref1.0f/rw*x_dot_dot_ref*deltaT;1 ωref ───────── · ∫ ẍref dt Rw7.5 一帧控制流程把上面的内容串起来每个控制周期 deltaT 执行一次 1. 读取传感器 θ ← MPU6050pitch θ̇ ← MPU6050gyro ω ← 编码器 2. 角度环 PID θ̇ref PID_angle(θref − θ) Kp 4 3. 角速度环 PID θ̈ref PID_rate(θ̇ref − θ̇) Kp 10, Ki 10 4. 输出限幅 θ̇ref, θ̈ref 限幅到 ±4π, ±40π 5. 逆解算 ẍref (g·sinθ − lp·θ̈ref) / cosθ 6. 换算 积分 omega_ref (1.0f / rw) * ẍref * deltaT ← 1/(Rw·s) 7. 电机调速 Motor_SetSpeed(omega_ref) ← 电机环闭环已在底层实现7.6 代码文件组织文件内容建议包含app_control.h参数宏、结构体、函数声明Kp / Ki / Kd、轮半径 Rw、限幅值、PID 结构体app_control.c实现串级 PID 计算、逆解算、积分、限幅、调用电机调速接口附录 A公式速查表#公式含义出处1G(s) G1·G2串联化简7.2 p42G(s) G1 G2并联化简7.2 p43Φ(s) G/(1 GH)负反馈闭环7.2 p44G_PID(s) Kp Ki/s Kd·s标准 PID7.2 p55G_PID(s) 0.5(s14)/s 7(s/141)/s速度环 PID零点在 14 rad/s7.2 p56Gm(s) 5.591/[(s/14.21)(s/1962.21)]电机模型7.2 p57穿越 ≈ 38.8 rad/s课件写 39.4速度环开环穿越频率7.2 p88Φ(s) 1/(s/40 1)主极点 −40.8电机环闭环传递函数7.2 p99FT mp·g·sinθ − mp·ẍ·cosθ切向合力7.3 p3 / 7.7 p310T FT·lpJp mp·lp²T Jp·θ̈扭矩与转动定律7.3 p311θ̈ (g·sinθ − ẍ·cosθ)/lp倒立摆数学模型非线性7.3 p412x Rw·θ ⇒ ẍ(s)/ω(s) Rw·s车轮运动学7.4 p613θ̇(s)/θ̈(s) 1/s积分关系7.4 p814ẍ (g·sinθ − lp·θ̈)/cosθ逆解算7.5 p6 / 7.7 p215Φ_θ̇(s) 1/[(s/14.81)(s/20.41)]角速度环闭环课件值存疑7.6 p916G_θ(s) 1/[s(s/14.81)(s/20.41)]角度环开环对象7.6 p1117ωref (1/Rw)·∫ ẍref dt代码中的换算与积分7.7 p6附录 B数值验算表所有关键数字用 Python 复算精确值与课件值对照。项目复算值课件值是否一致电机 PID 零点 Ki/Kp14.000 rad/s14✔电机 PID 零极点形式0.5(s14)/s 7(s/141)/s7(s/141)/s✔开环低频增益 Kp·Gm(0)2.7955低频渐近线 2.80/s写 2.8✔开环穿越频率38.78 rad/s39.4✔39.4 是穿越点等效相位裕度89.1°未给—闭环极点精确−13.89 / −40.84 / −1921.66零极点对消 忽略✔主导极点 / 时间常数−40.84 → 1/40.84 s1/40 s✔角速度环开环穿越9.05 rad/s未给—角速度环相位裕度70.9°未给—角速度环闭环极点−439.09 / −0.91114.8 / 20.4✘ 不符角速度环闭环直流增益1.0001.0✔角度环穿越用课件极点2.92 rad/s裕度 70.7°未给—角度环穿越用精确极点1.50 rad/s裕度 31.0°未给—正/逆解互逆校验误差 1e−12—✔ 自洽1 rad/s ? rpm9.549 rpm未给换算用限幅 4π / 40π12.566 rad/s / 125.66 rad/s²4π / 40π✔附录 C课件勘误汇总#位置问题处理17.3 p3 式(1)FT mp·ẍ·cosθ − mp·g·sinθ与最终方块符号相反 以 7.7 p3 绿色修订版为准FT mp·g·sinθ − mp·ẍ·cosθ27.6 p9角速度环闭环极点写 14.8 / 20.4按图面参数应为 −439.09 / −0.911 已在正文标注考试若沿用课件数值则照抄37.2 p8把 39.4/s 当作化简终式易被误读为低频渐近线实际是 2.80/s⚠️ 已在正文说明39.4 是穿越点等效增益47.6 p3图面把对象画成 1/[s(s/401)]但若 1/s 真在环内则系统不稳定⚠️ 说明 1/s 应在环外角度环那一级才引入附录 D自测清单为什么要在角度环和电机环之间再加一层角速度环提示MPU6050 能直接测 θ̇写出从 G(s) 39.14(s/141)/[s(s/14.21)(s/1962.21)] 化简到只剩积分环节的两条近似理由并说明 39.4 是穿越点等效增益而不是低频渐近线。倒立摆模型为什么不能直接写成传递函数逆解算是怎么绕过这个问题的框图里 Rw·s 在物理世界一侧代码里为什么变成1.0f / rw三个环的 Kp / Ki 分别是多少整定顺序为什么必须「从内向外」输出限幅限的是哪两个量限幅值是多少不加限幅会怎样如果发现摆往 x 侧倒下时小车也跟着往 x 走却越倒越快最可能是哪个符号写错了电机闭环的主极点是 −40.8角速度环按图面参数算出的闭环极点是 −439.1 与 −0.91而课件写 14.8/20.4。哪个对为什么「内环必须比外环快」用 9.05 rad/s角速度环与 2.92 rad/s角度环两个穿越频率说明。