【機器人學】機器人開源專案KDL原始碼學習:(3)機器人操作空間路徑規劃(Path Planning)和軌跡規劃(Trajectory Planning)示例
阿新 • • 發佈:2018-11-25
很多同學會把路徑規劃(Path Planning)和軌跡規劃(Trajectory Planning)這兩個概念混淆,路徑規劃只是表示了機械臂末端在操作空間中的幾何資訊,比如從工作臺的一端(A點)沿直線移動到另一端(B點)。而軌跡規劃則加上了時間律,比如它要完成的任務是從A點開始到B點結束,中間是以梯形的速度規律來執行的(先以一個加速度a加速運動到一定的速度Vmax,然後再以固定的速度Vmax巡航,最後再以加速度-a減速到0,同時到達B點),經過軌跡規劃後,得到的是操作空間位置和時間的關係式。
(orocos_kinematics_dynamics-master\orocos_kdl\examples\trajectory_example.cpp)
關鍵程式碼詳細解讀:int main(int argc,char* argv[]) { using namespace KDL; // Create the trajectory: // use try/catch to catch any exceptions thrown. // NOTE: exceptions will become obsolete in a future version. try { // Path_RoundedComposite defines the geometric path along // which the robot will move. // Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis()); path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))); path->Finish(); VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1); velpref->SetProfile(0,path->PathLength()); Trajectory* traject = new Trajectory_Segment(path, velpref); Trajectory_Composite* ctraject = new Trajectory_Composite(); ctraject->Add(traject); ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)))); // use the trajectory double dt=0.1; std::ofstream of("./trajectory.dat"); for (double t=0.0; t <= traject->Duration(); t+= dt) { Frame current_pose; current_pose = traject->Pos(t); for (int i=0;i<4;++i) for (int j=0;j<4;++j) of << current_pose(i,j) << "\t"; of << "\n"; // also velocities and accelerations are available ! //traject->Vel(t); //traject->Acc(t); } of.close(); // you can get some meta-info on the path: for (int segmentnr=0; segmentnr < path->GetNrOfSegments(); segmentnr++) { double starts,ends; Path::IdentifierType pathtype; if (segmentnr==0) { starts = 0.0; } else { starts = path->GetLengthToEndOfSegment(segmentnr-1); } ends = path->GetLengthToEndOfSegment(segmentnr); pathtype = path->GetSegment(segmentnr)->getIdentifier(); std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends; switch(pathtype) { case Path::ID_CIRCLE: std::cout << " circle"; break; case Path::ID_LINE: std::cout << " line "; break; default: std::cout << " unknown "; break; } std::cout << std::endl; } std::cout << " trajectory written to the ./trajectory.dat file " << std::endl; delete ctraject; } catch(Error& error) { std::cout <<"I encountered this error : " << error.Description() << std::endl; std::cout << "with the following type " << error.GetType() << std::endl; } }
這幾段程式碼表示在操作空間中插入6箇中間路徑點,注意需要以path->Finish()結束。path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0))); path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0))); path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1))); path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0))); path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))); path->Finish();
VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1);
velpref->SetProfile(0,path->PathLength());
Trajectory* traject = new Trajectory_Segment(path, velpref);
這幾段表示將從起始到終點的速度設定為梯形波,最大速度為0.5,加速度為0.1。
Trajectory_Composite* ctraject = new Trajectory_Composite();
ctraject->Add(traject);
ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
這幾段沒看出起到了什麼作用。
double dt=0.1;
std::ofstream of("./trajectory.dat");
for (double t=0.0; t <= traject->Duration(); t+= dt) {
Frame current_pose;
current_pose = traject->Pos(t);
for (int i=0;i<4;++i)
for (int j=0;j<4;++j)
of << current_pose(i,j) << "\t";
of << "\n";
}
of.close();
這幾段用來輸出軌跡,步長為0.1,將軌跡的位姿存放在trajectory.dat檔案中。
for (int segmentnr=0; segmentnr < path->GetNrOfSegments(); segmentnr++) {
double starts,ends;
Path::IdentifierType pathtype;
if (segmentnr==0) {
starts = 0.0;
} else {
starts = path->GetLengthToEndOfSegment(segmentnr-1);
}
ends = path->GetLengthToEndOfSegment(segmentnr);
pathtype = path->GetSegment(segmentnr)->getIdentifier();
std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends;
switch(pathtype) {
case Path::ID_CIRCLE:
std::cout << " circle";
break;
case Path::ID_LINE:
std::cout << " line ";
break;
default:
std::cout << " unknown ";
break;
}
std::cout << std::endl;
}
std::cout << " trajectory written to the ./trajectory.dat file " << std::endl;
這幾段是打印出各個相鄰的路徑點的軌跡形式,在笛卡爾空間中,軌跡基元有直線和圓兩種。
下面兩張圖是利用matlab繪出的軌跡曲線和機械臂末端與時間的關係,可以看出在明顯的圓弧過渡。
Path velocity呈明顯的梯形形狀。