[{"content":"欢迎来到我的博客，这里是 CJY719HNU 的个人站点。📝\n关于这个博客 这个博客用来记录我学习和实践过程中的思考与产出，主要包括：\n技术笔记：平时踩过的坑、解决过的问题、用过的工具 项目记录：自己写的小项目，从想法到实现的完整过程 知识整理：把零散的知识点整理成有体系的内容，方便自己回顾，也希望能帮到别人 碎碎念：大概类似可以公开的日记以一种生活化的语言讲出来？ 写博客不是目的，把问题想清楚才是。一篇文章从\u0026quot;大概懂\u0026quot;写到\u0026quot;能讲给别人听\u0026quot;，本身就是一个很好的学习方式。\n关于我 常驻 GitHub：CJY719HNU 研究方向 / 工作方向：机器人/嵌入式/控制工程 常用语言 / 技术栈：C/C++/现代控制理论，曾任RoboMaster湖南大学跃鹿战队26赛季电控组长 联系方式：QQ：86430451 技术栈 这个博客本身也是一个学习项目，基于：\nHugo —— 用 Go 写的静态站点生成器，构建快、无数据库 Stack 主题 —— 简洁现代的主题，支持中文、深色模式、GitHub 评论 GitHub Pages —— 免费托管，推送即部署 关于这套博客是怎么搭起来的、怎么用，可以看我的另一篇文章：《Hugo + Stack 主题完整使用教程》。\n评论与交流 文章下方的评论区用的是 Utterances：用你的 GitHub 账号登录即可评论，评论会以 Issue 的形式存在我的仓库里。\n欢迎指正、讨论，或者直接去 GitHub 找我。\n如果这篇文章对你有帮助，点个赞或者留个言就是对我最大的鼓励。😄\n","date":"2026-08-10T00:00:00+08:00","permalink":"/p/about/","title":"关于这个博客"},{"content":"线性时不变系统（定常系统） 以下讨论线性定常齐次状态方程解：即自由解\n对于 $\\dot x=Ax$，若开始时刻 $t_0$ 状态若定为 $x(t_0)=x_0$，则 $$ x(t)=e^{A(t-t_0)}x_0,\\quad t\\ge t_0 $$ 考虑从0开始，即 $t_0=0$ 则 $$ x(t)=e^{At}x_0,\\quad t\\ge0 $$这是 $e^{At}$ 的定义，有题目会让我们求 $e^{At}$，通常也称其为状态转移矩阵 $\\Phi(t)=e^{At}$\n系统例子：\n例 对于无重根，例为 $$ A=\\begin{bmatrix}0\u00261\\\\-2\u0026-3\\end{bmatrix} $$①根据定义求 $$ e^{At}=I+At+\\frac{1}{2!}A^2t^2+\\dots+\\frac{1}{n!}A^nt^n+\\dots $$ 注:定理有若 $A=P\\Lambda P^{-1}$ 即对角化，那么 $e^{At}=Pe^{\\Lambda t}P^{-1}$ 或 $e^{\\Lambda t}=P^{-1}e^{At}P$ 。这很好证明，只要知道 $A^n=P\\Lambda^nP^{-1}$\n这个前提代上式即求得\n$$ e^{At}= \\begin{bmatrix}1\u00260\\\\0\u00261\\end{bmatrix}+ \\begin{bmatrix}0\u00261\\\\-2\u0026-3\\end{bmatrix}t+ \\begin{bmatrix}0\u00261\\\\-2\u0026-3\\end{bmatrix}^2\\frac{t^2}{2!}+ \\begin{bmatrix}0\u00261\\\\-2\u0026-3\\end{bmatrix}^3\\frac{t^3}{3!}+\\dots $$$$ =\\begin{bmatrix} 1-t^2+t^3+\\dots \u0026 t-\\frac{3}{2}t^2-\\frac{7}{6}t^3+\\dots\\\\ -2t+3t^2-\\frac{7}{3}t^3+\\dots \u0026 1-3t+\\frac{7}{2}t^2-\\frac{5}{2}t^3+\\dots \\end{bmatrix} $$②对角化求 快速求得 $A$ 矩阵特征值为 $\\lambda_1=-1,\\ \\lambda_2=-2$ 对应于 $$ \\Lambda=\\begin{bmatrix}-1\u00260\\\\0\u0026-2\\end{bmatrix},\\quad P=\\begin{bmatrix}2\u00261\\\\-2\u0026-2\\end{bmatrix},\\quad P^{-1}=\\begin{bmatrix}1\u0026\\frac12\\\\-1\u0026-1\\end{bmatrix} $$$$ e^{At}=Pe^{\\Lambda t}P^{-1}= \\begin{bmatrix}2\u00261\\\\-2\u0026-2\\end{bmatrix} \\begin{bmatrix}e^{-t}\u00260\\\\0\u0026e^{-2t}\\end{bmatrix} \\begin{bmatrix}1\u0026\\frac12\\\\-1\u0026-1\\end{bmatrix} $$$$ =\\begin{bmatrix} 2e^{-t}-e^{-2t} \u0026 e^{-t}-e^{-2t}\\\\ -2e^{-t}+2e^{-2t} \u0026 -e^{-t}+2e^{-2t} \\end{bmatrix} $$ ③解耦求通解 从解耦的角度思考，可求出他的通解形式 取 $x=Py$，则 $\\dot x=Ax\\Rightarrow P\\dot y=APy$ 左右乘 $P^{-1}$，得 $\\dot y=P^{-1}APy \\Rightarrow \\dot y=\\Lambda y$\n$$ \\begin{cases} \\dot y_1=-y_1\\\\ \\dot y_2=-2y_2 \\end{cases} \\Rightarrow \\begin{cases} y_1=C_1e^{-t}\\\\ y_2=C_2e^{-2t} \\end{cases} $$回到 $x=Py$ 得 $$ \\begin{bmatrix}x_1\\\\x_2\\end{bmatrix}= \\begin{bmatrix}2\u00261\\\\-2\u0026-2\\end{bmatrix} \\begin{bmatrix}y_1\\\\y_2\\end{bmatrix} $$$$ \\begin{cases} x_1=2y_1+y_2=2C_1e^{-t}+C_2e^{-2t}\\\\ x_2=-2y_1-2y_2=-2C_1e^{-t}-2C_2e^{-2t} \\end{cases} $$验证把 $t=0$ 代入， $$x_1(0)=2C_1+C_2,\\ x_2(0)=-2C_1-2C_2$$$$C_1=x_1(0)+\\frac12x_2(0),\\quad C_2=-(x_1(0)+x_2(0))$$故 $$ x_1(t)=2\\big(x_1(0)+\\frac12x_2(0)\\big)e^{-t}+\\big(-x_1(0)-x_2(0)\\big)e^{-2t} $$$$ x_2(t)= -2\\big(x_1(0)+\\frac12x_2(0)\\big)e^{-t}-2\\big(-x_1(0)-x_2(0)\\big)e^{-2t} $$写成矩阵形式，其中中间的 $2\\times2$ 矩阵正是 $e^{At}$：\n$$\\begin{bmatrix}x_1(t)\\\\ x_2(t)\\end{bmatrix}= \\begin{bmatrix} 2e^{-t}-e^{-2t} \u0026 e^{-t}-e^{-2t}\\\\ -2e^{-t}+2e^{-2t} \u0026 -e^{-t}+2e^{-2t} \\end{bmatrix} \\begin{bmatrix}x_1(0)\\\\x_2(0)\\end{bmatrix}$$与定义完全对应。\n对于有重根的情况，例为 $$ A=\\begin{bmatrix}0\u00261\u00260\\\\0\u00260\u00261\\\\2\u0026-5\u00264\\end{bmatrix} $$快速求出 $\\lambda_1=\\lambda_2=1,\\ \\lambda_3=2$\n约当标准型： $$ J=\\begin{bmatrix} 1\u00261\u00260\\\\ 0\u00261\u00260\\\\ 0\u00260\u00262 \\end{bmatrix} $$所以 $$ e^{Jt}= \\begin{bmatrix} e^t \u0026 te^t \u0026 0\\\\ 0 \u0026 e^t \u0026 0\\\\ 0 \u0026 0 \u0026 e^{2t} \\end{bmatrix} $$ （这个特殊形式看书）\n对应于 $$ P=\\begin{bmatrix} 1\u0026-1\u00261\\\\ 1\u00260\u00262\\\\ 1\u00261\u00264 \\end{bmatrix},\\quad P^{-1}= \\begin{bmatrix} -2\u00265\u0026-2\\\\ -2\u00263\u0026-1\\\\ 1\u0026-2\u00261 \\end{bmatrix} $$那么也可由 $e^{At}=Pe^{Jt}P^{-1}$ 求出 $e^{At}$\n再考虑拉氏反变换法： $$ e^{At}=\\Phi(t)=\\mathcal L^{-1}\\big[(sI-A)^{-1}\\big] $$对应于 $$ A=\\begin{bmatrix}0\u00261\\\\-2\u0026-3\\end{bmatrix},\\quad sI-A=\\begin{bmatrix}s\u0026-1\\\\2\u0026s+3\\end{bmatrix} $$$$ (sI-A)^{-1}=\\frac{1}{|sI-A|}(sI-A)^* =\\frac{1}{s(s+3)+2}\\begin{bmatrix}s+3\u00261\\\\-2\u0026s\\end{bmatrix} $$$$ =\\begin{bmatrix} \\frac{2}{s+1}-\\frac{1}{s+2} \u0026 \\frac{1}{s+1}-\\frac{1}{s+2}\\\\[4pt] \\frac{-2}{s+1}+\\frac{2}{s+2} \u0026 \\frac{-1}{s+1}+\\frac{2}{s+2} \\end{bmatrix} $$$$ e^{At}=\\mathcal L^{-1}\\big[(sI-A)^{-1}\\big]= \\begin{bmatrix} 2e^{-t}-e^{-2t} \u0026 e^{-t}-e^{-2t}\\\\ -2e^{-t}+2e^{-2t} \u0026 -e^{-t}+2e^{-2t} \\end{bmatrix} $$ 在线性齐次系统的基础上加入输入的卷积，即线性定常系统非齐次方程的解\n对于 $\\dot x=Ax+Bu$，初态时刻为 $t_0=0$，初为状态 $x(t_0)$ 时，其解为 $$ x(t)=\\Phi(t)x(0)+\\int_{0}^{t}\\Phi(t-\\tau)Bu(\\tau)d\\tau $$ 分别对应 零输入响应 零状态响应\n或写成 $$ x(t)=e^{At}x(0)+\\int_{0}^{t}e^{A(t-\\tau)}Bu(\\tau)d\\tau $$在特定控制作用下，如脉冲函数、阶跃函数和斜坡函数的激励下，系统解式可简化\n脉冲：$u(t)=K\\delta(t),\\ x(0_{-})=x_0$ 时， $$ x(t)=e^{At}x_0+e^{At}BK $$阶跃：$u(t)=K\\cdot 1(t)$ , $x(0_{-})=x_0$ 时， $$ x(t)=e^{At}x_0+A^{-1}(e^{At}-I)BK $$斜坡：$u(t)=Kt\\cdot 1(t),\\ x(0_{-})=x_0$ 时， $$ x(t)=e^{At}x_0+\\big[A^{-2}(e^{At}-I)-A^{-1}t\\big]BK $$ 注：这里用 $x(0_{-})$ 不是 $x(0_{+})$，这是因为冲激要发挥作用只能在 $x(0_{-})$ 也就是冲激发生前一时刻开始算。\n线性时变系统 在此基础上再加入线性时变的条件，则一般写不出解析形式的状态方程的解\n先考虑一个标量系统 $$ \\frac{dx(t)}{dt}=a(t)x(t) $$可解出 $$ x(t)=e^{\\int_{t_0}^{t}a(\\tau)d\\tau}x(t_0) $$ 这个也能表示为状态矩阵\n用 $\\Phi(t,t_0)$ 表示，即 $$ \\Phi(t,t_0)=\\exp\\Big(\\int_{t_0}^{t}a(\\tau)d\\tau\\Big) $$如果要推广到矢量方程 $\\dot x=A(t)x(t)$ 使之有 $$ x(t)=\\exp\\Big[\\int_{t_0}^{t}A(\\tau)d\\tau\\Big]x(t_0) $$ 则要求 $A(t)$ 和 $\\int_{t_0}^{t}A(\\tau)d\\tau$ 满足交换律，即 $$ A(t)\\int_{t_0}^{t}A(\\tau)d\\tau=\\int_{t_0}^{t}A(\\tau)d\\tau\\,A(t) $$这条件很严苛，可满足的情况有\n$A$ 是定常矩阵，可以交换 $A(t)=K(t)\\cdot M$，即 $A(t)$ 是时变标量乘以矩阵 $A(t)=f_1(t)M_1+f_2(t)M_2+\\dots+f_r(t)M_r$，$M_1,M_2\\dots M_r$ 全为常数矩阵 对于不满足这个条件的时变系统，一般采用级数近似法，即\n$$ \\Phi(t,t_0)=I+\\int_{t_0}^{t}A(\\tau_0)d\\tau_0 +\\int_{t_0}^{t}A(\\tau_0)\\int_{t_0}^{\\tau_0}A(\\tau_1)d\\tau_1d\\tau_0 +\\int_{t_0}^{t}A(\\tau_0)\\int_{t_0}^{\\tau_0}A(\\tau_1)\\int_{t_0}^{\\tau_1}A(\\tau_2)d\\tau_2d\\tau_1d\\tau_0+\\dots $$验证只需证 $$ \\dot\\Phi(t,t_0)=A(t)\\Phi(t,t_0) $$","date":"2026-09-04T12:09:53+08:00","permalink":"/notes/2026-09-04_note_3/","title":"现代控制理论学习笔记（3）：控制系统状态空间表达式的解"},{"content":"经典引入 下面以经典的质量-弹簧-阻尼系统为例，说明状态空间矩阵的构建过程及其物理含义。 系统描述 弹簧阻尼系统的微分方程来源于牛顿第二定律（或达朗贝尔原理）结合弹簧力与阻尼力的物理模型。\n受力分析 取质量块$m$为研究对象，规定位移$x$向右为正。\n作用在质量块上的力有：\n弹簧力 $F_k$ 弹簧满足胡克定律：力与位移成正比，方向与位移相反。 当位移为 $x$时，弹簧力 $F_k = -kx$（负号表示恢复力，始终指向平衡位置）。 阻尼力$F_c$ 粘性阻尼力与速度成正比，方向与速度相反。 当速度为 $\\dot{x}$时，阻尼力 $F_c = -c \\dot{x}$。 外力$u(t)$ 由外部施加，方向与正方向一致。 牛顿第二定律 合外力等于质量乘以加速度： $$\\sum F = m \\ddot{x}$$ 代入各力（注意符号）： $$u(t) - k x - c \\dot{x} = m \\ddot{x} $$整理得微分方程 质量为 $m$ 的物体，受到弹簧刚度$k$、阻尼系数$c$ 和外部作用力$u(t)$ 的作用。系统的动力学方程为： $$m \\ddot{x}(t) + c \\dot{x}(t) + k x(t) = u(t) $$ （如果你现在在学大物上，会发现这个方程和阻尼振动很有关系）\n状态变量选取 选取状态变量为位移和速度：（状态变量由自己设置） $$x_1(t) = x(t), \\quad x_2(t) = \\dot{x}(t) $$ 状态方程与输出方程 由动力学方程可得：\n$$\\dot{x}_1 = x_2$$$$ \\dot{x}_2 = -\\frac{k}{m}x_1 - \\frac{c}{m}x_2 + \\frac{1}{m}u $$假设输出为位移：\n$$ y(t) = x_1(t) $$写成矩阵形式：\n$$ \\begin{bmatrix} \\dot{x}_1 \\\\ \\dot{x}_2 \\end{bmatrix} = \\begin{bmatrix} 0 \u0026 1 \\\\ -\\frac{k}{m} \u0026 -\\frac{c}{m} \\end{bmatrix} \\begin{bmatrix} x_1 \\\\ x_2 \\end{bmatrix} + \\begin{bmatrix} 0 \\\\ \\frac{1}{m} \\end{bmatrix} u(t) $$$$y(t) = \\begin{bmatrix} 1 \u0026 0 \\end{bmatrix} \\begin{bmatrix} x_1 \\\\ x_2 \\end{bmatrix} +0 \\cdot u(t) $$状态空间矩阵 $$\\mathbf{A} = \\begin{bmatrix} 0 \u0026 1 \\\\ -\\frac{k}{m} \u0026 -\\frac{c}{m} \\end{bmatrix},\\quad \\mathbf{B} = \\begin{bmatrix} 0 \\\\ \\frac{1}{m} \\end{bmatrix},\\quad \\mathbf{C} = \\begin{bmatrix} 1 \u0026 0 \\end{bmatrix},\\quad \\mathbf{D} = 0 $$ 自动控制原理 自动控制原理的核心之一即为 传递函数。传递函数用以描述线性定常系统的输出与输入对应变换关系，其定义式表达为（在拉普拉斯变换域下） $$ G(s)=\\frac{Y(s)}{R(s)} $$ 对应于上面的例子，我们很容易由牛顿第二定律 $$m \\ddot{x}(t) + c \\dot{x}(t) + k x(t) = u(t) $$ 进行拉普拉斯变换后得出传递函数 $$ G(s) = \\frac{1}{ms^2+bs+k} $$ 对应输入$R(s)=U(s)$，输出$Y(s)=X(s)$\n现代控制理论 在现代控制理论中，传递函数的代数表达式对应的矩阵形式被称为传递函数阵。\n回到状态空间表达式$\\dot{x}=Ax+Bu$，左右进行拉式变换得$sX(s)=AX(s)+Bu(s)$。移项整理后得到$X(s)=(sI-A)^{-1}BU(s)$\n同样对输出方程拉式变换$Y(s)=CX(s)+DU(s)$把上面的$X(s)$表达式带入得到$Y(s)=C(sI-A)^{-1}BU(s)+DU(s)$\n从而传递函数阵\n$$G(s)=\\frac{Y(s)}{U(s)}=C(sI-A)^{-1}B+D$$ 现在计算$(sI-A)^{-1}$：$(sI-A)^{-1}=\\frac{(sI-A)^{*}}{|sI-A|}$，在上面的弹簧阻力质量系统中$\\mathbf{A} = \\begin{bmatrix} 0 \u0026amp; 1 \\ -\\frac{k}{m} \u0026amp; -\\frac{c}{m} \\end{bmatrix}$，则 $$ (sI-A)^{-1}=\\frac {\\begin{bmatrix} s+\\frac{b}{m} \u0026 1 \\\\ -\\frac{k}{m} \u0026 s \\\\ \\end{bmatrix}}{s(s+\\frac{B}{m})-(-1)\\frac{k}{m}}=\\frac {\\begin{bmatrix} s+\\frac{b}{m} \u0026 1 \\\\ -\\frac{k}{m} \u0026 s \\\\ \\end{bmatrix}}{s^2+\\frac{B}{m}s+\\frac{k}{m}} $$左乘矩阵$C$：\n$$ C(sI-A)^{-1}=\\begin{bmatrix} 1 \u0026 0 \\end{bmatrix}\\frac{\\begin{bmatrix} s+\\frac{b}{m} \u0026 1 \\\\ -\\frac{k}{m} \u0026 s \\\\ \\end{bmatrix}} {s^2+\\frac{B}{m}s+\\frac{k}{m}} =\\frac{\\begin{bmatrix} s+\\frac{b}{m} \u0026 1 \\\\\\end{bmatrix}}{s^2+\\frac{B}{m}s+\\frac{k}{m}} $$再右乘矩阵$B$，并加上矩阵$D$（对应0）： $$ C(sI-A)^{-1}B+D=\\frac{\\frac{1}{m}}{s^2+\\frac{B}{m}s+\\frac{k}{m}}+0 =\\frac{1}{ms^2+bs+k} $$ 形式与代数一模一样\n分析 他们形式能一致，是因为我们将其当作一个SISO系统处理，此时传递函数矩阵退化为一个 $1X1$的标量矩阵，而这个标量就是传统的传递函数 我们发现${ms^2+bs+k}$这段式与$|sI-A|$息息相关，而如果把$s$替换为$\\lambda$，很容易发现这就是求$A$矩阵特征值的公式；而分母的解在传递函数中又代表着极点。那么我们可以得出结论：$A$矩阵的特征值即为传递函数的极点，与系统稳定性息息相关 更新\n2026.9.4把本篇作为新的自控学习笔记\n","date":"2026-09-04T11:57:28+08:00","permalink":"/notes/2026-08-10_fromac2mc/","title":"现代控制理论学习笔记（2）：从自控到现控"},{"content":"为什么需要若尔当标准型 $n\\times n$ 矩阵对角化需要$n$个线性无关的特征向量。若特征值有重根会导致特征向量数量不足，矩阵无法对角化。于是使用若尔当块拼出一个最接近对角的矩阵。\n若尔当块 对于重根特征值$\\lambda$，若尔当块对角线元素为$\\lambda$，紧挨着主对角线上方全为$1$，其余为$0$。\n$$ J_2(\\lambda) = \\begin{bmatrix} \\lambda \u0026 1 \\\\ 0 \u0026 \\lambda \\end{bmatrix}，J_3(\\lambda) = \\begin{bmatrix} \\lambda \u0026 1 \u00260\\\\ 0 \u0026 \\lambda \u0026 1\\\\ 0 \u0026 0 \u0026 \\lambda \\end{bmatrix}…… $$求若尔当标准型步骤 求特征值及其代数重数$m$。 例：$(\\lambda-1)^2(\\lambda-2)=0$，$\\lambda=1$ 代数重数$m=2$。\n计算零化度序列$d_k$ 定义： $$ d_k=\\mathrm{nullity}(A-\\lambda I)^k = n-\\mathrm{rank}\\big((A-\\lambda I)^k\\big) $$ 边界条件：令$d_0=0$，一直计算直到$d_k=m$为止。\n关于零化度序列的下文会有更详细讲解。\n计算大小为$j$的若尔当块个数$c_j$ $$ c_j=2d_j-d_{j-1}-d_{j+1} $$ 他的原始公式展开为 $$ c_j=(d_j-d_{j-1})-(d_{j+1}-d_j)=\\Delta_j-\\Delta_{j+1} $$ $\\Delta_k = d_k-d_{k-1}$，这个等式后面会讲。\n得到各个大小若尔当块数量后就可以写出若尔当矩阵$J$；书写惯例：左上角放高重次块，右下角放低重次块。\n零化度序列 设$A$为$n\\times n$矩阵，取某一个特征值$\\lambda$，记 $$ N = A-\\lambda I $$ $N$是对应这个特征值的幂零部分。\n零化度序列$d_k$定义： 对任意非负整数$k$，考虑齐次方程组 $ N^k \\boldsymbol{X} = \\boldsymbol{0} $ （$\\boldsymbol{X}$为矩阵自变量）。 $N^k$表示$N$自乘$k$次（规定$N^0=I$），方程组全部解向量构成线性空间，称为$N^k$的核，记作$\\ker(N^k)$。\n定义$d_k$为方程组$N^k\\boldsymbol{X}=\\boldsymbol 0$解空间的维数： $$ \\boldsymbol{d_k=\\dim\\big(\\ker(N^k)\\big)} $$ 边界条件：$\\boldsymbol{d_0=0}$。\n零化度序列两条性质 对任意$k\\ge0$，有$\\boldsymbol{d_k \\le d_{k+1}}$。 当$k$足够大，$d_k$停止增长，最终等于该特征值的代数重数$m$。 $\\Delta_k$描述的是：对于这个特征值，所有若尔当块里尺寸大于等于$k$的块的总个数，满足$\\boldsymbol{\\Delta_k = d_k-d_{k-1}}$。\n例题 已知矩阵 $$ A=\\begin{bmatrix} -1 \u0026 1 \u0026 0\\\\ -1 \u0026 3 \u0026 0\\\\ 0 \u0026 0 \u0026 2 \\end{bmatrix} $$① 计算特征值 令 $|A-\\lambda I|=0$ $$ \\begin{vmatrix} 1-\\lambda \u0026 1 \u0026 0\\\\ -1 \u0026 3-\\lambda \u0026 0\\\\ 0 \u0026 0 \u0026 2-\\lambda \\end{vmatrix} =(2-\\lambda)\\big[(1-\\lambda)(3-\\lambda)+1\\big] =(2-\\lambda)(\\lambda^2-4\\lambda+4) =(2-\\lambda)^3 $$ 仅有特征值 $\\lambda=2$，代数重数 $m=3$。\n② 计算 $N=A-\\lambda I$ $$ N=A-2I=\\begin{bmatrix} -1 \u0026 1 \u0026 0\\\\ -1 \u0026 1 \u0026 0\\\\ 0 \u0026 0 \u0026 0 \\end{bmatrix} $$③ 求零化度序列 $d_k$ $d_1=\\dim\\ker(N)$，即解 $N\\boldsymbol{X}=\\boldsymbol{0}$。\n设 $\\boldsymbol{x}=\\begin{bmatrix} x_1\\ x_2\\ x_3 \\end{bmatrix}$， $$ \\begin{bmatrix} -1 \u0026 1 \u0026 0\\\\ -1 \u0026 1 \u0026 0\\\\ 0 \u0026 0 \u0026 0 \\end{bmatrix} \\begin{bmatrix} x_1\\\\ x_2\\\\ x_3 \\end{bmatrix} = \\begin{bmatrix} 0\\\\0\\\\0 \\end{bmatrix} \\Rightarrow \\begin{cases} x_1=x_2 \\end{cases} $$ 有两个自由变量，$\\boldsymbol{d_1=2}$。\n$d_2=\\dim\\ker(N^2)$，解 $N^2\\boldsymbol{X}=\\boldsymbol{0}$。\n$$ N^2=\\begin{bmatrix} 0 \u0026 0 \u0026 0\\\\ 0 \u0026 0 \u0026 0\\\\ 0 \u0026 0 \u0026 0 \\end{bmatrix} $$ $N^2\\boldsymbol{x}=\\boldsymbol{0}$ 中 $x_1,x_2,x_3$ 全部为自由变量，$d_2=3$，已经等于代数重数 $m=3$，停止。\n当然，敏锐的人也观察到$d_k=n-\\mathrm{rank}(N^k)$，如$ d_1=3-1=2$；$d_2=3-0=3$。\n得到： $d_0=0,\\ d_1=2,\\ d_2=3$；且可推$d_3=3,d_4=0\\cdots$\n计算$\\Delta_k = d_k-d_{k-1}$： $$ \\Delta_1 = d_1-d_0 = 2 $$ $\\Delta_1$：尺寸至少为1的块，一共2个。\n$$ \\Delta_2 = d_2-d_1 = 1 $$ $\\Delta_2$：尺寸至少为2的块，一共1个。\n$$ \\Delta_3 = d_3-d_2 = 0 $$ $\\Delta_3$：尺寸至少为3的块，一共0个。\n根据逻辑不难推测，尺寸为2的块有1个，尺寸为1的块有1个\n也不难发现$\\Delta_{k+1}\\le \\Delta_k$，否则逻辑上无法说明“至少为1的块有2个但至少为2的块有114514个？”。\n当然也能用块个数公式 $c_j=\\Delta_j-\\Delta_{j+1}$： $$ c_1=\\Delta_1-\\Delta_2 = 2-1=1,\\quad c_2=\\Delta_2-\\Delta_3=1-0=1 $$从而可以写出该矩阵的若尔当标准型： $$ J=\\begin{bmatrix} 2 \u0026 1 \u0026 0\\\\ 0 \u0026 2 \u0026 0\\\\ 0 \u0026 0 \u0026 2 \\end{bmatrix} $$关键概念回顾与应该注意的地方 $d_k=\\dim\\ker(N^k),\\ N=A-\\lambda I$ $\\boldsymbol{\\Delta_k = d_k-d_{k-1}}$：大小 $\\ge k$ 的若尔当块总数量 $\\boldsymbol{c_k=\\Delta_k-\\Delta_{k+1}}$：恰好大小等于 $k$ 的若尔当块数量 性质：$\\Delta_1\\ge \\Delta_2\\ge \\Delta_3\\ge\\cdots$，不会递增。 所有块阶数加起来 $=$ 代数重数 $m$。 不要把矩阵阶数n和特征值$\\lambda$的重根数m搞混 过渡矩阵P的求解 在已知若尔当标准型的基础上若想求使$P^{-1}AP =J$的过渡矩阵P：\n上式等价于求$AP=PJ$，把$P$拆成三个列向量$P=[p_1 \\ p_2 \\ p_3]$，对应于上式例子 $$ J= \\begin{bmatrix} 2 \u0026 1 \u0026 0 \\\\ 0 \u0026 2 \u0026 0 \\\\ 0 \u0026 0 \u0026 2 \\\\ \\end{bmatrix} $$ 由$AP=PJ$有\n$Ap_1=2p_1$ （$p_1为普通特征向量$） $Ap_2=p_1+2p_2 \\implies (A-2I)p_2=p_1$（$p_2$为广义特征向量） 从1式可解得到两个自由变量关系$x_1=x_2$，$x_3$任取，基础解系为$\\xi_1= \\begin{bmatrix} 1 \\ 1 \\ 0 \\end{bmatrix} ，\\xi_2= \\begin{bmatrix} 0 \\ 0 \\ 1 \\end{bmatrix} $\n（两个自由变量对应属于这个特征值的若尔当块个数，也就是上面说的$d_1$）\n我们取$p_1=\\xi_1$作为链头，$p_3=\\xi_2$作为一阶块，然后根据$(A-2I)p_2=p_1$找到$p_2$，带入后发现解得$x_1,x_3$是自由变量，同时有$x_2=x_1+1$，取$x_1=0,x_3=0$，则$p_2=\\begin{bmatrix} 0 \\ 1 \\ 0\\end{bmatrix}$ ，从而从左到右放入$p_1,p_2,p_3$得 $$ P = \\begin{bmatrix} 1 \u0026 0 \u0026 0 \\\\ 1 \u0026 1 \u0026 0 \\\\ 0 \u0026 0 \u0026 1 \\\\ \\end{bmatrix} $$ 心得 学习若尔当标准型时，我遇到了许多令人困惑的问题，比如若尔当矩阵存在的意义是什么？零化度序列又是什么？\n而在学习这些问题的过程中我发现，我们的教材在过度追求严谨的同时，却对一些重要的推导公式进行了简化，大大增加了同学们的困惑。例如部分书中零化度序列的块公式 $$ c_j=2d_j-d_{j-1}-d_{j+1} $$ 即使有数学直觉的同学能很快看出这显然存在一个等差序列，大多数人很难一眼得出这个式子在表达什么，只好死记硬背。\n在提防此类防自学教材的同时，我们也要在听课时认真听讲，提高检索信息的能力，只有“穷究原理”方可“洞悉本质”。\n","date":"2026-08-20T23:27:40+08:00","permalink":"/notes/2026-8-20_jcf-learn/","title":"现代控制理论学习笔记（1）：若尔当标准型"},{"content":"前言 作为技术小白且非计科专业学生，配置Git时我是一头雾水地跟着教程走又一头雾水的完成了配置，看着一切正常却忽略了许多底层原理。以至于每当我点开一个开源项目的Code，Clone那一栏的HTPS和SSH两个方式总会让我疑惑（现在看更疑惑的是我直接忽略掉了Github CLI）：同样可以达到克隆项目的目的，这两种方式有何区别？ 直观的感受上，https的clone速度较慢，但通常一定时间后能下载下来；ssh则似乎有点看网络条件，条件良好则不一会即可clone完成，但大多数不配置代理工具的情形下收获的都是一个Timeout。 除此之外，Git配置过程中在本地配置的私钥与复制到Github个人帐号的公钥又是如何完成 “上锁”与“解锁” 的？ 这些问题曾困扰我，而当最近学习Linux系统编程时，这些词汇再次频繁出现，我不能再将其视作 房间里的大象了。因此我查阅了一些资料并将自己的收获总结为笔记，仅代表个人见解，望各位批评指正。\nHTTPS与SSH 本质区别\nHTTPS链接：使用443端口。这是网页浏览的标准端口，所有防火墙默认放行。你需要输入用户名+密码（或Token令牌）来验证身份。 SSH链接：使用22端口。这是远程登录的标准端口。你需要配置公钥+私钥来进行加密身份验证，无需每次输密码。 在国内，由于某些网络策略，GitHub的22端口（SSH）时常遭受干扰，而443端口（HTTPS）因为有TLS加密且是网页流量，反而更稳定。 SSH的公钥和私钥 签名和验签算法的流程 在Git配置时，通常会在Git Bash输入\n1 ssh-keygen -t ed25519 -C \u0026#34;your_email@example.com\u0026#34; Ed25519 是一种椭圆曲线数字签名算法（EdDSA），专门做数字签名，是用来做身份校验、防篡改。SSH、git、密钥登录大量在用。椭圆曲线数字签名算法下面会提及，暂且不表。\n此时，在~/.ssh下会生成id_ed25519和id_ed25519.pub文件，前者是私钥，万不可泄露；后者是公钥，需复制给Github中个人Settings里的SSH Keys\n重点来了，当git clone时，如果选的 SSH 方式（比如 git@github.com:用户名/仓库名.git）， Git 并不会直接发送请求，而是先调用电脑里的 SSH 客户端程序。SSH 客户端向 GitHub 的 22 端口发起连接请求，并告诉 GitHub：“我是 git 用户，我想用公钥认证”。 SSH 连接到服务器的那一瞬间，GitHub 服务端会抛出一个“随机字符串（质询）”给我的 SSH 客户端。SSH 客户端拿到这个随机字符串后，立刻用电脑里藏着的私钥对该字符串进行数字签名，然后把签名结果发回给 GitHub。 GitHub 收到签名后，立刻去它刚才存好的公钥进行验签，GitHub 才确认“对面这台电脑确实持有匹配的私钥，是本尊”。 整个过程叫“签名-验签”，不传输密码，也不传输公钥本身，只传输签名结果，所以极其安全。 验签成功后，GitHub 打开一个加密通道（SSH 隧道）。此时本地 Git 程序才通过这条加密隧道发送 git-upload-pack 请求，告诉 GitHub“我要拿这个仓库的所有对象”。GitHub 把仓库打包成压缩流，通过隧道加密后传回。本地 Git 收到数据后，在本地工作目录里解包、重建目录树，最终把代码文件还原到指定的路径下。\n签名和验签算法的实现 公钥与私钥的特点在于\n正向容易（私钥 → 公钥） 反向极难（公钥 → 私钥） 在数学上，两者称为是一个 “非对称陷门函数”的配对 ，算法（如Ed25519）会基于一个巨大的随机数种子，通过 椭圆曲线点乘运算，同时生成两个数\n私钥（d）：一个随机大整数。 公钥（Q）：由私钥通过“单向陷门函数”计算得出（$Q=d×G$，G是公开基点）。 这个运算的意义仅凭笔者贫乏的描述能力难以表达，可参考此视频 公钥加密技术ECC椭圆曲线加密算法原理。\n上述运算的 $×$ 也不是简单意义的标量乘法，实际数学逻辑是：\nQ = d × G 在椭圆曲线中，表示“将点 G 与自己进行 d 次点加运算”。 点加规则（特殊运算）：给定两个点 P 和 R，经过“画切线、找交点、取对称点”的几何规则，生成第三个点 S。这种运算不满足普通的数乘分配律，而是基于椭圆曲线群上的加法。 听上去有点绕，但表达的意思简而言之，如果将G（公开的）和Q（公开的）视作曲线上两个点，现在你能从G通过d次变换得到Q，但反推回原点甚至反推出“正步骤”难如登天。正如上面视频的“颜色”比喻：给你两桶颜料，把它们搅匀，得到一桶新颜料，这非常容易；给你这桶混合好的新颜料，让你逆向分析出原本那桶私钥颜料的确切成分和分量，这在物理和数学上都是几乎不可能完成的任务。\n事实上，公钥和私钥的设计使用远比混合颜料更复杂（毕竟混合颜料还可视作线性的过程），具体操作可分为 哈希凝练、签名生成、验签三步。\n哈希凝练：将消息 M、随机点 R 和公钥 A 压缩（实际上是一个 $h = SHA-512(R||A||M)$ 的公式）成固定长度整数 h。其中M为验证方（只拿着公钥的一方，例如Github方）抛过来的随机字符串，随机点R的计算第二步会提及，但该过程不涉及私钥，意味着验证方可以独立计算出h。 签名生成：由持有私钥的一方，用数学方法把 h 和私钥绑定，生成不可伪造的签名对 (R, S)。持有方生成一个一次性随机盲因子整数r（生成遵循一定规律），通过椭圆曲线乘法 $ R = r \\cdot G $ ，算出随机点；使用私钥标量 a，计算 $S=(r + h * a)\\ mod \\ L $（模 L 是曲线群的阶，确保结果落在有限域内）。序列化 (R, S)，长度共 64 字节（R 占 32 字节，S 占 32 字节）。 验签验证：验证方此时拥有：公钥A，原始消息M，收到的签名（R,S）。反解h后，验证方在等号左侧放上 $LHS=S \\cdot G$ ，右侧放上 $RHS=R+h \\cdot A$ 。由于椭圆曲线点乘满足结合律，验证$LHS=RHS$恒成立则验签通过。 TODO：补充哈希部分\n","date":"2026-08-12T23:18:59+08:00","permalink":"/notes/2026-08-12_keylearn/","title":"从git clone到密码学的思考"},{"content":"第一部分：环境搭建与编译哲学 1. 编译器与工具链 Linux系统编程首选C语言，编译器为gcc。Ubuntu安装编译链：\n1 sudo apt update \u0026amp;\u0026amp; sudo apt install build-essential -y 编译流程回顾： 预处理(.c-\u0026gt;.i) -\u0026gt; 编译(.i-\u0026gt;.s汇编) -\u0026gt; 汇编(.s-\u0026gt;.o目标文件) -\u0026gt; 链接(.o-\u0026gt;可执行文件)。在Linux下可执行文件无后缀，靠x权限运行。\n2. 第一个系统调用（文件IO） 概念： 系统编程不是改写内核，而是调用内核提供的服务（open, write, close）。\n1 2 3 4 5 6 7 8 #include \u0026lt;fcntl.h\u0026gt; #include \u0026lt;unistd.h\u0026gt; int main() { int fd = open(\u0026#34;test.txt\u0026#34;, O_CREAT | O_WRONLY, 0777); write(fd, \u0026#34;Hello Linux Kernel!\u0026#34;, 19); close(fd); return 0; } 编译： gcc first.c -o first \u0026amp;\u0026amp; ./first\n第二部分：进程控制（Process） 1. 进程分叉（fork） 概念： fork() 创建子进程，父子进程共享代码段但数据隔离（写时拷贝）。\n返回值： 父进程返回子进程PID；子进程返回0。 2. 僵尸进程与 wait 痛点： 子进程死后若父进程不管，会变成僵尸（\u0026lt;defunct\u0026gt;）占着内核资源。 解法： 父进程必须调用 wait() 来“收尸”，并获取子进程退出状态码。\n1 2 3 4 #include \u0026lt;sys/wait.h\u0026gt; int status; wait(\u0026amp;status); printf(\u0026#34;子进程退出码：%d\\n\u0026#34;, WEXITSTATUS(status)); 第三部分：多线程与竞态条件（Thread \u0026amp; Race Condition） 1. 线程的创建与共享内存 概念： 线程是进程内的轻量级执行流，共享全局变量和堆内存（不同于进程的隔离）。\n1 2 3 4 #include \u0026lt;pthread.h\u0026gt; // 编译必须加 -lpthread 链接库 pthread_create(\u0026amp;tid, NULL, work_func, NULL); pthread_join(tid, NULL); // 阻塞等待线程结束 2. 竞态条件的发生（单核也会出事） 实验： 两条线程同时对全局变量 money 做 10万次 money++。 结果： 结果不是 200000，因为 money++ 在CPU层面拆为“读-改-写”三步，时间片切换导致丢失更新。 结论： 只要有调度器（中断/抢占），非原子操作就有竞态，与单核多核无关。\n第四部分：同步原语（解决并发问题） 1. 互斥锁（Mutex）—— 厕所门锁 概念： 有所有权（谁加锁谁解锁），计数值只能是0或1，用于保护共享资源的修改。\n1 2 3 4 5 6 pthread_mutex_t mutex; pthread_mutex_init(\u0026amp;mutex, NULL); pthread_mutex_lock(\u0026amp;mutex); // 临界区（共享数据操作） pthread_mutex_unlock(\u0026amp;mutex); pthread_mutex_destroy(\u0026amp;mutex); 心法： 只要是“改数据”，条件反射就是加互斥锁。\n2. 信号量（Semaphore）—— 停车场显示屏 概念： 无所有权，计数值可以是N，用于限流（控制并发数） 或同步。 致命误区： 初始值为1的二值信号量不能替代互斥锁！因为信号量没有“所有者”机制，第三方线程可以随意 post 放行，破坏互斥。\n3. 条件变量（Condition Variable）—— 门铃 概念： 解决“线程等待”问题，避免CPU空转（忙等待）。必须配合互斥锁使用。 铁三角规则：\nlock 锁住共享资源。 while(条件不满足) { pthread_cond_wait(\u0026amp;cond, \u0026amp;mutex); }（内部原子性解锁+挂起，唤醒时自动加锁）。 生产者满足条件后 pthread_cond_signal 按门铃。 重点： 判断条件必须用while不能用if，防止虚假唤醒（Spurious Wakeup）。\n第五部分：网络编程基础（TCP Socket） 1. Socket 就是“电话” 服务器： socket(买电话) -\u0026gt; bind(绑定号码) -\u0026gt; listen(开机) -\u0026gt; accept(接电话)。 客户端： socket(买电话) -\u0026gt; connect(拨号)。 通信： send/recv(通话) -\u0026gt; close(挂断)。 2. 单线程服务器的死穴 单线程 accept 后会卡在 recv，如果客户端不发数据，服务器永远无法服务下一个客户。\n第六部分：多线程TCP服务器（老板与工人模型） 1. 架构设计 老板（主线程）： 死循环 accept，只负责接客。 工人（子线程）： 每接一个客，创建一线程负责 recv/send，互不干扰。 2. 关键代码与踩坑笔记 （1）保护在线人数计数器（互斥锁实战）\n1 2 3 4 5 6 7 int online_count = 0; pthread_mutex_t count_mutex; // 连接建立时加锁 pthread_mutex_lock(\u0026amp;count_mutex); online_count++; pthread_mutex_unlock(\u0026amp;count_mutex); （2）大坑：传参必须 malloc（堆内存）\n错误写法： pthread_create(..., \u0026amp;client_fd) （传栈上局部变量地址）。 后果： 主循环极快，client_fd 被下一个连接覆盖，子线程读到错误的fd，数据错乱。 正确写法： 1 2 3 4 int* fd_ptr = (int*)malloc(sizeof(int)); *fd_ptr = client_fd; pthread_create(..., handle_client, fd_ptr); // 在线程函数里 free(fd_ptr) （3）线程分离（detach）\n1 pthread_detach(tid); // 线程干完自动销毁，主线程无需 join，避免阻塞。 （4）端口复用（解决重启报错）\n1 2 int opt = 1; setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, \u0026amp;opt, sizeof(opt)); 附：C语言新手常见语法疑问（FAQ） 为什么线程函数是 void* func(void* arg)？ 因为 pthread_create 设计为通用型，不限定参数和返回值类型，void* 就是“万能包裹”，由开发者自己强转。 为什么全局变量要定义在函数外面？ 因为定义在函数内部是“栈内存”，线程间互相不可见。必须定义在全局区或堆区才能共享。 pthread_cond_wait 为什么传锁进去？ 因为内部需要原子性地完成“释放锁 + 挂起线程”两个动作，防止丢失唤醒。 编译时为什么总加 -lpthread？ 因为线程库不是C标准库默认链接的，需要显式指定。 当前进度与下一步规划 DONE： Linux基础IO、进程管理（fork/wait）、线程同步（互斥锁/信号量/条件变量）、TCP协议栈、多线程高并发服务器骨架。 TODO： 虚拟GPIO控制（通过读写 /sys/class/gpio 文件模拟物理电平）以及 SocketCAN（汽车总线通信），最终把网络指令转化为硬件动作。 ","date":"2026-08-11T20:51:11+08:00","permalink":"/notes/2026-08-11_linuxsyscode/","title":"Linux系统编程从零实战笔记（1）：从Hello World到多线程TCP服务器"},{"content":"绪论 双足轮腿机器人，在RoboMaster比赛中一般以平衡步兵形态出现（25赛季上交以其轮腿英雄震惊全场）。因轮腿式机器人系统具有非线性、欠驱动、强耦合、多输入多输出（MIMO）特性，传统PID算法非常难以使其达到收敛稳定状态（因为即使理想情况下，每一个腿长都对应着左右腿各三个电机，共六组参数，非常难调），故需要使用LQR、MPC等使用最优思想的控制算法，并对其建模仿真，才能使系统趋于稳定。\n幸运的是自从23年底哈工程的王洪玺（很多交流群提到的王工、玺佬）发布了LQR控制五连杆轮腿机器人的电控开源，各个队伍调轮腿的难度也大大降低，后续也出现了基于LQR串联MPC（电科中山柳幸之）、强化学习（交龙等队伍）等更优的控制方法，RM赛场上的轮腿越来越多也越发强大。\n本文主要针对尚未系统学习过自动控制原理（自动化大二下）、现代控制理论（自动化大三上），但希望能调腿的队员设计，主要针对控制理论中一些较晦涩难懂的概念及名词用较为通俗的例子进行解释，点明这里是什么，为什么，该怎么做。由于笔者水平有限，部分内容可能解释略微浅显，各队员也可结合我推荐的众多队伍电控开源进行学习理解。\n轮腿机器人是跃鹿战队“年年不忘”的每年电控必吃榜。自23赛季曾老师首次调出五连杆并联腿，到26赛季换成上交构型四连杆串腿平步，在轮腿上的电控技术积累逐渐趋于稳定，已经积累了跃鹿自己的文档和仿真（来自24赛季腿控王硕凯）。在保证电控组 人手、资金充足的条件下， 我个人认为无论规则倾向如何，队伍的新队员都应该尝试进行轮腿机器人的调试，这会成为电控组员控制水平提高的一个很好的机会。\n学习路径 轮腿机器人门槛愈发降低，但对电控的基本数理要求依然摆在那里，为了尽可能减少理论不扎实带来的调车痛苦，要求腿控至少要学习的基础理论有：\n高数、线性代数、大学物理力学部分 （必学）\n自动控制原理中的闭环控制、反馈等概念\n现代控制理论中的状态空间方程、代价函数、观测器等概念\n理论力学中的广义力、虚位移、虚功原理等概念\n也就是说，腿控不建议早于大一下开始调腿，最佳时间为赛季末至下赛季初的暑假开始知识储备，以保证新赛季初能在机械出车后立刻开始调车。\n接下来我会以王工的论文（见绪论超链接）为范本进行顺序解释，希望大家在看本篇之前至少看过他的论文一遍。\n力学分析与系统建模 以下的所有分析都基于单侧腿\n文章将五连杆腿简化为了一个二阶倒立摆，为了计算和建模方便，可以将腿简化成的单杆视作一个匀质杆，杆的中点视作质心（需要注意无论四连杆还是五连杆轮腿都可以如此等效，而实际上四连杆构型会存在些许质心偏移，但总体对我们后续的分析影响不大）\n文中使用的是经典力学分析，也可通过拉格朗日方程进行分析，式子更简略，对数理要求更高，此处暂不叙述。这部分大家不需要自己手算分析，因为过程和结果都比较复杂，可以用MATLAB solve函数进行求解。\n经典力学分析后得到的核心方程即原文中的式(3)(6)(9)\n$$ \\ddot{x} = \\frac{T - NR}{\\frac{I_{\\text{W}}}{R} + m_{\\text{W}} R} $$ $$I_{\\text{P}} \\ddot{\\theta} = \\left(PL + P_{\\text{M}} L_{\\text{M}}\\right) \\sin\\theta - \\left(NL + N_{\\text{M}} L_{\\text{M}}\\right) \\cos\\theta - T + T_{\\text{P}} $$ $$I_{\\text{m}} \\ddot{\\varphi} = T_{\\text{P}} + N_{\\text{M}} l \\cos\\varphi + P_{\\text{M}} l \\sin\\varphi$$ 其中\n$$x = x_b - (L + L_{\\text{M}})\\sin\\theta $$ $$\\dot{x} = \\dot{x}_b (L + L_{\\text{M}})\\dot{\\theta}\\cos\\theta$$ $$\\ddot{x} = \\ddot{x}_b - (L + L_{\\text{M}})\\ddot{\\theta}\\cos\\theta + (L + L_{\\text{M}})\\dot{\\theta}^2\\sin\\theta$$ 进行运动学分解，目的是为了给状态空间方程建立做准备。\n3.状态空间方程 首先大家需要理解一件事，矩阵本质是描述线性映射/线性变换的数学工具，我们在校所学的线性代数可能未能很好阐述矩阵、行列式、线性的作用和概念，建议大家去看MIT的线代教学或3B1B的线代科普，会有新的收获。 状态空间方程基本形式如下 $$ \\dot{x} = Ax+Bu \\\\ y = Cx+Du $$ 其中， A是状态矩阵，描述了系统自身演化规律 B是输入矩阵，描述输入对系统状态变化的影响（中学学过，导数可以理解为系统的变化率） C是输出矩阵，描述系统状态x到实际输出的线性映射关系 D是前馈矩阵，代表了输入对输出的直接实际影响 为了使大家更好理解状态空间矩阵，可以从此超链接到弹簧阻尼系统的例子。 同时，你需要理解以下这句话： LQR本身是线性系统的最优控制方法，处理形如$\\dot{x} = Ax+Bu$的线性状态空间模型。而轮腿是典型的非线性系统，真实动力学满足$\\dot{x} = f(x,u)$这个函数（或者说，映射，这个f是什么在这里你不用在意），因此需要用雅可比矩阵在平衡点处进行线性化。 （非线性关系“线性化”比较直观的体现）\n雅可比矩阵第一次接触应该会在高数下，从直角坐标系到球坐标系柱坐标系的变换，其具体作用可参考3B1B的视频。 定义系统非线性模型$\\dot{x} = f(x,u)$，其中状态向量$x $与输入向量$u$分别为 $$ \\boldsymbol{x} = \\begin{bmatrix} \\theta\\\\\\dot{\\theta} \\\\ x_b \\\\\\dot{x}_b \\\\\\varphi\\\\\\dot{\\varphi} \\end{bmatrix}, \\quad\\boldsymbol{u} = \\begin{bmatrix} T \\\\ T_P \\end{bmatrix} $$ 然后按照论文部分往下推导，在平衡点处进行线性化变成$\\dot{x} = Ax+Bu$的形式，$AB$矩阵均基于我们上述的力学运动学推导，那么把矩阵展开会有 $$ \\begin{bmatrix} \\dot{\\theta}\\\\\\ddot{\\theta} \\\\ \\dot{x_b} \\\\\\ddot{x}_b \\\\\\dot{\\varphi}\\\\\\ddot{\\varphi} \\end{bmatrix} = \\begin{bmatrix} 0 \u0026 1 \u0026 0 \u0026 0 \u0026 0 \u0026 0 \\\\ A_1 \u0026 0 \u0026 0 \u0026 0 \u0026 A_2 \u0026 0 \\\\ 0 \u0026 0 \u0026 0 \u0026 1 \u0026 0 \u0026 0\\\\A_3 \u0026 0 \u0026 0 \u0026 0 \u0026 A_4 \u0026 0 \\\\ 0 \u0026 0 \u0026 0 \u0026 0 \u0026 0 \u0026 1 \\\\ A_5 \u0026 0 \u0026 0 \u0026 0 \u0026 A_6 \u0026 0 \\end{bmatrix} \\begin{bmatrix} \\theta\\\\\\dot{\\theta} \\\\ x_b \\\\\\dot{x}_b \\\\\\varphi\\\\\\dot{\\varphi} \\end{bmatrix} + \\begin{bmatrix} 0 \u0026 0 \\\\ B_1 \u0026 B_2 \\\\ 0 \u0026 0 \\\\B_3 \u0026 B_4\\\\ 0 \u0026 0 \\\\ B_5 \u0026 B_6 \\end{bmatrix} \\begin{bmatrix} T \\\\ T_P \\end{bmatrix} $$其中\n$$ \\begin{gather*} A_1 = \u0026\\;\\frac{g\\,(L m_p + L M + L_M M)\\left(I_{\\text{M}} I_w + I_{\\text{M}} M R^2 + I_w M l^2 + I_{\\text{M}} R^2 m_p + I_{\\text{M}} R^2 m_w + M R^2 l^2 m_p + M R^2 l^2 m_w\\right)}{\\Delta} \\\\[4pt] A_2 = \u0026\\;\\frac{M^2 g l^2 \\left(I_w L + I_w L_M + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w\\right)}{\\Delta} \\\\[4pt] A_3 = \u0026\\;\\frac{g\\,(I_{\\text{M}} + M l^2)\\,(L m_p + L M + L_M M)\\left(I_w L + I_w L_M + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w\\right)}{\\Delta} \\\\[4pt] A_4 = \u0026\\;\\frac{M^2 g l^2 \\left(I_w L^2 + I_w L_M^2 + I_p R^2 + L^2 R^2 m_w + L_M^2 R^2 m_p + L_M^2 R^2 m_w + 2 I_w L L_M + 2 L L_M R^2 m_w\\right)}{\\Delta} \\\\[4pt] A_5 = \u0026\\;\\frac{M g l\\,(L m_p + L M + L_M M)\\left(I_w L + I_w L_M + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w\\right)}{\\Delta} \\\\[4pt] A_6 = \u0026\\;\\frac{M g l\\left(I_p I_w + I_w L^2 M + I_w L_M^2 M + I_p M R^2 + I_w L^2 m_p + I_p R^2 m_p + I_p R^2 m_w + L^2 M R^2 m_w + L_M^2 M R^2 m_p + L_M^2 M R^2 m_w + 2 I_w L L_M M + L^2 R^2 m_p m_w + 2 L L_M M R^2 m_w\\right)}{\\Delta} \\\\[4pt] B_1 = \u0026\\;-\\frac{I_{\\text{M}} I_w + I_{\\text{M}} M R^2 + I_w M l^2 + I_{\\text{M}} R^2 m_p + I_{\\text{M}} R^2 m_w + I_{\\text{M}} L M R + I_{\\text{M}} L_M M R + M R^2 l^2 m_p + M R^2 l^2 m_w + I_{\\text{M}} L R m_p + L M R l^2 m_p}{\\Delta} \\\\[4pt] B_2 = \u0026\\;\\frac{I_{\\text{M}} I_w + I_{\\text{M}} M R^2 + I_w M l^2 + I_{\\text{M}} R^2 m_p + I_{\\text{M}} R^2 m_w + M R^2 l^2 m_p + M R^2 l^2 m_w + I_w L M l + I_w L_M M l + L M R^2 l m_w + L_M M R^2 l m_p + L_M M R^2 l m_w}{\\Delta} \\\\[4pt] B_3 = \u0026\\;-\\frac{(I_{\\text{M}} + M l^2)\\left(I_w L + I_w L_M - I_p R + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w + L L_M R m_p\\right)}{\\Delta} \\\\[4pt] B_4 = \u0026\\;\\frac{I_{\\text{M}} I_w L + I_{\\text{M}} I_w L_M + I_w L M l^2 + I_w L^2 M l + I_w L_M M l^2 + I_w L_M^2 M l + I_p M R^2 l + I_{\\text{M}} L R^2 m_w + I_{\\text{M}} L_M R^2 m_p + I_{\\text{M}} L_M R^2 m_w + L M R^2 l^2 m_w + L^2 M R^2 l m_w + L_M M R^2 l^2 m_p + L_M^2 M R^2 l m_p + L_M M R^2 l^2 m_w + L_M^2 M R^2 l m_w + 2 I_w L L_M M l + 2 L L_M M R^2 l m_w}{\\Delta} \\\\[4pt] B_5 = \u0026\\;-\\frac{M l\\left(I_w L + I_w L_M - I_p R + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w + L L_M R m_p\\right)}{\\Delta} \\\\[4pt] B_6 = \u0026\\;\\frac{I_p I_w + I_w L^2 M + I_w L_M^2 M + I_p M R^2 + I_w L^2 m_p + I_p R^2 m_p + I_p R^2 m_w + L^2 M R^2 m_w + L_M^2 M R^2 m_p + L_M^2 M R^2 m_w + 2 I_w L L_M M + L^2 R^2 m_p m_w + I_w L M l + I_w L_M M l + 2 L L_M M R^2 m_w + L M R^2 l m_w + L_M M R^2 l m_p + L_M M R^2 l m_w}{\\Delta} \\\\[4pt] \\Delta = \u0026\\;I_{\\text{M}} I_p I_w + I_p I_w M l^2 + I_{\\text{M}} I_w L^2 m_p + I_{\\text{M}} I_p R^2 m_p + I_{\\text{M}} I_p R^2 m_w \\\\\u0026+ I_{\\text{M}} I_w L^2 M + I_{\\text{M}} I_w L_M^2 M + I_{\\text{M}} I_p M R^2 + I_{\\text{M}} L^2 M R^2 m_w + I_{\\text{M}} L_M^2 M R^2 m_p \\\\\u0026+ I_{\\text{M}} L_M^2 M R^2 m_w + 2 I_{\\text{M}} I_w L L_M M + I_w L^2 M l^2 m_p + I_p M R^2 l^2 m_p + I_p M R^2 l^2 m_w \\\\\u0026+ I_{\\text{M}} L^2 R^2 m_p m_w + 2 I_{\\text{M}} L L_M M R^2 m_w + L^2 M R^2 l^2 m_p m_w \\end{gather*} $$以上公式由MATLAB直接导出，可以看出篇幅确实不小，在MATLAB中进行仿真时也不必我们计算，使用solve函数即可，只需要记住$AB$矩阵中的各个系数可视作仅以腿长$L$作为变量，其余参数在建模之初便已赋值为常量。而系统输出设为$y=Cx=I_{6}x$，其中$I_{6}$是六维单位阵，即不经过系数转化直接将状态作为输出；输入不直接对输出产生影响，$D$矩阵为0。\n注：在经过关于控制系统可控性和可观性的学习后，我们可以知道系统的能控性矩阵和能观性矩阵均满秩，从而可控且可观。\n4.控制器设置 4.1线性二次调节器LQR 轮腿机器人的控制系统框图如下，估计器中既包含了姿态解算得到的$\\hat{\\psi}$（即IMU读到的航向角yaw），也有经过速度解算得到的$\\dot{x}$( 这个值还可能经过卡尔曼滤波进行速度估计，后面会讲到)，控制器中可以是LQR、MPC或二者串联的方法，当前我们只考虑最基本的LQR控制。\n先思考一下，我们从状态空间方程$\\dot{x} = Ax+Bu $得到了状态变化量与当前状态和输入的关系，但我们并未建立起反馈机制，即状态又如何作用于我们的输入使我们做出调整。\n这就好像我们开车时需要变道，我们知道打方向盘可以使车身偏离，这是输入对状态的影响；但我们不能一直打方向，看到变道成功后需要把方向盘回正，这是状态给输入的反馈。\n在PID中，我们通过$error=measure-target$得到误差，再对误差进行比例微分积分计算，得到新输出量；在LQR中，这一机制是由$u=-Kx$（负号体现反馈性）此线性控制率决定的，同时为了使我们能跟踪参考输入，取参考值矩阵为$x_d$，于是有$$ u=K({x_d}-x) \\\n意识流地感受一下，$x$越接近参考值，控制量$u$越小，符合我们的直觉，感觉上有点像纯P控制。在LQR中，我们的最优化过程即体现在对$K$求最优解上。那么，我们需要一个工具来衡量我们这个$K$是不是我们需要的最优解，于是我们引入代价函数： $J=\\int_{0}^{\\infty} (x^TQx+u^TRu) dt $\n对时间从0积分到无穷体现的是LQR在无限时域中的最优性（和MPC等控制器的代价函数有很大区别），不必纠结这个结论怎么来的，先接受他。代价函数中的两项均为二次型，也就是说$x^TQx$作为矩阵的运算就如同一般代数中的$qx^2$，$Q$我们称为状态权重矩阵（对角半正定），$R$我们称为控制权重矩阵（对角正定）；$x 、u$代表状态和输入，$J$是全局代价。\n关于正定半正定矩阵：正定矩阵$\\boldsymbol{A}$可使$x^TAx\u0026gt;0$，半正定矩阵$A$可使$x^TAx≥0$。为了方便调参、解耦等需求，我们取了QR为对角阵，此时则要求Q对角线上每一项严格≥0，R对角线上每一项严格\u0026gt;0\n也许看到这里你会有点晕：这个式子里明明没有$K$，为何会说$J$与反馈矩阵有关呢，而且凭空多出来一个$Q$和$R$是为什么？在此我再举一个浅薄的例子：\n作为一名普通高中生，做数学题时，我们总要制定做题策略以争取拿到更高分数：是慢慢写追求正确率，还是追求省多点时间做多点题？这些策略可以抽象为一个个权重，对于一系列状态和输入，自然有一系列权重，这就是我们的Q、R矩阵。\n$QR$矩阵即为整个LQR系统中最需要我们动态调整的参数矩阵。当我们希望系统更稳定，就对应调大$Q$中的参数，比如希望机体俯仰角$\\theta$快速收敛，就调大对应的$Q_1$权重；当调大$R$中的参数，会导致$J$变大，对应的输入$u$更小。 现在我们知道$QR$矩阵对$J$的影响，那么大概可以想象出这个二次型的图像。由于QR是半正定/正定矩阵，“碗”的开口始终向上，J总能求出一个最小值。 想象一个静态问题：$J=Qx^2+Ru^2$，我们能控制的是输入$u$（$u$是变量），求导得$\\frac{\\partial J}{\\partial u} =2Q\\frac{\\partial x}{\\partial u} + 2Ru= 0 $，如果J、Q、R、K都是标量，且u=-Kx，则会有K=Q/R，也就得出了K的表达式，这当然很容易；而实际上他们都是矩阵，为使代价函数$J$最小，输入$u$应满足下式\n$$ u=-R^{-1}B^TPx$$其中$P$矩阵满足黎卡提方程 $$A^TP+PA−PBR^{−1}B^TP+Q=0$$黎卡提方程推理请查看超链接。这需要一些泛函分析的知识，大家有兴趣可以去学习。\n因此，我们得出结论$K=-R^{-1}B^TP$，在MATLAB中，我们可以很容易地用lqr函数求出增益矩阵（求解代码形如$K=lqr(A,B,Q,R)$）。此时还有一个问题：我们的$AB$矩阵会因不同腿长$L$的变化而变化，造成增益矩阵均有所不同，而每次变腿长时都对增益矩阵求解计算量太大，无法将该算法部署在单片机上。因此如论文所述，我们在腿长每变化10mm时进行一次线性化并求此时的增益矩阵，拟合则相当于对此时每个增益矩阵中的参数$K_{ij}$进行记录后，用一条“光滑的曲线”把它们连接起来，同时预测没有计算的点会以一个什么样的趋势变化。 理论上，6X2大小的增益矩阵$K$中每个$K_{ij}$均进行三次多项式拟合后应共有12*(3+1)=48个系数，而经过实测检验二次多项式也可以满足需求，因此战队24赛季的平衡步兵采用了一个[12][3]的数组存放系数，便于直接调用。\n4.2运动学解算，虚拟模型控制VMC 好了，费了那么大劲我们总算是求出了$K$矩阵——在单片机中每个$K_{ij}$的形式是一个与“腿长”$L_0$有关的函数。但你理应意识到这个“腿长”只是我们简化分析设置的等效杆长，我们求出的$T_p$也只是等效中心轴输出力矩，我们接下来要做的工作就是通过虚拟模型控制，把这个等效量转化为我们实际需要让电机输出的关节力矩。\n正运动学解算 (五连杆机构简化图，实际模型轮毂在C的位置，对于四连杆可等效为$AE=0$) 为了方便新学习的同学补充一些知识，这里简单解释何为正逆运动学解算\n正运动学解算：关节空间-\u0026gt;工作空间，即知道角度($\\theta,\\phi\u0026hellip;$)求位姿($x,y,z$) 逆运动学解算：工作空间-\u0026gt;关节空间，即知道位姿反求角度 在此也就是通过$\\phi_{i},l_j$求出C点坐标。根据图像，左右两边分别列出$x_c,y_c$的表达式 $$\\begin{cases} x_B + l_2 \\cos\\phi_2 = x_D + l_3 \\cos\\phi_3 \\\\ y_B + l_2 \\sin\\phi_2 = y_D + l_3 \\sin\\phi_3 \\end{cases}$$ 解出$\\phi_2$：\n$$\\phi_2 = 2 \\arctan\\left( \\frac{B_0 + \\sqrt{A_0^2 + B_0^2 - C_0^2}}{A_0 + C_0} \\right)$$其中的参数为以下式子，而$x_D,y_D$又与$\\phi_4$相关 $$ \\begin{gather*} \u0026A_0 = 2l_2 (x_D - x_B) \\\\ \u0026B_0 = 2l_2 (y_D - y_B) \\\\ \u0026C_0 = l_2^2 + l_{BD}^2 - l_3^2\\\\ \u0026l_{BD} = \\sqrt{(x_D - x_B)^2 + (y_D - y_B)^2} \\\\ \\end{gather*}$$ 从而解出C的坐标，仅使用可直接观测的量$l_1,\\phi_1$和解算可得的$\\phi_2$（与$\\phi_4$相关）表示： $$ \\begin{cases} x_C = l_1 \\cos\\phi_1 + l_2 \\cos\\phi_2 \\\\ y_C = l_1 \\sin\\phi_1 + l_2 \\sin\\phi_2 \\end{cases} $$并进一步得到极坐标，即等效的单摆倒立摆和真实角度与腿长的映射： $$\\begin{cases} L_0 = \\sqrt{(x_C - l_5/2)^2 + y_C^2} \\\\ \\phi_0 = \\arctan\\left( \\frac{y_C}{x_C - l_5/2} \\right) \\end{cases} $$ 正运动学解算即完成，该过程理解上并不复杂，也不需要我们进行手算，直接将公式套入MATLAB中即可算出来，不必在这上面过多纠结停留。\nVMC雅可比矩阵 王工关于此处更详细的解释如下：五连杆运动学解算与VMC 对于上面的参数我们定义向量$\\boldsymbol{x} = {\\begin{bmatrix} L_0 \u0026amp; \\phi_0\\ \\end{bmatrix}}^T$，$\\boldsymbol{q} = {\\begin{bmatrix} \\phi_1 \u0026amp; \\phi_4\\ \\end{bmatrix}}^T$，在式(13)中如果我们结合式(12)展开$x_c,y_c$，那么显然可以得到函数映射关系$\\boldsymbol{x}=f(\\boldsymbol{q})$。再提醒一次，$L_0$是腿的等效杆，$\\phi_0$是等效杆与实际髋关节连线的夹角。 再定义向量$\\boldsymbol{T} = {\\begin{bmatrix} T_1 \u0026amp; T_2\\ \\end{bmatrix}}^T$和$\\boldsymbol{F} = {\\begin{bmatrix} F_0 \u0026amp; T_p\\ \\end{bmatrix}}^T$，其中$T_1,T_2$分别为前后两关节电机输出力矩，$F_0$为沿等效单杆机构的推力（新的未知量，等下提到如何解），$T_p$为沿等效中心轴的力矩。在论文中，玺佬核心思路是求出将关节速度$\\boldsymbol{\\dot{q}}$映射为五连杆姿态变化率$\\boldsymbol{\\dot{x}}$ 的雅可比矩阵$J$，再由虚功原理得出矩阵$T$与$F$的关系，从而解出真实髋关节力矩参考值$T_1,T_2$，从而核心结论为： $$ \\begin{gathered} \\delta\\boldsymbol{x} = \\boldsymbol{J}\\delta\\boldsymbol{q} \\\n\\boldsymbol{T} = \\boldsymbol{J}^\\mathrm{T} \\boldsymbol{F} \\end{gathered} $$ 于是到目前为止，我们对各个参数可知道的信息为：\n$\\boldsymbol x$：其中的$L_0,\\phi_0$ 经过一系列变换可直接由观测得到的$\\phi_1,\\phi_4$解得，已知量 $\\boldsymbol J$：雅可比矩阵，可知，待求量 $\\boldsymbol F$：$T_p$由反馈$u=-Kx$可求得，已知量；$F_0$，尚未得到它与已知量的关系 $\\boldsymbol T$：真实髋关节力矩，目标值 对于待求的雅可比矩阵，论文中提供了两种方法： ①直接对$\\boldsymbol{x}=f(\\boldsymbol{q})$求全微分，从而得到雅可比矩阵： $$ \\begin{gathered} \\begin{cases} \\delta L_0 = \\dfrac{\\partial f_1}{\\partial \\varphi_1} \\partial \\varphi_1 + \\dfrac{\\partial f_1}{\\partial \\varphi_4} \\partial \\varphi_4 \\\\[6pt] \\delta \\varphi_0 = \\dfrac{\\partial f_2}{\\partial \\varphi_1} \\partial \\varphi_1 + \\dfrac{\\partial f_2}{\\partial \\varphi_4} \\partial \\varphi_4 \\end{cases} \\\\ \\boldsymbol J = \\begin{bmatrix} \\dfrac{\\partial f_1}{\\partial \\varphi_1} \u0026 \\dfrac{\\partial f_1}{\\partial \\varphi_4} \\\\[8pt] \\dfrac{\\partial f_2}{\\partial \\varphi_1} \u0026 \\dfrac{\\partial f_2}{\\partial \\varphi_4} \\end{bmatrix} \\end{gathered} $$ 然而，只凭感觉我们会发现上述的模型表达式中包含大量平方与三角函数及其嵌套运算（展开式里既有arctanx也有他的平方与开根运算），即使用了MATLAB求解也会无法部署至单片机上。\n②由于雅可比矩阵$J$实际描述的是两坐标微分的线性映射关系，因此我们可以通过计算速度映射关系来得到雅可比矩阵，即论文1.2.3静力学解算的方法\n对式(12)求导： $$ \\begin{cases} \\dot{x}_C = -l_1 \\dot{\\phi}_1 \\sin\\phi_1 - l_2 \\dot{\\phi}_2 \\sin\\phi_2 \\\\\\dot{y}_C = l_1 \\dot{\\phi}_1 \\cos\\phi_1 + l_2 \\dot{\\phi}_2 \\cos\\phi_2 \\end{cases} $$ 经过一系列消去变换得到： $$ \\begin{cases} \\dot{x}_C = -l_2 \\dot{\\phi}_1 \\sin\\phi_1 - l_2 \\left( \\frac{(\\dot{x}_D - \\dot{x}_B)\\cos\\phi_3 + (\\dot{y}_D - \\dot{y}_B)\\sin\\phi_3}{l_2 \\sin(\\phi_3 - \\phi_2)} \\right) \\sin\\phi_2 \\\\\\dot{y}_C = l_2 \\dot{\\phi}_1 \\cos\\phi_1 + l_2 \\left( \\frac{(\\dot{x}_D - \\dot{x}_B)\\cos\\phi_3 + (\\dot{y}_D - \\dot{y}_B)\\sin\\phi_3}{l_2 \\sin(\\phi_3 - \\phi_2)} \\right) \\cos\\phi_2 \\end{cases} $$用MATLAB进行符号计算后： $$ \\begin{bmatrix} \\dot{x}_C \\\\\\dot{y}_C \\end{bmatrix} = \\begin{bmatrix} \\dfrac{l_1 \\sin(\\phi_1 - \\phi_2) \\sin\\phi_3}{\\sin(\\phi_2 - \\phi_3)} \u0026 \\dfrac{l_4 \\sin(\\phi_3 - \\phi_4) \\sin\\phi_2}{\\sin(\\phi_2 - \\phi_3)} \\\\[1em] -\\dfrac{l_1 \\sin(\\phi_1 - \\phi_2) \\cos\\phi_3}{\\sin(\\phi_2 - \\phi_3)} \u0026 -\\dfrac{l_4 \\sin(\\phi_3 - \\phi_4) \\cos\\phi_2}{\\sin(\\phi_2 - \\phi_3)} \\end{bmatrix} \\begin{bmatrix} \\dot{\\phi}_1 \\\\\\dot{\\phi}_4 \\end{bmatrix} $$ 记作： $$ \\begin{bmatrix} \\dot{x}_C \\\\\\dot{y}_C \\end{bmatrix} = \\boldsymbol{J} \\begin{bmatrix} \\dot{\\phi}_1 \\\\\\dot{\\phi}_4 \\end{bmatrix} $$当然，方法②的$J$与式(14)的$J$必然不同，从而$\\boldsymbol{T}$与$\\boldsymbol{F}$的关系也有所不同。事实上，若用此雅可比矩阵，则真实关节力矩与虚拟力映射中间还应左乘一变换矩阵$\\boldsymbol{M}$和一旋转矩阵$\\boldsymbol{R}$，展开后的形式为：\n$$ \\begin{gathered} \\boldsymbol{R} = \\begin{bmatrix} \\cos(\\phi_0 - \\pi/2) \u0026 -\\sin(\\phi_0 - \\pi/2) \\\\\\sin(\\phi_0 - \\pi/2) \u0026 \\cos(\\phi_0 - \\pi/2) \\end{bmatrix}\\\\ \\boldsymbol{M} = \\begin{bmatrix} 0 \u0026 -\\dfrac{1}{L_0} \\\\ 1 \u0026 0 \\end{bmatrix}\\\\ \\begin{bmatrix} T_1 \\\\ T_2 \\end{bmatrix} = \\boldsymbol{J}^T \\boldsymbol{R} \\boldsymbol{M} \\begin{bmatrix} F_0 \\\\ T_p \\end{bmatrix} \\end{gathered} $$MATLAB求解后得到最终结果： $$ \\begin{bmatrix} T_1 \\\\ T_2 \\end{bmatrix} = \\begin{bmatrix} \\dfrac{l_1 \\sin(\\phi_0 - \\phi_3) \\sin(\\phi_1 - \\phi_2)}{\\sin(\\phi_3 - \\phi_2)} \u0026 \\dfrac{l_1 \\cos(\\phi_0 - \\phi_3) \\sin(\\phi_1 - \\phi_2)}{L_0 \\sin(\\phi_3 - \\phi_2)} \\\\[1.5em] \\dfrac{l_4 \\sin(\\phi_0 - \\phi_2) \\sin(\\phi_3 - \\phi_4)}{\\sin(\\phi_3 - \\phi_2)} \u0026 \\dfrac{l_4 \\cos(\\phi_0 - \\phi_2) \\sin(\\phi_3 - \\phi_4)}{L_0 \\sin(\\phi_3 - \\phi_2)} \\end{bmatrix} \\begin{bmatrix} F_0 \\\\ T_p \\end{bmatrix} $$ 十分简洁。\n4.3未求解的数 在上面，我们已经解决了雅可比矩阵的求解，找到了$\\boldsymbol T$与$\\boldsymbol F$之间的映射关系，但还有一个量$F_0$我们还未将其表示。容易想到对于单腿有固定腿长下沿杆推力$F_l = 0.5Mgcos\\theta$，其中$M$为机体质量；而若我们希望能实现变腿长且“不僵硬”，则需将腿杆建模为一个弹簧-阻尼系统，并使用PD控制器对其进行控制，将这个力叠加在$F_l$上，即$F_0=F_l+K_p(Height_{Ref}-Height)+K_d(0-\\dot{Height})$（注，为保持机身稳定，这里的参考值是高度） $$ \\begin{gathered} \\text{LegLength} = \\sqrt{(x_C - l_5/2)^2 + y_C^2} \\\\ \\text{Height} = \\text{LegLength} \\cdot \\cos\\theta \\end{gathered} $$ 4.4速度解算 王工的论文中对于轮速的处理为 $$ \\begin{gathered} \\dot{x_b} = \\omega R+(L_0sin\\theta)^{'} \\end{gathered} $$ 其中$\\omega$为驱动轮转子相对大地角速度。 根据式(23)，轮腿机器人的机体平动速度 $\\dot{x}_b$ 可通过驱动轮角速度 $\\omega$ 与腿部运动项 $(L_0 \\sin \\theta)\u0026rsquo;$ 直接解算得到。然而，在实际系统中，这一方法存在三方面固有缺陷：\n测量噪声放大：驱动轮角速度 $\\omega$ 由编码器测量，高频噪声明显；微分项 $(L_0 \\sin \\theta)\u0026rsquo;$ 进一步放大了角度 $\\theta$ 与腿长 $L_0$ 的噪声，导致速度估计剧烈抖动。 打滑与模型误差：式(23) 假设驱动轮纯滚动，一旦地面湿滑或加速过快，$\\omega R$ 不再等于轮心相对大地的平动速度，直接计算会引入不可观测的偏差。 积分漂移：若采用加速度积分估计速度（如 IMU 数据），短时精度高但长时间积分后零偏漂移严重；而式(23) 虽然无长期漂移，但动态响应滞后且易受腿部运动耦合影响。 为了融合两者的互补特性，卡尔曼滤波 成为一种最优的实时速度估计方案。它能根据系统动态模型与传感器噪声统计特性，动态调整加速度积分（预测）与轮速测量（校正）的权重，输出最小方差意义下的最优速度估计。 卡尔曼滤波原理与速度估计模型 要推导卡尔曼滤波全式子所需篇幅过大，也不适合文档这个载体，因此此处我只会给出一些关键式子，具体的板书推导我会给出DR.CAN的视频卡尔曼滤波数学推导，我十分推荐大家把他的视频看完，感受何为醍醐灌顶。 先明确卡尔曼滤波是什么：他是一个观测器，它根据预测值（模型推演）和测量值（传感器），按各自的不确定性（方差）动态计算权重，实时估计出系统状态。 设置这么一个场景：我们有两个秤，对于同一物品，A称出来重$Z_1$kg，B称出来$Z_2$kg，同时我们已知A、B秤称重的标准差分别为$\\sigma_1$和$\\sigma_2$，且服从正态分布，我们需要通过数据融合的思想使其称重结果更可信，这个结果记为估计值$\\hat{Z}$。 由于正态分布的性质，我们设$\\hat{Z}=(1-K)Z_1+Z_2=Z_1+K(Z_2-Z_1)$。显然$K=0$时$\\hat{Z}=Z_1$，$K=1$时$\\hat{Z}=Z_2$ 。我们希望找到一个最“优”的$K$值使$\\hat{Z}$置信度最高，即求出使融合后的方差$\\sigma_{\\hat{Z}}$最小的$K$。 由概率论知识，$\\sigma_{\\hat{Z}}=Var((1-K)Z_1+Z_2)$，由于AB称出来的结果相互独立，则$\\sigma_{\\hat{Z}}=Var((1-K)Z_1)+Var(Z_2)=(1-K)^2Var(Z_1)+Var(Z_2)=(1-K)^2\\sigma_1^2+\\sigma_2^2$ 考虑有一维离散系统，状态变量为机体速度 $v_k$加速度计提供控制输入 $a_m$，轮速提供观测值 $z_k = v_m$。系统模型如下： 状态预测方程（基于加速度积分）：$\\hat{v}{k|k-1} = \\hat{v}{k-1} + a_m \\cdot \\Delta t$ 预测协方差：$P_{k|k-1} = P_{k-1} + Q$ 其中 $Q$ 为过程噪声协方差，反映加速度计积分的不确定性。 测量更新方程（利用轮速校正）： 卡尔曼增益： $$ \\begin{gathered} K_k = \\frac{P_{k|k-1}}{P_{k|k-1} + R} \\end{gathered} $$ 状态估计更新： $$ \\begin{gathered} \\hat{v}_k = \\hat{v}_{k|k-1} + K_k \\left( z_k - \\hat{v}_{k|k-1} \\right) \\end{gathered} $$ 估计协方差更新： $$ \\begin{gathered} P_k = (1 - K_k) P_{k|k-1} \\end{gathered} $$其中 $R$ 为测量噪声协方差，反映轮速测量的可信度（打滑时 $R$ 增大）。\n通过递归执行上述五步，卡尔曼滤波在每一时刻都给出当前速度的最优估计。该估计在统计意义上融合了加速度积分的高频响应与轮速测量的无漂移特性，同时对打滑和噪声具有鲁棒性。将滤波后的速度 $\\hat{v}_k$ 替代原有的直接微分项，可显著提升轮腿的纵向运动控制的稳定性与精度。\n4.5综合运动控制 该部分回到了我们熟悉的PID控制且几乎都使用了PD控制器。以IMU获取到的Yaw角度角速度作为反馈，即可完成转向控制，进一步实现轮腿小陀螺；以Roll的角度角速度作为反馈，即可完成横滚控制，从而保持单腿上坡等情景下机身平衡；腿长控制在4.3中也进行了叙述。进而我们可以将第4部分一开始的图加上各种控制器变成论文中的模样。\n该部分在理论上并没有难理解的部分，PD控制器在培训阶段应有涉及，实际参数需结合调车情况分析调整，可能会与仿真有较大差别。\n5.仿真软件选择 针对不同的建模与仿真需求，这里介绍四款常用的仿真软件，可根据自身平台与侧重点选择。\nMATLAB/Simulink\nMATLAB/Simulink是商业软件，也是最常用于算法原理验证与数学建模的工具，支持Windows、macOS、Linux。其在矩阵运算、符号计算和控制系统工具箱方面几乎无可替代，例如前文求解增益矩阵所用的lqr函数、solve符号求解，直接在MATLAB中一条命令即可完成。Simulink提供丰富的模块库，可快速搭建控制框图，验证LQR、MPC等算法的正确性。但其3D物理仿真能力很弱（Simscape Multibody在接触、碰撞、打滑等动力学细节上表现一般），无法模拟轮腿与地面真实的摩擦、打滑过程，因此更适合做\u0026quot;算法正确性\u0026quot;验证，而不是\u0026quot;整机真实感\u0026quot;仿真。\nGazebo\nGazebo是ROS生态中最流行的开源3D物理仿真软件，主要面向Linux（Ubuntu）平台，Windows/macOS支持较差。它通过URDF/SDF格式建模，支持ODE、Bullet、DART等多种物理引擎，能较真实地模拟接触、摩擦、打滑等细节，并可配合Rviz等工具搭建完整的感知-控制闭环，社区资料丰富、用户基数大。缺点是环境配置繁琐（需要Ubuntu+ROS+Gazebo版本严格匹配），学习曲线陡峭，物理引擎精度一般，高频运行时容易出现数值抖动，且对新手不太友好。\nWebots\nWebots是开源免费的3D机器人仿真软件，跨平台支持Windows/macOS/Linux，图形界面友好，上手门槛远低于Gazebo。它基于ODE物理引擎，内置丰富的轮式机器人模型，支持C/C++、Python、ROS等多种控制器语言，非常适合教学、算法快速验证和课程作业。缺点是ROS集成不如Gazebo成熟，社区生态相对较小，物理细节（如打滑、柔性接触）不如专业引擎精细，自定义场景需要花时间学习其节点式的建模方式。\nMuJoCo\nMuJoCo是DeepMind开源的物理仿真引擎，以速度快、数值稳定性好、接触动力学精度高著称，是当下强化学习（RL）训练的主流选择。轮腿这类欠驱动、强耦合的系统若采用RL方法训练，MuJoCo几乎是首选平台。其Python接口dm_control简洁易用，使用MJCF格式建模，也支持C++接口，跨平台（Windows/macOS/Linux）。缺点是它更偏\u0026quot;引擎\u0026quot;而非\u0026quot;软件\u0026quot;，缺少完整的三维可视化界面与调试面板，需要配合其他工具使用，对以理论学习为主、尚未接触RL的队员来说，入门曲线不如MATLAB直观。\n6.更新 2026.4.1 完成初版\n2026.4.15 完成控制器设置、新增仿真软件选择部分\n2026.5.7 完成卡尔曼滤波部分，略显粗糙\n2026.8.10 完成对博客格式适配，添加各仿真的说明\n","date":"2026-08-10T00:00:00Z","permalink":"/tutorials/2026-8-10_control_of_wheel_leg/","title":"轮腿机器人控制"}]