5. ROS编程入门--航迹推算（dead reckoning）

最新推荐文章于 2023-09-12 13:00:38 发布

w_tom

最新推荐文章于 2023-09-12 13:00:38 发布

阅读量1.2w

点赞数 4

分类专栏： ROS 算法文章标签：机器人算法

ROS 同时被 2 个专栏收录

11 篇文章 13 订阅

订阅专栏

算法

8 篇文章 2 订阅

订阅专栏

这篇教程介绍了如何在ROS环境中实现航迹推算（Dead Reckoning）算法，用于测量机器人里程计的误差。通过控制机器人沿着正方形路径行走，比较局部里程计和全局里程计的位置差异，评估结果并使用PID控制进行路径跟踪。教程涵盖了从创建ROS包、编写节点代码到编译和运行的完整过程。

摘要由CSDN通过智能技术生成

TASK

这篇教程主要是测量机器人的里程计误差的，让机器人重复的走固定的路线，在这里我让机器人走正方形，比较机器人的起始位置（理想状态是重合的）以及局部里程计（机器人自身测算得）和全局里程计的的位置（定位系统）。

仿真用的机器人里程计是准确的，因此这次实验要在自己的机器上进行。

Algorithm

DeadReckoning（航迹推算）算法在 NodeDeadReckoningI 类中实现，在你发送指令之前，你需要知道机器人在局部里程计和全局里程计的位置。两个位置都知道后才能运动。在 NodeDeadReckoning 类里面需要需要两个变量（globalOdomReceived and localOdomReceived）来判断是否接收到位置信息。
通过PID控制机器人直走多少米，转弯多少度，PID控制的反馈数据来源私有程计（local odometry：机器人自带).
机器人静止后，再次等待local和global的里程计并计算与起始点的误差。

输入处理

输入的是odometry消息。你只需要位置和航向角。坐标x，y存放在pose.pose.position.x,pose.pose.position.y，航向角（ $\varphi$ ）和四元数z元素之间的关系如下：

φ = 2 \cdot arcsin (Q z) (quaternion-angle)

$\varphi=2\cdot\arcsin(Q_{z})\tag{quaternion-angle}$

方形路径跟随

当收到local和global的里程计数据后，调用commander()函数控制机器人。让机器人跑几圈后，等待接收里程计数据。‘

评估结果

如果global和local的数据都受到了调用evaluateResults()函数，起始点的误差需要计算x、y、 $\varphi$ 的偏差。local里程计会相对于global里程计飘移。将local的初始位置和global的设定为一致。
$\theta$ 记作local和global里程计的初始位置角度偏差

θ = φ l o c S t a r t - φ g l o b S t a r t (odom-angle)

$\theta=\varphi_{locStart}-\varphi_{globStart}\tag{odom-angle}$
将系统1（local 里程计）坐标变换到系统0（global 里程计）下：

(x 0 y 0) = (cos θ sin θ - sin θ cos θ) (x 1 y 1)

$\left(\begin{matrix} x_{0}\ y_{0} \end{matrix}\right)=\left(\begin{matrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta \end{matrix}\right)\left(\begin{matrix} x_{1}\\ y_{1} \end{matrix}\right)$
展开：

x 0 = x 1 cos θ - y 1 sin θ (x0)

$x_{0} = x_{1}\cos\theta-y_{1}\sin\theta\tag{x0}$

y 0 = x 1 sin θ + y 1 cos θ (y0)

$y_{0} = x_{1}\sin\theta+y_{1}\cos\theta\tag{y0}$

Program

创建包：

catkin_create_pkg dem_dead_reckoning roscpp rospy std_msgs
跳转到新产生的目录dem_dead_reckoning 下面，创建文件：

include/node_deadReckoningI.h
src/node_deadReckoningI.cpp

添加主程序：

src/deadReckoningI.cpp

为了独立使用PID控制器，把上一节的内容拷贝到合理的文件夹下面：

src/myPoint.cpp
src/node_pid.cpp
src/pid.cpp
include/myPoint.h
include/node_pid.h

修改CMakeLists.txt，添加下面内容 :

set (SRCS1 ${SRCS1} src/myPoint.cpp)
set (SRCS1 ${SRCS1} src/node_pid.cpp)
set (SRCS1 ${SRCS1} src/pid.cpp)
set (SRCS2 ${SRCS2} src/node_deadReckoningI.cpp)
set (SRCS2 ${SRCS2} src/deadReckoningI.cpp)
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(cmd ${SRCS1})
target_link_libraries(cmd ${catkin_LIBRARIES})
add_executable(deadReckoningReg ${SRCS2})
target_link_libraries(deadReckoningReg ${catkin_LIBRARIES})

Code: node_deadReckoningI.h

#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include <string>

using namespace std;

class NodeDeadReckoningI
{
public:

  /* Constructor
   * pub Publisher, which can send commands to robot.
   * fw  Variable, which will be stored in forward.
   * an  Variable, which will be stored in angle.
   * num Variable, which will be stored in numberOfTrips.
   */
  NodeDeadReckoningI(int num, double fw, double an);

  ~NodeDeadReckoningI();

  /* This method receives global odometry data.
   * If #evaluate = false, than this method processes
   * start position.
   * If #evaluate = true, than this method processes
   * end position and start method evaluateResults()
   * 
   * msg   Message with global odometry data
   */
  void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);

  /* This method is similar to messageCallback,
   * but receives local odometry data.
   *
   * msg   Message with local odometry data.
   */
  void messageCallback2(const nav_msgs::Odometry::ConstPtr& msg);

  /* This method controls movement of robot (Trips forward
   * and turn around by using PID controller).
   */
  void commander();

  /* Evaluate difference between start and end
   * position in global odometry and compare
   * global with local odometry.
   */
  void evaluateResults();

  //variables
  double forward;     // Distance to go forward
  double angle;       // Angle to turn around
  int numberOfTrips;  // How many times will robot go forward and turn.
  //Initial position (Loc - local odometry)
  geometry_msgs::Point start;
  double startAngle;
  geometry_msgs::Point startLoc;
  double startLocAngle;
  //Final position (Loc - local odometry)
  geometry_msgs::Point end;
  double endAngle;
  geometry_msgs::Point endLoc; 
  double endLocAngle;
  bool evaluate; // True if robot finished its trajectory
  bool localOdomReceived;  // True, if local odometry is received
  bool globalOdomReceived; // True, if global odometry is received
};

