Go1四足机器人运动控制入门：从速度指令到闭环与低层关节控制

四足机器人这个东西，第一次上手最容易踩的坑不是代码写不出来，而是你明明把指令发出去了，机器狗却趴在地上一动不动，或者猛地一窜把你吓一跳。这篇内容围绕消费级四足平台 Go1 的运动控制开发展开，属于这个系列的第二篇，重点讲“怎么让它真的动起来、动得听话、动得安全”。我会把高层运动接口的速度指令、模式状态机、步态与身姿参数、状态反馈解析、低层关节控制这几块拆开讲，中间穿插我自己调试时的踩坑记录。适合已经能把 PC 和机器狗网络打通、但还没真正让机器狗走起来的开发者，也适合想把机器狗做成移动底盘、做巡检或做算法验证的同学。看完你应该能独立写出一套“站立—行走—转向—急停”的完整控制脚本，并且知道每一步为什么这么设参数。

1. 先想清楚：操控机器狗到底在操控什么

刚接触这套平台的人，脑子里通常有两种极端想法。一种是觉得它就是个“会走路的玩具”，发个前进指令就应该傻乎乎地走过去；另一种是觉得必须从十二个电机的力矩开始一点点算，才算“真正的控制”。这两种想法都会让人在第一天就卡住。真实情况介于两者之间：厂商已经把“怎么抬腿、怎么维持平衡”这层最难的活封在底层控制器里了，露给你的是一套运动学与步态的参数化接口，你要做的是在这套接口之上，表达清楚“我想让它以什么姿态、什么速度、朝哪个方向走”。

1.1 从“发速度指令”到“十二条腿关节出力”的控制链路

把控制链路拆开看，大致是四层。第一层是你写的应用代码，产生一个期望速度向量，包含前后速度、左右速度、转向角速度三个分量，有时再加一个身高的期望值。第二层是机器狗机载的主控，它拿到这个向量后，结合当前 IMU 姿态、足端触地信息、关节编码器读数，跑一遍步态规划，算出每条腿在当前这个步态周期里应该落在哪个落足点、抬多高、支撑多久。第三层是单腿逆运动学，把落足点换算成髋、大腿、小腿三个关节的角度。第四层才是电机驱动，把角度指令变成电流，通过关节里的行星减速器输出力矩。

这条链路意味着一个很重要的事实：你发的是“意图”，不是“动作”。你发“向前 0.4 米每秒”，机器狗内部会自己决定先迈哪条腿、抬多高、身体前倾多少。所以当你看到机器狗走得不直、走着走着歪了、或者原地踏步不走，问题往往不在“速度给错了”，而在模式没切对、姿态估计漂了、或者地面摩擦系数和默认步态不匹配。

理解这一点之后，调试的排查顺序就顺了：先确认模式对不对，再确认指令有没有被接收，然后才去看步态参数，最后才怀疑硬件。

1.2 为什么优先选高层运动接口而不是直接拧关节

几乎每个新手都会问我同一个问题：能不能不用高层接口，直接给十二个关节角度？答案是能，但强烈不建议作为第一个上手方案。原因有三个。

第一是安全。高层接口内部有平衡控制器兜底，你给一个略微超范围的速度，它会限幅；你给一个突然的姿态变化，它会做平滑过渡。低层关节控制没有这层兜底，你给错一个角度，机器狗会直接把腿硬掰到那个位置，轻则关节过载保护停机，重则扭伤结构件、摔机。

第二是效率。用高层接口，二十行代码就能让机器狗走起来；用低层接口，你得先标定零点、测出各关节的角度限位、设计站立姿态、写插值、调 PD 参数，没有几天时间跑不通。

第三是可维护性。高层接口的输入是物理量，速度单位是米每秒，转向单位是弧度每秒，换一台机器狗、换一个场地，参数大体还能用。低层接口的输入是关节角度，一旦换了机器型号、换了腿部长度，全部要重新标定。

