自动回充

ancientcc

本文给的雷达点云相关图像，用的雷达是镭神N10。

目前只支持机器人背部充电，不支持底部。对充电桩的硬件要求

  1. 对准误差不超过3cm。这意味着，每张接触铜片至少6cm。当然，这两铜片可能放在机器人端。
  2. 识别特征是直线。会用激光雷达识别充电桩，因而雷达扫描平面扫到的那一行得是直线。但雷达扫描平面不是严格在一个高度，上、下得留一定误差。建议至少各2cm（须雷达产家确认）。
  3. 宽度至少15cm。最终确定这个宽度时需要考虑机器人后背宽度，建议取15cm和“后背宽度+3cm”最大值。举个例子，机器人后背15cm，那充电桩宽度至少18cm。原因是一旦机器人对准充电桩时出现误差，像3cm。会出现机器人后背有3cm和充电桩是悬空的，这时机器人径直向后退时，由于3cm悬空，会给充电桩一个斜向力，这个力导致充电桩有向左或向右的移动趋势。
  4. 厚度至少5cm。可能的话留大点，像6cm。要给厚度留最小值，是为了区分充电桩和墙。要没意外，充电桩会靠着墙放。由于充电桩识别特征是直线，左、右两墙也是直线，虽然墙的宽度可能不在充电桩宽度范围，但也会遇到差不多情况。这时就要靠厚度区分哪是墙、哪是充电桩。
  5. 得能承受住机器人后退时产生的压力。快要对准时，机器人依旧会用几次后退来确保真的对准。这几次后退，机器人后背可能就紧贴着充电桩，充电桩得受得住这些压力。对这压力，一个指标是机器人后退的线速度会不大于0.05m/s。

设想中的一种充电方案。在机器人尾部开两条槽，内粘两6cm铜片，作为充电口母头。在充电桩，对应两槽的中心，放两个pogopin弹簧触点，作为充电口公头。当机器人对准充电桩后，弹簧触点会碰到槽内铜片，实现充电。对两条槽位置，可能上下或左右。在都可以时，为容许更大误差，以及降低机器人最小宽度，建议用上下。

充电逻辑涉及到机器人硬件有没有提供是否正在充电标志。软件考虑了没有提供，这种情况下，会用每次间隔3秒的后退来判断。如果连续三次后退后，雷达到充电桩两端的距离基本没变，认为已在充电。对两条槽位置，可能开在上下或左右，为容许更大误差，以及降低机器人最小宽度，建议用上下。

由于会出现几次紧贴着充电桩的后退命令，可此时充电桩正挡着，其实机器人没有移动，这时在轮子电机处会“积蓄”着这段未移动距离，像弹簧一直被压着。一旦拿着机器人离开充电桩，压力被释放，轮子就自动转几圈。这个“积蓄”状态可能会烧坏电机，机器人须要提供个命令，让软件在要进入充电时，发这命令释放掉“积蓄”状态。

机器人放置雷达时，x方向可以不放在底盘中心。在y方向，雷达需放在底盘的y=0，即雷达和底盘的y偏移需是0。

 

一、总逻辑

建完地图后，地图上一定会有个叫“充电”的位置。用户在放置这位置时，须要离充电桩半米内，角度和充电桩垂直，且背对充电桩。

图1 回充逻辑

机器人判断出得去充电了，会先启动导航，自主导航到“充电”位置。导航结束，机器人离充电桩不到半米，且背对充电桩。然后不断重复收到雷达点云，用此次点云识别、对准充电桩，直到进入充电。

导航到了“充电”位置，如果3分钟后依旧没让进入充电，会结束此次任务。等着下一次判断出要充电了，重复上图的充电过程。

 

二、图像化雷达点云

1.1 由LaserScan点云转为cv::Mat

