机器人嵌入式硬件开发移动开发【免费下载链接】foc-wheel-legged-robotOpen source materials for a novel structured legged robot, including mechanical design, electronic design, algorithm simulation, and software development. | 一个新型结构的轮腿机器人开源资料包含机械设计、电子设计、算法仿真、软件开发等材料项目地址https://gitcode.com/gh_mirrors/fo/foc-wheel-legged-robot点击查看免费下载本文基于 foc-wheel-legged-robot 项目的matlab模块完整梳理该轮腿机器人“模型搭建与算法仿真”环节的技术脉络如何用 MATLAB 符号计算把双腿机构解析化简为单根虚拟腿并导出leg_pos/leg_spd/leg_conv三个 M 函数如何建立整机状态空间方程、离散化后求解 LQR 反馈矩阵并拟合出lqr_k以及这些符号化简结果如何经 MATLAB Coder 转换为 C 代码后在 ESP32 实机控制中被调用。读完后你可以掌握该机器人 VMC LQR 平衡算法背后的完整数学模型与推导链路并能按仓库文件逐一对照复现。一、仿真环境与模块概览本项目的算法仿真使用MATLAB (R2022b)完成围绕平衡算法的设计和仿真主要做了以下五件事引自 matlab/README.md腿部连杆姿态计算系统状态空间方程建立LQR 反馈矩阵计算和拟合Simscape Multibody 物理模型搭建Simulink 控制算法仿真模块内各文件的职责如下继承原文档文件介绍并补充链接文件职责导出的 M 函数leg_func_calc.m腿部解算和 VMC 映射计算流程步骤 1~2leg_pos.m由关节电机角度求腿部姿态leg_spd.m由关节电机角速度求腿部运动速度leg_conv.m由虚拟腿目标扭矩和推力求电机所需输出的力矩sys_calc.m求系统状态方程与反馈矩阵流程步骤 3~4lqr_k.m代入腿长 L0 后返回该腿长对应的反馈矩阵 Kleg_sim.slx腿部仿真 Simulink 模型含腿部 Multibody 物理模型用于腿部 VMC 算法验证—leg_sim_2020a.slx导出的旧版本模型适用 R2020a 及以上版本—sys_sim.slx整机平衡仿真 Simulink 模型含整机 Multibody 物理模型用于 VMC LQR 整体算法验证—sys_sim_2020a.slx导出的旧版本模型适用 R2020a 及以上版本—注原文档说明导出的 M 函数都在仿真模型中直接使用使用 MATLAB Coder 工具箱直接转换为 C 语言后即可直接在实机代码中使用在本项目主控代码中可以找到同名文件对应 esp32-controller/software/src 下的leg_pos.c、leg_spd.c、leg_conv.c、lqr_k.c。版本说明.slx为二进制模型文件需在对应版本 MATLAB 中打开默认文件面向 R2022b低版本用户请使用带_2020a后缀的导出模型。二、平衡控制算法的整体流程平衡算法参考了 matlab/README.md 中给出链接的一篇专栏文章仅有少许不同原文档因此只描述大致过程。可以概括为以下 5 个步骤与原文档保持一致对腿部结构进行化简求得化简前后各符号的对照关系从而可以使用电机数据计算出腿部的姿态求得化简后的虚拟腿受力到电机扭矩的映射关系从而可以使用 VMCVirtual Model Control虚拟模型控制思想对化简后的腿部进行控制进行经典力学分析和符号化简求得系统状态空间方程设定 LQR 中的权重系数求取反馈矩阵 K并对不同腿长求出的 K 进行拟合进行状态反馈控制。原文档注代码中变量名称与参考文章所述基本保持一致读者可以对照查看。从整体结构看该算法分为两层VMC 层负责“腿”——把期望的虚拟腿推力 F 和髋部力矩 Tp 折算成两个关节电机的输出扭矩LQR 层负责“整机”——以机体姿态、位置等状态量做全状态反馈输出轮端驱动扭矩 T 和髋部力矩 Tp。两个脚本leg_func_calc.m与sys_calc.m分别对应步骤 1~2 与步骤 3~4导出函数则对应步骤 5 的实机实现。三、腿部结构解算与 VMC 映射步骤 1~2以下分析基于 leg_func_calc.m它是一个符号计算脚本用关节电机可采集的phi1、phi4、dphi1、dphi4计算腿长l0和腿部角度phi0导出leg_pos及其移动速度dl0和dphi0导出leg_spd并求出 VMC 所需函数leg_conv由推力 F 和扭矩 Tp 转换为电机扭矩 T。3.1 腿部几何模型与杆长参数脚本 matlab/leg_func_calc.m 中设定的符号与腿部杆长为符号取值 (m)含义l10.05第一段连杆长度l20.105第二段连杆长度l3 l2第三段连杆长度l4 l1第四段连杆长度l50.06几何偏置量几何计算过程第 14~28 行为xb l1*cos(phi1); % b 点坐标第一段连杆末端 yb l1*sin(phi1); xd l5 l4*cos(phi4); % d 点坐标含 l5 偏置 yd l4*sin(phi4); % 以 b、d 为圆心、l2、l3 为半径解两圆交点求 phi2 A0 2*l2*(xd - xb); B0 2*l2*(yd - yb); C0 l2^2 (xd - xb)^2 (yd - yb)^2 - l3^2; phi2 2*atan((B0 sqrt(A0^2 B0^2 - C0^2)) / (A0 C0)); xc xb l2*cos(phi2); % c 点质心/机身体连接点坐标 yc yb l2*sin(phi2); l0 sqrt((xc - l5/2)^2 yc^2); % 虚拟腿长 phi0 atan2(yc, xc - l5/2); % 虚拟腿与 x 轴夹角即已知两个关节角phi1、phi4后通过两圆相交求得中间铰点phi2进而得到机体系点c的坐标以(l5/2, 0)为参考原点把整条腿化简为一根虚拟腿——长度l0、姿态角phi0。这正是步骤 1 所述“化简前后符号对照关系”的具体实现phi1/phi4是电机可直接测量的关节量l0/phi0是控制律使用的虚拟腿量。关节与变量的对应关系可结合 esp32-controller/README.md 的说明靠前的关节电机对应phi4靠后的关节电机对应phi1。3.2 虚拟腿姿态解算leg_pos姿态结果导出为 M 函数leg_func_calc.m 第 30~32 行pos [l0; phi0]; matlabFunction(pos, File, leg_pos);对应 leg_pos.m 的函数签名为pos leg_pos(phi1, phi4) % pos(1) l0腿长pos(2) phi0腿部角度该文件由 Symbolic Math Toolbox 自动生成文件头注释注明内容为纯三角函数化简式无迭代、无查表适合实时调用。3.3 腿部速度解算leg_spd对l0、phi0分别关于phi1、phi4求偏导得到 2×2 雅可比矩阵第 34~39 行J [diff(l0,phi1) diff(l0,phi4); diff(phi0,phi1) diff(phi0,phi4)]; spd J * [dphi1; dphi4]; matlabFunction(spd, File, leg_spd);导出函数 leg_spd.m 的签名为spd leg_spd(dphi1, dphi4, phi1, phi4) % spd(1) dl0 腿长变化速度 % spd(2) dphi0 腿部角度变化速度即腿部线速度和角速度可由关节角速度经雅可比线性映射直接求得供实机做腿长速度/加速度估计使用。3.4 虚拟腿受力到关节扭矩的映射leg_convVMC 的核心是“期望力 → 关节力矩”的静力折算。脚本 第 45~48 行syms F Tp; % F虚拟腿推力Tp髋部力矩 T J * [F; Tp]; % 转置雅可比映射 matlabFunction(T, File, leg_conv);导出函数 leg_conv.m 的签名为T leg_conv(F, Tp, phi1, phi4) % T(1) T1T(2) T2两关节电机所需扭矩由虚功原理关节扭矩向量等于雅可比矩阵的转置与虚拟腿广义力[F; Tp]的乘积——这正是T J*[F; Tp]的含义。由此上位控制律只需给出“这条虚拟腿该用多大的推力 F、髋部该输出多大的力矩 Tp”leg_conv即可完成到两个关节电机电力的换算实现对化简后腿部的 VMC 控制步骤 2 完成。四、系统状态空间方程与 LQR 反馈矩阵步骤 3~4以下分析基于 sys_calc.m。脚本注释说明其用途对于每一个腿长 L0求解一次系统状态空间方程然后求得反馈矩阵 K对不同 K 关于 L0 进行拟合得到lqr_k(L0)。4.1 结构参数与集总模型sys_calc.m 第 18~20 行 定义的机器人结构参数为SI 单位制符号取值说明R0.02车轮半径 (m)L、Lm L0/2主质量、附加质量的力臂l0附加几何项mw0.251327车轮质量 (kg)mp0.0248附加质量 (kg)M0.325主质量 (kg)Iw5.02655e-05车轮转动惯量 (kg·m²)Ip(9.66667e-07 7.9975e-06)*2附加质量相关转动惯量 (kg·m²)Im0.000338542腿部转动惯量 (kg·m²)g9.8重力加速度 (m/s²)其中腿长L0在0.04:0.01:0.14共 11 个采样点范围内循环求解第 7 行。4.2 三个动力学方程脚本先写出主质量水平/竖直方向所需广义力Nm、Pm再加上附加质量mp的贡献得到N、P第 23~26 行随后联立三个方程第 28~31 行% equ1 车轮滚动方程由驱动力矩 T 与轮端负载 N 求平动加速度 x2 equ1 x2 - (T - N*R) / (Iw/R mw*R); % equ2 主质量绕接触点的转动方程重力/惯性力矩、驱动力矩 T、髋部力矩 Tp 平衡 equ2 (P*L Pm*Lm)*sin(theta) - (N*L Nm*Lm)*cos(theta) - T Tp - Ip*theta2; % equ3 腿部转动方程髋部力矩 Tp 与主质量作用力矩平衡 equ3 Tp Nm*l*cos(phi) Pm*l*sin(phi) - Im*phi2; [x2, theta2, phi2] solve(equ1, equ2, equ3, x2, theta2, phi2);符号含义见 第 13 行 注释theta1 dTheta、theta2 ddTheta即theta为腿部相对机体的摆角x为机体水平位置phi为机体俯仰角T为轮端驱动扭矩Tp为髋部输出力矩。用solve求出三个加速度后系统被表达为关于广义力[T; Tp]的显式动力学形式。4.3 线性化与状态空间方程随后脚本对状态[theta1; theta2; x1; x2; phi1; phi2]即[θ, θ̇, x, ẋ, φ, φ̇]分别关于状态变量和输入[T; Tp]求雅可比并在平衡点各角度为 0即竖直站立、无位移处取值得到状态空间矩阵第 34~37 行Ja jacobian([theta1;theta2;x1;x2;phi1;phi2], [theta theta1 x x1 phi phi1]); Jb jacobian([theta1;theta2;x1;x2;phi1;phi2], [T Tp]); A vpa(subs(Ja, [theta theta1 x x1 phi phi1], [0 0 0 0 0 0])); B vpa(subs(Jb, [theta theta1 x x1 phi phi1], [0 0 0 0 0 0]));4.4 离散化与 LQR 求解[G, H] c2d(eval(A), eval(B), 0.005); % 按 5ms200Hz离散化 Q diag([1 10 100 20 1000 1]); % 状态权重 R diag([1 1]); % 输入权重 Ks(:,:,step) dlqr(G, H, Q, R); % 离散 LQR 求反馈矩阵权重矩阵Q对应的状态依次为θ, θ̇, x, ẋ, φ, φ̇状态θθ̇xẋφφ̇权重 Q1101002010001可以看到机体俯仰角φ的权重最大1000即把“保持平衡”置于最高优先级位置x次之100腿摆角θ最小——这与“双轮腿机器人先求稳、再运动”的控制目标一致。5ms 的离散周期对应 200Hz与实机陀螺仪MPU6050DMP 的 200Hz 输出频率相对应见 esp32-controller/README.md 对 DMP 频率的说明。4.5 反馈矩阵对腿长的拟合lqr_k由于K依赖于当前腿长脚本对 11 个 L0 采样点分别求出K后对K的每个元素关于L0做三次多项式拟合第 51~62 行for x 1:2 for y 1:6 p polyfit(L0s, reshape(Ks(x,y,:), 1, length(L0s)), 3); K(x,y) p(1)*L0^3 p(2)*L0^2 p(3)*L0 p(4); end end matlabFunction(K, File, lqr_k); vpa(subs(K, L0, 0.07)) % 代入 L00.07 打印矩阵 K导出结果 lqr_k.m 的签名为K lqr_k(L0)返回 2×6 反馈矩阵第 1 行对应轮端扭矩T的增益第 2 行对应髋部力矩Tp的增益。以第 1 行为例系数取自 lqr_k.m 第 10~15 行K(1,1) 78.6044781*L0^3 - 39.9138887*L0^2 - 5.1269914*L0 0.024809986 K(1,2) -0.36215743*L0^3 0.061259177*L0^2 0.016418561*L0 - 0.0020128854 K(1,3) 9.9144642*L0^3 - 7.7528647*L0^2 - 0.71381840*L0 0.0063399588 K(1,4) 0.0080964608*L0^3 - 0.0053304803*L0^2 0.0025984670*L0 - 1.8096e-5 K(1,5) 39.0028359*L0^3 - 13.9123667*L0^2 - 2.5389326*L0 0.027201568 K(1,6) -0.56548413*L0^3 0.21739994*L0^2 - 0.032202579*L0 0.0039767820这样实机无需在 MCU 上运行任何矩阵求解给定当前腿长代入多项式即可得到K运算量仅为若干乘加。脚本最后打印的L0 0.07处的K与实机代码中默认目标腿长target.legLength 0.07fmain.cpp 第 544 行保持一致可视为标定工况。五、Simscape Multibody 与 Simulink 物理模型验证matlab目录中的两个.slx模型是上述解析算法的物理闭环验证环境二进制模型文件需 MATLAB 打开leg_sim.slx腿部仿真模型内含腿部 Simscape Multibody 物理模型用于腿部 VMC 算法验证——即在第 3 节leg_pos/leg_spd/leg_conv导出的 M 函数接入物理模型后检验“虚拟腿推力/力矩 → 关节扭矩”映射在真实动力学下的表现sys_sim.slx整机平衡仿真模型内含整机 Simscape Multibody 物理模型用于VMC LQR 整体算法验证——把lqr_k给出的状态反馈律与腿部 VMC 层串成完整控制链路在物理仿真中观察起立、行走、受扰恢复等平衡表现两个模型各提供*_2020a.slx旧版本导出供 R2020a 及以上低于 R2022b的版本打开。仿真流程与sys_calc.m的分工是脚本负责“纸面”推导解析状态方程 离线求 K.slx模型负责“物理”检验含摩擦、柔性、非线性等符号模型没有的因素。文章开头展示的整机仿真效果图即为sys_sim类模型的 Simscape Multibody 仿真表现。六、实机落地导出函数在 ESP32 控制链路中的调用esp32-controller/README.md 明确指出leg_pos.c/h、leg_spd.c/h、leg_conv.c/h、lqr_k.c/h是由 MATLAB 直接生成的 C 代码文件文件头注释为 “MATLAB Coder”不参与逻辑设计仅作为数学函数库使用。它们在 main.cpp 中的实际调用链如下。6.1 腿部姿态更新任务LegPos_UpdateTaskmain.cpp 第 452~491 行每 4ms 执行一次对左右两腿leg_pos(leftJoint[1].angle, leftJoint[0].angle, legPos); // 后关节 phi1、前关节 phi4 leftLegPos.length legPos[0]; // l0 leftLegPos.angle legPos[1]; // phi0 leg_spd(leftJoint[1].speed, leftJoint[0].speed, leftJoint[1].angle, leftJoint[0].angle, legSpd); // dl0, dphi0参数顺序与 esp32-controller/README.md 的约定一致靠前的关节电机对应phi4靠后的关节电机对应phi1。该任务还基于dLength差分估计腿长加速度ddLength一阶低通滤波系数 0.5供后续离地检测使用。6.2 主控制任务状态反馈闭环Ctrl_Taskmain.cpp 第 530 行起每 4ms 执行一次其状态向量构造与sys_calc.m的雅可比状态顺序完全对应stateVar.phi imuData.pitch; // 机体俯仰角MPU6050 stateVar.x (leftWheel.angle rightWheel.angle) / 2 * wheelRadius; // 机体位移 stateVar.theta (leftLegPos.angle rightLegPos.angle) / 2 - M_PI_2 - imuData.pitch; // 腿部相对机体摆角 float legLength (leftLegPos.length rightLegPos.length) / 2; // 1) 按当前腿长实时求取反馈矩阵多项式求值非查表 lqr_k(legLength, kRes); // 2) 状态向量 [theta, dTheta, x, dx, phi, dPhi]位置/速度项先与目标作差 float x[6] {stateVar.theta, stateVar.dTheta, stateVar.x, stateVar.dx, stateVar.phi, stateVar.dPhi}; x[2] - target.position; x[3] - target.speed; // 3) 全状态反馈 float lqrOutT k[0][0]*x[0] ... k[0][5]*x[5]; // 轮端扭矩 T float lqrOutTp k[1][0]*x[0] ... k[1][5]*x[5]; // 髋部力矩 Tp之后按第 2 节的分层结构输出车轮Motor_SetTorque(leftWheel, -lqrOutT * lqrTRatio - yawPID.output)右轮取yawPID.output用左右轮差速叠加 yaw 轴串级 PID 实现转向腿部VMC腿长串级 PID 输出加上 roll 轴 PID 的左右分配与 6N 前馈得到左右虚拟腿推力leftForce/rightForce髋部力矩为lqrOutTp * lqrTpRatio ± legAnglePID.output腿角差 PID 按腿长归一化最后leg_conv(leftForce, leftTp, leftJoint[1].angle, leftJoint[0].angle, leftJointTorque); leg_conv(rightForce, rightTp, rightJoint[1].angle, rightJoint[0].angle, rightJointTorque);即leg_conv把虚拟腿的F、Tp折算为两关节电机电力完成整个“LQR 状态反馈 → VMC 力折算 → 电机电流指令”的闭环。从源码结构还可以看到实机在纯 LQR 之外加的工程逻辑增益整定kRatio[2][6]、lqrTpRatio、lqrTRatio作为手动系数叠加在反馈矩阵与输出上用于实机微调离地处理由虚拟腿推力与腿长加速度估算左右腿支持力 3N判为触地1s 防抖离地时车轮电机关断、腿长目标改大0.12m并替换反馈矩阵仅保持腿部竖直第 570~583 行保护逻辑腿部相对机体角度或机体俯仰超过 ±45° 时关断全部电机等待回到安全范围后再延时 4s 恢复控制车轮半径在实机标定取wheelRadius 0.026m第 532 行含轮胎而sys_calc.m中R 0.02两者存在差异属于仿真参数与实机标定参数的区别。下图为该套 VMC LQR 算法在实机上运行时机器人加速过程的真实效果七、使用方式与工程权衡环境与复现方式使用 MATLAB R2022b或 R2020a 配合*_2020a模型打开matlab目录运行 leg_func_calc.m 可重新导出leg_pos.m、leg_spd.m、leg_conv.m注意脚本使用matlabFunction覆盖写出同名文件修改杆长参数后重跑即可再生成运行 sys_calc.m 可重新求取各腿长的K并导出lqr_k.m脚本结尾会打印L0 0.07处的K供核对打开 leg_sim.slx / sys_sim.slx 分别验证腿部 VMC 算法与 VMC LQR 整机算法。符号化简直出 C 代码的代价esp32-controller/README.md 在“改进方向”中给出了一个重要的工程事实关键算法使用 MATLAB 符号化简结果直接导出运算量非常大几乎吃满了 ESP32-C3 芯片性能无法进一步提升控制频率其改进建议是手动化简后再转为 C 代码。这与leg_conv.m这类由 Symbolic Math Toolbox 生成的、含大量临时变量与sqrt/atan的表达式规模是一致的——leg_conv单次调用即包含多次开方与反正切。因此该项目的仿真-实机一致性是以 MCU 算力为代价换取的同一套leg_pos/leg_spd/leg_conv/lqr_k在 Simulink 模型与 ESP32 控制任务中逐位对应保证了仿真结论可以平移到实机这也是本文第 3、4 节各 M 函数签名在两侧完全一致的原因。小结matlab模块以“符号推导 → 函数导出 → 物理仿真验证 → Coder 转 C 实机部署”为主线leg_func_calc.m完成步骤 1~2 的腿部化简与 VMC 映射sys_calc.m完成步骤 3~4 的状态空间建模与K(L0)拟合leg_sim.slx与sys_sim.slx提供 Simscape Multibody 物理闭环验证最终全部通过 MATLAB Coder 落入 ESP32 主控构成该轮腿机器人平衡算法从理论到实机的完整证据链。赞分享机器人嵌入式硬件开发移动开发【免费下载链接】foc-wheel-legged-robotOpen source materials for a novel structured legged robot, including mechanical design, electronic design, algorithm simulation, and software development. | 一个新型结构的轮腿机器人开源资料包含机械设计、电子设计、算法仿真、软件开发等材料项目地址https://gitcode.com/gh_mirrors/fo/foc-wheel-legged-robot点击查看免费下载相关推荐FOC 双轮腿机器人开源项目全解析从机械结构、LQR 算法仿真到 STM32/ESP32 驱动与 Android 遥控的完整实现FOC 双轮腿机器人开源项目全解析从机械结构、LQR 算法仿真到 STM32/ESP32 驱动与 Android 遥控的完整实现 foc wheel legg机器人嵌入式硬件开发移动开发FOC双轮腿机器人的安装与使用指南FOC双轮腿机器人的安装与使用指南 目录 一、项目的目录结构及介绍 FOC双轮腿机器人项目的目录结构是为了便于管理和查找各种资源而精心设计的。下面将详细介绍每个机器人嵌入式硬件开发移动开发探索FOC双轮腿平衡机器人技术原理与创新实践探索FOC双轮腿平衡机器人技术原理与创新实践 引言重新定义轮腿机器人的平衡艺术 当传统轮式机器人在复杂地形举步维艰而多足机器人又受限于能量效率时一机器人嵌入式硬件开发移动开发创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考