五自由度diy机械臂空间插补算法(直线和圆弧)简单测试

写在前面

下面程序中涉及到如下两部分内容：
正逆运动学算法：五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
简易矩阵运算库：自写的C语言矩阵运算库
空间直线插补仿真：

五自由度diy机械臂简易实验结果：

空间圆弧插补仿真：

五自由度diy机械臂简易实验结果：

实现代码

空间直线插补和圆弧插补头文件：

/*
 * simple_space_interpolation.h
 *
 *  Created on: Jul 18, 2019
 *      Author: xuuyann
 */
/*
 * 简单的空间插补程序：目前只更新空间直线插补和空间圆弧插补程序
 * 传统插补方法＋梯形加减速归一化处理
 *　归一化因子采用抛物线过渡的线性函数（梯形加减速），
 *　以保证整段轨迹上的位移和速度都连续。
 */
#ifndef HEADER_SIMPLE_SPACE_INTERPOLATION_H_
#define HEADER_SIMPLE_SPACE_INTERPOLATION_H_
// 定义的数组容量，注意溢出问题
#define n 10000
//　定义插值点结构体
struct InterPoint_Node
{
	double x[n];
	double y[n];
	double z[n];
	double alp[n];
	double beta[n];
	double gama[n];
};
typedef struct InterPoint_Node *PtrToInterPointNode;
typedef PtrToInterPointNode InterPoint;

//　定义设定点结构体
struct SetPoint_Node
{
	double x;
	double y;
	double z;
	double alp;
	double beta;
	double gama;
};
typedef struct SetPoint_Node *PtrToSetPointNode;
typedef PtrToSetPointNode SetPoint;
/*
 * 空间直线插补＋梯形加减速归一化处理
 * 参数：起点S位置和RPY角
 * 		终点D位置和RPY角
 * 		末端线速度vs、加减速度a
 * 		待赋值的插值点数N
 * 返回值：插值点位置和RPY角（不包括起点S和终点D）
 */
InterPoint SpaceLine(SetPoint S, SetPoint D, double vs, double a, int *N);

/*
 * 空间圆弧插补＋梯形加减速归一化处理
 * 参数：起点S位置和RPY角
 * 		中间点M位置和RPY角
 * 		终点D位置和RPY角、
 * 		末端角速度ws、加减速度a
 * 		待赋值的插值点数N
 * 返回值：插值点位置和RPY角(不包括起点S和终点D)
 * 为方便起见，角速度和角加速度均为角度制
 */
InterPoint SpaceCircle(SetPoint S, SetPoint M, SetPoint D, double ws, double a, int *N);

/*
 * 归一化处理：归一化因子采用抛物线过渡的线性函数（梯形加减速）
 * 参数：机械臂末端运动总位移（角度）pos
 * 		机械臂末端线速度（角速度）vel
 * 		加减速度accl（设定加减速度相同）
 * 		插值点数N
 * 		空的lambda数组
 */
void Normalization(double pos, double vel, double accl, int N, double lambda[]);

#endif /* HEADER_SIMPLE_SPACE_INTERPOLATION_H_ */

空间直线插补和圆弧插补.c文件

/*
 * simple_space_interpolation.c
 *
 *  Created on: Jul 18, 2019
 *      Author: xuuyann
 */
#include "../header/MyMatrix.h"
#include "../header/simple_space_interpolation.h"
#include 
#include 
#include 

#define PI 3.141592653

// 归一化处理：归一化因子采用抛物线过渡的线性函数（梯形加减速）
void Normalization(double pos, double vel, double accl, int N, double lambda[])
{
	//　加减速段的时间和位移
	double T1 = vel / accl;
	double S1 = (1.0/2.0) * accl * pow(T1, 2);
	// 总时间
	double Te = 2.0*T1 + (pos - 2.0*S1) / vel;
	// 归一化处理
	double S1_ = S1 / pos;
	double T1_ = T1 / Te;
	double T2_ = 1 - T1_;
	double accl_ = 2.0*S1_ / pow(T1_, 2);
	// 求解归一化因子
	for (int i = 0; i <= N; i++){
		double t = (double)(i) / (double)(N);
		if (t >= 0 && t <= T1_)
			lambda[i] = (1.0/2.0) * accl_ * pow(t, 2);
		else if (t >T1_ && t <= T2_)
			lambda[i] = (1.0/2.0)*accl_*pow(T1_, 2) + accl_*T1_*(t - T1_);
		else if (t > T2_ && t <= 1)
			lambda[i] = (1.0/2.0)*accl_*pow(T1_, 2) + accl_*T1_*(t - T1_) - (1.0/2.0)*accl_*pow(t - T2_, 2);
	}
}

