機器人兩點五次軌跡規劃程序設計

機器人軌跡泛指工業機器人在運動過程中的運動軌跡，即運動點的位移、速度和加速度。
下面是五次多項式插值算法，關鍵求解過程是數值分析裏的高斯列主元消去法，將下列寫成矩陣形式，A矩陣是關於時間的矩陣，B矩陣是約束條件，求解得到是多項式參數

#include "stdafx.h"
#include <iostream>
#include <iomanip>
#include <math.h>

#include <iostream>

#include <fstream>

#include <vector>
#include <cassert>
#include <string>
#include <sstream>
#include <vector>
#include <stdlib.h>
using namespace std;

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

string s;
vector<string> v;

int filesread(void);

int size;
vector<float> num1;
vector<float> 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<mmm;iii++)
		aa[iii]=(double*)malloc(nnn*sizeof(double));
	




	k = 0;
	for (i = 0; i < size; i++)
	{
		for (j = 0; j < size; j++)
		{
		
			//a[i][j] = num1[k];
			aa[i][j] = num1[k];
			k++;
		}
	}

	for (i = 0; i < size; i++)
	{
		
		
		//a[i][size] =num2[i];
		aa[i][size] =num2[i];
		
	}
	for (k = 0; k < size - 1; k++) //找列主元最大值
			{
				double max = 0;
				int hang=0,num=0;
				for (i = k; i < size; i++)
				{
					if (fabs(aa[i][k]) > 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<string> 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