所以我的建议很明确：先把高层接口玩透，能稳定地走、稳定地转、稳定地停，再去碰低层。低层接口的定位不是“更高级的控制”，而是“做特殊动作、做算法研究、做故障诊断”时才需要动用的工具。

2. 落地前的环境准备与网络打通

运动控制的调试，八成的时间浪费在通信上。我见过太多人代码写得没问题，就是网线插错了口、本机网卡没配对地址、防火墙把 UDP 拦了，然后对着屏幕怀疑人生。

2.1 有线直连的地址规划与网卡配置

消费级四足平台通常留了一个以太网口，机载主控自己有一个固定网段。常见的做法是：机器狗侧占一个网段，你的开发机在同一网段里配一个不同尾号的地址，直连网线，或者通过一个小交换机接进来。

以常见的地址规划为例，机器狗主控可能是192.168.123.161这类地址，你的开发机配成192.168.123.100，子网掩码255.255.255.0。配置方式在 Linux 下用命令行最直接：

# 查看网卡名，一般是有线口 ip link show # 临时配置地址，重启失效 sudo ip addr add 192.168.123.100/24 dev eth0 sudo ip link set eth0 up # 确认路由和地址 ip addr show eth0

要提醒一句，如果你的开发机同时还连着无线网，两个网卡的默认路由可能打架。稳妥的做法是把无线网卡的默认路由权重调低，或者干脆调试期间只留有线和本地回环。

配完地址第一件事是ping。ping 得通，说明链路层和网络层都通了，后面才谈得上应用层。

ping -c 4 192.168.123.161

注意：ping 不通先别改代码。按“网线是否插对端口—本机地址是否同网段—防火墙是否放行—机器狗是否已经上电启动完成”这个顺序查，四步之内基本都能定位。

2.2 指令端口与状态端口的分工

这套平台的通信设计是上下行分离的：你往一个端口发指令，从另一个端口收状态。指令走的是“高层运动控制端口”，状态上报走的是“状态查询端口”，两者端口号不同，一条 UDP 连接管不了两件事。

为什么要这么设计？因为上行是低频的、小包的、要求低时延；下行是高频的、大包的、可以容忍少量丢包。分开之后，状态包再大也不会堵住指令通道，指令通道再忙也不影响状态刷新。理解了这个设计意图，你就不会犯“把状态包发到指令端口上”这种低级错误。

实际写代码的时候，我会开两个 socket，一个绑定本地任意端口用于发送指令，另一个绑定状态端口用于接收。如果状态端口是 8007 这类需要独占的端口，注意别让别的进程占着。

import socket CMD_ADDR = ("192.168.123.161", 8080) # 指令下发目标 STATE_PORT = 8007 # 本地监听状态端口 cmd_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) state_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) state_sock.bind(("0.0.0.0", STATE_PORT)) state_sock.settimeout(1.0)

端口号的具体数值请以你手上这版官方头文件和示例代码为准，不同固件版本可能有差异，我这里给的是便于理解的结构。写代码前先去翻一遍官方的通信说明，比事后猜要省时间。

2.3 通信链路自检的三步法

我自己固化的自检流程是三步，每次换机器、换电脑、换系统都跑一遍。

第一步，看状态包有没有进来。只收不发，跑十秒，看能不能收到稳定的状态数据。如果一包都收不到，问题在链路层或者端口占用，别往下走。

第二步，看状态包内容是否合理。打印 IMU 的俯仰、横滚，机器狗平放在地上时这两个值应该接近零；打印电池电压，应该在正常区间。如果打印出来全是零或者全是乱码，说明结构体解析的字节序或者字段偏移错了。

第三步，发一条“最安全”的指令。什么是安全指令？就是让它趴下或者保持阻尼状态的那条。确认这条指令能让机器狗从站立切到趴下，说明你的上行通道是通的，且解析是对的。只有做完这三步，我才会去发运动指令。