// 空间直线插补＋梯形加减速归一化处理
InterPoint SpaceLine(SetPoint S, SetPoint D, double vs, double a, int *N)
{
	InterPoint Inter_p;
	Inter_p = (InterPoint)malloc(sizeof(struct InterPoint_Node));
	double x1, y1, z1, alp1, beta1, gama1;
	double x2, y2, z2, alp2, beta2, gama2;
	x1 = S->x;
	y1 = S->y;
	z1 = S->z;
	alp1 = S->alp;
	beta1 = S->beta;
	gama1 = S->gama;
	x2 = D->x;
	y2 = D->y;
	z2 = D->z;
	alp2 = D->alp;
	beta2 = D->beta;
	gama2 = D->gama;
	int P = 1; // 插值参数，增加插值点数，避免过小
	// 总位移
	double dis = sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2) + pow(z2 - z1, 2));
	//　插值点数
	*N = (int)(P*dis/vs) + 1;
	printf("N: %d\n", *N);
	// 归一化参数
	double lambda[*N + 1];
	Normalization(dis, vs, a, *N, lambda);
	double delta_x = x2 - x1;
	double delta_y = y2 - y1;
	double delta_z = z2 - z1;
	double delta_alp = alp2 - alp1;
	double delta_beta = beta2 - beta1;
	double delta_gama = gama2 - gama1;
	for (int i = 0; i < *N + 1; i++){
		Inter_p->x[i] = x1 + delta_x*lambda[i];
		Inter_p->y[i] = y1 + delta_y*lambda[i];
		Inter_p->z[i] = z1 + delta_z*lambda[i];
		Inter_p->alp[i] = alp1 + delta_alp*lambda[i];
		Inter_p->beta[i] = beta1 + delta_beta*lambda[i];
		Inter_p->gama[i] = gama1 + delta_gama*lambda[i];
	}
	return Inter_p;
}