SDL_2Point tleagor_charge_api::did_scan_subscribed(const sensor_msgs::LaserScan& msg)
{
  const std::vector<float>& ranges = msg.ranges;
  float angle = msg.angle_min;

  const double max_consider_range = 1.5;
  const float range_max = SDL_min(msg.range_max, max_consider_range);

雷达最远半径一般可12米，对识别充电桩来说，一般查1.2米内的点云就够了。为减少后绪计算量，要减少半径。这里设了1.5米。

  for (int idx = 0; idx < scan_msg_range_size; ++ idx, angle += msg.angle_increment) {
    const float echo = ranges[idx];

    if (msg.range_min <= echo && echo <= range_max) {
      float dx = cos(angle) * echo;
      float dy = sin(angle) * echo;

      tangle_range& angle_range = angle_ranges[ep_count ++];
      angle_range.int250_x = (int)(dx * PIXEL_2_METER);
      angle_range.int250_y = (int)(dy * PIXEL_2_METER);

      posix_touch_i32(angle_range.int250_x, angle_range.int250_y, &min_x, &min_y, &max_x, &max_y);

posix_touch_i32将找出点云中最小的x、y，以及最大的x、y。

    }
  }
  SDL_Rect rect{0, 0, max_x - min_x + 1, max_y - min_y + 1};
  const SDL_Point origin{0 - min_x, rect.h - (0 - min_y) - 1};

posix_touch_i32计算出图像矩形rect。存储在msg.ranges的点，用的是数学坐标系，也就是说，在y轴，是越往下y值越小，这和图像坐标系相反。cv::Mat是图像坐标系，origin是雷达中心在图像中的坐标，因则它的y是“rect.h - (0 - min_y) - 1”。

  cv::Mat laser_scan_mat = cv::Mat(rect.h, rect.w, CV_8UC1);
  ...
}

laser_scan_mat是由点云转换出的矩形，因为后面要用opencv，这里就把图像以着cv::Mat格式存储。origin是雷达中心在图像中的坐标。下图是一个由雷达点云转换出的cv::Mat。是个灰度图，点云中点所在像素值是255(白色)，否则是0(黑色)。 左下较亮那个小方块是雷达中心（这9个像素是为调试特意设置到255），充电桩是左下三段中的中间那段。

PIXEL_2_METER用于把米表示的距离转换到用像素表示的像素数。当前PIXEL_2_METER值是250，即每像素4mm，这意味着图像精度是4mm。就识别效果来说，这精度一般是越大越好，不过有几个原因限制不能太大。

  1. PIXEL_2_METER变大，图像尺寸会数倍增加。后面cv::HoughLinesP消耗时间要变大。
  2. 图像精度太大，一旦超过了雷达距离精度，那同样1cm下，像由4像素变大到5像素，由于像素之间不连贯，可能会让cv::HoughLinesP连线时，不成功。

个人认为，上限不要让出现500，下限至少250。

 

二、识别充电桩

2.1 清除雷达点云中的孤悬点

对某个雷达点，如果半径3格内的48个点都不是雷达点，认为这个点是个孤悬点。充电桩靠墙放时，墙和充电桩点云离得较近，“连接线”上可能出现孤悬点。为避免让cv::cv::HoughLinesP在“连接线”生成直线，要清除这些点。

 

2.2 以半径1进行膨胀

膨胀目的是为让后面cv::HoughLinesP，容易在充电桩生成直线。

 

2.3 find_line

在叙述find_line函数前，先看下一个叫tline_C结构，它用于封装一条线。

2.3.1 tline_C

  • start、end(SDL_Point)。线的两个端点。x不一样时，start总是指向x小那个。否则start指向y小那个。
  • start_dist_m、end_dist_m(double)。雷达到两个端点的距离。
  • center(SDL_DPoint)。线的中心点，为更精准，这里用了浮点。
  • width_m(double)。线的长度，考虑到它要比较的是充电桩宽度，用了“width”字样。用户预先会给定一个充电桩真实宽度，一旦width_m不在充电桩宽度范围，那该线不可能是充电桩。
  • theta(double)。单位弧度。线的角度，值总在[-90度, 90度]。绝对值在90附近，可能会出现正、负摇摆。像这次是86.9度，下次可能是-85.3度。
  • height_m(double)。线在垂直方向高度。目前没使用。
  • y_diff_m(double)。线中心到原点中心、在y方向的偏移。用的是“center.y - origin.y”，于是线中点位于雷达下面时，y_diff_m大于0，否则小于0。

对以double表示的距离，单位都是米，“_m”的语义就是米。图中红色还显示了如何得到三个变量，abs_to_90_theta_deg、abs_y_diff_m和dist，后面对准会用到这些变量。为叙述方便，abs_to_90_theta_deg称为夹角，从图中可看到，它是“90度减去theta“。

2.3.2 find_line逻辑

第一步cv::HoughLinesP得到的直线plines，这些直线好多会出现重叠。

示例找到了32条直线，上图用一种颜色表示一条直线，为直观，每条直线进行了加粗。让看下索引4直线，它和好多条有重叠。是充电桩的直线23，内中也有几条。免得这种“更短”直线产生干扰，在第3步，对有重叠的，要找出最长那条。于是第3步后，可能是充电桩的就剩2、4、13、23、25。

