PyKDL与CoppeliaSim联合仿真:机械臂爬行运动实现
简介:运动学仿真是机器人研发中验证算法与控制逻辑的核心手段,而物理引擎则提供接触、摩擦等真实交互反馈。PyKDL作为高效的刚体运动学库,负责求解关节空间与笛卡尔空间的映射;CoppeliaSim通过Bullet/ODE物理引擎模拟重力与接触力。将二者联合,可在虚拟环境中实现机械臂的爬行运动仿真,通过URDF统一模型描述,利用速度级逆解完成尺蠖式步态规划,并结合接触力反馈动态切换支撑相与摆动相,有效避免末端打滑。该方法适用于具身智能、仿生机器人等场景,为复杂运动控制提供低成本验证平台。
1. 用PyKDL算逆解、CoppeliaSim跑物理,搭一套机械臂爬行运动仿真
在移动机器人里,多关节机械臂通常负责抓取,但当机械臂需要穿过狭窄管道、废墟缝隙,或者临时充当移动平台时,爬行运动变成另一类任务:末端和基座都成了支撑点,身体弯曲来推动整体重心前移。要验证这种运动,常见做法是把PyKDL和CoppeliaSim联合起来:PyKDL算关节角到支撑点的运动学映射,CoppeliaSim算接触、摩擦和重力,两边在仿真步长内交换数据。这套方案适合做具身智能、机械臂轨迹规划或仿生机器人控制的工程师,既避开真实机器人磨损,又能把运动学闭环和物理反馈都练到。
2. 用同一份URDF同时喂给PyKDL和CoppeliaSim
2.1 为什么爬行运动需要两条模型链
PyKDL和CoppeliaSim是两套互补系统:PyKDL是一套纯粹的刚体运动学库,处理链式结构的正解和逆解,但不知道质量、摩擦和碰撞;CoppeliaSim用物理引擎Bullet或ODE模拟重力、接触和关节电机,但没有内建的运动学求解器。爬行运动本质上是一个受接触约束的连续运动,机械臂末端接触地面时,关节角变化与基座位移必须同时满足“末端近似不动”的约束;只用PyKDL,接触点会穿透或打滑;只用CoppeliaSim,每次设定身体怎么弯非常麻烦。所以联合仿真中,PyKDL负责“该让哪个关节转多少”,CoppeliaSim负责“这么转了以后地面给多少反力,是否推得动”。
此外PyKDL和CoppeliaSim的模型必须来自同一份URDF,因为运动学映射是严格依赖坐标系和关节轴的。如果PyKDL里定义的基座朝向和CoppeliaSim导入的URDF差一个旋转,逆解出来的关节角会全部带偏,最后表现成“关节乱转”或“末端够不到预定的脚印”。
2.2 用kdl_parser构建PyKDL Tree
常见的做法是用ROS包kdl_parser_py把URDF解析成KDL树。下面这段代码不依赖完整ROS环境,直接读文件就能用:
from kdl_parser_py import urdf
from PyKDL import ChainIkSolverVel_pinv, ChainFkSolverPos_recursive
# 读取URDF,返回(ok, tree)
ok, tree = urdf.treeFromFile("crawl_arm.urdf")
if not ok:
raise RuntimeError("URDF解析失败,检查XML格式和mesh路径")
# 获取从基座到末端的运动链
chain = tree.getChain("base_link", "ee_link")
# 为同一个链建立正解和速度级逆解
fk_solver = ChainFkSolverPos_recursive(chain)
ik_solver = ChainIkSolverVel_pinv(chain)
print(chain.getNrOfJoints())
代码逻辑比较直白:tree是整个URDF的树结构,getChain把两个link之间的关节段抽出来;ChainIkSolverVel_pinv在构造时就保存了雅可比矩阵,后面每次调用CartToJnt都会更新雅可比。注意getChain的两个参数必须和URDF里的link名字完全一致,CoppeliaSim导入后如果自动加了 _joint 之类的后缀,会造成名字对不上,这在联合仿真中经常出问题。
这里还有个针对爬行运动的建议:不要只建一条从base到ee的链。爬行运动经常需要倒着算,比如末端固定在地面、基座往前移动,这时用PyKDL的tree.getChain("ee_link", "base_link")重新取一次链,逆解的目标就从“末端该在哪”变成“基座该在哪”。我一般会做一个solver缓存:
solver_cache = {}
def get_chain_solver(root, tip):
key = (root, tip)
if key not in solver_cache:
c = tree.getChain(root, tip)
solver_cache[key] = ChainFkSolverPos_recursive(c)
solver_cache[key + "_ik"] = ChainIkSolverVel_pinv(c)
return solver_cache[key], solver_cache[key + "_ik"]
这样在支撑相和摆动相切换时不用反复创建对象,CoppeliaSim仿真循环里保持稳定。
2.3 CoppeliaSim导入URDF后的关节参数
在CoppeliaSim中导入URDF,一般用菜单File > Import > URDF,或者直接把.urdf拖进场景。导入后每个关节默认会根据URDF的transmission配置来设置,但很多URDF不包含transmission,所以必须在每个joint属性里手动打开Dynamics。建议先按下面这张表把关节参数调到位:
| 参数 | 爬行仿真建议 | 原因 |
|---|---|---|
| Dynamics enabled | 勾选 | 关闭动态时CoppeliaSim只做运动学校验,无法反映地面反力 |
| Motor enabled | 勾选 | 关节电机是Torque/Force模式的执行器 |
| Control loop enabled | 勾选 | 开启位置控制,PyKDL输出的目标关节角才能作为target position |
| Target position | 初始值设为当前关节角 | 防止仿真开始瞬间关节猛跳 |
| Max. torque | 按臂上各关节负载给200-500 N*m | 爬行时末端要撑起整个基座,扭矩不够会趴在地上不动 |
| Friction | 静摩擦0.8,动摩擦0.6 | 支撑相时末端与地面要有足够切向力,否则基座往前推时末端先打滑 |
除了joint参数,还要把每个link的碰撞体简化。CoppeliaSim导入URDF时会保留Visual mesh和Collision mesh,爬行仿真里把碰撞mesh换成box或cylinder会稳定很多,因为Bullet对凸包网格的接触点计算更快,不容易出现高频抖动。这个调整不影响PyKDL,因为PyKDL只取URDF的joint和link树,不关心碰撞形状。
接着需要在场景里添加一个child script到base_link或者一个空的scene object,用一条Lua脚本拉起remote API server。服务端启动代码:
-- child script中运行
if sim.getSimulationState() ~= sim.simulation_stopped then
simRemoteApi.start(19999)
end
在Python侧连接这个端口后,PyKDL算出的每个关节目标通过sim.simxSetJointTargetPosition写入,CoppeliaSim在下一个仿真步长里执行。到这里,运动学与物理仿真的数据通道就打通了。
3. 爬行步态的三相位规划与PyKDL逆解实现
3.1 尺蠖步态:末端抓地、身体前移、复位
机械臂爬行不是让末端在空中乱晃,而是像一个尺蠖虫一样交替使用支撑点。我把一个完整周期拆成三个相位:支撑相、摆动相和复位相。支撑相时末端接触地面,肘关节和肩关节弯曲,把基座沿爬行方向拉过去;摆动相时基座不动,末端抬起并向前移动到下一落点;复位相是末端落地的瞬间,防止刚发生碰撞反弹。这样每一步的模型都是开链,PyKDL不需要去解闭合环,简化了大部分约束问题。
用Python实现一个相位状态机,返回值包括当前相位名和相位内进度。仿真控制循环只要利用这两个值生成目标位置,再交给PyKDL求解:
class CrawlPhase:
def __init__(self):
self.phases = ["stance", "swing", "reset"]
self.duration = {"stance": 0.8, "swing": 0.6, "reset": 0.2}
def at_time(self, t):
cycle = sum(self.duration.values())
t_mod = t % cycle
for name in self.phases:
if t_mod < self.duration[name]:
return name, t_mod / self.duration[name]
t_mod -= self.duration[name]
return "stance", 0.0
相位持续时间不是等分的,因为支撑相要拖动整个基座,时间留长一点;复位相只是缓冲落地冲击,短一点。可以在CoppeliaSim里观察基座位移曲线,如果支撑相里基座根本没动,说明末端抓地力不够或者关节扭矩不够,把支撑相时间从0.8秒提高到1.5秒,而不是增加末端速度。
3.2 末端脚印轨迹与虚拟链求解
爬行方向上的目标位置由脚印规划给定。假设机械臂沿着世界坐标X方向爬,末端落点每周期前进一个步长step=0.08m,抬升高度h=0.1m:
def foot_target(phase, progress, start_x, step):
if phase == "stance":
x = start_x - step * progress
z = 0.0
elif phase == "reset":
x = start_x + step * progress
z = 0.0
else:
x = start_x + step * progress
z = 0.1 * (1.0 - abs(2.0 * progress - 1.0))
return x, z
这里给的是相对目标:支撑相末端实际抓地位置不变,但爬行机械臂的参考点是基座,所以PyKDL求解时要用倒链。倒链就是把机械臂模成一个从“末端”到“基座”的开链,末端固定在地面作为虚拟根,基座作为虚拟末端;这样stance相的目标就是基座的新位置。
PyKDL求解速度级逆解的核心代码:
def calc_joint_velocity(solver, q_now, pos_now, pos_target, dt, linear_gain=2.0):
# 构造速度Twist:线速度为位置误差,角速度这里忽略
err = Vector(pos_target[0] - pos_now[0],
pos_target[1] - pos_now[1],
pos_target[2] - pos_now[2])
twist = Twist(err * linear_gain, Vector())
q_dot = JntArray(q_now.rows())
ret = solver.CartToJnt(q_now, twist, q_dot)
if ret < 0:
return None
return q_dot
代码说明:CartToJnt把期望的笛卡尔速度映射到关节速度。linear_gain归一化位置误差,单位是1/s,通常取1.5到3.0,太大会让关节速度突变。得到q_dot后做欧拉积分,把目标关节角发送给CoppeliaSim:
q_next = JntArray(q_now.rows())
for i in range(q_now.rows()):
q_next[i] = q_now[i] + q_dot[i] * dt
每轮循环开始前,从CoppeliaSim读取当前关节角作为q_now,而不是用上一轮积分结果。这个偏差反馈特别重要,因为仿真里有电机响应延时,位置控制不会立刻到达target position,用“上一轮的q”积分会产生累积漂移。
3.3 速度级IK优于位置级IK的三个原因
很多从ROS移过来的人第一选择是ChainIkSolverPos_LMA,直接在PyKDL里求逆解。但爬行运动我更推荐速度级逆解,理由有三个。
第一,爬行过程中接触状态频繁变化,支撑相末端固定、基座移动会产生狭窄的运动空间,LMA初始猜测差一点就收敛到别的一支,出现关节角跳变;速度级每次只走一小步,轨迹连续性好。
第二,速度级能天然处理冗余自由度。机械臂爬行时如果自由度超过6,雅可比伪逆会给出最小范数解,避免多余的关节摆动。
第三,速度级求解器可以直接接到仿真循环里,不需要先把完整路径离散成位姿序列再批量逆解。
不过速度级也有弱点:存在累计误差。处理方法是每个相位结束时做一次FK校验:
fk_solver.JntToCart(q_next, frame)
dx_err = frame.p.x - pos_target[0]
if abs(dx_err) > 1e-3:
print("相位结束,末端位置误差超过1mm")
误差超过阈值时,在下一个相位开始前用位置级IK修正一下,这样两种求解器配合使用。注意PyKDL返回的Frame.p就是位置向量,可以直接读取。
4. CoppeliaSim联合仿真的通信、步长与排错参数
4.1 Remote API数据流向:Python侧写关节目标
联合仿真桥接用CoppeliaSim的Remote API。服务端脚本里启动simRemoteApi.start(19999)后,Python端连接:
import sim
sim.simxFinish(-1)
client_id = sim.simxStart("127.0.0.1", 19999, True, True, 5000, 5)
if client_id == -1:
raise ConnectionError("CoppeliaSim Remote API 连接失败")
# 先取关节句柄,建议用blocking模式建立连接
joint_handles = {}
for j in joint_names:
res, handle = sim.simxGetObjectHandle(client_id, j, sim.simx_opmode_blocking)
if res == sim.simx_return_ok:
joint_handles[j] = handle
# 主循环中写target position
for j in joint_names:
sim.simxSetJointTargetPosition(client_id, joint_handles[j], q_next[i], sim.simx_opmode_streaming)
simx_opmode_streaming是只做非阻塞写入,不会等仿真返回,适合在控制周期内发送多个关节目标;但要注意,不要对同一个句柄同时用streaming和blocking,会把通信模式弄乱。
4.2 仿真步长与PyKDL积分周期对齐
CoppeliaSim默认仿真步长是50ms,PyKDL本身没有时间概念,它的积分步长完全由外部控制循环决定。也就是说,远程API发一个关节目标,CoppeliaSim执行这个目标时,PyKDL已经按50ms走了一步,二者必须同步。如果步长不一致,关节运动会变得卡顿或超前。
一个常见做法是从CoppeliaSim读取当前仿真时间,pykdl只在这个时间差大于等于dt时继续积分:
res, sim_time = sim.simxGetFloatSignal(client_id, "simTime", sim.simx_opmode_streaming)
last_time = 0.0
if sim_time - last_time >= dt - 1e-6:
last_time = sim_time
q_next = integrate_ik(q_now, target)
仿真步长调小到20ms后,接触更稳定,但PyKDL每秒要解50次速度级逆解,每个逆解包括一次雅可比计算和伪逆,对CPU和Remote API都有压力。如果发现客户端跟随不上,不要无限调小步长,而是把机械臂的碰撞模型简化,或者把step control中的maximum step size保持25ms。
4.3 关节模式参数对照表
| 现象 | 参数项 | 推荐值/范围 |
|---|---|---|
| 关节松动,机械臂被重力压下 | Motor enabled + Control loop enabled | 必须同时打开 |
| 位置控制振荡或咔哒声 | Joint control loop PID | P降到10-30,I设为0,D给0.2-0.8 |
| 支撑相基座纹丝不动 | Max. torque | 从0.4倍link重力矩开始,逐步增大 |
| 末端打滑 | Contact properties的friction | 静摩擦0.8,动摩擦0.6,比默认0.5高 |
| 仿真开始瞬间关节飞走 | Target position初始值 | 设成URDF初始关节角,而不是0 |
其中PID参数要根据机械臂重量调整,我一般先不开control loop,用手动拖动观察关节自然下垂,再逐渐加P。爬行运动对位置精度要求不如抓取高,关节角误差3度以内都可以,所以不必把PID调到最小超调。
4.4 三个最常踩的联合仿真坑
第一个坑是关节角从PyKDL的弧度到CoppeliaSim的默认角度单位不一致。PyKDL全部采用弧度制,而CoppeliaSim场景里的joint参数可以设置成degree。如果不做转换,爬行运动看起来就像疯狂抽搐。解决方法是写入前统一乘上math.pi/180,或者写入前查一下joint的单位设置。
第二个坑是URDF中revolute joint的limit没有设好。PyKDL不会自动约束关节角度,但CoppeliaSim会在关节位置达到limit时锁住,导致实际执行趋势和PyKDL积分趋势相反。最后看到的机械臂要么顶着limit不动,要么关节突然以最大速度反弹。必须在积分循环里clip:
for i in range(n_joints):
if q_next[i] < jmin[i]:
q_next[i] = jmin[i]
if q_next[i] > jmax[i]:
q_next[i] = jmax[i]
clip之后q_dot与期望的笛卡尔速度已经不一致,但至少不会撞限位。
第三个坑是末端接触判断用错信号。很多人用CoppeliaSim的sim.GetContactCallback,但Remote API下回调函数不会自动push到Python端。更稳妥的是在末端link上加一个force sensor,用simxReadForceSensor读法向力。如果法向力低于阈值,就说明接触还没建立,这时不要切到支撑相,否则爬行会变成“推着地面滑”。
5. 用接触力反馈驱动支撑相切换,避免爬行变滑行
爬行运动能不能稳定推进,核心不在逆解速度多快,而在支撑相和摆动相的切换时机。固定时间切换虽然代码简单,但当CoppeliaSim里的摩擦系数、关节扭矩或者地面略有变化时,末端没有真正压住地面就切换成支撑相,基座就会在推挤过程中产生滑移。常见做法是在末端连杆上加一个force sensor,作为接触反馈信号,直接接入爬行状态机。
在CoppeliaSim中把force sensor插入URDF的ee_link,然后用Remote API读取:
res, handle = sim.simxGetObjectHandle(client_id, "ee_force_sensor", sim.simx_opmode_blocking)
res, force = sim.simxReadForceSensor(client_id, handle, sim.simx_opmode_streaming)
simxReadForceSensor返回的force是世界坐标系下的三轴力。爬行时末端垂直压地的力应该是负Z(坐标约定不同可能相反),取f_z的绝对值。把相位切换条件从时间触发改成“时间+力阈值”触发:
def can_switch(self, phase, t, f_z):
if phase == "swing" and f_z > 0.5 * self.threshold:
return False
if phase == "reset" and f_z < self.threshold:
return True
return False
threshold建议按机械臂重量的1.2倍取值。例如整条臂重4kg,末端支撑时法向力约40N,threshold就设48N。太低会导致支撑相里末端还悬空;太高会导致状态机一直卡在reset,整个周期无法推进。通过观察CoppeliaSim的Graph中的force曲线,能看到每次落地时f_z都会出现一个尖峰,尖峰后快速回落,取回落后的稳态值,再乘以1.2,就足够可靠。
另一个容易忽略的参数是支撑相里末端摩擦力的方向。即使f_z满足阈值,如果末端碰巧落在CoppeliaSim场景里的一块低摩擦碰撞板上,支撑相照样推不动。解决办法是在末端安装一个能够翻转的“抓地锚点”形状,比如把末端碰撞体从球改成锥体,锥尖在支撑相压入薄层。在PyKDL维度上不需要任何改动,因为锥体只是碰撞模型,不改变运动学框架。
这套力反馈机制对从零复刻机械臂的项目特别有价值:不需要昂贵的六维力传感器,仿真里一个虚拟force sensor就够;等真实机械臂落地时,同样可以用固定在传力路径上的单轴压力传感器替代。让机械臂像尺蠖一样稳定爬行的关键,始终是把“希望的运动学”和“地面事实”放在同一个闭环里,接触力就是那个事实。
更多推荐
所有评论(0)