// 空间圆弧插补＋梯形加减速归一化处理
InterPoint SpaceCircle(SetPoint S, SetPoint M, SetPoint D, double ws, double a, int *N)
{
	InterPoint Inter_p;
	Inter_p = (InterPoint)malloc(sizeof(struct InterPoint_Node));
	double x1, y1, z1, alp1, beta1, gama1;
	double x2, y2, z2, alp2, beta2, gama2;
	double x3, y3, z3, alp3, beta3, gama3;
	x1 = S->x;
	y1 = S->y;
	z1 = S->z;
	x2 = M->x;
	y2 = M->y;
	z2 = M->z;
	x3 = D->x;
	y3 = D->y;
	z3 = D->z;

	alp1 = S->alp;
	beta1 = S->beta;
	gama1 = S->gama;
	alp2 = D->alp;
	beta2 = D->beta;
	gama2 = D->gama;

	double A1 = (y1 - y3)*(z2 - z3) - (y2 - y3)*(z1 - z3);
	double B1 = (x2 - x3)*(z1 - z3) - (x1 - x3)*(z2 - z3);
	double C1 = (x1 - x3)*(y2 - y3) - (x2 - x3)*(y1 - y3);
	double D1 = -(A1*x3 + B1*y3 + C1*z3);

	double A2 = x2 - x1;
	double B2 = y2 - y1;
	double C2 = z2 - z1;
	double D2 = -((pow(x2, 2) - pow(x1, 2)) + (pow(y2, 2) - pow(y1, 2)) + (pow(z2, 2) - pow(z1, 2))) / 2.0;

	double A3 = x3 - x2;
	double B3 = y3 - y2;
	double C3 = z3 - z2;
	double D3 = -((pow(x3, 2) - pow(x2, 2)) + (pow(y3, 2) - pow(y2, 2)) + (pow(z3, 2) - pow(z2, 2))) / 2.0;
	Matrix A;
	Matrix b;
	A = Create_Matrix(3, 3);
	b = Create_Matrix(3, 1);
	double coef[9] = {A1, B1, C1, A2, B2, C2, A3, B3, C3};
	double b_[3] = {-D1, -D2, -D3};
	SetData_Matrix(A, coef);
	SetData_Matrix(b, b_);
	// 计算圆心
	Matrix C;
	C = Create_Matrix(b->row, b->column);
	C = Gauss_lie(A, b);
	double x0, y0, z0;
	x0 = PickInMat(C, 1, 1);
	y0 = PickInMat(C, 2, 1);
	z0 = PickInMat(C, 3, 1);
	printf("C: %f %f %f\n", x0, y0, z0);
	//　外接圆半径
	double r = sqrt(pow(x1 - x0, 2) + pow(y1 - y0, 2) + pow(z1 - z0, 2));
	// 新坐标系Z0的方向余弦
	double L = sqrt(pow(A1, 2) + pow(B1, 2) + pow(C1, 2));
	double av[3];
	av[0] = A1 / L;
	av[1] = B1 / L;
	av[2] = C1 / L;
	// 新坐标系X0的方向余弦
	double nv[3];
	nv[0] = (x1 - x0) / r;
	nv[1] = (y1 - y0) / r;
	nv[2] = (z1 - z0) / r;
	// 新坐标系Y0的方向余弦
	Matrix av_, nv_, ov_;
	av_ = Create_Matrix(3, 1);
	nv_ = Create_Matrix(3, 1);
	ov_ = Create_Matrix(3, 1);
	SetData_Matrix(av_, av);
	SetData_Matrix(nv_, nv);
	ov_ = Cross(av_, nv_);
	double ox, oy, oz;
	ox = PickInMat(ov_, 1, 1);
	oy = PickInMat(ov_, 2, 1);
	oz = PickInMat(ov_, 3, 1);
	// 相对于基座标系{O-XYZ}， 新坐标系{C-X0Y0Z0}的坐标变换矩阵
	double T[16] = {nv[0], ox, av[0], x0, nv[1], oy, av[1], y0, nv[2], oz, av[2], z0, 0, 0, 0, 1};
	Matrix T_ = Create_Matrix(4, 4);
	SetData_Matrix(T_, T);
	// 求在新坐标系{C-X0Y0Z0}下S、M和D的坐标
	double S_[4] = {x1, y1, z1, 1.0};
	double M_[4] = {x2, y2, z2, 1.0};
	double D_[4] = {x3, y3, z3, 1.0};
	Matrix S_M, M_M, D_M;
	S_M = Create_Matrix(4, 1);
	M_M = Create_Matrix(4, 1);
	D_M = Create_Matrix(4, 1);
	SetData_Matrix(S_M, S_);
	SetData_Matrix(M_M, M_);
	SetData_Matrix(D_M, D_);
	Matrix S__, M__, D__;
	S__ = Mult_Matrix(EleTransInv_Matrix(T_), S_M);
	M__ = Mult_Matrix(EleTransInv_Matrix(T_), M_M);
	D__ = Mult_Matrix(EleTransInv_Matrix(T_), D_M);
	double x1_, y1_, z1_;
	double x2_, y2_, z2_;
	double x3_, y3_, z3_;
	x1_ = PickInMat(S__, 1, 1);
	y1_ = PickInMat(S__, 2, 1);
	z1_ = PickInMat(S__, 3, 1);
	x2_ = PickInMat(M__, 1, 1);
	y2_ = PickInMat(M__, 2, 1);
	z2_ = PickInMat(M__, 3, 1);
	x3_ = PickInMat(D__, 1, 1);
	y3_ = PickInMat(D__, 2, 1);
	z3_ = PickInMat(D__, 3, 1);
	// 判断圆弧是顺时针还是逆时针，并求解圆心角
	double angle_SOM = 0.0, angle_SOD = 0.0;
	if (atan2(y2_, x2_) < 0)
	    angle_SOM = atan2(y2_, x2_) + 2*PI;
	else
	    angle_SOM = atan2(y2_, x2_);
	if (atan2(y3_, x3_) < 0)
	    angle_SOD = atan2(y3_, x3_) + 2*PI;
	else
	    angle_SOD = atan2(y3_, x3_);
	// 逆时针
	int flag;
	double theta;
	if (angle_SOM < angle_SOD){
	    flag = 1;
	    theta = angle_SOD; // 圆心角
	}
	// 顺时针
	if (angle_SOM >= angle_SOD){
	    flag = -1;
	    theta = 2*PI - angle_SOD; // 圆心角
	}
	// 插补点数Ｎ
	int P = 1; // 插补参数，增加插值点数，避免过小
	ws = ws * PI/180; // 将角度转换为弧度
	a = a * PI/180;
	*N = (int)(P * theta/ws) + 1;
	// 求归一化参数
	double lambda[*N + 1];
	printf("N: %d\n", *N);
	Normalization(theta, ws, a, *N, lambda);

	// 插补原理: 在新平面上进行插补（简化）
	// 在新坐标系下z1_,z2_,z3_均为0，即外接圆在新坐标系的XOY平面内
	// 此时转化为平面圆弧插补问题
	double delta_ang = theta;
	double delta_alp = alp2 - alp1;
	double delta_beta = beta2 - beta1;
	double delta_gama = gama2 - gama1;
	double x_ = 0, y_ = 0, z_ = 0;
	Matrix I, P_;
	I = Create_Matrix(4, 1);
	P_ = Create_Matrix(4, 1);
	// 求插值点
	for (int i = 0; i < *N + 1; i++){
		x_ = flag * r * cos(lambda[i]*delta_ang);
		y_ = flag * r * sin(lambda[i]*delta_ang);
		z_ = 0.0;
		double p_[4] = {x_, y_, z_, 1.0};
		SetData_Matrix(P_, p_);
		I = Mult_Matrix(T_, P_);
		Inter_p->x[i] = PickInMat(I, 1, 1);
		Inter_p->y[i] = PickInMat(I, 2, 1);
		Inter_p->z[i] = PickInMat(I, 3, 1);
		Inter_p->alp[i] = alp1 + delta_alp*lambda[i];
		Inter_p->beta[i] = beta1 + delta_beta*lambda[i];
		Inter_p->gama[i] = gama1 + delta_gama*lambda[i];
	}
	return Inter_p;
}