Code: node_deadReckoningI.cpp

#include "node_deadReckoningI.h"
#include <cstdlib>
#define PI 3.141592654

//Constructor and destructor
NodeDeadReckoningI::NodeDeadReckoningI(int num, double fw, double an)
{
  numberOfTrips = num;
  forward = fw;
  angle = an;
}

NodeDeadReckoningI::~NodeDeadReckoningI()
{
}

//Subscriber
void NodeDeadReckoningI::messageCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
  if (globalOdomReceived == false && evaluate == false)
  {
    globalOdomReceived = true;
    start = msg->pose.pose.position;
    startAngle = 2.0*asin(msg->pose.pose.orientation.z);
    if (localOdomReceived == true)
    {
      commander();
    }

  }
  else if (evaluate)
  {
    globalOdomReceived = true;
    end = msg->pose.pose.position;
    endAngle = 2.0*asin(msg->pose.pose.orientation.z);
    if (localOdomReceived == true)
    {
      evaluateResults();
    }
  }
}

//Subscriber
void NodeDeadReckoningI::messageCallback2(const nav_msgs::Odometry::ConstPtr& msg)
{
  if (localOdomReceived == false && evaluate == false)
  {
    localOdomReceived = true;
    startLoc = msg->pose.pose.position;
    startLocAngle = 2.0*asin(msg->pose.pose.orientation.z);
    if (globalOdomReceived == true)
    {
      commander();
    }

  }
  else if (evaluate)
  {
    localOdomReceived = true;
    endLoc = msg->pose.pose.position;
    endLocAngle = 2.0*asin(msg->pose.pose.orientation.z);
    if (globalOdomReceived == true)
    {
      evaluateResults();
    }
  }
}

void NodeDeadReckoningI::commander(){
    stringstream sf;
    sf << "./cmd -f " << forward;
    stringstream sr;
    sr << "./cmd -r " << angle;

    for (int tripsDone = 0; tripsDone < numberOfTrips ; tripsDone++)
    {
      std::system(sf.str().c_str());
      std::system(sr.str().c_str());
    }
    evaluate = true;
    localOdomReceived = false;
    globalOdomReceived = false;
}

void NodeDeadReckoningI::evaluateResults(){
  //evaluating results
    double diffx = end.x - start.x;
    double diffy = end.y - start.y;
    double diffAngle = endAngle - startAngle;

    //TRANSFORMATION
    double angleTransStart = startAngle - startLocAngle;    //How much is local angle different from global
    double xStartLocNew = startLoc.x*cos(angleTransStart)-startLoc.y*sin(angleTransStart);
    double yStartLocNew = startLoc.x*sin(angleTransStart)+startLoc.y*cos(angleTransStart);
    double xEndLocNew = endLoc.x*cos(angleTransStart)-endLoc.y*sin(angleTransStart);
    double yEndLocNew = endLoc.x*sin(angleTransStart)+endLoc.y*cos(angleTransStart);
    double diffxLoc = xEndLocNew  - xStartLocNew ;
    double diffyLoc = yEndLocNew - yStartLocNew ;
    double diffAngleLoc = endLocAngle - startLocAngle;
    while (fabs(diffAngle) > PI)
    {
      diffAngle -= copysign(2*PI,diffAngle);
    }
    //sending message
    ROS_INFO("DIFFERENCE BETWEEN LOCAL AND GLOBAL ODOMETRY:\nDIFFERENCES IN AXIS:\nDIFF_X = %f\nDIFF_Y = %f\nDIFF_ANGLE = %f",diffxLoc-diffx,diffyLoc-diffy,diffAngleLoc-diffAngle);
    ROS_INFO("DIFFERENCE FROM START POSITION:\nDIFERENCES IN AXIS:\nDIFF_X = %f\nDIFF_Y = %f\nDIFF_ANGLE = %f",diffx, diffy, diffAngle);
    exit(0);
}

Code: deadReckoningI.cpp

#include "node_deadReckoningI.h"
#define SUBSCRIBER_BUFFER_SIZE 1  // Size of buffer for subscriber.
#define NUMBER_OF_TRIPS 8 // How many times will robot go forward and turn
#define FORWARD 0.6  // Distance to go forward.
#define ANGLE 1.570796 // Robot will turn around by this angle
#define SUBSCRIBER_TOPIC "/syros/global_odom"
#define SUBSCRIBER2_TOPIC "/syros/base_odom"

int main(int argc, char **argv)
{
  //Initialization of node
  ros::init(argc, argv, "deadReckoningI");
  ros::NodeHandle n;

  //Creating object, which stores data from sensors and has methods for subscribing
  NodeDeadReckoningI *nodeDeadReckoningI = new NodeDeadReckoningI(NUMBER_OF_TRIPS, FORWARD, ANGLE);

  //Creating subscribers
  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeDeadReckoningI::messageCallback, nodeDeadReckoningI);
  ros::Subscriber sub2 = n.subscribe(SUBSCRIBER2_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeDeadReckoningI::messageCallback2, nodeDeadReckoningI);

  ros::spin();
  return 0;
}