第4步，要从剩下5条中选出最好的。因为line.width_m太长，可去掉2、4。剩下3条，23是充电桩，靠着墙放，25、13分别是左、右两侧墙体。结合上图，具体看怎么区分23、25哪是墙、哪是充电桩。

  1. 变量23_center表示23直线的中心坐标，25_center则是25的。在两坐标连出一条线（图中灰白线），并算出该线的中心坐标center，即图中绿点。
  2. 变量23_theta表示23直线角度，25_theta则是25的，算出这两角度平均值theta，abs_theta是theta的绝对值。
  3. 让23_center、25_center绕着绿点(center)顺时针旋转(90-abs_theta)度。旋转后，23、25都是变得会近乎垂直。
  4. 变量23_center2表示23_center旋转后的值，25_cener2则是25的，分别是图中红线的两个端点。比较23_center2.x、25_center2.x，大的那一个是充电桩，否则是墙。

此示例是只要去掉13、25这两墙，就剩充电桩了，但实际场景肯定会出现还有多条满足充电桩宽度的直线。这时就选theta最大的。原因是不论前进还是后退，机器人总是尽量保持垂直充电桩，于是认为theta大的更可能是充电桩。这里可能是那些和充电桩垂直的墙。

第5步，要计算更精确的线(tline_C)数值，首先是start、end。之前这两坐标来自cv::HoughLinesP()，这次要依据点云中的雷达点。查找是这么个过程。

  1. 走出一条从start到end的直线，从这条线生成个灰度图mat：check_2th_mat。
  2. 对check_2th_mat，也就是这条线涉及到的点，以半径4作膨胀，生成一个dilate2_mat。
  3. 在dilate2_mat，对它的非0像素，对比雷达点云mat，找到更准确的start、end。找到的start、end一定是点云中某个点的坐标。用它们重新生成result。result就是fine_line要返回的tline_C。

 

2.4 消除抖动

  • 雷达两次扫描时，即使障碍物一模一样，得到的各点坐标依旧会有偏差。
  • 机器人正移动时，充电桩位置会出现变动。后面对准逻辑须要等机器人不动后，再执行。
  • 某次find_line找到的result，其实不是充电桩。

为消除这些随机问题，需要有个除抖动机制。方法是内部维护个tline_C数组line_samples_[ltype_vel]。如果连续4次result，它们的start_dist_m、end_dist_m误差不超过3cm，那认为找到的result是充电桩、而且机器人不在移动。

此时line_samples_[ltype_vel]存储着最近4条tline_C，后面对准时只须要一条，那要选哪条。直观想到是取四条的平均值，的确，大都是这样，但有意外。假设最近四条的theta是以下四个数值（单位是度）。

{-88.994, -88.994, -88.994, 88.958} ==> 平均值：-44.506

theta绝对值在90度时，会出现正、负角度摇摆，此时若计算角度平均值，就出错了。如果四条中同时出现正负theta，改为选最后一条用于对准。

 

三、对准充电桩

  • y0对齐。机器人雷达中心的y尽量对齐充电桩中心的y。两个都是中心，于是叫y0。
  • 垂直对齐。机器人尽量和充电桩垂直。

对准充电桩，说来就是要同时做到y0对齐和垂直对齐。反应到数值上，如果达到abs_y_diff_m不大于2cm，那认为y0对齐了。abs_to_90_theta_deg(夹角)不大于15度，那认为垂直对齐了。

已连续三个移动周期，充电桩位置不变，abs_y_diff_m不大于2cm，并且夹角小于15度，为什么还不认为已在充电？——机器人要能充电，须要整个后背都碰到充电桩，而不能只是有某个点碰上了，但依然有个小角度。为此在即将要充电时，再分两阶段，finish_theta（对应图中finish_1th_theta）和finish_finished。先进入finish_theta，此时只是认为至少一个点和充电桩碰到，而不是整个后背。这时让试着转一个最多8度的角度，线速度则固定用-0.01。如果之后连着三个移动周期后，充电桩位置都没变，才认为整个后背碰上了。于是进入finish_finished阶段，同时认为已在充电。

只要abs_y_diff_m超过2cm，就要进入y0对齐。y0对齐分支下分三种处理。

  • y0_1th_largetheta。当前夹角太大了，必须先减少夹角。虽然放在y0对齐，但它做的其实是垂直对齐的事。
  • y0_2th_forward。正处在向前移动，同时让机器人y0向充电桩y0靠拢，宁可增大夹角。
  • y0_3th_backward。正处在向后移动，同时让机器人y0向充电桩y0靠拢，宁可增大夹角。

2th、3th都要让机器人y0向充电桩y0靠拢，至于此次移动把夹角增得过大了，那由收到下个雷达点云时，发现角度过大了，由1th进行弥合。