在前面的FiveDofKinematic.h和相应的c文件中加了如下子程序:

/*
 * 求RPY角（目前没有解决奇点问题）
 *　无返回值，但rpy_ang地址对应的数组发生改变
 */
void RPY_ang(Matrix fk_T, double rpy_ang[])
{
	double nx, ny, nz;
	double ox, oy, oz;
	double ax, ay, az;
	nx = PickInMat(fk_T, 1, 1);
	ny = PickInMat(fk_T, 2, 1);
	nz = PickInMat(fk_T, 3, 1);
	ox = PickInMat(fk_T, 1, 2);
	oy = PickInMat(fk_T, 2, 2);
	oz = PickInMat(fk_T, 3, 2);
	ax = PickInMat(fk_T, 1, 3);
	ay = PickInMat(fk_T, 2, 3);
	az = PickInMat(fk_T, 3, 3);
	rpy_ang[1] = atan2(-nz, sqrt(pow(nx, 2) + pow(ny, 2))); // beta
	if (cos(rpy_ang[1]) < 1.0e-004){
		printf("Singularity!The beta = +-pi/2\n");
		exit(1);
	}else{
		rpy_ang[0] = atan2(ny, nx); // alpha
		rpy_ang[2] = atan2(oz, az); // gama
	}
}

/*
 * 求RPY角的旋转矩阵，即定角旋转矩阵
 */
Matrix RPY_rot(double rpy_ang[])
{
	Matrix R;
	R = Create_Matrix(3, 3);
	double alp, beta, gama;
	alp = rpy_ang[0];
	beta = rpy_ang[1];
	gama = rpy_ang[2];
	double r11, r12, r13, r21, r22, r23, r31, r32, r33;
	r11 = cos(alp) * cos(beta);
	r12 = cos(alp)*sin(beta)*sin(gama) - sin(alp)*cos(gama);
	r13 = cos(alp)*sin(beta)*cos(gama) + sin(alp)*sin(gama);
	r21 = sin(alp)*cos(beta);
	r22 = sin(alp)*sin(beta)*sin(gama) + cos(alp)*cos(gama);
	r23 = sin(alp)*sin(beta)*cos(gama) - cos(alp)*sin(gama);
	r31 = -sin(beta);
	r32 = cos(beta)*sin(gama);
	r33 = cos(beta)*cos(gama);
	double R_[9] = {r11, r12, r13, r21, r22, r23, r31, r32, r33};
	SetData_Matrix(R, R_);
	return R;
}

