CeRLP：面向跨形态机器人视觉导航的统一局部规划框架
1. 简介
移动机器人导航是机器人学中最基础也最关键的能力之一。无论是仓储物流中的自主搬运车、灾难救援中的探测机器人，还是家庭服务场景中的扫地机器人，都需要在复杂环境中找到一条安全可行的路径到达目标位置。传统的导航方法，如动态窗口法（DWA）和时间弹性带（TEB），虽然在工程实践中被广泛使用，但它们严重依赖预先构建的精确地图，一旦环境发生变化或地图不可用，导航性能就会急剧下降。
近年来，深度强化学习（DRL）为机器人导航带来了新的范式。DRL方法可以直接从相机或激光雷达的原始传感器数据中学习导航策略，不再需要显式的地图构建过程。然而，当我们试图将一个在特定机器人上训练好的视觉导航策略迁移到另一台机器人时，往往会遭遇严重的性能退化。这种退化的根源在于三个层面的耦合问题：
第一，单目深度的尺度模糊。单目相机拍摄的图像天然丢失了深度的绝对尺度信息——一个近处的小物体和一个远处的大物体在图像上可能呈现完全相同的投影。当前主流的单目深度估计模型（如 Depth Anything V2，发表于 NeurIPS 2024）虽然具备出色的跨场景泛化能力，但其输出的是相对深度而非绝对米制深度。这意味着模型预测的"远近关系"是正确的，但具体"有多远"是未知的。对于需要精确判断障碍物距离的导航任务来说，这是一个致命缺陷。
第二，相机配置的敏感性。不同机器人上的相机安装高度、俯仰角度、视场角（FOV）和内参矩阵各不相同。即使是同一型号的相机，安装位置的微小变化也会导致观测图像的分布发生显著偏移。一个在相机高度 0.4 米处训练的策略，部署到相机高度 0.6 米的机器人上时，可能会系统性地高估或低估障碍物的距离。
第三，机器人几何信息的缺失。现有的大多数视觉导航方法将机器人简化为一个没有体积的质点。这种简化在开阔环境中或许可以接受，但在狭窄通道、密集障碍物等场景中，忽略机器人的实际长度和宽度会直接导致碰撞。一台宽度为 1 米的机器人和一台宽度为 0.3 米的机器人，面对同一个 0.8 米宽的通道，应该做出截然不同的决策。
图 1：CeRLP 的核心思想。面对不同体型、不同相机配置的异构机器人，CeRLP 将多样化的视觉输入统一转换为虚拟激光扫描，并将机器人建模为广义长方体，从而在无需微调的情况下实现跨形态零样本导航。
为了应对上述挑战，近年来出现了两类主流解决方案。一类是以 GNM（General Navigation Model）和 ViNT（Visual Navigation Transformer）为代表的大规模数据驱动方法，它们通过收集来自数十种不同机器人的海量导航数据来学习通用的视觉表征。另一类是以 SplitNet 和 FastRLAP 为代表的微调适配方法，它们在新机器人或新环境上进行额外的模型微调。然而，这两类方法都存在明显的局限性：前者对数据量的需求极高，后者的适配过程耗时且不够灵活，而且两者都没有显式地考虑机器人的几何尺寸信息，在安全性上存在隐患。
CeRLP（Cross-embodiment Robot Local Planning）正是在这一背景下被提出的。它提供了一个全新的视角：不去直接学习与相机像素分布相关的特征，而是将视觉信息抽象为统一的几何表示，从根本上解耦导航策略与特定传感器硬件和机器人平台之间的绑定关系。
2. CeRLP 框架总览：三个模块，一条流水线
CeRLP 的设计哲学可以用一句话概括：将异构的视觉输入标准化为统一的几何表示，让导航策略只关心"障碍物在哪里"和"机器人有多大"，而不关心"图像是什么样的"。
整个框架由三个核心模块串联组成，形成一条从 RGB 图像到速度指令的完整处理流水线：
图 2：CeRLP 完整框架。异构机器人的 RGB 图像经过深度估计和尺度校正后，被转换为米制深度图；随后通过视觉转扫描模块生成高度自适应的虚拟激光扫描；最终由维度可配置的策略网络输出安全的速度控制指令。整个过程中，相机参数和机器人尺寸作为显式输入参与计算，而非隐式地编码在训练数据中。
模块一：深度估计尺度校正（Scale Correction for Depth Estimation）。该模块解决的是"从相对深度到绝对深度"的问题。它利用预训练的 Depth Anything V2 模型从 RGB 图像中提取相对深度图，然后通过一次离线标定过程（使用 ArUco 标定板）计算出特定相机的尺度因子和偏移量，在线推理时将相对深度实时转换为米制深度。这个过程只需要对每个相机做一次标定，之后就可以持续使用。
模块二：视觉转扫描抽象（Visual-to-Scan Abstraction）。该模块解决的是"从深度图到统一表示"的问题。它将米制深度图通过相机内参反投影为三维点云，再利用相机外参将点云从相机坐标系变换到机器人坐标系，接着根据机器人的实际高度进行障碍物过滤（去除地面和过高物体），最后将过滤后的三维点投影到二维平面，生成一条标准的虚拟激光扫描线。无论原始相机是什么型号、装在什么位置，输出的激光扫描格式完全一致。
模块三：维度可配置局部规划（Dimension-Configurable Planning）。该模块解决的是"不同体型的机器人如何共享同一个策略"的问题。它基于 DRL-DCLP 方法，将机器人的前悬长度、后悬长度和宽度直接编码到每个激光扫描点的特征向量中，通过 PointNet 编码器提取全局几何特征，再结合目标位置和当前速度信息，由 SAC（Soft Actor-Critic）网络输出线速度和角速度指令。策略在仿真环境中通过课程学习训练，覆盖连续变化的机器人尺寸空间。
这三个模块的协作关系可以用以下伪代码来描述：
# 伪代码：CeRLP 完整推理流程
# 注意：该代码为根据论文描述编写的伪代码，原始代码尚未开源
import numpy as np
class CeRLPFramework:
"""CeRLP 跨形态视觉导航框架"""
def __init__(self, depth_model, scale_params, camera_intrinsic,
camera_extrinsic, robot_body, policy_network):
"""
Args:
depth_model: 预训练的 Depth Anything V2 模型
scale_params: 离线标定得到的 (s1, s2) 尺度参数
camera_intrinsic: 相机内参矩阵 K = {fx, fy, cx, cy}
camera_extrinsic: 相机外参 T_ext = [R_ext | t_ext]
robot_body: 机器人尺寸 [L_front, L_rear, W]
policy_network: 训练好的 SAC 策略网络
"""
self.depth_model = depth_model
self.s1, self.s2 = scale_params
self.K = camera_intrinsic
self.T_ext = camera_extrinsic
self.robot_body = robot_body
self.policy = policy_network
def infer(self, rgb_image, goal_position, current_velocity, dynamic_limits):
"""
单帧推理：从 RGB 图像到速度指令
Args:
rgb_image: 当前帧 RGB 图像 (H, W, 3)
goal_position: 相对目标位置 (distance, angle)
current_velocity: 当前速度 (v, omega)
dynamic_limits: 速度和加速度限制
Returns:
(v, omega): 线速度和角速度指令
"""
# 第一步：深度估计 + 尺度校正
relative_depth = self.depth_model(rgb_image) # 相对深度
metric_depth = 1.0 / (self.s1 * relative_depth + self.s2) # 米制深度
# 第二步：视觉转扫描
virtual_scan = self.visual_to_scan(metric_depth)
# 第三步：维度可配置规划
action = self.plan(virtual_scan, goal_position,
current_velocity, dynamic_limits)
return action
从工程实现的角度看，CeRLP 的一个重要优势在于其模块化设计。当需要部署到一台新机器人时，只需要三步操作：（1）用 ArUco 标定板对新相机做一次离线标定，获取尺度参数；（2）测量并输入机器人的物理尺寸和相机安装参数；（3）直接运行，无需重新训练或微调任何模型。这种**“即插即用”**的特性是 CeRLP 区别于现有方法的核心竞争力。
3. 单目深度估计的尺度校正
3.1 尺度模糊问题的本质
要理解 CeRLP 的第一个核心模块，首先需要理解单目深度估计中的尺度模糊问题。当我们用一台相机拍摄一张照片时，三维世界被投影到了二维平面上，深度信息在这个过程中被丢失了。虽然人类可以通过经验和上下文线索推断物体的大致距离，但从数学上严格恢复绝对深度是一个不适定问题（ill-posed problem）。
当前最先进的单目深度估计模型，如 Depth Anything V2（Yang et al., NeurIPS 2024），通过在大规模合成数据和伪标签真实数据上训练，已经能够产生质量极高的相对深度图。所谓"相对深度"，是指模型输出的深度值保持了场景中物体之间的远近顺序关系，但其绝对数值与真实的米制距离之间存在一个未知的仿射变换。具体来说，模型预测的逆深度 d p r e d d_{pred} dpred 与真实逆深度 d g t d_{gt} dgt 之间满足如下关系：
d g t = s 1 ⋅ d p r e d + s 2 d_{gt} = s_1 \cdot d_{pred} + s_2 dgt=s1⋅dpred+s2
其中 s 1 s_1 s1 是全局尺度因子， s 2 s_2 s2 是视差偏移量。 s 1 s_1 s1 反映了单张图像中未知的物理基线， s 2 s_2 s2 补偿了深度范围定义的差异。这两个参数对于每一种相机配置都是不同的，因为它们取决于相机的焦距、安装高度、俯仰角等物理属性。
对于导航任务来说，仅有相对深度是远远不够的。如果机器人不知道前方障碍物到底是 0.5 米远还是 5 米远，就无法做出正确的避障决策。因此，恢复绝对尺度是将单目深度估计应用于机器人导航的前提条件。
3.2 离线尺度标定方法
CeRLP 提出了一种基于 ArUco 标定板的离线尺度标定方法。其核心思想是：利用已知物理尺寸的标定板作为"尺子"，通过比较标定板的真实深度和模型预测的相对深度，反推出尺度参数 s 1 s_1 s1 和 s 2 s_2 s2。
整个标定过程分为以下几个步骤：
首先，在不同距离处（至少包含一个近距离和一个远距离）放置 ArUco 标定板，用待标定的相机拍摄若干张包含标定板的图像。之所以需要不同距离的数据，是因为仿射变换有两个未知数（ s 1 s_1 s1 和 s 2 s_2 s2），至少需要两个不同深度平面的约束才能唯一确定。如果所有标定点都集中在同一深度，线性系统将会病态（ill-conditioned），导致求解不稳定。
然后，对每张标定图像执行以下处理：检测 ArUco 标记的四个角点像素坐标，利用 PnP（Perspective-n-Point）算法结合相机内参和畸变系数求解标记相对于相机的 6-DoF 位姿，从位姿中提取每个角点的真实深度值，同时从深度模型的输出中采样对应像素位置的预测值。
最后，将所有角点的数据汇总，构建一个超定线性系统，通过**岭回归（Ridge Regression）**求解最优的尺度参数。
以下是离线标定过程的完整伪代码实现：
# 伪代码：离线尺度标定算法
# 注意：该代码为根据论文 Algorithm 1 编写的伪代码
import numpy as np
import cv2
def offline_scale_calibration(calibration_images, marker_size, K, dist_coeffs,
depth_model, lambda_reg=1.0):
"""
离线尺度标定：通过 ArUco 标定板恢复深度估计的尺度参数
Args:
calibration_images: 标定图像列表（需包含近距离和远距离的标定板图像）
marker_size: ArUco 标记的物理边长（米）
K: 相机内参矩阵 (3x3)
dist_coeffs: 相机畸变系数
depth_model: 预训练的深度估计模型（如 Depth Anything V2）
lambda_reg: 岭回归正则化系数
Returns:
s1: 尺度因子
s2: 视差偏移量
"""
# 定义 ArUco 标记在物体坐标系中的角点坐标
half = marker_size / 2.0
obj_points = np.array([
[-half, half, 0],
[ half, half, 0],
[ half, -half, 0],
[-half, -half, 0]
], dtype=np.float32)
# 初始化线性系统的数据收集
A_rows = [] # 系数矩阵的行
b_rows = [] # 观测向量的行
# 初始化 ArUco 检测器
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
detector_params = cv2.aruco.DetectorParameters()
for image in calibration_images:
# 步骤 1：用深度模型预测相对深度图
D_rel = depth_model(image) # 输出为逆相对深度
# 步骤 2：检测 ArUco 标记
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
corners, ids, _ = cv2.aruco.detectMarkers(
gray, aruco_dict, parameters=detector_params
)
if ids is None:
continue
for i in range(len(ids)):
# 步骤 3：通过 PnP 算法估计标记的 6-DoF 位姿
success, rvec, tvec = cv2.solvePnP(
obj_points, corners[i][0], K, dist_coeffs
)
if not success:
continue
# 步骤 4：将标记角点变换到相机坐标系
R, _ = cv2.Rodrigues(rvec)
for k in range(4):
# 计算角点在相机坐标系中的三维坐标
p_cam = R @ obj_points[k] + tvec.flatten()
# 真实深度 = 相机坐标系中的 Z 分量
z_real = p_cam[2]
# 获取角点的像素坐标
u_p = int(round(corners[i][0][k][0]))
v_p = int(round(corners[i][0][k][1]))
# 从深度模型输出中采样对应位置的预测值
d_pred = D_rel[v_p, u_p]
# 构建线性方程：d_pred * s1 + 1 * s2 = 1 / z_real
A_rows.append([d_pred, 1.0])
b_rows.append(1.0 / z_real)
# 步骤 5：通过岭回归求解尺度参数
A = np.array(A_rows)
b = np.array(b_rows)
# 闭式解：x = (A^T A + lambda * I)^{-1} A^T b
I = np.eye(2)
x = np.linalg.inv(A.T @ A + lambda_reg * I) @ A.T @ b
s1, s2 = x[0], x[1]
return s1, s2
3.3 在线尺度校正
标定完成后，在线推理阶段的尺度校正非常简单高效。对于每一帧 RGB 图像，只需要两步计算：
# 伪代码：在线尺度校正
# 注意：该代码为根据论文公式 (12) 编写的伪代码
def online_scale_correction(rgb_image, depth_model, s1, s2):
"""
在线推理时的尺度校正
Args:
rgb_image: 当前帧 RGB 图像
depth_model: 预训练的深度估计模型
s1, s2: 离线标定得到的尺度参数
Returns:
D_metric: 米制深度图（单位：米）
"""
# 预测相对深度（逆深度）
D_rel = depth_model(rgb_image)
# 通过仿射变换恢复米制深度
# 公式：D_metric = 1 / (s1 * D_rel + s2)
D_metric = 1.0 / (s1 * D_rel + s2)
return D_metric
这个设计的精妙之处在于：深度模型本身不需要任何修改或重新训练，尺度恢复完全通过后处理实现。当需要更换相机时，只需要重新做一次离线标定（通常只需几分钟），就可以获得新相机的尺度参数。这比重新训练一个 metric depth 模型或者收集新的训练数据要高效得多。
论文中的实验数据表明，经过尺度校正后的深度估计精度显著优于直接使用 metric depth 模型的结果，尤其是在不同相机配置之间切换时，校正后的深度误差保持稳定，而未校正的模型误差会随相机变化剧烈波动。
4. 视觉转扫描抽象——从深度图到统一激光表示
4.1 为什么需要转换为激光扫描？
在获得了精确的米制深度图之后，一个自然的问题是：为什么不直接用深度图作为导航策略的输入？答案在于深度图仍然与相机的具体配置紧密耦合。不同相机的分辨率、视场角、安装位置各不相同，产生的深度图在像素分布上差异巨大。如果直接用深度图训练策略，策略就会隐式地学习到特定相机的像素模式，换一台相机就会失效。
相比之下，二维激光扫描是一种天然的"标准化"表示。一条激光扫描线本质上就是一组**"角度-距离"对**，它描述的是机器人周围各个方向上最近障碍物的距离。这种表示与相机的具体参数无关，只与环境的几何结构有关。更重要的是，已有研究（DRL-DCLP, Zhang et al.）已经证明，基于激光扫描的导航策略可以在不同尺寸的机器人之间实现零样本迁移。因此，将视觉信息转换为激光扫描，就可以直接复用这些成熟的策略框架。
4.2 三维反投影与坐标变换
视觉转扫描的第一步是将二维深度图"还原"为三维点云。这个过程利用相机的针孔模型，将每个像素的深度值反投影到三维空间中。对于深度图中的每个有效像素 ( u , v ) (u, v) (u,v)，其对应的三维点在相机坐标系中的坐标为：
X c = ( u − c x ) ⋅ Z f x , Y c = ( v − c y ) ⋅ Z f y , Z c = Z X_c = \frac{(u - c_x) \cdot Z}{f_x}, \quad Y_c = \frac{(v - c_y) \cdot Z}{f_y}, \quad Z_c = Z Xc=fx(u−cx)⋅Z,Yc=fy(v−cy)⋅Z,Zc=Z
其中 f x , f y f_x, f_y fx,fy 是焦距， c x , c y c_x, c_y cx,cy 是光心坐标， Z Z Z 是该像素的米制深度值。
接下来，需要将点云从相机坐标系变换到机器人坐标系。这一步至关重要，因为不同机器人上的相机安装位置和角度各不相同。通过相机的外参矩阵（包含旋转矩阵 R e x t R_{ext} Rext 和平移向量 t e x t t_{ext} text），可以将相机坐标系中的点变换到机器人坐标系：
p r o b = R e x t ⋅ p c a m + t e x t \mathbf{p}_{rob} = R_{ext} \cdot \mathbf{p}_{cam} + t_{ext} prob=Rext⋅pcam+text
这个变换有效地消除了相机俯仰角造成的透视畸变，将地面平面对齐到统一的坐标系中。
4.3 高度自适应障碍物过滤
在机器人坐标系中，并非所有三维点都是需要避让的障碍物。地面上的点不是障碍物，高于机器人的悬挂物体（如天花板上的管道）也不需要避让。CeRLP 根据机器人的实际高度定义了一个有效障碍物高度范围 [ h m i n , h m a x ] [h_{min}, h_{max}] [hmin,hmax]，只保留落在这个范围内的点：
P o b s = { p r o b ∣ h m i n < z r o b < h m a x } P_{obs} = \{ \mathbf{p}_{rob} \mid h_{min} < z_{rob} < h_{max} \} Pobs={prob∣hmin<zrob<hmax}
其中 h m i n h_{min} hmin 是略高于地面的阈值（用于过滤地面噪声）， h m a x h_{max} hmax 对应机器人的物理高度。
这个设计赋予了 CeRLP 天然的跨形态适应能力：一台高度为 0.8 米的机器人会检测到 0.7 米高的横杆作为障碍物，而一台高度仅为 0.3 米的机器人则会自动忽略它，因为横杆在其通行高度之上。这种基于物理约束的过滤逻辑，比纯数据驱动的方法更加可靠和可解释。
4.4 虚拟激光扫描投影
最后一步是将过滤后的三维障碍物点投影到二维平面，生成标准的激光扫描格式。对于每个障碍物点，计算其在机器人坐标系中的极坐标：
r = x r o b 2 + y r o b 2 , θ = atan2 ( y r o b , x r o b ) r = \sqrt{x_{rob}^2 + y_{rob}^2}, \quad \theta = \text{atan2}(y_{rob}, x_{rob}) r=xrob2+yrob2,θ=atan2(yrob,xrob)
然后将视场角离散化为若干个角度区间（扇区），对于每个扇区，取其中所有点的最小距离作为该方向上的扫描值。如果某个扇区内没有障碍物点，则将其设为传感器的最大量程。
以下是完整的视觉转扫描模块的伪代码实现：
# 伪代码：视觉转扫描抽象模块（对应论文 Algorithm 2）
# 注意：该代码为根据论文描述编写的伪代码，原始代码尚未开源
import numpy as np
def visual_to_scan(D_metric, K, R_ext, t_ext, h_min, h_max,
angle_min=-np.pi, angle_max=np.pi,
num_beams=720, max_range=10.0):
"""
将米制深度图转换为虚拟二维激光扫描
Args:
D_metric: 米制深度图 (H, W)，单位：米
K: 相机内参 {fx, fy, cx, cy}
R_ext: 相机到机器人坐标系的旋转矩阵 (3x3)
t_ext: 相机到机器人坐标系的平移向量 (3,)
h_min: 障碍物最低有效高度（过滤地面噪声）
h_max: 障碍物最高有效高度（对应机器人高度）
angle_min: 扫描起始角度（弧度）
angle_max: 扫描终止角度（弧度）
num_beams: 激光扫描的角度分辨率（光束数量）
max_range: 最大探测距离（米）
Returns:
scan: 虚拟激光扫描数组 (num_beams,)
"""
fx, fy, cx, cy = K['fx'], K['fy'], K['cx'], K['cy']
H, W = D_metric.shape
# ---- 第一步：三维反投影 + 坐标变换 ----
# 生成像素坐标网格
u_coords, v_coords = np.meshgrid(np.arange(W), np.arange(H))
# 获取有效深度的掩码（排除无效值）
valid_mask = (D_metric > 0.1) & (D_metric < max_range)
Z = D_metric[valid_mask]
u = u_coords[valid_mask].astype(np.float64)
v = v_coords[valid_mask].astype(np.float64)
# 反投影到相机坐标系
X_c = (u - cx) * Z / fx
Y_c = (v - cy) * Z / fy
Z_c = Z
# 组装为 (N, 3) 的点云矩阵
points_cam = np.stack([X_c, Y_c, Z_c], axis=1) # (N, 3)
# 变换到机器人坐标系
points_rob = (R_ext @ points_cam.T).T + t_ext # (N, 3)
# ---- 第二步：高度自适应障碍物过滤 ----
z_rob = points_rob[:, 2]
obstacle_mask = (z_rob > h_min) & (z_rob < h_max)
points_obs = points_rob[obstacle_mask]
# ---- 第三步：投影为二维激光扫描 ----
scan = np.full(num_beams, max_range) # 初始化为最大量程
if len(points_obs) == 0:
return scan
x_rob = points_obs[:, 0]
y_rob = points_obs[:, 1]
# 计算极坐标
r = np.sqrt(x_rob ** 2 + y_rob ** 2)
theta = np.arctan2(y_rob, x_rob)
# 将角度映射到扫描区间的索引
angle_range = angle_max - angle_min
bin_indices = ((theta - angle_min) / angle_range * num_beams).astype(int)
bin_indices = np.clip(bin_indices, 0, num_beams - 1)
# 对每个角度区间取最小距离
for i in range(len(r)):
k = bin_indices[i]
if r[i] < scan[k]:
scan[k] = r[i]
return scan
这段代码的计算复杂度主要取决于深度图的分辨率。在实际部署中，通过向量化操作和 GPU 加速，整个视觉转扫描过程可以在毫秒级完成，满足 10 Hz 的规划频率要求。
5. 维度可配置的局部规划策略
5.1 将机器人体型编码进决策过程
传统的导航策略通常假设机器人是一个没有体积的质点，或者是一个固定半径的圆形。这种简化在开阔环境中问题不大，但在狭窄通道、密集障碍物等场景中会导致严重的碰撞风险。更关键的是，当同一个策略需要部署到不同尺寸的机器人上时，质点假设完全无法区分一台小型扫地机器人和一台大型物流车之间的通行能力差异。
CeRLP 的局部规划模块基于 DRL-DCLP（Deep Reinforcement Learning based Dimension-Configurable Local Planner）构建，其核心创新在于将机器人的物理尺寸直接编码到感知特征中。具体来说，机器人被建模为一个广义长方体，由三个参数描述：前悬长度 L f r o n t L_{front} Lfront（从驱动中心到前端的距离）、后悬长度 L r e a r L_{rear} Lrear（从驱动中心到后端的距离）和宽度 W W W。
对于虚拟激光扫描中的每一个点，其特征向量被构造为：
p i = ( sin ϕ i , cos ϕ i , 1 d i − β , L f r o n t , L r e a r , W 2 ) \mathbf{p}_i = \left(\sin\phi_i,\ \cos\phi_i,\ \frac{1}{d_i - \beta},\ L_{front},\ L_{rear},\ \frac{W}{2}\right) pi=(sinϕi, cosϕi, di−β1, Lfront, Lrear, 2W)
其中 ϕ i \phi_i ϕi 和 d i d_i di 分别是第 i i i 个扫描点的角度和距离， β \beta β 是一个可训练的缩放参数。注意最后三个维度——它们就是机器人的物理尺寸参数。这意味着策略网络在处理每一个障碍物点时，都能**"感知"到当前机器人的体型大小**，从而做出与体型匹配的避障决策。
5.2 策略网络架构
策略网络的输入由四部分组成：
# 伪代码：策略网络输入构造
# 注意：该代码为根据论文公式 (2)(3) 编写的伪代码，原始代码尚未开源
import numpy as np
import torch
import torch.nn as nn
class DimensionConfigurablePolicy(nn.Module):
"""维度可配置的导航策略网络"""
def __init__(self, num_scan_points=720, point_feature_dim=6,
global_feature_dim=256, hidden_dim=256):
super().__init__()
# PointNet 编码器：处理带有维度信息的扫描点集
self.pointnet = nn.Sequential(
nn.Linear(point_feature_dim, 64),
nn.ReLU(),
nn.Linear(64, 128),
nn.ReLU(),
nn.Linear(128, global_feature_dim),
)
# 状态融合网络
# 输入：全局几何特征 + 目标位置(2) + 当前速度(2) + 动力学限制(4)
state_dim = global_feature_dim + 2 + 2 + 4
self.actor = nn.Sequential(
nn.Linear(state_dim, hidden_dim),
nn.ReLU(),
nn.Linear(hidden_dim, hidden_dim),
nn.ReLU(),
nn.Linear(hidden_dim, 2), # 输出：(v, omega)
nn.Tanh(),
)
def encode_scan_points(self, scan, robot_body, beta=0.1):
"""
将激光扫描编码为带有维度信息的点集特征
Args:
scan: 虚拟激光扫描 (num_beams,) 距离值
robot_body: [L_front, L_rear, W]
beta: 可训练的缩放参数
Returns:
point_features: (num_beams, 6) 每个点的特征向量
"""
L_front, L_rear, W = robot_body
num_points = len(scan)
# 计算每个扫描点的角度
angles = np.linspace(-np.pi, np.pi, num_points, endpoint=False)
# 构造特征向量：[sin(phi), cos(phi), 1/(d-beta), L_front, L_rear, W/2]
features = np.zeros((num_points, 6))
features[:, 0] = np.sin(angles)
features[:, 1] = np.cos(angles)
features[:, 2] = 1.0 / (scan - beta + 1e-6) # 避免除零
features[:, 3] = L_front
features[:, 4] = L_rear
features[:, 5] = W / 2.0
return features
def forward(self, scan_features, goal, velocity, dyn_limits):
"""
前向推理
Args:
scan_features: (B, N, 6) 带维度信息的扫描点特征
goal: (B, 2) 相对目标位置 [distance, angle]
velocity: (B, 2) 当前速度 [v, omega]
dyn_limits: (B, 4) 动力学限制 [v_max, omega_max, a_v_max, a_omega_max]
Returns:
action: (B, 2) 速度指令 [v, omega]
"""
# PointNet 编码：逐点特征提取 + 全局最大池化
point_features = self.pointnet(scan_features) # (B, N, 256)
global_feature = point_features.max(dim=1)[0] # (B, 256)
# 拼接所有状态信息
state = torch.cat([global_feature, goal, velocity, dyn_limits], dim=1)
# 输出动作
action = self.actor(state)
return action
…详情请参照古月居
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献131条内容
所有评论(0)