敬畏自然绿水青山

Category: AI/ML/DL/CV/NLP/RL/Robotics/BigModels/AIGC

  • SLAM系列之cartographer系统中的基于Probability栅格的建图实现分析

    本篇文章将向大家介绍cartographer系统中基于概率表示方法的栅格地图建图过程分析，有的地方分析的较为细致，官方没有给出详细的说明，有借助于大模型做了额一定的辅助理解，如对部分内容的理解有疑问或补充欢迎联系作者加以修订，谢谢。 首先向大家介绍基于概率的栅格地图的具体表示方法，基于概率的栅格地图表示其被物体占据的概率(occupancy probability)，在cartographer系统中，采用了特殊的设计，使得其存储表示更新等更加高效，下面先就其表示方法的细节做一下具体的说明。 首先我们介绍一下概率的相关的表示方法和数学原理，首先介绍概率上几个相关的概念定义：(1)、Odds=P/(1-P)，P为概率，Odds称为几率，对数几率(一般称为Log-Odds或Logit)，Logit 将 P 从 [0, 1] 映射到 (-∞, +∞)，这样也符合一般在神经网络里的分类层裸输出的结果(线性输出的范围没有显示约束)一般称为logits，将其进行sigmoid或softmax计算后所得即为实际的概率值。；(2)、点云帧的数据可以理解为观测测量数据，占用概率如果用几率表示，在现有状态下的几率为先验几率Odds_prior，则在给定观测数据条件下的占有几率为后验几率，基于贝叶斯的几率更新规则，可以直接在 Odds 空间进行的贝叶斯更新(可以通过贝叶斯的概率公式进行证明)：Odds_posterior = Odds_measurement * Odds_prior，如当hit_probability =0.55时，odds= 0.55 / (1 – 0.55) = 0.55 / 0.45 ≈ 1.222，Odds_posterior = 1.222 * Odds_prior，因此单元格的一次hit观测，其占用概率/几率会增加。当miss_probability=0.49时，odds= 0.49 / (1 – 0.49) = 0.49 / 0.51 ≈ 0.961，因此单元格的一次miss观测，其占用概率几率会降低。基于odds的贝叶斯公式实现了hit_table_和miss_table_的提前预计算，下面为相关代码。 在基于概率的地图表示方法的基础上，其再次用到了一些技巧实现了高效的计算，如将概率值转化为无符号短整型(具体的实现在代码文件probability_values.h里)，以及将短整型分成左右两半区间，以保证同一个点云帧的所有点云的遍历时概率栅格地图的单元格的值最多只更新一次(更新逻辑在CastRays函数里实现)。接下来根据代码加以分析根据点云帧建图的逻辑。这里关于地图更新的逻辑将以问题的形式进行说明。 问题1：子地图更新的逻辑总体思路是什么样的，如何更新hit或miss单元格栅格？ 回答：更新算法在CastRays函数里进行实现，顾名思义，算法基于射线投射的思路，当前点帧的原点位置作为起始位置begin，点云点的坐标位置作为end形成一条射线ray，对点云帧的returns和misses点云点集合形成的rays分别进行相应处理。如下为相关代码和逻辑的解释分析： 问题2：为什么在基于hit和miss的查找表更新的过程中采用超分辨率进行计算，有哪些考虑？ 回答：采用超分辨率的计算方式主要在于对miss的更新更加准确。这里用下面的图示进行说明。 如图1左图所示，由于黄色点所在cell的index和begin的y坐标相同，但x坐标比begin还要小，Bresenham等光线追踪算法可能判定如图所示的三条光束“跳过”了本应标记为 “miss” 的单元格。右图通过进一步细化分辨率，可以看到光线在原先大的单元格的小的单元格的cell的y轴坐标会比begin要大，红色箭头表明这些sub cell会被选中，因此超分辨率主要解决miss” 单元格由于离散化和单元格尺寸过大而导致的精度损失问题。 问题3：子图的维护在ActiveSubmaps2D类里进行，前面的文章中有描述，当前活动的子地图的个数不会超过两个，整个全局的地图信息在哪里维护？ 回答：下面的函数调用堆栈及代码片断说明了2D SLAM关键部分的调用及处理流程，全局的数据作为轨迹节点数据会更新到位姿图里作为轨迹节点。 References

  • SLAM系列之cartographer系统2D建图模块类图关系结构和应用功能简要分析说明

    这篇文章主要的内容为cartographer系统中2D建图相关代码类图关系结构和应用功能的简要分析说明，以总览性的方式向大家介绍2D建图基础类的定义，应用功能及类对象间相互通信和依赖关系。具体的2D建图表示方法和详细逻辑将在后续专门的文章中加以详细分析阐述，欢迎大家关注并期待任何形式的意见和建议，谢谢。 如室内地面等二维应用场景中，cartographer采用基于Grid栅格的形式来表示地图，如上类结构图所示，下面对相关类的定义，实现和相互通信和依赖关系做相关说明。 1、二维地图的表示方法所涉及的相关类中，Grid2D为表示栅格结构的基础类，其中：(1)、MapLimits类型变量limits_表示了地图结构基本信息的元数据，该对象中的resolution变量表示每个cell单元格的物理分辨率尺寸，如0.05则表示每个单元格的长宽物理尺寸为0.05米(0.05米/像素，类似地图中的scale比例尺度），max_代表MapLimits类对象x，y方向上的的最大物理尺寸，CellLimits类为单元格数目的简单封装类，cell_limits对象保存了栅格地图横向和纵向单元格数目。以上三个变量的关系通过公式表达为：max_.x() = min_x + cell_limits_.num_x_cells * resolution_。(2)、Grid2D类的成员函数GrowLimits实现地图的动态增长，如果给定点超出了当前地图的边界时，通过扩展地图的方法将给定点纳入到新的地图边界内；(3)、correspondence_cost_cells_为保存的具体栅格值变量，其类型为uint16短整型数组，具体该值的表示含义将会根据具体的地图类型不同而不同，后面将会根据地图类型做专门具体的介绍。 2、实际中具体会用到的二维地图类别有两种，其中：(1)、ProbabilityGrid为概率栅格地图类，表示栅格地图中单元格位置是否存在物体或障碍物某部分的表面的概率；(2)、TSDF2D为截断有符号性距离场(Truncated Signed Distance Field, TSDF)栅格地图类，其表示栅格里物体表面的距离值，关于具体的表示方法和含义将会在专门的文章中做相关分析说明。 3、2D建图的入口类为ActiveSubmaps2D，如图1所示，其主要的成员变量和成员函数说明如下：(1)、submaps_为Submap2D类型的指针数组，Submap2D为2D子地图的具体的实例类别，其grid_成员变量指向Grid2D基础类，在实际的系统运行时，会根据配置初始化具体的地图类别；(2)、range_data_inserter_成员变量的类型为接口类型RangeDataInserterInterface指针(C++里接口为纯虚类)，该接口可实例化继承类重载了成员变量函数Insert，以实现具体的地图更新逻辑，该变量实例化类型会和地图实例化类型相匹配，trajectory_builder_2d.lua文件中就对上述两个变量类型进行了设置，这里截取相关的一段如下: ActiveSubmaps2D中最多维护了两个活跃的子地图，两个子地图有一定的重叠，可以参考如下的代码和注释： References

  • SLAM系列之cartographer系统中激光传感器的点云数据流转变换过程分析

    上篇文章介绍到传感器消息在到达SLAM核心算法模块之前的数据预处理的方法和过程，这篇文章介绍激光点云数据在cartographer系统中统一适配处理的方法细节。 由于精心设计的代码框架，cartographer系统能较好适配多种机器人不同传感器组合。如支持兼容LaserScan 2D激光雷达(室内地面平面空间轮式机器人的配置，如仓库AGV Automated Guided Vehicle，小车自动导引运输系统小车，扫地机器人等)和PointCloud 3D激光雷达(空中无人机3D建图，室外复杂地面3维空间场景自主导航机器人，如自动泊车应用等)，而且系统自动适配机器人本体设备上多个3D Lidar传感器的点云数据的融合以支持更高精度建图(RangeDataCollator类的成员变量expected_sensor_ids_的支持，可以通过配置文件定义激光雷达的数目)。 当数据流转到LocalTrajectoryBuilderND(先以LocalTrajectoryBuilder2D类为例进行介绍)的AddRangeData函数时，就到达了SLAM算法处理的核心部分。在该函数里，主要处理逻辑有： 1、首先调用range_data_collator_.AddRangeData()函数实现一到多个Lidar传感器的点云数据的同步和融合，返回同步后的逻辑点云帧。该函数内部主要调用了CropAndMerge函数将不同来源点云传感器的数据在时间重叠区域外进行切割裁剪，在时间重叠区域内进行合并融合。裁剪融合后逻辑点云帧中部分点云点由于截至时间的变化，其相对时间信息需要做一个校正，具体的代码和文字注释如下： 下面部分的代码实现了融合区间之外的点云帧的数据的切分，其中data为引用数据类型，直接修改了原始电源帧的数据，保留裁剪剩下的部分供和下一次融合使用。最后还对多个传感器融合后的点云数据结果根据时间进行了严格的排序。 2、在对点云数据帧进行裁剪融合同步后，对返回的逻辑点云帧的每一个点云点对应的时间启用姿态外推器进行位姿估计(关于姿态外推器的实现原理将在专门的文章中进行介绍[1])，将估计的结果存入到range_data_poses的数组里。紧接着将基于估计出的点云点时刻的位姿对点云点进行坐标变换，变换到统一的全局坐标系下的坐标以解决点云扫描时间的不同形成的点云位置畸变。统一坐标后将点云点测得的距离按range区间进行过滤。具体的代码和分析注释如下： 在对当前逻辑点云帧进行了数据处理后，会将状态变量num_accumulated_进行递增，当满足条件num_accumulated_ >= options_.num_accumulated_range_data()时，就将多个逻辑点云帧的数据进行聚合并调用局部位姿优化得出局部位姿优化结果。具体的形成聚合数据的方法参见下面的代码和分析。 这里对TransformToGravityAlignedFrameAndFilter变换做一下具体说明。在进行变换之前，所有点云点accumulated_range_data_的坐标已经变换到全局的坐标系下(通过点云点时刻time车体相对于起始世界坐标的位姿对点云点相对于车体坐标系下的坐标点进行坐标变换实现，既点云点去畸变的实现，详见第2部分的说明)。这里的变换为两个变换的组合变换，关于这两个变换分别说明如下。 1、range_data_poses.back().inverse()为最后一个点云点对应的车体位姿的逆变换，其作用于当前逻辑点云帧的所有点云点上，将最后一个点云点的坐标定义坐标原点且坐标轴为标准方向，基于此坐标系定义，将点云点坐标进行变换到局部的坐标系下。range_data_poses基于ScanMatch算法获得较为精确的基础位姿，和局部时间段基于ImuTracker估计所得位姿变化量叠加实现点云点时刻的位姿估计。 2、gravity_alignment.cast<float>() 为重力对齐变换，其为ImuData数据根据陀螺仪的角速度和加速度计加速度进行积分和双重校正得以实现，返回的是time时刻预估的方向旋转四元数估计orientation_(和标准的坐标轴方向不一致，定义了车身的姿态的方向角度部分以及重力相对于垂直方向的角度偏移)。gravity_alignment的估计只用到了ImuData的数据，比range_data_poses.back()中的重力方向的估计更加的鲁棒和稳定，所以用gravity_alignment变换去和重力方向做对齐操作。 gravity_alignment.cast<float>() * range_data_poses.back().inverse()为两者变换的叠加，实现了将点云点转换到点云帧局部标准坐标系下后再根据当时的重力方向进行对齐(相当于将标准坐标z轴对齐到重力方向后点云点的坐标值的变化）。 将点云点坐标进行上述的变换后，AddAccumulatedRangeData函数将实现具体SLAM算法的核心部分，主要包括：1、基于上述变换后的点云点和时间通过调用函数ScanMatch进行局部位姿优化；2、InsertIntoSubmap函数基于点云数据对局部地图进行更新；3、返回匹配优化和更新后的地图结果的综合信息。关于位姿优化和局部地图更新的具体实现将在后续文章中加以详细介绍，欢迎关注。 References

  • SLAM系列之cartographer系统中PoseExtrapolator实现分析

    本篇文章将向大家介绍cartographer中的姿态外推器PoseExtrapolater的实现，姿态外推器主要基于ImuData和/或OdometryData以及局部SLAM优化后的位姿实现较近的未来时刻的车体(机器人本体)的姿态估计。下面将和代码相结合进行较为详细具体的分析介绍。 姿态外推器的主要作用体现在：1、在同步后的点云数据的每一个点云点上都会调用ExtrapolatePose函数预估出即时姿态，以便基于点云坐标的去畸变操作(将点云点相对于车体坐标系的坐标和车体姿态叠加获取点云点在统一坐标系下的坐标从而实现了去畸变的操作，和FAST-LIVO2中的运动补偿实现去畸变的思路和实现方法不同)；2、当点云帧的最后一个点的坐标校正完成后调用ScanMatch函数对基于外推器预测的姿态和点云数据基础上进行位姿优化获得更加精确的姿态，同时调用extrapolator_->AddPose函数将优化后的姿态更新到姿态外推器，姿态外推器基于更加准确的姿态的基础上进行后续时间点的位姿估计。如下图所示，在帧内和帧的边界分别调用了姿态外推器的上述两个函数。 下面就具体的实现代码细节做一些说明。 PoseExtrapolater的成员变量及其含义和作用见下面的代码和对应的注释说明。 AddPose函数在局部SLAM算法优化位姿后调用，通过ScanMatch算法优化后的位姿和对应的时间作为参数调用AddPose函数，具体关于AddPose函数的实现细节及分析请参考如下的代码和注释。 关于姿态外推器中的旋转和位移估计的代码实现，下面的段落文字给出具体的代码和分析注释。 References

  • SLAM系列之cartographer系统中ImuTracker实现分析

    这篇文章将向大家介绍cartographer系统中姿态外推器PoseExtrapolator实现分析。姿态外推器可以看成是一种软件逻辑实现的里程计，但是其利用了局部位姿优化的结果及时更新更高精度的位姿，从而有效防止出现位姿的严重累积漂移现象。本篇文章将从两个方面进行介绍，第一部分介绍IMU传感器的基本作用及采用高频ImuData进行预积分估计姿态的方法(主要体现在ImuTracker类里)；第二部分将介绍姿态外推器和局部位姿优化的结果如何结合实现更精确的位姿估计(主要体现在PoseExtrapolator类里)。 惯性测量单元（IMU, Inertial Measurement Unit）传感器包含陀螺仪(主要测量角速度angular_velocity，频率高)和加速度计(主要测量linear_acceleration，频率低)。但在IMU的硬件驱动层一般会对低频率的线性加速度值进行插值或直接填充，以保持和角速度的测量同样的频率输出，因此输出的消息同时有这两个测量值。IMU的消息标准中msg->header.frame_id 是 sensor_msgs::Imu 消息中声明的IMU数据所在的坐标系名称（例如 “imu_link”）,IMU和 tracking_frame 必须共址（The IMU frame must be colocated with the tracking frame，否则由于imu和tracking_frame的位置上平移关系的存在导致车体的线性加速度的估计变得困难），以便IMU相关算法简化计算。 IMU的线性加速度的读数值为一个三维的向量，测量的是比力（specific force），加速度计的测量值里天然包含重力方向的加速度分量(比如车体在平面上静止时IMU的加速度计读数为[0,0,9.8]，当车体做自由落体运动时，加速度计的读数为0。 Imu在做time时刻的位姿估计的时候，其主要估计的是旋转的部分(位姿估计的平移部分主要在姿态外推器中利用预估的线速度和时间差进行估计)。 如上ImuData所述，ImuTracker一般会同时接收到角速度和线性加速度的消息，接收角速度消息时，直接更新角速度变量的值即可(用于后续基于此进行角度增量预估)，在接收线性加速度时的处理稍微复杂一些，下面是具体的分析。 一般的使用方式如下(在姿态外推器中调用），通过旋转姿态估计函数Advance和更新线性加速度观测值值函数AddImuLinearAccelerationObservation实现了旋转角度和重力向量的双重校正。实现了旋转姿态和重力向量的双重校正，使用IMU的陀螺仪测得的角速度和线性加速度计测得的线性加速度两个观测值相互校正和纠偏，实现更加稳定的姿态估计。 References

  • SLAM系列之cartographer系统中多种类型传感器数据的统一预处理

    在cartographer系统中，激光点云作为定位和建图的核心数据，在系统中不同的类对象中的流转和变换(包括坐标变换和数据类型的变换)是最频繁和复杂的，这篇文章将向大家梳理一下cartographer系统中多种类型传感器数据的统一预处理，包括传感器数据类型从标准ROS消息到cartographer内部处理所需数据类型的转换，消息多队列缓存和分发派遣机制等，如对本文理解和分析的过程有疑问或发现有问题的地方欢迎联系作者讨论并校正，谢谢。 1、首先在cartographer_ros的节点中，接收到的点云消息为标准通用的点云消息定义（根据物理传感器的不同，点云的标准消息有三类: sensor_msgs::PointCloud2，sensor_msgs::LaserScan，sensor_msgs::MultiEchoLaserScan，sensor_msgs 包用来专门存放消息的标准定义格式，其中msg.header.stamp定义了点云帧扫描的起始时间)，ROS节点收到点云消息后，在SensorBridge类对象中调用了三个函数名相同(函数名均为ToPointCloudWithIntensities，具体的实现参考msg_conversion.cc代码)但参数不同的重载函数将上述三种不同的点云数据转换成为一致的cartographer内部的PointCloudWithIntensities类型对象，该类型对象结构含有成员变量points和intensities，其中points的每一个点云点含有时间信息（TimedCloudPoint，时间为相对于点云扫描帧结束时间的偏移，为负值），intensities为点云点对应的强度信息，为浮点值数组。 以sensor_msgs::PointCloud2点云消息为例，SensorBridge类的函数HandlePoint2Message对接收到的消息进行处理，其中会通过函数调用ToPointCloudWithIntensities(*msg)进行的转换，在sensor_msgs::PointCloud2类型转换的工程中，用到了较为常用的pcl点云处理库，先将数据转换成pcl::PointCloud<PointXYZT>数据类型（在pcl::PointCloud数据类型汇总，点云点的时间戳为相对于点云帧扫描开始的时间偏移，为正值），然后基于该类型数据转换为PointCloudWithIntensities类型。 2、SensorBridge中通过函数HandleRangefinder将TimedPointCloud数据（PointCloudWithIntensities的points成员变量）转化为TimedPointCloudData类型的数据对象，该转换涉及到了点云数据从传感器坐标系到机器人本体坐标系（车身坐标系）的坐标变换，该变换定义为sensor_to_tracking->cast<float>()，而tracking_frame一般在lua配置文件中定义为“base_link”（一般为车体底盘后面中间位置）。同时TimedPointCloudData的time成员变量值为点云帧扫描结束时间，其origin成员变量定义了Lidar传感器相对于tracking_frame的位置偏移（ sensor_to_tracking->translation().cast<float>()）。将转换后的数据通过调用trajectory_builder_->AddSensorData转入到cartographer的核心库进行后续的算法处理。具体的变换代码为： 3、上面的trajectory_builder_变量的类型为CollatedTrajectoryBuilder，在MapBuilderBridge::AddTrajectory函数里通过调用map_builder_->AddTrajectoryBuilder函数来生成，在map_builder.cc代码文件里的实现片段可供参考： 其中map_builder对象在node_main.cc文件里通过调用cartographer::mapping::CreateMapBuilder(node_options.map_builder_options)进行的初始化），CollatedTrajectoryBuilder类为local_trajectory_builder（局部SLAM，前端）和PoseGraphND（全局SLAM，后端）的对象的集成封装，为SLAM前后端的集成入口。trajectory_builders_为vector数组容器，因此cartographer系统默认支持多个trajectory的构建。在cartographer核心库的实现中，不同的传感器数据将会在一起集中进行队列缓存管理和进一步分发。相关的数据队列缓存管理和分发的对象为sensor_collator_ ，其内部实现中使用了多个队列来分别缓存不同的传感器消息，并基于类似归并排序的思想从多个队列中取出最早的sensor数据进行分发(Dispatch)。 综合上述的描述，数据的处理流转主要表现为：CollatedTrajectoryBuilder::AddSensorData(…)->CollatedTrajectoryBuilder::AddData(…)->sensor_collator_->AddSensorData(…)->OrderedMultiQueue::Add()->OrderedMultiQueue::Dispatch()->HandleCollatedSensorData()->data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get())->trajectory_builder->AddSensorData(sensor_id_, data_);->local_trajectory_builder_->AddRangeData()。 当数据流转到LocalTrajectoryBuilderND的AddRangeData函数时，就基本到达了SLAM算法的核心部分，包括外推器的姿态估计，ScanMatch的局部位姿优化(Local SLAM)，以及全局位姿优化(PoseGraph )等过程中的Lidar数据的进一步的流转和变换，限于篇幅，将在下一篇文章中向大家介绍。 References

  • SLAM系列之cartographer传感器数据模块介绍

    本篇文章将向大家介绍cartographer系统的sensor模块的功能的代码实现，希望对感兴趣的读者朋友们有所帮助，欢迎读者朋友阅读并对说明描述不足之处提出意见和建议。 首先，cartographer库是SLAM的算法处理核心部分，其关于sensor的数据的定义主要用于内部算法的处理，而从智能设备本体接受的sensor原始数据会由cartographer_ros的节点进行接收和发送，这里将对cartographer_ros和cartographer算法库这两部分的sensor的数据表示在一起做一下说明。 如下图所示为cartographer系统（包括ros节点）代码中数据相关的类图。其中左边一列为IMU，Odometry等其他传感器的类图，右边两列为Lidar传感器相关类图，其中右上方描述了Lidar数据在系统中的流转转换流程过程。 在这里需要特别加以说明的有：在ROS节点接收通用点云数据ROS消息在SensorBridge类中转换为TimedPointCloudData类型的数据后发送给cartographer库进一步处理，TimedPointCloudData类对象数据的time成员表示为当前扫描帧点云的最晚时间，原点坐标为lidar传感器相对于tracking_frame的相对位置（一般tracking_frame定义为base_link，lua中有定义tracking_frame = “base_link”，原点定义为lidar传感器相对于base_link的位移大小，为固定的标定的值)。cartographer系统统一支持2D的LaserScan Lidar（较低成本的平面Lidar传感器）和3D Lidar传感器，对于2D LaserScan点云数据，一般会在预处理时进行切分处理(如backpack_2d.lua中定义了参数num_subdivisions_per_laser_scan = 10)，以便充分利用时间及时进行点云去畸变的计算。 在cartographer库内部，RangeDataCollator类实现了兼容多个Lidar(如backpack_3d.lua中定义了num_point_clouds = 2)传感器数据的同步融合，融合后的数据类型为TimedPointCloudOriginData，保留传感器相对于机器人本体坐标系的位移origin信息，可以计算range的大小以便进行点云的过滤操作。经过进一步的处理后的点云数据转换为RangeData，基于该数据调用ScanMatch算法优化局部位姿后对局部地图进行更新，关于整个SLAM算法流程中点云数据的坐标变换过程，ScanMatch算法实现局部位姿优化以及PoseGraph算法实现全局位姿优化等细节将在后续文章中做更加具体的说明分析。 ImuData为高频数据（频率几百到上千hz），PoseExtrapolate类实现了基于ImuData和/或OdometryData实现的机器人的位姿的较为精确的初始估计值。其中预估的大体方法为基于线速度和时间差估计位移，基于角速度和时间差积分出旋转增量估计，并且姿态外推器也基于ScanMatch的局部位姿优化结果更新time时刻更加准确的位姿，并预估出两个pose之间的匀速速度用于位移估计，在cartographer中没有采用IMU的线性加速度去进行预积分计算位姿中的translation部分(IMU的线性加速度的噪声较大，容易导致位姿漂移，而基于较为准确的位姿的变化去估计速度则较为稳定和准确，也可以基于OdometryData进行位移估计，如果有提供且数据量组足够时）。 后续文章将对cartographer的细节算法，系统架构等做进一步的介绍，欢迎关注。 References

  • 具身智能论文和开源系统之-VoxPoser实现思路介绍

    这篇文章向大家介绍VoxPoser算法的思路和创新方法。该论文提出了机器人应用中通过开放指令（通用任务）操作开放（通用）物体的一种技术实现方案，实现通过开放语言任务指令到密集的6自由度的终端执行器的任务轨迹合成，为未来日常通用任务机器人的实现提供一种技术思路和发展方向。总体思路和技术途径为：通过大语言模型在基于自由开放的任务指令上的任务理解和任务分解的优势，将自然语言指令转化为系统执行代码。 如图所示为VoxPoser算法流程示意图，一个完整的机器人任务从语言指令下达到路径点轨迹执行的整个过程主要由以下几个部分组成：1、基于代码生成微调后的大语言模型将开放语言指令进行子步骤分解，2、基于每一个步骤的子任务，基于感知模块（后面段落中有具体详细的说明）来构建多种类型的价值地图，过程1和2为如图1中的a部分；3、将各种价值图提供给路径规划算法，路径规划算法首先根据贪心算法找到局部最优的路径点序列，然后基于MPC相关算法来优化运动控制执行参数以驱动机器人执行相应动作，下面就相关过程做一下较为详细的说明。 首先，这篇文章基于LMPS的方法实现机器人通用任务，未来的理想机器人将不限于执行硬编码到系统中固定的操作任务，通过更前瞻的智能模型使得机器人通用操作任务功能成为可能。具体的LMP的原始想法来自于Code as Policies一文[1]，这里对相关的多个层级的LMP的分工做一下简要说明（具体实例见引文[2]中的各个类别的LMP的Shots样例说明）：1、planner，任务规划，将任务分解为多个子任务；2、composer，根据当前的子任务根据语言参数通过感知模块生成相应的value map LMPs(affordance, avoidance, gripper, rotation等等)。 论文中的感知模块部分pipeline过程较为复杂，包含几个步骤（但代码中并没有给出开源实现，也可以根据实际的物理和机器人部署软硬件环境对感知的pipeline进行自定义）：1、基于开放词汇目标检测算法模型（Open-Vocab Detector）来检测特定物体，文中Open-Vocab Detector采用了google提出的OWL-ViT（Open-World Localization via Vision Transformer），该模型可以基于特定目标的语言描述，将图像和文本描述作为输入，输出目标物体的几何信息；2、然后将物体的boundingbox作为SAM（Segment Anything Model）模型的输入获取物体的mask；3、通过视觉跟踪算法对mask进行跟踪；4；跟踪的mask作为RGB-D图像的输入来获取目标物体或（物体的某个部分）的点云信息，在对较为稀疏的点云信息进行smooth插值后获得稠密的地图信息。 价值地图构建(Value Map Composition)， 价值地图的结果以(100,100,100,k)形状的张量来表示体素值地图（vox value map），在可供性和约束性地图时k=1，表示cost代价值，在表示旋转地图时，k=4，表示旋转向量。 其次，动作规划（Motion Planner）基于可供性地图和约束性地图构成的价值地图采用贪婪搜索（greedy search）的方法来求解局部最优路径并进行平滑下采样等后处理，然后采用MPC相关算法优化执行参数并驱动硬件执行相关动作，在下一个执行时刻，会基于最新的观测数据重新调用动作规划以保证较高的实时准确性。 图2为相关任务场景和价值地图可视化示例，其中第一行中的“entity of interest”为objects（paper trash和 top drawer handler），可视化轨迹路径为黄色的path线条，下方两行的entity of interest为机械臂的夹爪，轨迹为红色的路径点连接而成的虚线条。 References

  • 具身智能论文和开源系统之-Code as Policies实现介绍

    这篇文章主要向大家介绍基于大模型的LMPS实现机器人通用任务的方法，该方法名为Code as Policies，这里将向大家介绍其基本的原理和一些实践样例。也将以问答的形式来进行介绍。 问题1：Code as Policies这篇文章的主要思想和实现思路和方法是什么？ 回答：基于在代码生成的数据集上微调通用大语言模型增强其生成代码的能力，机器人任务执行的策略代码通过大语言辅助编程得以实现。利用大语言的开放自然语言词汇描述任务，常识知识，推理逻辑等优势为实现机器人通用任务提供了一种概念验证实现，为具身智能的通用智能的实现提供了一种发展方向和路径。 问题2：LMP具有什么样的功能，以及如何通过LLM实现？直接用训练好的通用大模型可以吗？ 回答：LMP(language model generated programs)是通过向大模型输入任务描述（用代码生成的数据集微调后的大语言模型）的方式生成可执行的代码。为了使得生成的代码具有面向特定任务，模式和场景，以及特定的感知模块接口，特定的机器人控制等接口的需求，可以采用了few-shot prompting的方法进行In-Context Learning（无需模型再训练更新参数，通过在提示中的样例隐式的推断出满足特定的格式和约束等满足期望的输出），即提供少量的（如5-20个）样例（Examplar/Shots， Prompt Engineering）作为多轮会话大模型的上下文，但它们通常是在实际提问之前一次性提供给模型的，作为初始设定或指令的一部分，以大模型对话历史的格式[2]作为提问prompt的prefix实现特定任务，模式和场景的需求，更多的描述可以参考引文2中问题2的描述。 问题3：LMP在语言生成方面的能力的具体表现有哪些？ 回答：LMP在语言生方面的能力具体体现在语言的低层次（low-level）的代码生成上和高层次（high-level）的代码生成上。在low-level的生成能力主要表现为：1、根据语言样例按任务逻辑描述生成特定语言的代码语句的能力；2、特定语言第三方库的使用生成能力，如python的numpy库的一些使用方法；3、应用自定义的接口调用代码生成能力；4、自然语言的推理逻辑在代码中的体现，具体每一方面的语言生成能力示例见引文[2]问题3的第一部分的描述；high-level的生成能力主要表现为：1、语言的控制逻辑的生成，如if-else条件判断，while循环等；2、代码组合的能力和代码嵌套生成的能力等； 问题4：完整的代码组合和嵌套如何在面向机器人领域中用python编程语言进行实现? 回答：这里主要用到了python编程语言的AST 解析功能和exec动态注册延迟执行的特性，使得生成的代码中函数的定义中可以调用其他函数，如引文[2]中的问题4的 lmp_fgen实现了基于LMP的函数生成，该LMP通过AST和exec机制实现了通过不同的LMP实现嵌套子函数的定义以及实现整个代码的生成和执行，详细的机制描述参考引文[2]中的问题4的详细说明。 问题5： Code as Policys论文中的代码生成和问题2中的一般的LMP的代码生成能力有什么不同和特殊性？优势体现在什么地方？ 回答：特殊性是使用LMP的原理应用在机器人的策略代码生成这样的领域，其中感知和控制的接口可以作为样例提供给特定的LMP（如调用感知的目标检测的已有算法和API等），以实现约束或背景条件生成。具体更加详细的描述参考引文[2]中的问题4的描述。 References

  • 具身智能论文和开源系统之-RLBench简介

    本篇文章将向大家介绍RLBench，一个针对机器人研发的基准平台，旨在推动机器人操作常用任务的研发和对比评估。下面将以问答的形式来对RLBench进行介绍(RLBench的安装使用参考引文[1])。 问题1：RLBench和Gym提供的机器人模拟器环境对比，各有什么特色？ 回答：RLBench为机器人强化学习等提供了一个基准平台，特别是单臂机器人的常用操作任务提供高保真仿真模拟环境，而gym是强化学习中更加通用的平台，提供了众多基础的物理仿真环境（如小车平衡杆CartPole-v0等，参考引文[2]）进行强化学习基础算法实验。RLBench默认使用的是Franka Panda机械臂，也可以支持其他机器人，有人已经移植了UR5等，仿真环境要和真实的机器人尽可能保持一致性（包括外形等几何结构一致性，动力学运动一致性以及仿真控制操作接口一致性），这样Sim2Real时会保持最大限度的兼容，迁移也更为方便和有效。RLBench的源代码里有一个gym.py文件，通过RLBenchEnv Wrapper类将RLBench的物理仿真环境进和Gym仿真环境进行了适配，如果研发人员熟悉了gym的环境，就可以采样gym的接口实现RLBench里定义的任务相关的强化学习等模型的研发。Issac Gym（引文[4]）是Nvidia提出的模拟仿真环境，其利用GPU强大的计算能力基于大量并行数据训练人形机器人的运动功能的强化学习模型算法上有较大优势，但CoppeliaSim的仿真度更高，适合高精度操作的工业机器人，更加适合sim2real的实验。具体的更多的信息可以参考引文[3]中的问题1的详细说明。 问题2：RLBench源代码结构及实现上关键点介绍 回答：关于RLBench的核心组件主要有：Scene，Observation，Task，Robot，ActionMode，Enrionment等等。具体每个部分的详细介绍和代码示例请参考引文[3]中的问题2的详细说明。 问题3：具体个性化任务的定义 回答：主要重载Task类中的几个关键函数：init_task，init_episode，variation_count等。具体关于每一个函数的作用及示例介绍请参考引文[3]中问题3的详细说明。 问题4：模拟仿真数据采集过程介绍 回答：主要通过TaskEnvrionment的get_demos函数获取多个episodes的rollout数据，具体的代码可以参考引文[3]中的问题4的详细说明。 References