测试主程序（用来打印检查结果是否和matlab计算一致）：

#include 
#include 
#include 
#include "../header/FiveDofKinematic.h"
#include "../header/MyMatrix.h"
#include "../header/simple_space_interpolation.h"
#define PI 3.141592653

int main()
{
//空间插补
	Matrix T_S, T_M, T_D;
	Input_data DH_para;
	DH_para = (Input_data)malloc(sizeof(struct DH_Node));
	Init_DH(DH_para);
	double theta_S[5] = {-40*PI/180, 90*PI/180, -10*PI/180, -85*PI/180, 0*PI/180};
	double theta_M[5] = {-40*PI/180, 100*PI/180, -50*PI/180, -50*PI/180, 0*PI/180};
	double theta_D[5] = {-40*PI/180, 45*PI/180, -100*PI/180, 50*PI/180, 0*PI/180};
	//　各点末端执行器位姿
	T_S = five_dof_fkine(DH_para, theta_S);
	T_M = five_dof_fkine(DH_para, theta_M);
	T_D = five_dof_fkine(DH_para, theta_D);
	Show_Matrix(T_S, "T_S = ");
	Show_Matrix(T_M, "T_M = ");
	Show_Matrix(T_D, "T_D = ");
	// 求起点和终点对应的RPY角
	double S_RPY[3], D_RPY[3];
	RPY_ang(T_S, S_RPY);
	RPY_ang(T_D, D_RPY);
	double ws = 2, a = 0.5;
	double vs = 0.08, a_ = 0.02;
	SetPoint S, M, D;
	InterPoint p;
	p = (InterPoint)malloc(sizeof(struct InterPoint_Node));
	S = (SetPoint)malloc(sizeof(struct SetPoint_Node));
	D = (SetPoint)malloc(sizeof(struct SetPoint_Node));
	M = (SetPoint)malloc(sizeof(struct SetPoint_Node));
	// 起点对应的位置坐标和RPY角
	S->x = PickInMat(T_S, 1, 4);
	S->y = PickInMat(T_S, 2, 4);
	S->z = PickInMat(T_S, 3, 4);
	S->alp = S_RPY[0];
	S->beta = S_RPY[1];
	S->gama = S_RPY[2];
	printf("S alpha = %f beta = %f gama = %f\n", S->alp, S->beta, S->gama);
	// 中间点对应的位置坐标
	M->x = PickInMat(T_M, 1, 4);
	M->y = PickInMat(T_M, 2, 4);
	M->z = PickInMat(T_M, 3, 4);
	// 终点对应的位置坐标和RPY角
	D->x = PickInMat(T_D, 1, 4);
	D->y = PickInMat(T_D, 2, 4);
	D->z = PickInMat(T_D, 3, 4);
	D->alp = D_RPY[0];
	D->beta = D_RPY[1];
	D->gama = D_RPY[2];
	int N = 0;
//	p = SpaceLine(S, D, vs, a, &N);
	p = SpaceCircle(S, M, D, ws, a, &N);
	for (int i = 0; i <= N; i++)
		printf("x: %f, y: %f, z: %f, alp: %f, beta: %f, gama: %f, i: %d\n", p->x[i], p->y[i], p->z[i], p->alp[i], p->beta[i], p->gama[i], i);
	printf("\n");
	// 将各插值点对应的关节角设为数组，数组大小为*N+2
	double th1[N+2], th2[N+2], th3[N+2], th4[N+2], th5[N+2];
	// 先初始化起点对应的各关节角度
	//　初始化起点对应的关节角度需要考虑坐标关系，不能简单淡入起点角度初始值
	Matrix theta_S_;
	theta_S_ = five_dof_ikine(DH_para, T_S);
	th1[0] = PickInMat(theta_S_, 2, 1);
	th2[0] = PickInMat(theta_S_, 2, 2);
	th3[0] = PickInMat(theta_S_, 2, 3);
	th4[0] = PickInMat(theta_S_, 2, 4);
	th5[0] = PickInMat(theta_S_, 2, 5);
	Free_Matrix(theta_S_);
	// printf("th1 = %f, th2 = %f, th3 = %f, th4 = %f, th5 = %f\n", th1[0], th2[0], th3[0], th4[0], th5[0]);
	Matrix R, T, ik_T; // RPY的旋转矩阵，插值点对应的齐次变换矩阵，逆解
	T = Create_Matrix(4, 4);
	for (int i = 0; i < N + 1; i++){
		double rpy_ang[3] = {p->alp[i], p->beta[i], p->gama[i]};
		R = RPY_rot(rpy_ang);
		double r11, r12, r13, r21, r22, r23, r31, r32, r33;
		r11 = PickInMat(R, 1, 1);
		r12 = PickInMat(R, 1, 2);
		r13 = PickInMat(R, 1, 3);
		r21 = PickInMat(R, 2, 1);
		r22 = PickInMat(R, 2, 2);
		r23 = PickInMat(R, 2, 3);
		r31 = PickInMat(R, 3, 1);
		r32 = PickInMat(R, 3, 2);
		r33 = PickInMat(R, 3, 3);
		double T_p[16] = {r11, r12, r13, p->x[i],
						  r21, r22, r23, p->y[i],
						  r31, r32, r33, p->z[i],
						    0,   0,   0,     1};
		SetData_Matrix(T, T_p);
		ik_T = five_dof_ikine(DH_para, T);
		double th[5];
		// 简单取第２行逆解
		for (int j = 0; j < 5; j++){
			th[j] = PickInMat(ik_T, 2, j+1);
		}
		// 给关节角数组向量赋值
		th1[i+1] = th[0];
		th2[i+1] = th[1];
		th3[i+1] = th[2];
		th4[i+1] = th[3];
		th5[i+1] = th[4];
		 printf("th1 = %f, th2 = %f, th3 = %f, th4 = %f, th5 = %f, i = %d\n", th1[i+1], th2[i+1], th3[i+1], th4[i+1], th5[i+1], i+1);
	}
}
	Free_Matrix(T_S);
	Free_Matrix(T_M);
	Free_Matrix(T_D);
	Free_Matrix(R);
	Free_Matrix(T);
	Free_Matrix(ik_T);
	return 0;

