机器人软件
简介
数字孪生
数字孪生不仅有相同的外观（Geometry），还有相同的物理属性（Physics），能接收传感器数据（Sensing）并响应控制指令（Actuation）。
| 传统仿真 | 数字孪生 | |
|---|---|---|
| 焦点 | 特定过程 | 完整实体 |
| 范围 | 局部、离线 | 全局、实时同步 |
| 目的 | 分析与验证 | 预测、监控、闭环交互 |
| 例子 | 一个齿轮的受力分析，一个电路的性能模拟 | 在数字空间创建物理实体的动态虚拟副本 |
在robot 领域，就是为实体的机械臂创建一个它的数字孪生体，作为主要的开发、测试和实验平台。
- 物理成本，实体机器人价格昂贵，在仿真中，我们可以”撞坏“无数个虚拟机器人，成本为零。
- 时间成本，物理实验受场地、时间限制，在仿真中，我们可以同时运行成百上千个实验，24小时不间断。
- 迭代成本，修改一个物理样机的设计可能需要数周时间，而在仿真中，修改一个模型参数（如臂长、电机扭矩）可能只需要几秒钟。
- 设备安全、人员安全。
- 前端开发与后端解耦，算法工程师可以在仿真中开发调试，无需等待物理样机制造完成。这实现了软硬件开发的并行化。 仿真提供一个”纯净“的、排除了物理不确定性的理想环境，便于我们首先验证算法逻辑的正确性。
数据驱动AI的困境：现代AI，尤其是深度学习，是名副其实的”数据饕餮“。训练一个图像识别模型需要数百万张图片，那么，训练一个机器人抓取模型需要什么？让一个实体机器人尝试抓取100w次，可能需要数年时间，并且硬件早已报废。仿真环境是强化学习算法最理想的训练场，Agent可以在虚拟世界里进行数亿次的试错，学习从失败中总结经验，而这一切都可以在几个小时内完成。此外，还能通过”领域随机化“技术，生成千奇百怪的数据，随机改变光照、纹理、物体位置、物理参数……，用这些数据训练出的模型，在真实世界中的泛化能力（Generalization）会更强。“在仿真中大规模训练，在现实中少量微调”已经成为公认的、最高效的机器人AI开发范式。
Gazebo
ROS2 官方原生仿真，
MuJoCo
MuJoCo stands for Multi-Joint dynamics with Contact. It is a general purpose physics engine that aims to facilitate research and development in robotics, biomechanics, graphics and animation, machine learning, and other areas which demand fast and accurate simulation of articulated(铰接的) structures interacting with their environment.
<mujoco>
<option timestep="0.002" gravity="0 0 -9.81"/> <!--gravity重力沿 Z 轴向下, timestep每个物理步推进 0.002 秒-->
<worldbody>
<!-- ground一个地面-->
<geom name="ground"
type="plane"
size="2 2 0.1"/>
<body name="ball">
<!--关节：这里限制小球只能沿 Z 轴上下移动-->
<joint name="vertical"
type="slide"
axis="0 0 1"/>
<!--一个物体，有半径 0.05 米、质量 1 千克的球形几何体/geom-->
<geom type="sphere"
size="0.05"
mass="1"/>
</body>
</worldbody>
</mujoco>
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
# 把小球放在 1 米高，位移就是球心的高度。
data.qpos[0] = 1.0
# 让物理世界运行一秒, 500 步 × 每步 0.002 秒 = 1 秒仿真时间
for step in range(500):
mujoco.mj_step(model, data)
if (step + 1) % 100 == 0:
print(
f"时间={data.time:.2f}s，"
f"高度={data.qpos[0]:.3f}m，"
f"竖直速度={data.qvel[0]:.3f}m/s"
)
# 小球在重力作用下自然下落
ROS（Robot Operating System）
2006 年，Eric Berger 和 Keenan Wyrobek 通过调查发现，当时的机器人公司在开发机器人时把八成的精力都花在了搭建机器人的通信机制和基础工具上，反而没时间完善机器人的决策系统。
硬件是机器人的载体，软件则赋予智能机器人灵魂，智能机器人开发需要一个机器人操作系统作为基础。ROS 是开源的，由一组软件库和若干工具组成，追求的是稳定、安全和实时的通信能力（核心是通信），并非经典意义上的操作系统。
ros2 系统架构
- ROS 2 的核心通信是采用第三方的通信组件来实现的，这个第三方就是数据分发服务（Data Distribution Service，DDS），DDS 基于实时发布订阅协议（Real-time Publish-Subscribe，RTPS）来实现数据分发。
- 因为要支持不同厂家的DDS，同时又需要对外保持一致，所以ROS 2定义了RMW（ROS Middleware Interface，ROS中间件接口），再由不同DDS进行实现，为ROS 2客户端层提供统一的调用接口。举例来说，DDS接口层类似于USB接口的标准，而DDS实现层就是不同厂家根据标准生产的USB设备。
- ROS 2 客户端层提供了不同编程语言的 ROS 2 客户端库（ROS 2 Client Library，RCL），使用这些库提供的接口，可以完成对 ROS 2 核心功能的调用，如话题、服务、参数和动作通信机制。
- 所有基于 RCL 开发的程序都属于应用层，后续开发的机器人应用也属于这一层。
四大通信机制
- topic，基于发布-订阅模式的通信方式，允许节点之间异步交换数据，单向，即 发布者 ( Publisher) 将消息 发 布 到 某 个 话题 上,订阅者 ( Subscriber) 订阅话题即可获取数据
- service，双向(client/server)，同步通信方式，客户端发送请求，服务端处理并返回结果
- parameter，用于机器人参数的设置和读取
- action，client/server, 支持复杂行为的通信方式，服务端可以反馈处理进度，客户端可以取消请求。
ROS 2 为基础的软件栈
- 编程模型，以 Node（节点）作为基本的功能和通信单位，逻辑功能单元，一个进程可以运行多个 Node(反之不可以)，ros2 提供了 rclpy.node.Node 父类。通过topic、service和action与其他Node协作
- 机器人通用能力
- 坐标变换 ( Coordinate Transformation, TF)，,相机 固定 在右上 方的camera_link处,机 械 臂的底座固定在 base_link 处 。从 base_link 到 camera_link 的位 置是固定 不 变 的 ,图中已经给出了变换关系,平移分量(0.5,0.3,0.6),旋 转分量 ( 180,0,0)。相 机通过识别得到瓶子 bottle_link 的坐标 ,其中平移分量为〈0.2,0.3,0.5), 旋转分量为(0,0,0)。如果机械臂想夹取瓶子 ,就需要知道base_link到bottle_link之间的坐标关系。
- 机器人领域框架。nav2（导航）/movelt2（机械臂运动规划）
机器人世界的建筑蓝图
|||| |—|—|—| |在xml的基础上对标签的名字有规定|URDF|MJCF| |仿真器|Gazebo|MuJoCo + MuJoCo Warp| URDF 和MJCF都能描述身体，可以相互转换。MJCF 还可以直接描述整个仿真场景、执行器及仿真选项。
MJCF
mujoco 专用格式规范 <mujoco> ...</mujoco> 下包含3大类
- 设置 compiler/asset/default
- worldbody 物理模型，运动学tree
- body, 包含joint（两个body 之间有约束关系）/geom/body/inertial/site等
- geom, 结合体，静态的、动态的
- inertial(指定geom 的一些性质)/site( 一个点位，挂在某个body上)
- actuator，我们对机器人实现控制， 实际上就是对驱动器进行控制。拿到一个xml 首要看的就是有哪些actuator以及它所驱动的joint
data.ctrl[actuator index]=xx 就是给actuator 输入- tendon，传动器
- sensor，传感器，挂在某个site 上
mujoco.mj_step(model,data)，每运行mj_step 一次之后，mujoco会根据传入的模型以及模型对应的数据进行计算（比如小球重力往下落）
URDF
仿真的第一步是对机器人进行建模。URDF(Unified Robot Description Format) 是 ROS 世界中描述机器人的标准语言，可以看成是机器人三维模型说明书
- 链接/Links
- 关节/Joints，一般对应一个电机
比如我们要向计算机描述一个人的身体结构，我们会怎么说？ 这个人有一个驱干，驱干上连着一个脖子，脖子上连着一个头……，我们需要一种标准化的语言来精确描述这种连接关系和每个部件的属性。urdf 基于xml格式的文件，专门用来描述机器人的运动学模型和动力学模型，主流机器人软件和仿真器都可以加载和使用。
这个简化机械臂由 4 个 link 和 3 个 joint 组成：肩、肘关节负责旋转，夹爪关节负责开合。夹爪外壳与固定爪指和小臂没有相对运动，因此合并为同一个 link。URDF 描述装配结构和运动约束，当前关节角度及夹爪位移由 /joint_states 提供。robot_state_publisher 结合两者计算各 link 的坐标变换，并发布到 TF。
<?xml version="1.0"?>
<!-- Teaching model: two revolute arm joints and one sliding gripper finger.
XY plane, +Z rotation; metres and radians. Visual geometry only:
collision, inertial properties and hardware control are omitted. -->
<robot name="two_joint_arm">
<material name="base_gray"><color rgba="0.39 0.45 0.55 1"/></material>
<material name="arm_blue"><color rgba="0.15 0.39 0.92 1"/></material>
<material name="forearm_teal"><color rgba="0.05 0.58 0.53 1"/></material>
<material name="finger_purple"><color rgba="0.58 0.20 0.92 1"/></material>
<link name="base_link">
<visual>
<origin xyz="0 0 -0.025"/>
<geometry><box size="0.10 0.09 0.03"/></geometry>
<material name="base_gray"/>
</visual>
</link>
<link name="upper_arm_link">
<visual>
<origin xyz="0.15 0 0"/>
<geometry><box size="0.30 0.035 0.02"/></geometry>
<material name="arm_blue"/>
</visual>
</link>
<link name="forearm_link">
<visual>
<origin xyz="0.125 0 0"/>
<geometry><box size="0.25 0.03 0.02"/></geometry>
<material name="forearm_teal"/>
</visual>
<!-- Palm and fixed finger belong to the same rigid body as the forearm. -->
<visual>
<origin xyz="0.24 0.008 0"/>
<geometry><box size="0.026 0.06 0.02"/></geometry>
<material name="forearm_teal"/>
</visual>
<visual>
<origin xyz="0.285 -0.012 0"/>
<geometry><box size="0.07 0.008 0.02"/></geometry>
<material name="forearm_teal"/>
</visual>
</link>
<link name="finger_link">
<visual>
<origin xyz="0.035 0 0"/>
<geometry><box size="0.07 0.008 0.02"/></geometry>
<material name="finger_purple"/>
</visual>
</link>
<joint name="shoulder_joint" type="revolute">
<parent link="base_link"/>
<child link="upper_arm_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-1.57" upper="1.57" effort="5" velocity="1"/>
</joint>
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin xyz="0.30 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-1.57" upper="1.57" effort="5" velocity="1"/>
</joint>
<joint name="gripper_joint" type="prismatic">
<parent link="forearm_link"/>
<child link="finger_link"/>
<!-- At q=0 the two finger surfaces touch; positive local Y opens them. -->
<origin xyz="0.25 -0.004 0" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="0" upper="0.04" effort="10" velocity="0.05"/>
</joint>
</robot>
下面是让机械臂挥手的ros2 代码
- STEPS 预先定义好了 各个joint 的指令参数
- 定义了3个 topic，用于给joint 发指令
- 对仿真器 Gazebo 配置了一个topic= “/so2/joint_states”，用于接收joint 状态
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
JOINTS = ["shoulder_joint", "elbow_joint", "gripper_joint"]
# 每一步：肩角度(rad)、肘角度(rad)、夹爪开度(m)
STEPS = [
(0.9, -0.6, 0.03), # 准备
*[(0.9, e, 0.03) for e in [0.25, -0.9] * 3], # 挥手
*[(0.9, -0.9, g) for g in [0.038, 0.002] * 2], # 张合
(0.65, -1.1, 0.03), # 回到初始姿态
]
class HelloArm(Node):
def __init__(self):
super().__init__("hello_arm")
self.step = 0
self.positions = {}
self.publishers_by_joint = {
name: self.create_publisher(
Float64, f"/so2/{name}/target", 10
)
for name in JOINTS
}
self.create_subscription( # 订阅关节状态
JointState,
"/so2/joint_states",
self.on_feedback,
qos_profile_sensor_data,
)
self.timer = self.create_timer(0.1, self.tick)
def on_feedback(self, msg):
self.positions = dict(zip(msg.name, msg.position))
def tick(self):
if not all(name in self.positions for name in JOINTS):
return
target = STEPS[self.step]
# 告诉各个关节：“你应该转到 / 移到这里。”
for name, value in zip(JOINTS, target):
self.publishers_by_joint[name].publish(Float64(data=value))
# 根据实际反馈判断到位，而不是发完指令就当作完成。
tolerances = [0.025, 0.025, 0.0015]
reached = all(
abs(self.positions[name] - value) < tolerance
for name, value, tolerance in zip(JOINTS, target, tolerances)
)
if reached:
self.step += 1
if self.step == len(STEPS):
self.get_logger().info("打招呼完成")
self.timer.cancel()
def main():
rclpy.init()
node = HelloArm()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
算法
机器人导航
Nav2/Navigation2 是基于 ROS 2 构建的导航框架，包含 导航任务编排、代价地图、规划服务、控制服务、恢复行为等，具体到某个组件（比如规划器），Nav2 定义了规划器接口GlobalPlanner ，具体算法以插件的形式被加载使用。
- 要想导航 ,就要先确定当前位置和目标点的位置 , 如何确定位置其实就是定位问题。
- 知道了当前位置和目标位置信息后 ,要想规划出行走路线 ,还需要知道哪里是可以走的，哪里是不能走的。此时就需要一张标有环境信息的地图
- SLAM ( Simultaneous Localization andMapping, 同步定位与地图构建)
- 解决了地图和定位问题 后 ,我们再来考虑如何走的问题
- 全局路径规划。确认一条从当前位置到目标位置之间的路线 ,理想中这条路线应该符合某些要求 ,比如能避开所有障碍物且耗时最少。要实现这一效果就需要在地图上标记代价信息, 比如走路况差的路线要花费更多的时间, 付出更大的代价。
- 局部路径规划。在按照全局路线行走时,前方可能会出现一些障碍物 ,比如开车时发现前方一段车道正在封闭维修,此时就需要绕开维修车道 。但机器人要想绕过去 ,第一步要把障碍物加到地图中 ,然后对当前这一小段路线重新进行路径规划。因为这个障碍物是小范围和动态的 ,所以我们往往会针对当前小范围环境,重新创建一张局部的代价地图,然后进行路径规划 。
- 考虑全局路径和局部路径以后 ,有时我们还要面对一些特殊情况 ,比如行走过程中卡住或者被行人挡住 ,且找不到局部路径绕过去。此时就需要有一些相应的行为帮助机器人脱离困境 , 比如当前进卡住时尝试后退 ,当被人挡住时播放“请让路 ”语音。对于这些遇到故障问题时控制动作，我们称之为恢复行为。
机器人控制系统
部署
实际运行时
- 板载计算机。机器人内部的计算机运行 Linux + ROS2 进程，通过驱动程序、控制板连接电机
- 外置计算机。开发者电脑（上位机）运行 Linux + ROS2 进程，通过 USB、串口、CAN、以太网等连接装置（下位机）的MCU控制板。电脑一般不会直接操作电机的供电线。
- MCU 通常运行固件或实时操作系统（RTOS），不运行完整 Ubuntu。 常见例子有 STM32、ESP32。 micro-ROS。
仿真时，开发者电脑运行 Linux + ROS2 进程，仿真器提供虚拟硬件及其控制、状态反馈接口；独立运行的 ROS2 程序通过适配层发送关节指令并接收反馈，形成控制闭环。
留下评论