【机器人学】机器人开源项目KDL源码学习:(3)机器人操作空间路径规划(Path Planning)和轨迹规划(Trajectory Planning)示例

166
0
2021年2月20日 09时16分

很多同学会把路径规划(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;
	}
 
}

 

关键代码详细解读:

 

		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();

 

这几段代码表示在操作空间中插入6个中间路径点,注意需要以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绘出的轨迹曲线和机械臂末端与时间的关系,可以看出在明显的圆弧过渡。

 

 

微信图片_20210218123453

 

微信图片_20210218123456

 

Path velocity呈明显的梯形形状。

发表评论

后才能评论