打印结果(以圆弧插补为例)，与matlba仿真数据进行验证，证明c程序正确：

T_S = 
0.066765	-0.642788	0.763129	1.390297	
-0.056023	-0.766044	-0.640342	-1.166597	
0.996195	0.000000	-0.087156	2.122286	
0.000000	0.000000	0.000000	1.000000	

T_M = 
0.000000	-0.642788	0.766044	1.583017	
0.000000	-0.766044	-0.642788	-1.328309	
1.000000	0.000000	-0.000000	2.039603	
0.000000	0.000000	0.000000	1.000000	

T_D = 
0.066765	-0.642788	0.763129	2.247747	
-0.056023	-0.766044	-0.640342	-1.886084	
0.996195	0.000000	-0.087156	0.085876	
0.000000	0.000000	0.000000	1.000000	

S alpha = -0.698132 beta = -1.483530 gama = 3.141593
C: 0.448045 0.640980 -0.078341
N: 24
x: 1.390297, y: -1.166597, z: 2.122286, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 0
x: 1.399697, y: -1.174470, z: 2.111731, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 1
x: 1.427721, y: -1.197746, z: 2.079656, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 2
x: 1.473811, y: -1.235390, z: 2.024857, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 3
x: 1.535062, y: -1.284121, z: 1.947889, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 4
x: 1.597240, y: -1.331969, z: 1.864574, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 5
x: 1.657626, y: -1.376741, z: 1.778229, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 6
x: 1.716125, y: -1.418366, z: 1.688989, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 7
x: 1.772647, y: -1.456779, z: 1.596992, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 8
x: 1.827103, y: -1.491921, z: 1.502383, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 9
x: 1.879408, y: -1.523736, z: 1.405309, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 10
x: 1.929481, y: -1.552176, z: 1.305920, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 11
x: 1.977244, y: -1.577195, z: 1.204374, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 12
x: 2.022622, y: -1.598756, z: 1.100826, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 13
x: 2.065545, y: -1.616823, z: 0.995440, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 14
x: 2.105944, y: -1.631369, z: 0.888379, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 15
x: 2.143759, y: -1.642371, z: 0.779811, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 16
x: 2.178929, y: -1.649813, z: 0.669904, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 17
x: 2.211399, y: -1.653682, z: 0.558831, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 18
x: 2.241120, y: -1.653972, z: 0.446763, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 19
x: 2.268044, y: -1.650684, z: 0.333877, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 20
x: 2.291209, y: -1.644169, z: 0.224962, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 21
x: 2.306590, y: -1.637267, z: 0.144403, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 22
x: 2.315124, y: -1.632272, z: 0.095980, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 23
x: 2.317852, y: -1.630466, z: 0.079827, alp: -0.698132, beta: -1.483530, gama: 3.141593, i: 24

