简介面向机器人控制与并联机构研究者的MATLAB实践文档聚焦六自由度Stewart平台运动学逆解与动力学仿真的完整实现路径。内容从坐标变换、支腿长度计算切入结合雅可比矩阵推导并采用牛顿-欧拉法建立动力学方程进一步引入Simscape Multibody完成三维仿真与实时可视化对奇异位形检测、轨迹规划不合理等常见问题给出排查思路。压缩包共1个PDF文件约116KB篇幅紧凑便于按章节检索算法要点、代码片段与求解器配置。已有192人学习适合具备一定MATLAB基础、希望打通理论推导到仿真验证环节的技术人员参考可直接借鉴其中的函数写法、向量化计算与仿真参数设置减少调试试错成本。1. 从一台晃动的六自由度平台说起Stewart 逆解到底在算什么六自由度 Stewart 并联平台第一次动起来最容易出现的场面不是轨迹多漂亮而是指令刚发下去平台就抖、支腿力乱跳。基于 MATLAB 的 Stewart 并联平台运动学逆解与动力学仿真要解决的正是这个从几何到力的断层逆解给出六根支腿该多长动力学仿真回答每根支腿要出多大力、平台会不会失稳。它适合做并联机构控制、试验台调试、课程设计复现的从业者也适合刚接手一套六轴平台、手里只有尺寸表的人。先把逆解写成能检查的函数再把模型搭到能跑最后用几条踩坑记录把仿真结果的可信度兜住这条路比一上来就调控制器稳得多。2. 把平台几何参数先钉死Stewart 结构建模与逆解方程的 MATLAB 落点逆解不是背一条公式就完事。六根支腿的长度只跟上下平台铰点位置和动平台位姿有关但铰点编号、旋向、零位高度、角度单位任何一处对不上后面搭出来的动力学模型就会以“力突然很大”“平台拧麻花”的方式翻车。常见做法是先在 MATLAB 里用纯几何函数把逆解跑通用零位长度和单轴微位移做两个最小检查再进 Simscape Multibody。这样动力学模型出问题时至少能判断是几何错了还是驱动参数错了。2.1 上平台与下平台铰点布置先统一坐标系和编号固定坐标系 B 放在下平台几何中心z 轴垂直向上动坐标系 P 放在上平台几何中心零位时与 B 平行。第 i 条支腿连接下铰点 b_i 和上铰点 p_i编号必须上下对应。常见的 6 铰点布置是下平台按 60° 间隔、上平台错开 30°这样零位时六根腿长相等加工和装配也容易对齐。参数不需要多但每个都要能追溯到图纸。参数符号示例值单位说明下平台铰点半径rb0.8m铰点分布圆半径上平台铰点半径rp0.6m铰点分布圆半径零位高度h1.0m上平台中心相对下平台中心下铰点角beta[0 60 120 180 240 300]deg按逆时针排列上铰点角alpha[30 90 150 210 270 330]deg与下铰点一一对应局部坐标里下铰点 b_i [rb·cos(beta_i); rb·sin(beta_i); 0]上铰点 p_i [rp·cos(alpha_i); rp·sin(alpha_i); 0]。这两个式子看着简单实际最容易错在“从 CAD 里量了角度直接抄”。CAD 的俯视图可能是顺时针MATLAB 里按逆时针算编号一错位零位长度仍然可能相等但一给姿态角六根腿的长度变化方向就全乱。我的习惯是先在纸上画俯视图把六条腿按逆时针编号再在 MATLAB 里打印零位六根腿长确认它们一致后才继续。2.2 逆解核心从位姿到六根支腿长度的闭式推导位姿用 pose [x y z roll pitch yaw] 表示单位分别是 m 和 rad。旋转矩阵按 ZYX 顺序R Rz(yaw)·Ry(pitch)·Rx(roll)。上铰点在固定坐标系里的坐标是 q_i R·p_i t其中 t [x; y; z] 是上平台中心位置。支腿向量 s_i q_i - b_i长度 L_i ||s_i||单位方向 u_i s_i / L_i。逆解到这一步就结束了六根腿长直接由几何闭式给出不需要迭代。速度层面同样值得一起写出来因为后面要把逆解轨迹喂给动力学模型支腿速度是否连续会直接影响求解器。对 s_i 求导支腿速度 ldot_i u_i^T·(v ω×r_i)其中 r_i R·p_iv 是平台中心速度ω 是平台角速度。整理成雅可比矩阵 J 的每一行J_i [u_i^T, (r_i×u_i)^T]则 ldot J·[v; ω]。这个 J 是 6×6 的后面检查奇异位形、看条件数都用它。注意逆解本身不含质量、惯量、摩擦它只回答“腿该多长”不回答“力该多大”。2.3 用 MATLAB 写一个可复现的逆解函数下面这个函数把几何参数收进 geom 结构体输入位姿输出六根腿长、单位方向和雅可比。代码不依赖额外工具箱旋转矩阵自己写方便在没有 Robotics 工具箱的机器上直接跑。function [L, u, J] stewart_ik(pose, geom) % pose: [x y z roll pitch yaw], 单位 m 和 rad % geom: 包含 rb, rp, beta, alpha % L: 6x1 支腿长度; u: 6x3 单位方向; J: 6x6 雅可比 x pose(1); y pose(2); z pose(3); roll pose(4); pitch pose(5); yaw pose(6); % ZYX 欧拉角旋转矩阵 Rx [1 0 0; 0 cos(roll) -sin(roll); 0 sin(roll) cos(roll)]; Ry [cos(pitch) 0 sin(pitch); 0 1 0; -sin(pitch) 0 cos(pitch)]; Rz [cos(yaw) -sin(yaw) 0; sin(yaw) cos(yaw) 0; 0 0 1]; R Rz * Ry * Rx; t [x; y; z]; L zeros(6,1); u zeros(6,3); J zeros(6,6); for i 1:6 % 下铰点与上铰点局部坐标 b [geom.rb*cos(geom.beta(i)); geom.rb*sin(geom.beta(i)); 0]; p [geom.rp*cos(geom.alpha(i)); geom.rp*sin(geom.alpha(i)); 0]; % 上铰点世界坐标与支腿向量 q R*p t; s q - b; L(i) norm(s); u(i,:) (s / L(i)); % 雅可比第 i 行平台速度到支腿速度 r R*p; J(i,:) [u(i,:), cross(r, u(i,:))]; end end代码里最需要解释的是三个地方。第一旋转顺序必须和后续 Simscape 模型里的姿态定义一致ZYX 只是常见选择换成 XYZ 就要同步改轨迹生成和模型初始姿态。第二b 和 p 的 z 分量都是 0意味着铰点都布置在上下平台中面上如果实际铰点有高度差要在这里加 z 偏移不要等到动力学模型里再补。第三J 的角速度分块用 cross(r, u)顺序不能反反了之后腿长速度符号会整体错控制器一闭环就发散。2.4 用零位长度和单轴微位移做最小验证逆解函数写完不要急着搭模型。先做两个检查零位时六根腿长是否相等给一个单轴平移腿长变化方向是否符合几何直觉。零位检查能抓住铰点角错位和半径单位混用单轴检查能抓住旋转矩阵或编号顺序错误。下面这段脚本可以直接接在函数后面。geom.rb 0.8; geom.rp 0.6; geom.beta deg2rad([0 60 120 180 240 300]); geom.alpha deg2rad([30 90 150 210 270 330]); pose0 [0 0 1.0 0 0 0]; [L0, ~, J0] stewart_ik(pose0, geom); fprintf(零位腿长: %s\n, mat2str(L0, 6)); assert(max(abs(L0 - mean(L0))) 1e-9, 零位腿长不一致检查铰点角); % 沿 x 方向平移 1 mm看六根腿长变化 pose1 pose0; pose1(1) 0.001; L1 stewart_ik(pose1, geom); fprintf(x 向微位移腿长变化: %s\n, mat2str((L1 - L0), 6)); fprintf(零位雅可比条件数: %.3f\n, cond(J0));这段脚本不验证绝对精度只验证一致性。零位六根腿长相等说明上下铰点角度配对和半径没有明显错x 向微位移后靠近 x 正方向的腿应该缩短、远离的腿应该伸长符号不对就回去查编号。条件数在零位一般不大如果零位条件数就很高说明铰点布置接近奇异后面轨迹要避开这个区域。把这两个检查固定成脚本换一套几何参数先跑一遍比在 Simscape 里看平台乱晃再回头找原因省时间。3. 逆解跑通之后用 Simscape Multibody 搭一个能动的 Stewart 动力学模型逆解给的是长度动力学仿真给的是力。支腿长度对了不代表模型能跑质量、惯量、质心、关节摩擦、驱动刚度阻尼、求解器步长任何一项缺失支腿力都可能出现高频抖动或者第一帧就飞出去。Simscape Multibody 的好处是能把几何、关节、力和求解器放在一个模型里坏处是过约束和初始状态不匹配会以很隐蔽的方式出现。这一章按“先搭机构、再配驱动、最后喂轨迹”的顺序走。3.1 为什么逆解不够动力学仿真要补哪些量逆解假设平台和支腿是刚性的关节没有间隙驱动能瞬间跟上位置。真实平台不是这样上平台有质量质心可能偏离几何中心支腿有质量移动副有摩擦球铰有间隙和阻尼电动缸有刚度、带宽和饱和。动力学仿真要补的就是这些量。最小心集包括上平台质量与惯量、负载质量与质心偏移、六根支腿的移动质量、球铰摩擦、移动副摩擦、驱动刚度和阻尼。少一两个量模型也能跑但支腿力的峰值和相位会偏拿它去选电机就会偏保守或偏冒险。常见做法是先把模型分成“几何体”和“关节”两层上下平台用 Solid 块六条支腿每条拆成下腿、上腿和中间的移动副两端用球铰。上平台不额外加 6-DOF 关节否则和六条腿形成过约束求解器会报冗余约束或者力突变。让六条腿自己约束上平台的六个自由度这是并联机构在 Simscape 里比较稳的搭法。3.2 从几何到 Simscape 多体建模步骤与参数表如果手里有 STEP 或 URDF可以用 smimport 导入但导入后一定要检查关节轴向和单位。更可控的做法是在 Simscape Multibody 里直接用 Solid 块搭简化几何下平台固定在世界坐标系上平台按铰点半径画一个圆柱或长方体六条支腿按零位长度摆放。每条支腿的结构是下球铰 → 下腿 Solid → 移动副 → 上腿 Solid → 上球铰。移动副的轴向要沿支腿方向零位时六根腿方向不同所以每条腿的局部坐标系要单独对齐。参数示例值单位注意点上平台质量30kg含负载时要合并上平台惯量diag(0.8,0.8,1.2)kg·m²按实际质心坐标系单根支腿移动质量2.5kg下腿和上腿分别设置支腿行程0.71.3m零位 1.0 m球铰摩擦0.5N·m先小后大做对比移动副摩擦20N可用连续化库仑摩擦建模时先不要加驱动让模型在重力下自己静平衡。如果静平衡时平台下沉量很大说明支腿移动副没有约束或者质量单位错了如果求解器报冗余约束多半是上平台多加了关节。静平衡跑通之后再加位置驱动这样初始状态容易对齐。参数表里的惯量不要直接填 CAD 默认值CAD often 给的是关于质心的惯量Simscape Solid 里要确认参考坐标系一致否则平台转动惯量会差一个量级。3.3 驱动、关节与求解器配置位置驱动还是力驱动Simscape Multibody 的移动副可以配置成位置驱动或力驱动。做运动学验证时用位置驱动直接把逆解得到的腿长作为输入看平台位姿是否跟预期一致做动力学分析时用力驱动把作动器模型输出的力加到移动副上看支腿力和平台误差。位置驱动容易起步但它会掩盖驱动饱和和摩擦所以最终选型还是要回到力驱动。一个折中是先位置驱动跑通轨迹再把位置环换成 PD 力环比较两者的支腿力峰值。求解器建议用 ode23t 或 ode15s最大步长 1e-3 s相对容差 1e-6。摩擦和球铰阻尼会让系统变刚步长太大时支腿力会出现高频毛刺看起来像“力突然翻倍”实际是数值振荡。下面这段脚本只做模型配置和逆解轨迹写入不负责搭块搭块部分在模型里完成。mdl stewart_dyn; load_system(mdl); % 求解器配置变步长限制最大步长 set_param(mdl, Solver, ode23t, ... MaxStep, 1e-3, ... RelTol, 1e-6, ... AbsTol, 1e-6); % 生成 5 s 逆解轨迹采样 1 ms t (0:0.001:5); pose zeros(numel(t), 6); pose(:,3) 1.0 0.02*sin(2*pi*0.5*t); % 示例仅升沉幅值 20 mm geom.rb 0.8; geom.rp 0.6; geom.beta deg2rad([0 60 120 180 240 300]); geom.alpha deg2rad([30 90 150 210 270 330]); L zeros(numel(t), 6); for k 1:numel(t) L(k,:) stewart_ik(pose(k,:), geom); end leg_ts timeseries(L, t); leg_ts.Name leg_position; assignin(base, leg_ts, leg_ts);这段代码的关键不是 timeseries 本身而是“采样 1 ms”和“初始腿长”两个点。采样太粗五次多项式轨迹的加速度段会被漏掉位置驱动会像阶跃一样冲击模型初始腿长不写进关节状态仿真第一帧会把平台硬拽到轨迹起点支腿力出现一个尖峰。模型里六个移动副的位置输入端口要按列拆开用 Demux 或 Matrix 拆分顺序和 stewart_ik 输出的列顺序一致。3.4 用逆解轨迹驱动模型先对零位再切轨迹位置驱动跑通的标准是平台中心位姿和输入 pose 的误差在毫米级六根腿长和逆解输出的误差在 0.1 mm 级。如果平台能动但轨迹滞后检查移动副的驱动方向是否和腿长增加方向一致如果平台动到一半突然反向检查铰点编号是否和逆解函数一致。常见做法是先在零位给一个 1 mm 的阶跃看六根腿的响应方向再切到连续轨迹。零位对齐这一步不能省我见过不少模型逆解和 Simscape 各自都对但编号错一位平台一动就拧。4. 动力学仿真的输入从哪来轨迹规划、作动器模型与负载设置逆解和模型都跑通之后仿真结果可不可信取决于输入。轨迹给得太猛支腿力峰值是假的作动器模型太理想选型会偏小负载质心随手填在几何中心平台俯仰力矩会漏掉。这一章把输入拆成三块一条速度、加速度连续的运动轨迹一个带刚度、阻尼和摩擦的作动器模型一个把负载质量与质心偏移写清楚的设置。最后再看输出该盯哪些量。4.1 给平台一条可用的运动轨迹五次多项式与姿态插值阶跃和正弦都能让模型动但做支腿力分析时五次多项式更接近实际点位运动起点和终点的速度、加速度都为零冲击小。下面这个函数把归一化时间映射到 01 的位移、速度和加速度位置轨迹用 p0 (p1-p0)·s 生成。姿态角在小角度范围内可以直接插值大角度建议转成旋转矩阵或四元数再插值避免欧拉角奇异。function [s, sd, sdd] quintic(t, T) % t: 当前时间; T: 总时长 tau min(max(t/T, 0), 1); s 10*tau.^3 - 15*tau.^4 6*tau.^5; sd (30*tau.^2 - 60*tau.^3 30*tau.^4) / T; sdd (60*tau - 180*tau.^2 120*tau.^3) / T^2; end用的时候把六个位姿分量分别从 p0 插到 p1位置单位用米姿态用弧度。T 取 2 s、行程 50 mm 时最大速度大约 0.047 m/s最大加速度大约 0.07 m/s²这个量级对小型平台比较温和。行程加大或 T 缩短支腿力峰值会明显上升不要拿一条“看起来平滑”的轨迹直接去选电机先把加速度峰值打印出来。4.2 电动缸/液压缸的简化模型刚度、阻尼、摩擦怎么放作动器模型不需要一上来就上完整液压系统。做支腿力估算时一个弹簧-阻尼-摩擦的简化模型够用F K·(x_cmd - x) C·(v_cmd - v) - Fc·tanh(v/vs) - b·v。K 是位置环等效刚度C 是阻尼Fc 是库仑摩擦力vs 是摩擦连续化速度b 是粘滞阻尼。tanh 代替 sign 是为了让求解器好收敛vs 太小会重新引入刚性一般取 0.01 m/s 量级。function F actuator_force(x, v, x_cmd, v_cmd, p) % x, v: 当前位移和速度; x_cmd, v_cmd: 指令 e x_cmd - x; de v_cmd - v; F p.K*e p.C*de - p.Fc*tanh(v/p.vs) - p.b*v; end参数示例值单位调整方向K2e5N/m越大跟踪越硬力峰值越高C2000N·s/m抑制超调太大会滞后Fc50N低速爬行和死区vs0.01m/s太小求解器吃力b100N·s/m高速段阻尼这组参数不是“标准答案”只是让模型先跑起来的起点。K 和 C 要跟实际驱动器的带宽、减速比、丝杠导程对得上Fc 和 b 可以从空载匀速试验里估。位置驱动改力驱动时先把 K 调小、C 调大看平台能不能稳住再逐步加刚度不然一开始就高频振荡。4.3 负载与质心偏移把“看起来对”变成“算出来对”上平台质量 30 kg、负载 10 kg如果负载质心偏离几何中心 20 mm重力产生的俯仰力矩大约是 10×9.81×0.02 ≈ 1.96 N·m。这个力矩不大但六根支腿的力分配会明显变化尤其是对角线上的支腿。Simscape 里要把负载合并进上平台 Solid 的质量和惯量或者单独加一个 Solid 并设置 Center of Mass。惯量矩阵要按负载质心重新算平行轴定理不要直接把两个惯量矩阵相加。质心偏移还会影响零位静平衡。如果模型零位时支腿力不对称先别怀疑逆解检查质心是不是在几何中心。做动力学仿真时建议先跑一个“重力-only”工况看六根支腿的静反力之和是否等于总重力对角线支腿力是否合理。这个检查比看平台轨迹更早暴露质量属性错误。4.4 仿真输出看什么支腿力、驱动功率、误差带仿真跑完不要只看平台轨迹动画。至少导出四类量六根支腿力、六根支腿速度、驱动功率、平台位姿误差。支腿力看峰值和波动速度看是否超过作动器额定功率看峰值是否超过电源或驱动器能力位姿误差看位置驱动和力驱动的差距。下面这段代码从 logsout 里取支腿力并算 RMS前提是模型里把支腿力信号记成 leg_force。logs out.logsout; legForce logs.get(leg_force).Values.Data; t logs.get(leg_force).Values.Time; F_rms sqrt(mean(legForce.^2, 1)); F_peak max(abs(legForce), [], 1); fprintf(各支腿力 RMS: %s\n, mat2str(F_rms, 4)); fprintf(各支腿力峰值: %s\n, mat2str(F_peak, 4));指标看什么异常信号支腿力峰值选型裕度单根腿明显大于其他腿支腿力 RMS热负荷高频毛刺导致 RMS 虚高驱动功率电源和驱动器短时尖峰超过额定位姿误差模型可信度位置驱动下误差仍很大支腿力单根明显偏大先查质心偏移和铰点编号六根腿都有高频毛刺先查求解器步长和摩擦连续化位姿误差在位置驱动下仍然大查移动副方向和初始状态。把这些量做成一张固定表每换一套参数跑一遍比只看动画可靠。5. Stewart 仿真避坑五条血泪经验对应现象、原因和解决这一章不展开新理论只记录五条在 Stewart 逆解和动力学仿真里反复出现的翻车点。每条都按“现象 → 原因 → 解决”写方便对照自己的模型。有些问题在逆解阶段就能拦住有些要等 Simscape 跑起来才暴露越早检查越省时间。5.1 铰点编号错一位零位长度全对仿真却“拧麻花”现象逆解零位六根腿长完全一致但一给俯仰或滚转平台在 Simscape 里明显扭动支腿力出现方向相反的尖峰。原因上下平台铰点编号没有按同一旋向配对第 i 条腿连接的下铰点和上铰点错位零位时腿长碰巧相等姿态一变就暴露。解决在俯视图上按逆时针给六条腿编号打印每组 b_i 和 p_i 的角度确认第 i 条腿的上下铰点夹角一致再用单轴微位移检查腿长变化符号符号不对就回去改编号。5.2 毫米与米混用支腿力突然放大一千倍现象模型能跑但支腿力比估算值大三个数量级或者平台几乎不动。原因CAD 导出单位是毫米MATLAB 逆解按米算几何参数一边是 800 一边是 0.8也有求解器里长度单位没统一的情况。解决所有几何参数进 MATLAB 前统一成 SI导入 STEP 后检查包围盒尺寸在逆解函数入口加一条断言零位腿长在 0.110 m 之间超出就报错。单位混用不会让模型立刻崩但会让后面所有力、功率、刚度的数全错。5.3 奇异位形附近逆解数值爆炸腿长速度出现尖峰现象轨迹经过某个姿态时雅可比条件数突然升高支腿速度出现尖峰Simscape 里支腿力也跟着抖。原因平台接近奇异位形六根支腿接近共面或共线逆解对位姿变化非常敏感。解决在轨迹生成阶段算每个采样点的 cond(J)超过阈值就减速或改路径控制器里用阻尼最小二乘代替纯逆避免速度指令饱和。不要试图用更小的求解器步长“压住”奇异根子在轨迹。[~, ~, J] stewart_ik(pose_k, geom); cj cond(J); if cj 1e4 warning(接近奇异位形条件数 %.2e, cj); end5.4 求解器步长与摩擦不匹配支腿力高频抖动现象支腿力曲线像锯齿RMS 明显偏高减小 MaxStep 后毛刺变小。原因摩擦用 sign 或很陡的 tanh系统变刚ode45 或大步长 ode23t 跟不上也有位置驱动和力驱动混用导致的约束冲突。解决摩擦改成连续化 tanhvs 取 0.01 m/s 量级求解器换 ode23tMaxStep 限制到 1e-3 sRelTol 1e-6位置驱动改力驱动时先降刚度加阻尼确认稳定后再加刚度。5.5 初始状态不匹配第一帧平台飞出去现象仿真时间 0 刚过平台位移或支腿力出现一个巨大尖峰然后才回到轨迹附近。原因Simscape 模型初始腿长和逆解轨迹第一帧不一致或者上平台初始位姿没有按零位设置求解器在第一步强行把状态拉过去。解决用逆解零位算出的六根腿长写进六个移动副的初始位置上平台初始位姿用零位姿态先跑重力静平衡再把轨迹切进来。位置驱动尤其要注意第一帧的指令值必须等于初始状态。6. 把仿真做成能复用的工具参数化脚本、批量扫参与结果校验的一个具体技巧一套 Stewart 模型跑通一次不难难的是换铰点半径、换负载、换轨迹之后还能快速复现。我自己的习惯是把几何、驱动、轨迹参数全部收进一个 config 结构体模型只认结构体不认散落在脚本里的数字。这样批量扫参时只改变量不用改模型结果校验也容易固定成两个检查点能量守恒和支腿力闭环。6.1 参数化封装一个 config 结构体管住全部几何与驱动参数cfg.geom.rb 0.8; cfg.geom.rp 0.6; cfg.geom.h 1.0; cfg.geom.beta deg2rad([0 60 120 180 240 300]); cfg.geom.alpha deg2rad([30 90 150 210 270 330]); cfg.act.K 2e5; cfg.act.C 2000; cfg.act.Fc 50; cfg.act.vs 0.01; cfg.act.b 100; cfg.traj.T 2.0; cfg.traj.dz 0.05; save(stewart_cfg.mat, cfg);模型初始化回调里 load 这个 mat逆解函数从 cfg.geom 取参数作动器从 cfg.act 取参数。好处是批量扫参时不会出现“脚本改了、模型里还是旧值”的情况。6.2 批量扫参用 parfor 跑不同铰点半径与负载rb_list 0.6:0.05:1.0; base_cfg cfg; results cell(numel(rb_list),1); parfor k 1:numel(rb_list) c base_cfg; c.geom.rb rb_list(k); results{k} run_case(c); % run_case 内部完成逆解、仿真、指标提取 endrun_case 返回支腿力峰值、RMS、功率峰值和条件数最大值。扫参不是为了找“最优”而是看趋势rb 增大时支腿力峰值怎么变条件数有没有变差。每次只改一个变量结果才解释得动。6.3 结果校验能量守恒与支腿力闭环的两个检查点能量检查看总输入功是否等于动能变化、势能变化和耗散之和误差在 5% 以内说明力和位移的符号没有大面积错。支腿力闭环看逆解腿长和模型实际腿长的 RMS 误差位置驱动下应在 1e-4 m 量级力驱动下会大一些但不应随时间发散。两个检查点都过再去看平台轨迹。我自己的习惯是先把零位六根腿长打印出来再跑重力静平衡最后才切轨迹少了这一步第一帧飞出去的概率很高。这个方向值不值得做取决于你是否需要支腿力而不是只看位姿如果需要先把逆解和动力学模型对上再谈控制器。希望帮到你。本文还有配套的精品资源点击获取