实操心得：很多人跳过第三步直接发前进指令，结果机器狗状态不对，一下窜出去撞到桌腿。用一条“趴下”指令做终点验证，成本最低、风险最小。

3. 高层运动模式：用一套速度指令把机器狗“赶”起来

高层运动接口的核心是一个状态机，不是一个连续可调的旋钮。这一点必须先说清楚，否则你会一直在“为什么我发了速度它不动”这个问题上打转。

3.1 模式编号与状态机的正确切换顺序

机器狗内部维护着一个当前模式，你每发一包指令，里面都带着一个模式字段。机载控制器看到模式变了，会做相应的过渡动作。切换是有顺序要求的，不能从“阻尼趴地”直接跳到“高速行走”。

常见的模式分类大致是这几类：

正确顺序是：上电 → 阻尼状态 → 站立模式（等身体完全撑起）→ 低速行走模式 → 给速度 → 减速到零 → 回站立 → 阻尼。每一步之间留出足够的过渡时间，站立到行走我一般留 1 到 2 秒，行走回站立也留 1 秒以上。

为什么不给过渡时间会出问题？因为站立时四条腿是同时着地的四足支撑，行走是两两交替的步态。如果模式一切换你立刻给一个大速度，控制器还没完成步态初始化，就会出现“腿不知道该往哪落”的瞬间，表现为抽搐或者趔趄。

def switch_mode(mode, hold=1.5): """切换模式并按指定时间保持，期间持续以 20Hz 发同一条指令""" t0 = time.time() while time.time() - t0 < hold: send_cmd(mode=mode, vx=0.0, vy=0.0, vyaw=0.0) time.sleep(0.05)

这段代码里的 20 赫兹不是随便写的。指令下发频率太低，控制器会认为你掉线了，自动降级到安全状态；频率太高也没意义，反而占带宽。实测 10 到 50 赫兹之间都比较稳，我习惯用 20 赫兹。

3.2 速度指令的四个分量与量纲换算

一条运动指令里最关键的几个量是：前后速度、左右速度、转向角速度，有时再加一个侧向偏移或者身高的微调。它们的量纲必须搞清楚，这是新手最容易翻车的地方。

关于正负号，我必须强调：不同固件版本、不同坐标定义，符号可能是反的。别照抄别人的代码就上电。我的做法是先把所有速度设成很小的值，比如前后速度给 0.1，观察机器狗是往前还是往后；转向角速度给 0.2，看是左转还是右转。确认方向之后再写进常量里。

速度给多少合适？我的经验值是这样：室内木地板或者瓷砖地面，前后速度 0.3 到 0.6 米每秒比较舒服，转向 0.5 到 1.0 弧度每秒；室外平整水泥地可以提到 0.8 到 1.0；地毯这种软地面要往下降，因为足端会陷进去，实际步幅会被吃掉。

还有一个细节：速度指令是渐变的，不是跳变的。你要从 0 加速到 0.5，中间应该有几个中间值。直接跳变会让控制器内部的参考轨迹出现阶跃，表现为机器狗猛地一顿。我一般写一个斜坡函数，用 0.3 到 0.5 秒完成加速。

def ramp_to(target, current, step=0.05): """简单的线性斜坡，避免速度阶跃""" if abs(target - current) <= step: return target return current + step * (1 if target > current else -1)

3.3 步态与身姿参数的调法

除了速度，高层接口一般还暴露几个“手感”参数，调对了机器狗走起来像模像样，调不对就像喝多了。

抬腿高度决定越障能力和稳定性。抬高一点，过门槛、过线缆不容易绊；抬太高，落足冲击变大，机身晃动明显，能耗也上去了。室内平地我一般用偏低的档位，需要过障碍时临时提高。

步频决定单位时间迈几步。步频高、步幅小，走起来稳但慢，适合精细对位；步频低、步幅大，速度快但对地面平整度要求高。有个经验关系：步幅乘以步频约等于前进速度，所以你想要更快，要么加步频要么加步幅，但两者都有上限。

