反光柱導(dǎo)航開發(fā)與實驗

VEnus算法對于反光柱導(dǎo)航的基本思路，其主要分為了高反點提取、高反點聚類查找中心、高反點與已知反光柱位姿匹配以及調(diào)用ceres庫進行位姿優(yōu)化等步驟。

然后在這個算法的基礎(chǔ)上，再進行一定的開發(fā)達到一個比較穩(wěn)定且可視化的版本。

使用：

ffmpeg -f x11grab -framerate 25 -video_size 1920*1080 -i :0.0 out.mp4

這里使用的是自己錄制的一個包，其中包含了激光數(shù)據(jù)以及里程計信息odom。為了使結(jié)果看起來更舒適，主要進行了以下幾個部分的改進：

1、發(fā)布map到odom變換關(guān)系

為了在rviz中進行點云以及反光柱的顯示，需要綁定一個固定TF，一開始使用的是以odom作為固定坐標(biāo)系，但是由于odom本身存在比較大的累積誤差，所以后面發(fā)現(xiàn)效果并不好。

所以這里需要重新計算一下從map到odom的累計漂移發(fā)布到TF樹中，以形成一個較為穩(wěn)定的TF關(guān)系。

tf::Quaternion quat1;
  tf::quaternionMsgToTF(new_odom.pose.pose.orientation, quat1);
  double roll1, pitch1, yaw1;//定義存儲rpy的容器
  tf::Matrix3x3(quat1).getRPY(roll1, pitch1, yaw1);//進行轉(zhuǎn)換
  //上一時刻機器人的角度
  double angel2 = last_pose.theta();
  //yaw1為機器人在odom下的角度，相減得到兩個坐標(biāo)系之間的角度差
  double angel = yaw1-angel2;
  odom_trans.header.frame_id = "map";
 odom_trans.child_frame_id = "odom";
  odom_trans.header.stamp = ros::now();
 odom_trans.transform.translation.x = (last_pose.x() - new_odom.pose.pose.position.x * cos(-angel) + new_odom.pose.pose.position.y * sin(-angel));
 odom_trans.transform.translation.y = (last_pose.y() - new_odom.pose.pose.position.x * sin(-angel) - new_odom.pose.pose.position.y * cos(-angel));
 odom_trans.transform.translation.z = 0;
 odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(-angel);
  odom_trans.header.stamp = ros::now();
  ROS_INFO("tf map_to_odom: x = %f,y = %f,theta = %f",new_odom.pose.pose.position.x - last_pose.x(),new_odom.pose.pose.position.y - last_pose.y(),angel);
  broadcaster.sendTransform(odom_trans);

通過已知機器人在map坐標(biāo)系下的pose以及odom下的位姿，反求odom到map的TF。先計算兩個坐標(biāo)系下的位姿的角度差，這個代表了兩個TF之間的旋轉(zhuǎn)分量。

在已知旋轉(zhuǎn)分量的情況下再計算兩個TF之間的平移分量。最后再將具體的TF關(guān)系通過broadcaster發(fā)布出來。

2、添加前端可視化插件

為了能夠在rviz中更加方便的查看機器人的運動以及匹配情況，需要在rviz中添加一下反光柱與機器人的一些可視化消息。

對于反光柱的消息，分為兩類：已經(jīng)確定的以及當(dāng)前的。已經(jīng)確定的是指那些被放到feature中我們認(rèn)為其已經(jīng)代表一個正常的反光柱的信息的那些點。

當(dāng)前的則是指當(dāng)前時間下我通過TF關(guān)系將當(dāng)前坐標(biāo)系下的點變換到map坐標(biāo)系下后的點。對于這兩種點。我們使用markarray來進行可視化操作，同時以不同的顏色(紅色/綠色)來區(qū)分它們，類似于這樣：

std::unordered_map>::iterator ite;
  for(ite=feature_points.begin();ite!=feature_points.end();ite++)
  {
    cylinder_arrow.ns = "cylinder";  //命名空間namespace                                     
    cylinder_arrow.type = visualization_msgs::CYLINDER;   //類型
    cylinder_arrow.action = visualization_msgs::ADD;
    cylinder_arrow.lifetime = ros::Duration(); //(sec,nsec),0 forever
    cylinder_arrow.header.frame_id = "map";
    cylinder_arrow.header.stamp = ros::now();
    cylinder_arrow.id = ite->first;
    cylinder_arrow.pose.position.x = ite->second.first;
    cylinder_arrow.pose.position.y = ite->second.second;
    cylinder_arrow.pose.position.z = 0;
    cylinder_arrow.pose.orientation.w = 1.0;
    cylinder_arrow.pose.orientation.x = 0;
    cylinder_arrow.pose.orientation.y = 0;
    cylinder_arrow.pose.orientation.z = 0;
    //設(shè)置標(biāo)記尺寸
    cylinder_arrow.scale.x = 0.09; //m
    cylinder_arrow.scale.y = 0.09;
    cylinder_arrow.scale.z = 0.50;


     ///設(shè)置標(biāo)記顏色
    cylinder_arrow.color.a = 1.0; // Don't forget to set the alpha!
    cylinder_arrow.color.r = 1.0;
    cylinder_arrow.color.g = 0.0;
    cylinder_arrow.color.b = 0.0;
    cylinder_Array.markers.push_back(cylinder_arrow);
  }
  mark_info_pub.publish(cylinder_Array);
  cylinder_Array.markers.clear();

