最近在ROS ANSWERS中看到这样一个问题：

Hi, I'm a beginner and I'm struggling to make my robot move to a certain point. How should I set the angular and linear velocity for my robot to move to a certain point ? For example, if my robot initial position is (0,0), what angular and linear velocity should I use for it to go to (5,5) ?

其实，这位同学所说的就是机器人导航，也就是让机器人从一个地方移动到另一个地方。

 

当然，为了实现这个过程，你需要考虑至少以下三件事情：

 

  • 坐标(0,0)和(5,5)是在哪个坐标系下?

  • 你需要设置障碍物躲避吗（也就是在你的机器人移动到目标位置的时候，躲避那些可能出现在(0,0)和(5,5)之间的障碍物)？

  • 机器人移动的位置是已知还是未知？

 

对于以上问题我们设想出最简单的答案。

 

  • 我们是在里程计坐标系下进行的坐标计算（假设你知道什么是里程计）。里程计坐标系存在一个问题，随着时间的推移会有漂移，但对于这位同学提到的问题已经够用了。

  • 我们路径上没有设置障碍物。因此，我们不需要采用势场或其它避障算法。

  • 假设这是一个未知的环境，我们不会在该环境中构建地图。

 

即使这是一个非常简单的设置，也可以使用该设置将机器人移动到检测到的目标位置。想象下机器人上安装有一个摄像头，并且有一个检测行人的程序。然后，一旦程序检测到机器人前方5m内有行人，也就意味着能用我们在上面问题中定义的简单设置让机器人走到行人所在位置。

 

现在，你需要构建一个控制器，该控制器将机器人当前姿态到目标的距离信息转为速度指令，并将其发送给机器人的/cmd_vel，以使其移动到目标位置。控制器的构建有很多种方法。

 

一个简单的控制器的做法如下：

 

  1. 如果机器人（里程计）的朝向不指向目标，那么旋转机器人直到指向目标。

    1

  2. 一旦机器人朝向目标，那么径直移动机器人直到抵达目标位置。

    2

 

可以使用更多复杂的控制器，来优化到达目标的时间，或者其它因素。比如当远离目标时移动快一点，靠近目标时移动慢一点。

 

假设你想让机器人能够避障，在一个更大的环境里面（可以建图的那种）导航，那么你需要使用ROS导航包，里面包含了所有这些东西，还有更多其它的东西。我们用stage进行移动机器人的仿真。

 

STEP 0. 在ROS下创建工程

4

 

STEP 1.移动机器人

 

首先，我们创建工程名为my_robot的package，运行以下命令：

 

为了将机器人从A移动到B，我们需要使用一个控制器。我们在my_robot的src中创建my_robot.cpp文件，核心代码如下：

double inc_x = goal.x -x; double inc_y = goal.y -y; double angle_to_goal = atan2(inc_y, inc_x); ​ if abs(angle_to_goal - theta) > 0.1 {     speed.linear.x = 0.0;     speed.angular.z = 0.3; } else {     speed.linear.x = 0.5     speed.angular.z = 0.0 } ​

 

在my_robot目录下新建launch文件夹，在launch文件夹下新建my_robot.launch文件，注意在robot.world中创建一个目标点:

<launch> <node name = "stage" pkg="stage_ros" type = "stageros" args="$(find stage_ros)/world/robot.world"/> <node name = "my_robot" pkg = "my_robot" type = "my_robot" output = "screen" /> </launch>

 

运行：

cd ~/robot_ws   //跳转到工作空间目录 catkin_make  roslaunch my_robot my_robot.launch

 

结果显示：

 

5

 

可以看到，通过比对设置的目标点和里程计之间的差异，机器人最终到了我们设置的目标点。

 

附上本文的完整源代码：

#include <iostream>
#include <math.h>

#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Odometry.h>

using namespace std;

double x = 0.0;
double y = 0.0;
double theta = 0.0;
double rot_q = 0.0;
double roll = 0.0;
double pitch = 0.0;

void NewOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
x = msg->pose.pose.position.x;
y = msg->pose.pose.position.y;

rot_q = msg->pose.pose.orientation;
(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w]);
}

int main(int argc, char **argv){

ros::init(argc, argv, "speed_controller");
ros::NodeHandle n;//create a handle node

ros::Subscriber sub = n.subscribe("/odometry/filtered", &NewOdom, this);
ros::Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

ros::Rate loop_rate(10);

geometry_msgs::Twist speed;
geometry_msgs::Point goal;

goal.x = 5;
goal.y = 5;
while(ros::ok())
{
double inc_x = goal.x -x;
double inc_y = goal.y -y;

double angle_to_goal = atan2(inc_y, inc_x);

if (abs(angle_to_goal - theta) > 0.1){
speed.linear.x = 0.0;
speed.angular.z = 0.3;
}
else{
speed.linear.x = 0.5;
speed.angular.z = 0.0;
}
pub.publish(speed);

ros::spinOnce();
}

return 0;
}