机身高度影响重心和腿部余量。降低身高，重心低，抗侧向扰动能力强，但腿部伸展余量小，过坎容易顶到；升高身高，越障余量大，但重心高，急转弯时更容易侧倾。

姿态角包括俯仰和横滚的期望值。这两个参数在爬坡和走斜面时特别有用。我做过一个斜坡测试，纯靠速度指令上坡，机器狗会一直低头；把俯仰角设一个小的抬头补偿值，走起来就顺多了。注意这个补偿值别设太大，超过十几度控制器会认为姿态异常。

注意：这些参数不是调一次就完事。换地面、换负载、换电池电量，最优值都会变。我的做法是把常用的几套参数写成配置字典，按场景切换，而不是每次现场手调。

4. 闭环控制与状态反馈的实际用法

只会发速度不叫开发，叫遥控。真正让机器狗“听话”的关键，是把状态反馈用起来，形成一个闭环。

4.1 从状态包里挖出可用的观测量

状态包里的信息量比大多数人以为的多。常用的几类观测量是：

姿态类：横滚、俯仰、偏航三个角，以及对应的角速度。这是做航向保持和防倾倒的基础。偏航角是累积的，会漂，但短时间内的变化量是可信的。

速度类：机体坐标系下的三轴线速度，以及足端的估计速度。这个值是估出来的，不是直接测的，会有延迟和噪声，做控制时要滤波。

位置类：机身位置和足端位置。机身位置是积分出来的，长时间会漂；足端位置相对可靠，可以用来判断是否打滑。

状态类：电池电压、电流、各电机温度、足端接触力。这几项是做安全保护的关键，尤其是电机温度和电池电压，我建议每次调试都打印出来。

状态机类：当前模式、当前步态、是否有故障标志位。故障标志位一定要读，有些异常控制器会自己降级，但如果你不读标志位，就会以为是自己的代码有问题。

解析状态包时最容易出的错是结构体对齐和字节序。我建议先用官方的解析示例跑通，确认字段值合理，再自己写。自己从头写解析，一不小心少算一个填充字节，后面所有字段全错位。

4.2 用偏航角做航向保持的简易闭环

这是最实用、收益最高的一个小闭环。机器狗走直线时，因为地面摩擦不均、腿部差异、IMU 零偏，走十几米就会偏出好几度。加一个航向闭环，直行精度立刻上一个档次。

思路很朴素：记下起步时的偏航角作为目标，运行中读当前偏航角，算误差，用比例控制输出转向角速度。

Kp_yaw = 1.2 yaw_target = None def heading_hold(current_yaw, base_yaw): err = wrap_to_pi(base_yaw - current_yaw) # 处理跨 ±pi 的情况 vyaw = Kp_yaw * err return max(-1.0, min(1.0, vyaw)) # 限幅

这里有两个坑。第一个坑是角度环绕：偏航角从接近正 pi 跳到负 pi，如果不做归一化，误差会瞬间变成两倍 pi，机器狗会突然原地转一圈。所以一定要写wrap_to_pi。

第二个坑是增益。比例系数给大了，机器狗会左右摇摆，像蛇一样；给小了，纠偏太慢，走完十米还是歪的。我的经验是从 0.8 开始试，逐步加到出现轻微摆动，再回调 30%。这个系数和速度有关，速度越快，同样的增益越容易振荡，所以高速时要把增益降下来。

同样的思路可以用在横滚保持上，比如走斜坡时防止侧倾。但横滚闭环要谨慎，因为横滚的可用调节手段少，调过头容易翻。

4.3 频率、时延与平滑处理

闭环控制的性能，一半取决于增益，一半取决于时延。这条链路里至少有四个时延源：你的采集周期、网络传输、机载控制器的处理周期、执行机构的响应。

实测下来，从你发指令到机器狗真正动起来，端到端延时有几十毫秒量级。这个时延决定了你的闭环带宽上限，也就决定了增益不能给太大。