這里只是feature_point的可視化發(fā)布，另外一個類似就不摘抄了。最后得到的就是類似于下圖這樣子的對應(yīng)關(guān)系：

3、優(yōu)化反光柱位姿匹配問題

在上一章中分析源代碼的過程我們大致了解了算法的原理主要是基于對不同反光柱點的距離匹配的。例如對于我現(xiàn)有的反光柱ABCD。

A到B的距離為5，A到C的距離為8，A到D的距離為6，B到C的距離為7，B到D的距離為4，C到D的距離為3。然后當(dāng)前幀檢測到幾個反光柱點A1，B1，C1。A1到B1的距離為5，A1到C1的距離為4。

則其分別與BA、BD兩條邊對應(yīng)，所以A1與B點對應(yīng)。這種情況適合大部分反光柱，但是會存在一種不適合的情況：當(dāng)全局反光柱中存在A、D兩個反光柱其到周圍其他反光柱之間的距離都相等的時候。

那么新檢測到的反光柱可能會匹配到A也可能會匹配到D。這種情況下就會存在比較大的誤匹配問題。

對于這個問題，我們在算法中的位姿匹配部分新添加了一個新的約束關(guān)系：在檢測與匹配當(dāng)前時刻的反光柱與全局反光柱的時候，我們不僅對其存在的邊進行約束關(guān)系的判斷。

還對這兩個匹配上的點之間進行一定距離上的約束。如果當(dāng)前幀下的投影點在全局坐標(biāo)系下的位姿與對應(yīng)匹配上的點的全局坐標(biāo)差別較大的話，我們認(rèn)為這是可能存在問題的。

舍棄這一個匹配結(jié)果繼續(xù)尋找其他可能存在的匹配點。這樣子可以保證基本每次匹配上的點都是比較正確的。類似于這樣子：

測試時在終端打印出了對應(yīng)匹配點的坐標(biāo)關(guān)系，point1是map坐標(biāo)系下的反光柱的位姿，而point2是當(dāng)前時刻下檢測到的高反點對應(yīng)在map坐標(biāo)系下的坐標(biāo)。整個匹配結(jié)果基本上是正確的。

4、優(yōu)化位姿匹配錯誤的跳點問題

跳點問題是指當(dāng)反光柱匹配錯誤是，導(dǎo)致后面優(yōu)化后的位姿突然一下子出現(xiàn)在離上一時刻很遠的地方。

這個在沒有優(yōu)化匹配之前還是較容易出現(xiàn)的，但是匹配結(jié)果正確的話基本上不會出現(xiàn)。只是作為一個保險的手段還是在這里添加一下。

根據(jù)運動邏輯我們知道機器人運動過程不可能一下子出現(xiàn)在離上一時刻很遠的地方。所以在優(yōu)化完位姿后，算法會在更新位姿之前對新的位姿進行一次判斷。

如果位姿與前一時刻想差較大，例如一下子跳到2-3米遠。時間間隔才0.1S，那肯定是有問題的。這個位姿不應(yīng)該給到其他地方進行運動計算。所以這時候需要舍棄這個結(jié)果重新計算。

一般場景中反光柱數(shù)量夠多的話是不會出現(xiàn)這個問題的，測試時基本位姿也都是正確的。

5、優(yōu)化同一位置出現(xiàn)多個反光柱的問題

在上面關(guān)于rviz中的圖片中存在一個BUG，就是在同一個位置出現(xiàn)了兩個不同的紅色反光柱。

這個反光柱其實是代表的同一個反光柱，出現(xiàn)這個問題的原因是再于：在進行位姿匹配的過程中，當(dāng)前幀的反光柱點與現(xiàn)有的反光柱點之間的距離超過了閾值，所以這兩個點沒有被匹配為一個點。

于是在求完新的位姿后，這個點作為一個新的反光柱點被添加到了feature_point里面。所以在這個位置就出現(xiàn)了一個新的反光柱。這個問題有兩種解決方案：第一種比較簡單就是增大范圍判斷的閾值。

這樣子同一個位置的反光柱就不會出現(xiàn)第二個了。另外一種是在新增反光柱的時候判斷一下這個點周圍多少范圍內(nèi)是否存在反光柱，如果有一個的話就不用新增了。

審核編輯：湯梓紅