th1 = -0.698132, th2 = 1.570796, th3 = -0.174533, th4 = -1.483530, th5 = 0.000000, i = 1
th1 = -0.698064, th2 = 1.605926, th3 = -0.261528, th4 = -1.431664, th5 = 0.000006, i = 2
th1 = -0.697229, th2 = 1.665150, th3 = -0.426881, th4 = -1.325536, th5 = 0.000079, i = 3
th1 = -0.694552, th2 = 1.718537, th3 = -0.609835, th4 = -1.195969, th5 = 0.000312, i = 4
th1 = -0.689565, th2 = 1.757921, th3 = -0.792909, th4 = -1.052281, th5 = 0.000747, i = 5
th1 = -0.683444, th2 = 1.778534, th3 = -0.945985, th4 = -0.919825, th5 = 0.001280, i = 6
th1 = -0.676703, th2 = 1.785677, th3 = -1.076342, th4 = -0.796622, th5 = 0.001868, i = 7
th1 = -0.669474, th2 = 1.782478, th3 = -1.190941, th4 = -0.678839, th5 = 0.002498, i = 8
th1 = -0.661811, th2 = 1.770460, th3 = -1.293423, th4 = -0.564361, th5 = 0.003167, i = 9
th1 = -0.653735, th2 = 1.750407, th3 = -1.385885, th4 = -0.451874, th5 = 0.003872, i = 10
th1 = -0.645246, th2 = 1.722708, th3 = -1.469581, th4 = -0.340515, th5 = 0.004614, i = 11
th1 = -0.636331, th2 = 1.687532, th3 = -1.545252, th4 = -0.229712, th5 = 0.005393, i = 12
th1 = -0.626970, th2 = 1.644930, th3 = -1.613301, th4 = -0.119116, th5 = 0.006213, i = 13
th1 = -0.617137, th2 = 1.594909, th3 = -1.673889, th4 = -0.008572, th5 = 0.007074, i = 14
th1 = -0.606799, th2 = 1.537487, th3 = -1.727009, th4 = 0.101892, th5 = 0.007982, i = 15
th1 = -0.595917, th2 = 1.472752, th3 = -1.772531, th4 = 0.212057, th5 = 0.008939, i = 16
th1 = -0.584447, th2 = 1.400901, th3 = -1.810244, th4 = 0.321512, th5 = 0.009951, i = 17
th1 = -0.572339, th2 = 1.322279, th3 = -1.839894, th4 = 0.429656, th5 = 0.011021, i = 18
th1 = -0.559536, th2 = 1.237401, th3 = -1.861220, th4 = 0.535713, th5 = 0.012157, i = 19
th1 = -0.545972, th2 = 1.146946, th3 = -1.873994, th4 = 0.638766, th5 = 0.013364, i = 20
th1 = -0.531576, th2 = 1.051739, th3 = -1.878039, th4 = 0.737815, th5 = 0.014651, i = 21
th1 = -0.516905, th2 = 0.956776, th3 = -1.873625, th4 = 0.828137, th5 = 0.015969, i = 22
th1 = -0.505506, th2 = 0.885021, th3 = -1.865162, th4 = 0.891239, th5 = 0.016998, i = 23
th1 = -0.498411, th2 = 0.841404, th3 = -1.857970, th4 = 0.927539, th5 = 0.017640, i = 24
th1 = -0.496001, th2 = 0.826787, th3 = -1.855221, th4 = 0.939364, th5 = 0.017859, i = 25

问题记录：

欢迎各位交流指正：

• 没有解决运动过程中的奇异性问题；
• 逆解精度不高；
• diy机械臂过于简单，不稳定，无编码器，无法实现反馈控制。

拟采取的解决思路：琢磨琢磨一下旋量理论和四元数，位置插补就这样吧，但是姿态插补需要用四元数再琢磨琢磨，至于diy臂的问题，emmmm赚钱了再换个好的。