写在前面
下面程序中涉及到如下两部分内容： 正逆运动学算法：五自由度机械臂正逆运动学算法（Ｃ语言+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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#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 <math.h>
#include <stdio.h>
#include <stdlib.h>
#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赚钱了再换个好的。
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明五自由度diy机械臂空间插补算法(直线和圆弧)简单测试！