另外，状态包和指令包的频率最好对齐。我习惯统一在 20 到 50 赫兹。如果你用 100 赫兹发指令，但状态只刷新 20 赫兹，那中间四帧你用的是旧数据，闭环效果反而差。

还有一点容易被忽略：发送线程和接收线程要分开。如果用单线程“发一包、收一包、睡一会”，一旦收到一个超大状态包或者发生一次丢包重传，指令节奏立刻乱掉。用两个线程，发送线程只管按固定周期发，接收线程只管收和解析，中间用共享变量交换最新状态。

实操心得：我在接收线程里加了一个“最后一包时间戳”，主控线程每次读状态前检查这个时间戳，超过 200 毫秒没更新就触发安全逻辑，把速度归零并切回站立。这个机制救过我好几次——网线被扯掉的时候，机器狗不会一直按着最后的指令往前冲。

5. 低层关节控制：什么时候需要，怎么不炸机

前面说了不建议新手直接上低层，但有些场景绕不开：做一个特定的蹲起动作、做降落缓冲、做单腿支撑的算法验证、或者排查某个关节是否异常。这时候就必须用低层接口。

5.1 低层模式的准入条件与安全前置

进入低层模式之前，我强制自己做四件事，一件都不能省。

第一件，把机器狗吊起来或者放在软垫上。用支架把机身架空，四条腿悬空，这样即使控制量给错，腿也只是在空中乱蹬，不会带着机身摔倒。

第二件，确认关节编号和正方向。十二个关节的编号顺序、每个关节的正方向，必须实测确认。方法是给某一个关节一个很小的正角度指令，观察实际转动方向。正负号搞反是低层控制最常见的事故来源。

第三件，设置保守的限幅。角度限幅、角速度限幅、力矩限幅，三个都要设。角度限幅防止超出机械限位，角速度限幅防止突然抽动，力矩限幅防止过载。我的习惯是第一次测试把力矩限幅设到额定值的 30% 以内。

第四件，准备好随时断电。手放在电源开关旁边，或者用一个能立刻执行的急停脚本。这一条听起来很土，但真的有用。

低层控制还有一个前提：电机必须先使能。使能之后电机会保持当前位置，这时候腿是硬的，推不动。这个状态下你才能开始发角度指令。如果不使能直接发角度，指令会被忽略。

5.2 KP/KD 与力矩限幅的取值逻辑

低层控制的每个关节都要配两个增益：位置增益和速度增益（有人叫阻尼增益）。它们的物理意义是：

• 位置增益越大，关节越“硬”，越努力回到目标角度，但太大会振荡；
• 速度增益越大，关节运动越“黏”，起到阻尼作用，抑制过冲。

这里有个生活化的类比：位置增益像弹簧的劲度系数，速度增益像减震器的阻尼。弹簧太硬，一碰就弹；阻尼太小，弹起来停不下来。

取值没有通用答案，因为和机器狗自重、腿部惯量、关节减速比都有关系。我的实操路径是：先在悬空状态给一个很小的增益，命令一个十度的小角度，看响应；慢慢加位置增益直到出现轻微过冲，再回退；然后加速度增益把过冲压下去。整个过程可能调几十组参数，做好记录。

这里有个反直觉的点：增益不是越高性能越好。太高的位置增益会让电机持续高频修正，电流波动大、发热快，长时间跑下来电机温度会顶到保护阈值。我吃过这个亏，一次连续测试二十分钟后机器狗自动趴下，查了半天代码，最后发现是电机过热保护。

5.3 从站立姿态插值开始的第一个动作

低层控制的第一个动作，我建议做“从站立姿态小幅摆动一条腿”，而不是“让机器狗走”。走涉及步态规划、重心转移、触地检测，变量太多。

具体做法是：读当前十二个关节的角度作为起始姿态，定义一组目标角度（比如只改右前腿的髋关节五度），然后做插值过渡。