布尔变量forword_state_用于表示机器人当前是处在要向前移动，还是向后移动。从充电桩到雷达方向，依次划出两个距离，分别叫a线、b线。雷达位置一旦小于a线，forword_state_要变成true。表示机器人得前进。一直前进，过了b线，forword_state_变成false，表示机器人得后退了。

不过，一旦机器人y0、垂直都对齐了，那即使雷达位置小于a线，forword_state_也是false，表示得后退。接下看用forword_state_如何解决一种拉扯情况。

在y0_1th_largetheta，不是用一个能弥补y0对齐的线速度(linear_x)，而是用forword_state_来决定线速度的正负。——要是y0_1th_largetheta用上一个弥补y0对齐线速度，那会让1th和后面2th、3th出现前、后拉扯。举个例子，机器人正处在2th_forward，向前移动了一段距离，方向是顺时针。下次雷达点云到来，发现夹角超过了20度，于是进入1th。这时使用的角速度方向得和前面2th时的相反，得是逆时针，同时要弥补y0对齐，线速度得向后退。于是就出现了向前、向后拉址。

为不让出现这种拉扯，采用办法是在1th时，linear_x不再是为弥补y_diff_m，而是使用一个很小的0.01，其符号和此时的forword_state_一致。这时说来只须要转动角度就行，没必要给线速度。依旧给线速度原因，是一些底盘，当改变linear_x方向时，第一次相反时的移动会出现机器人就不动，或只有一个比期望值小得多的速度。有个方向不变的小速度，或许能减缓这种“呆滞”。虽然只有0.01，但这个linear_x难免会加大y_diff_m，不过期望后面的forward/backward会弥补回来。

设想下机器人向后移动，小于了a位置，并且没做到y0、垂直同时对齐，于是forword_state_变成true，改为向前移动。按forword_state_，下次极可能也是向前，于是也用forword_state_解决了在a位置可能出现的前、后拉扯。

 

四、调试充电任务

充电逻辑涉及较多参数，有些还是经验值。你可能希望看到修改参数后结果，对此增加了些方便调试的功能。要调试，必须修改源码、然后重编译，只适合开发者。

请在windows下调试。

 

4.1 让game_config::is_dbg_charge()返回true

<apps>/external/3rdparty/rose_config_3rdparty.cpp
-------
const uint32_t dbg_flags = DBG_FLAG_CHARGE;

修改dbg_flags初始值，至少带DBG_FLAG_CHARGE标记，game_config::is_dbg_charge()便会返回true。一旦is_dbg_charge()是true，launcher行为会有以下改变。

  1. game_instance::app_did_navigation_bh_use_cpp。在cfg_task.type == aplt::task_charge时，会调用ros_instance_.stop_dwa_local_planner_node()，使得在导航到指定位置后，结束三个node：cartographer_occupancy_grid、cartographer、move_base。免得调试充电任务时，这三个node输出日志，干扰查看。
  2. tmap_scene::did_run_state_changed。enable_teleop_when_map_viewer会变成true。让有后台任务导航时，“地图”窗口会显示发送速度框。
  3. 在MoveBase，不论充电前要去哪个位置，会不移动，并立即返回成功。
  4. tleagor_charge_api::move_public_vel。disble_public_vel值会是true，由于是true，此函数实际不发送速度。

有了以上改变后，你改为做以下操作去调试充电过程。

  1. 在“中心”，执行“充电”。由于上面的(3)，机器人不用移动，便认为成功移动了指定位置。接下立即执行小程序提供的“充电”任务。
  2. 打开“地图”，在output，通过查看move_public_vel()输出日志，知道此次计算出的是啥速度，右下角速度面版切到“自定义”，改由这里发送速度。
  3. 每发送一个速度，评估相关结果。

 

4.2 抓取某个的点云，然后针对此点云调试

  1. 在find_line()，得到best_line2后，设置断点，像“if (best_line2 != nullptr) {”这一行。
  2. 在did_scan_subscribed，让msg_type值是msg_write。
  3. 在“中心”，执行“充电”。触发1)设置的断点后，看output输出，看是不是你想要调试的点云。如果不是，结束此次、再次执行充电，直到抓到一个。
  4. 在did_scan_subscribed，让msg_type值是msg_read。此后使用的点云将总是你上次抓取的点云。

msg_write后，点云被存储在<RoseApp>/launcher/aplt_leagor_basic__documents/LaserScan.msg。可以把这文件改名，等将来再用。

“测试数据”中“charge”目录放有数个点云文件，文件名格式：LaserScan-[xxxxx].msg。可以把下载它们，然后重命名为LaserScan.msg，本机看结果。当中LaserScan-example.msg对应着本文示例。

发布于 2025-02-20 19:05:14

全部评论: 0 条

    写评论: