pioneer 移动机器人实现平滑轨迹

#include "Aria.h"
#include <math.h>
#include <iostream>
int main(int argc, char **argv)
{
  Aria::init();
  ArRobot robot;
  ArArgumentParser parser(&argc, argv);
  parser.loadDefaultArguments();
  ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides.");
  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
  // and then loads parameter files for this robot.
  ArRobotConnector robotConnector(&parser, &robot);
  if(!robotConnector.connectRobot())
  {
    ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot.");
    if(parser.checkHelpAndWarnUnparsed())
    {
        Aria::logOptions();
        Aria::exit(1);
        return 1;
    }
  }
  if (!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::exit(1);
    return 1;
  }
  
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected.");
  // Start the robot processing cycle running in the background.
  // True parameter means that if the connection is lost, then the 
  // run loop ends.
  robot.runAsync(true);
  double v=0,w=0;
  double k1 = 0.6;
  double k2 = 50;
  double k3 = -50;
  int xg = 2000;
  int yg = -1000;
  int tg = -3.14/2 ;

  for(;;)
  {
	ArUtil::sleep(100);
	double x0 = robot.getX();
	double y0 = robot.getY();
	double t0 = robot.getTh()*3.14/180 + tg;
	std::cout <<x0<<"\t"<<y0<<"\t"<<t0<<std::endl; 

	double distance0 = sqrt((xg-x0)*(xg-x0) +(yg-y0)*(yg-y0));
	double alpha = -t0 + atan2((yg-y0),(xg-x0));
	double beta = -t0 - alpha;

	v = k1 * distance0;
	if(v>150)
		v = 150;
	w = k2 * alpha +k3 * beta;
	if(w>45)
		w = 45;
	if(w < -45)
		w= -45;
	robot.lock();
	robot.setVel(v);
	robot.setRotVel(w);
	robot.unlock();

	if(abs(robot.getX()-xg)<2 && abs(robot.getY()-yg)<2 && abs(robot.getTh()  - tg*180/3.14)<2)
	{
		break;
	}
  }
  robot.stopRunning();
  // wait for the thread to stop
  robot.waitForRunExit();
  // exit
  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
  Aria::exit(0);
  return 0;
}

其实根本性实现就是定义了x 轴速度和z 轴旋转；