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 轴旋转；