def interpolate(start, target, duration, rate=100): """从 start 到 target 平滑插值，duration 单位秒""" steps = int(duration * rate) for i in range(steps + 1): alpha = i / steps # 用余弦插值，两端速度为零，过渡更柔和 s = 0.5 - 0.5 * math.cos(math.pi * alpha) q = [a + (b - a) * s for a, b in zip(start, target)] send_joint_cmd(q) time.sleep(1.0 / rate)

这段代码里用余弦插值而不是线性插值，原因是线性插值在起点和终点速度不为零，会有“一顿”的感觉；余弦插值两端速度为零，过渡更柔和，对机械结构更友好。这个技巧在低层控制里非常常用，几乎所有平滑过渡都能用。

插值完成后，保持在目标位置几百毫秒，观察有没有振荡、有没有异响，再决定是否继续下一步。整套动作确认无误后，才考虑把它串成一个序列。

6. 常见问题与排查实录

这一节是我实际调试中积累的，按“症状—可能原因—排查动作”整理成表，遇到问题可以直接对照。

6.1 指令发了没反应

这是最高频的问题。九成的情况不是通信问题，而是模式没切对或者指令格式不对。

我遇到过一次特别典型的：状态包收得好好的，指令也发出去了，机器狗就是不动。折腾了半小时，最后发现我发的是站立模式，站立模式下速度字段是被忽略的。也就是说，你在站立模式下给前后速度，机器狗会老老实实站着。

还有一种情况是：模式切对了，但指令包里少算了一个校验字段。有些固件版本会对指令做校验，校验不通过直接丢弃，不报错。这种最难查，因为抓包看数据是发出去了。解决办法是拿官方的示例代码原样跑一遍，确认能动能走，再往里面加自己的逻辑。

6.2 抖动、抽搐与“走路发飘”

抖动分三种。第一种是高频小幅抖动，通常是指令频率和状态频率不同步，或者你的控制循环有抖动。检查方法很简单：把控制循环改成固定周期，看抖动是否消失。

第二种是周期性大幅度摆动，一般是增益给大了。把转向或者横滚的比例系数降下来，立竿见影。

第三种是偶发抽搐，很可能是丢包或者时延尖峰。用抓包工具看 UDP 的间隔，如果有超过几百毫秒的空洞，那就是网络问题。检查网线质量、交换机、以及你的接收线程有没有被阻塞。

“走路发飘”是另一种表现：机器狗走起来脚底像踩棉花，轨迹不直，落脚点乱。这种情况先看地面。地毯、草地、松散砂石，这些软地面会让默认步态失效，需要提高抬腿高度、降低速度、加大步频。如果地面没问题，再看 IMU 零偏，把机器狗静置几秒看姿态角是否稳定，如果静止时姿态角还在缓慢变化，说明需要重新标定。

6.3 安全与硬件保护清单

运动控制这块，安全不是一句口号，是要写成代码的。我的脚本里固定有这几条保护：

注意：这些保护逻辑要在发送线程之外实现，不要写在发送循环里。发送循环一旦卡住，保护逻辑也跟着卡住，等于没有保护。

另外一个容易被忽略的点是电池。四足机器狗在高速运动时电流很大，电压会瞬间被拉低。如果你设的电压保护阈值太高，稍微一加速就触发保护停机；设太低又起不到保护作用。我的做法是看空载电压和满载电压的差值，把阈值设在两者之间偏下的位置，并且加一个持续时间的判断，避免瞬时压降误触发。

最后说个小技巧：调试阶段我强烈建议在机器狗周围清出一片空地，至少留出两米的余量，并且先把速度上限钉死在 0.3 米每秒以内。等所有逻辑跑通、所有保护验证过，再往上调。我见过太多人一上手就给满速，结果机器狗一头撞上墙，修一次腿的钱够买好几块开发板。慢一点，稳一点，后面调参的时候你会感谢当初的自己。