机器人两点五次轨迹规划程序设计

机器人轨迹泛指工业机器人在运动过程中的运动轨迹，即运动点的位移、速度和加速度。
下面是五次多项式插值算法，关键求解过程是数值分析里的高斯列主元消去法，将下列写成矩阵形式，A矩阵是关于时间的矩阵，B矩阵是约束条件，求解得到是多项式参数

#include "stdafx.h"
#include 
#include 
#include 

#include 

#include 

#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

int i,j,k;
float d;
float temp[15];

string s;
vector v;

int filesread(void);

int size;
vector num1;
vector num2;


int main()
{
	filesread();

	//动态数组
	double **aa;
	int mmm,nnn,iii;
	mmm = size +1;
	nnn = size +1;

	//double *temp;	
	//temp = (double*)malloc(sizeof(int)*mmm);
	//for (i = 0; i < mmm; i++)
	//	temp[i] = 0;
	//
	
	 //输入你此次想要创建数组的行数和列数，储存在m和n中
	aa=(double**)malloc(mmm*sizeof(double*)); /*malloc函数在stdlib.h里面，用的时候加入这个头文件*/
	for(iii=0;iii max)
					{
						max = fabs(aa[i][k]);
						hang = i;
					}
				}
				if (aa[hang][k] == 0)
				{
					cout << "无法计算" << endl;
					return 0;
				}
				if (k != hang) //换行
				{
					for (i = 0; i < size+1; i++)
					{
						temp[i] = aa[k][i];
						aa[k][i] = aa[hang][i];
						aa[hang][i] = temp[i];
					}
				}
				for (i = k + 1; i < size; i++) //消元
				{
					d = aa[i][k] / aa[k][k];
					for (j = 0; j < size + 1; j++)
					{
						aa[i][j] = aa[i][j] - d * aa[k][j];
					}
				}
	}
	//memset(temp, 0, 15 * sizeof(float)); //将temp清0，准备存放解向量
	memset(temp, 0, 15 * sizeof(float)); //将temp清0，准备存放解向量
		for (i = size-1; i >= 0; i--) //求解向量
		{		  
			d = 0;
			for (k = 0; k < size; k++)
			{
				d = d + temp[k] * aa[i][k];
			}
			temp[i] = (aa[i][size] - d) / aa[i][i];
		}
		ofstream datafile;
		datafile.open("qiujie.txt",ofstream::app);
		
		for (i = 0; i < size; i++) 
		{
			datafile << " "<< fixed << setprecision(5) << temp[i];//5位小数
		}
	
		

	//system("pause");
	return 0;
}


int filesread()
{
	ifstream infile;
	infile.open("Ajuzheng.txt");   //将文件流对象与文件连接起来 
	assert(infile.is_open());   //若失败,则输出错误消息,并终止程序运行 

	while(getline(infile,s))
	{		
		v.push_back(s);
	}
	infile.close();             //关闭文件输入流 
	 size = v.size();

	for (int j =0; j< v.size(); j++)
	{
		
		string word= v[j];
		double result;
		stringstream input(word);
		while(input>>result)
			num1.push_back(result);			
	}

	ifstream infile2;
	infile2.open("Bjuzheng.txt");   //将文件流对象与文件连接起来 
	assert(infile2.is_open());   //若失败,则输出错误消息,并终止程序运行 

	string ss;
	vector vv;
	int size2;
	while(getline(infile2,ss))
	{		
		vv.push_back(ss);
	}
	infile2.close();             //关闭文件输入流 
	size2 = vv.size();

	for (int j =0; j< vv.size(); j++)
	{

		string wordd= vv[j];
		double resultt;
		stringstream inputt(wordd);
		while(inputt>>resultt)
			num2.push_back(resultt);			
	}
	return size;
}

示例结果

实例：

PS：这种规划不好，做阻抗控制，本来想着规划走上图曲面的轮廓的，但由于各种原因最后决定先尝试用robotmaster规划离线轨迹再进行实时修改（行不行的通还在尝试中）。

参考链接：https://blog.csdn.net/aic1999/article/details/82532525