Chen Huan
/
StewartOlatform
StewartOlatform
Revision 0:2b80f11eb1d3, committed 2017-10-11
- Comitter:
- heroistired
- Date:
- Wed Oct 11 07:05:25 2017 +0000
- Commit message:
- stewart platform
Changed in this revision
diff -r 000000000000 -r 2b80f11eb1d3 StewartPlatform.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/StewartPlatform.cpp Wed Oct 11 07:05:25 2017 +0000 @@ -0,0 +1,768 @@ +#include "StewartPlatform.h" + +//******************************************** +//功能:计算矩阵乘法 C=A*B +//输入参数:A、B:参加运算的矩阵 +//输出参数:C:运算结果 +//返回值:计算是否成功 成功返回0 否则返回1 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C) +{ + int i, j, k; //循环控制变量 + int posA, posB, posC; //矩阵下标索引 + + //判断异常 + if(A->Size[1] != B->Size[0]) //维度不匹配 + return 1; + if(A->Size[0] * B->Size[1] > 50) //C矩阵规模太大 + return 1; + + //计算矩阵乘法 + C->Size[0] = A->Size[0]; //得到C的行列数 + C->Size[1] = B->Size[1]; + for(i = 1; i <= C->Size[0]; i++) //行循环 + { + for(j = 1; j <= C->Size[1]; j++) //列循环 + { + posC = f2(i,j,C->Size[1]); + C->Elements[posC] = 0; + for(k = 1; k <= A->Size[1]; k++) //计算乘法结果 + { + posA = f2(i,k,A->Size[1]); + posB = f2(k,j,B->Size[1]); + C->Elements[posC] += A->Elements[posA] * B->Elements[posB]; + } + } + } + return 0; +} + +//******************************************** +//功能:计算矩阵转置 +//输入参数:A:被转置的矩阵 +//输出参数:B:转置后的矩阵 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void MatrixTransposition(MatrixType* A, MatrixType* B) +{ + int i, j; + int posA, posB; + + B->Size[0] = A->Size[1]; + B->Size[1] = A->Size[0]; + + for(i = 1; i <= A->Size[0]; i++) + { + for(j = 1; j <= A->Size[1]; j++) + { + posA = f2(i,j,A->Size[1]); + posB = f2(j,i,B->Size[1]); + B->Elements[posB] = A->Elements[posA]; + } + } +} + +//******************************************** +//功能:获得矩阵的子阵 +//输入参数:A:原矩阵 StartRow、StartColumn、EndRow、EndColumn:子阵起始元素 子阵终了元素 +//输出参数:B:子阵 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B) +{ + int i, j; //循环控制变量 + int posA, posB; //矩阵索引 + + B->Size[0] = EndRow - StartRow + 1; //计算子阵的维度 + B->Size[1] = EndColumn - StartColumn + 1; + for(i = 1; i <= B->Size[0]; i++) + { + for(j = 1; j <= B->Size[1]; j++) + { + posA = f2(StartRow + i - 1, StartColumn + j - 1, A->Size[1]); + posB = f2(i, j, B->Size[1]); + B->Elements[posB] = A->Elements[posA]; + } + } +} + +//******************************************** +//功能:填充矩阵 将一个矩阵填充到另一个矩阵中 +//输入参数:A:被填充的矩阵 Row、Column:矩阵填充的位置 B:要填充到被填充矩阵的矩阵 +//输出参数:A:被填充的矩阵 +//返回值:0 代表成功 1代表失败 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B) +{ + int i, j, posA, posB; + + if((Row + B->Size[0] - 1) > A->Size[0]) + { + return 1; + } + else if((Column + B->Size[1] - 1) > A->Size[1]) + { + return 1; + } + else + { + for(i = 1; i <= B->Size[0]; i++) + { + for(j = 1; j <= B->Size[1]; j++) + { + posA = f2(Row + i - 1, Column + j - 1, A->Size[1]); + posB = f2(i, j, B->Size[1]); + A->Elements[posA] = B->Elements[posB]; + } + } + return 0; + } +} + +//******************************************** +//功能:指定动平台变换矩阵参数x,y,z,a,b,c,计算动平台上的点A在绝对坐标系下的坐标B A可以是多个点 一行一个点 +//输入参数:x,y,z,a,b,c:动平台变换矩阵参数 A:动平台上点的相对坐标 +//输出参数:B:点在绝对坐标系下的坐标 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B) +{ + int i; + + MatrixType T; //原点平移 + MatrixType Ra; //俯仰 YAW + MatrixType Rb; //横滚 ROLL + MatrixType Rc; //偏航 PITCH + MatrixType temp1, temp2, temp3; + + T.Size[0] = 4; + T.Size[1] = 4; + T.Elements[f2(1,1,T.Size[1])] = 1; + T.Elements[f2(1,2,T.Size[1])] = 0; + T.Elements[f2(1,3,T.Size[1])] = 0; + T.Elements[f2(1,4,T.Size[1])] = x; + T.Elements[f2(2,1,T.Size[1])] = 0; + T.Elements[f2(2,2,T.Size[1])] = 1; + T.Elements[f2(2,3,T.Size[1])] = 0; + T.Elements[f2(2,4,T.Size[1])] = y; + T.Elements[f2(3,1,T.Size[1])] = 0; + T.Elements[f2(3,2,T.Size[1])] = 0; + T.Elements[f2(3,3,T.Size[1])] = 1; + T.Elements[f2(3,4,T.Size[1])] = z; + T.Elements[f2(4,1,T.Size[1])] = 0; + T.Elements[f2(4,2,T.Size[1])] = 0; + T.Elements[f2(4,3,T.Size[1])] = 0; + T.Elements[f2(4,4,T.Size[1])] = 1; + + Ra.Size[0] = 4; + Ra.Size[1] = 4; + Ra.Elements[f2(1,1,Ra.Size[1])] = 1; + Ra.Elements[f2(1,2,Ra.Size[1])] = 0; + Ra.Elements[f2(1,3,Ra.Size[1])] = 0; + Ra.Elements[f2(1,4,Ra.Size[1])] = 0; + Ra.Elements[f2(2,1,Ra.Size[1])] = 0; + Ra.Elements[f2(2,2,Ra.Size[1])] = cosd(a); + Ra.Elements[f2(2,3,Ra.Size[1])] = -sind(a); + Ra.Elements[f2(2,4,Ra.Size[1])] = 0; + Ra.Elements[f2(3,1,Ra.Size[1])] = 0; + Ra.Elements[f2(3,2,Ra.Size[1])] = sind(a); + Ra.Elements[f2(3,3,Ra.Size[1])] = cosd(a); + Ra.Elements[f2(3,4,Ra.Size[1])] = 0; + Ra.Elements[f2(4,1,Ra.Size[1])] = 0; + Ra.Elements[f2(4,2,Ra.Size[1])] = 0; + Ra.Elements[f2(4,3,Ra.Size[1])] = 0; + Ra.Elements[f2(4,4,Ra.Size[1])] = 1; + + Rb.Size[0] = 4; + Rb.Size[1] = 4; + Rb.Elements[f2(1,1,Rb.Size[1])] = cosd(b); + Rb.Elements[f2(1,2,Rb.Size[1])] = 0; + Rb.Elements[f2(1,3,Rb.Size[1])] = sind(b); + Rb.Elements[f2(1,4,Rb.Size[1])] = 0; + Rb.Elements[f2(2,1,Rb.Size[1])] = 0; + Rb.Elements[f2(2,2,Rb.Size[1])] = 1; + Rb.Elements[f2(2,3,Rb.Size[1])] = 0; + Rb.Elements[f2(2,4,Rb.Size[1])] = 0; + Rb.Elements[f2(3,1,Rb.Size[1])] = -sind(b); + Rb.Elements[f2(3,2,Rb.Size[1])] = 0; + Rb.Elements[f2(3,3,Rb.Size[1])] = cosd(b); + Rb.Elements[f2(3,4,Rb.Size[1])] = 0; + Rb.Elements[f2(4,1,Rb.Size[1])] = 0; + Rb.Elements[f2(4,2,Rb.Size[1])] = 0; + Rb.Elements[f2(4,3,Rb.Size[1])] = 0; + Rb.Elements[f2(4,4,Rb.Size[1])] = 1; + + Rc.Size[0] = 4; + Rc.Size[1] = 4; + Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(c); + Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(c); + Rc.Elements[f2(1,3,Rc.Size[1])] = 0; + Rc.Elements[f2(1,4,Rc.Size[1])] = 0; + Rc.Elements[f2(2,1,Rc.Size[1])] = sind(c); + Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(c); + Rc.Elements[f2(2,3,Rc.Size[1])] = 0; + Rc.Elements[f2(2,4,Rc.Size[1])] = 0; + Rc.Elements[f2(3,1,Rc.Size[1])] = 0; + Rc.Elements[f2(3,2,Rc.Size[1])] = 0; + Rc.Elements[f2(3,3,Rc.Size[1])] = 1; + Rc.Elements[f2(3,4,Rc.Size[1])] = 0; + Rc.Elements[f2(4,1,Rc.Size[1])] = 0; + Rc.Elements[f2(4,2,Rc.Size[1])] = 0; + Rc.Elements[f2(4,3,Rc.Size[1])] = 0; + Rc.Elements[f2(4,4,Rc.Size[1])] = 1; + + B->Size[0] = A->Size[0]; + B->Size[1] = A->Size[1]; + MatrixDot(&T, &Rc, &temp1); //相当于T * Rc * Rb * Ra + MatrixDot(&temp1, &Rb, &temp2); + MatrixDot(&temp2, &Ra, &temp1); + for(i = 1; i <= B->Size[0]; i++) + { + MatrixSub(A, i, 1, i, A->Size[1], &temp2); + MatrixTransposition(&temp2, &temp3); + MatrixDot(&temp1, &temp3, &temp2); + MatrixTransposition(&temp2, &temp3); + MatrixFill(B, i, 1, &temp3); + } +} + +//******************************************** +//功能:计算矩阵行向量所表示的坐标点之间的距离 +//输入参数:A, B:要计算距离的矩阵 +//输出参数:C:包含距离值信息的列向量 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +int Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C) +{ + float distance = 0; + int i = 0, j = 0; + if((A->Size[0] != B->Size[0]) || (A->Size[1] != 4) || (B->Size[1] != 4)) + { + return 1; + } + else + { + C->Size[0] = A->Size[0]; + C->Size[1] = 1; + for(i = 1; i <= A->Size[0]; i++) + { + for(j = 1; j <= A->Size[1]; j++) + { + distance = distance + (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]) * (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]); + } + C->Elements[f2(i,1,C->Size[1])] = sqrt(distance); + distance = 0; + } + return 0; + } +} + +//******************************************** +//功能:解析动感平台 +//输入参数:Platform:动感平台数据结构 包含各种输入输出 +//输出参数:无 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void CalStewartPlatform(StewartPlatformType* Platform) +{ + int x, y, z, a, b, c; //动平台变换矩阵参数 + float xx, yy, zz, r, l, AA, BB, CC, DD, EE, delta, mytheta1, mytheta2, d, e, f, l1, l2, l3, l4; //计算电机角度用的参数 + float theta1, theta2, theta3, theta4, theta5, theta6; + float topRadius, topInterval, bottomRadius, bottomInterval, lengthOfSteelWheel, lengthOfCardan, lengthOfBar; + //平台机械尺寸定义 + float theta0; //舵盘结构的倾角 + MatrixType temp1, temp2; + + MatrixType topPlatform; //动平台上的6个参考点 + MatrixType Rc; + MatrixType bottomPlatform; //定平台上的6个参考点 + MatrixType lengthOfBar1; + + topRadius = Platform->topRadius; //平台参数初始化 + topInterval = Platform->topInterval; + bottomRadius = Platform->bottomRadius; + bottomInterval = Platform->bottomInterval; + lengthOfSteelWheel = Platform->lengthOfSteelWheel; + lengthOfCardan = Platform->lengthOfCardan; + lengthOfBar = Platform->lengthOfBar; + + topPlatform.Size[0] = 6; + topPlatform.Size[1] = 4; + topPlatform.Elements[f2(1,1,topPlatform.Size[1])] = -topInterval / 2; + topPlatform.Elements[f2(1,2,topPlatform.Size[1])] = -topRadius; + topPlatform.Elements[f2(1,3,topPlatform.Size[1])] = 0; + topPlatform.Elements[f2(1,4,topPlatform.Size[1])] = 1; + topPlatform.Elements[f2(2,1,topPlatform.Size[1])] = topInterval / 2; + topPlatform.Elements[f2(2,2,topPlatform.Size[1])] = -topRadius; + topPlatform.Elements[f2(2,3,topPlatform.Size[1])] = 0; + topPlatform.Elements[f2(2,4,topPlatform.Size[1])] = 1; + + Rc.Size[0] = 4; + Rc.Size[1] = 4; + Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(120); + Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(120); + Rc.Elements[f2(1,3,Rc.Size[1])] = 0; + Rc.Elements[f2(1,4,Rc.Size[1])] = 0; + Rc.Elements[f2(2,1,Rc.Size[1])] = sind(120); + Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(120); + Rc.Elements[f2(2,3,Rc.Size[1])] = 0; + Rc.Elements[f2(2,4,Rc.Size[1])] = 0; + Rc.Elements[f2(3,1,Rc.Size[1])] = 0; + Rc.Elements[f2(3,2,Rc.Size[1])] = 0; + Rc.Elements[f2(3,3,Rc.Size[1])] = 1; + Rc.Elements[f2(3,4,Rc.Size[1])] = 0; + Rc.Elements[f2(4,1,Rc.Size[1])] = 0; + Rc.Elements[f2(4,2,Rc.Size[1])] = 0; + Rc.Elements[f2(4,3,Rc.Size[1])] = 0; + Rc.Elements[f2(4,4,Rc.Size[1])] = 1; + + MatrixSub(&topPlatform, 1, 1, 1, 4, &temp1); //等效于topPlatform(3,:) = (Rc * topPlatform(1, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&topPlatform, 3, 1, &temp2); + MatrixSub(&topPlatform, 2, 1, 2, 4, &temp1); //等效于topPlatform(4,:) = (Rc * topPlatform(2, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&topPlatform, 4, 1, &temp2); + MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&topPlatform, 5, 1, &temp2); + MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&topPlatform, 6, 1, &temp2); + + x = Platform->x; + y = Platform->y; + z = Platform->z; + a = Platform->a; + b = Platform->b; + c = Platform->c; + Inverse(x, y, z, a, b, c, &topPlatform, &temp1); //计算出动平台参考点的实际位置 + MatrixFill(&topPlatform, 1, 1, &temp1); + + bottomPlatform.Size[0] = 6; + bottomPlatform.Size[1] = 4; + bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])] = -bottomInterval / 2; + bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])] = -bottomRadius; + bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])] = 0; + bottomPlatform.Elements[f2(1,4,bottomPlatform.Size[1])] = 1; + bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])] = bottomInterval / 2; + bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])] = -bottomRadius; + bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])] = 0; + bottomPlatform.Elements[f2(2,4,bottomPlatform.Size[1])] = 1; + + MatrixSub(&bottomPlatform, 1, 1, 1, 4, &temp1); //等效于bottomPlatform(3,:) = (Rc * bottomPlatform(1, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&bottomPlatform, 3, 1, &temp2); + MatrixSub(&bottomPlatform, 2, 1, 2, 4, &temp1); //等效于bottomPlatform(4,:) = (Rc * bottomPlatform(2, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&bottomPlatform, 4, 1, &temp2); + MatrixSub(&bottomPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(5,:) = (Rc * bottomPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&bottomPlatform, 5, 1, &temp2); + MatrixSub(&bottomPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * bottomPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + MatrixFill(&bottomPlatform, 6, 1, &temp2); + + Distance2Point(&topPlatform, &bottomPlatform, &lengthOfBar1); + + Platform->BarLength[0] = lengthOfBar1.Elements[f2(1,1,lengthOfBar1.Size[1])]; //赋值计算出的杆长 + Platform->BarLength[1] = lengthOfBar1.Elements[f2(2,1,lengthOfBar1.Size[1])]; + Platform->BarLength[2] = lengthOfBar1.Elements[f2(3,1,lengthOfBar1.Size[1])]; + Platform->BarLength[3] = lengthOfBar1.Elements[f2(4,1,lengthOfBar1.Size[1])]; + Platform->BarLength[4] = lengthOfBar1.Elements[f2(5,1,lengthOfBar1.Size[1])]; + Platform->BarLength[5] = lengthOfBar1.Elements[f2(6,1,lengthOfBar1.Size[1])]; + + //计算角度 + r = sqrt(lengthOfCardan * lengthOfCardan + lengthOfSteelWheel * lengthOfSteelWheel); + l = lengthOfBar; + //点1 + xx = topPlatform.Elements[f2(1,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; + yy = topPlatform.Elements[f2(1,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; + zz = topPlatform.Elements[f2(1,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; + AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); + BB = 2 * r * xx / (2 * r * zz); + CC = BB * BB + 1; + DD = 2 * AA * BB; + EE = AA * AA - 1; + delta = DD * DD - 4 * CC * EE; + mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); + mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); + d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); + l1 = sqrt(d + e + f); + d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); + l2 = sqrt(d + e + f); + d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); + l3 = sqrt(d + e + f); + d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); + l4 = sqrt(d + e + f); + mytheta1 = mytheta1 / 3.1415926 * 180; + mytheta2 = mytheta2 / 3.1415926 * 180; + if(abs(l1 - l) <= 0.0001) + { + theta1 = mytheta1; + } + else if(abs(l2 - l) <= 0.0001) + { + theta1 = -mytheta1; + } + else if(abs(l3 - l) <= 0.0001) + { + theta1 = mytheta2; + } + else + { + theta1 = -mytheta2; + } + //点2 + xx = topPlatform.Elements[f2(2,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; + yy = topPlatform.Elements[f2(2,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; + zz = topPlatform.Elements[f2(2,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; + AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); + BB = -2 * r * xx / (2 * r * zz); + CC = BB * BB + 1; + DD = 2 * AA * BB; + EE = AA * AA - 1; + delta = DD * DD - 4 * CC * EE; + mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); + mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); + d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); + l1 = sqrt(d + e + f); + d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); + l2 = sqrt(d + e + f); + d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); + l3 = sqrt(d + e + f); + d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); + l4 = sqrt(d + e + f); + mytheta1 = mytheta1 / 3.1415926 * 180; + mytheta2 = mytheta2 / 3.1415926 * 180; + if(abs(l1 - l) <= 0.0001) + { + theta2 = mytheta1; + } + else if(abs(l2 - l) <= 0.0001) + { + theta2 = -mytheta1; + } + else if(abs(l4 - l) <= 0.0001) + { + theta2 = mytheta2; + } + else + { + theta2 = -mytheta2; + } + //点3 + MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixDot(&Rc, &temp1, &temp2); + MatrixTransposition(&temp2, &temp1); + xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; + yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; + zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; + AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); + BB = 2 * r * xx / (2 * r * zz); + CC = BB * BB + 1; + DD = 2 * AA * BB; + EE = AA * AA - 1; + delta = DD * DD - 4 * CC * EE; + mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); + mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); + d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); + l1 = sqrt(d + e + f); + d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); + l2 = sqrt(d + e + f); + d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); + l3 = sqrt(d + e + f); + d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); + l4 = sqrt(d + e + f); + mytheta1 = mytheta1 / 3.1415926 * 180; + mytheta2 = mytheta2 / 3.1415926 * 180; + if(abs(l1 - l) <= 0.0001) + { + theta3 = mytheta1; + } + else if(abs(l2 - l) <= 0.0001) + { + theta3 = -mytheta1; + } + else if(abs(l4 - l) <= 0.0001) + { + theta3 = mytheta2; + } + else + { + theta3 = -mytheta2; + } + //点4 + MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixDot(&Rc, &temp1, &temp2); + MatrixTransposition(&temp2, &temp1); + xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; + yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; + zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; + AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); + BB = -2 * r * xx / (2 * r * zz); + CC = BB * BB + 1; + DD = 2 * AA * BB; + EE = AA * AA - 1; + delta = DD * DD - 4 * CC * EE; + mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); + mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); + d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); + l1 = sqrt(d + e + f); + d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); + l2 = sqrt(d + e + f); + d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); + l3 = sqrt(d + e + f); + d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); + l4 = sqrt(d + e + f); + mytheta1 = mytheta1 / 3.1415926 * 180; + mytheta2 = mytheta2 / 3.1415926 * 180; + if(abs(l1 - l) <= 0.0001) + { + theta4 = mytheta1; + } + else if(abs(l2 - l) <= 0.0001) + { + theta4 = -mytheta1; + } + else if(abs(l4 - l) <= 0.0001) + { + theta4 = mytheta2; + } + else + { + theta4 = -mytheta2; + } + //点5 + MatrixSub(&topPlatform, 5, 1, 5, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; + yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; + zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; + AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); + BB = 2 * r * xx / (2 * r * zz); + CC = BB * BB + 1; + DD = 2 * AA * BB; + EE = AA * AA - 1; + delta = DD * DD - 4 * CC * EE; + mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); + mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); + d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); + l1 = sqrt(d + e + f); + d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); + l2 = sqrt(d + e + f); + d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); + l3 = sqrt(d + e + f); + d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); + l4 = sqrt(d + e + f); + mytheta1 = mytheta1 / 3.1415926 * 180; + mytheta2 = mytheta2 / 3.1415926 * 180; + if(abs(l1 - l) <= 0.0001) + { + theta5 = mytheta1; + } + else if(abs(l2 - l) <= 0.0001) + { + theta5 = -mytheta1; + } + else if(abs(l4 - l) <= 0.0001) + { + theta5 = mytheta2; + } + else + { + theta5 = -mytheta2; + } + //点6 + MatrixSub(&topPlatform, 6, 1, 6, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; + MatrixTransposition(&temp1, &temp2); + MatrixDot(&Rc, &temp2, &temp1); + MatrixTransposition(&temp1, &temp2); + xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; + yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; + zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; + AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); + BB = -2 * r * xx / (2 * r * zz); + CC = BB * BB + 1; + DD = 2 * AA * BB; + EE = AA * AA - 1; + delta = DD * DD - 4 * CC * EE; + mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); + mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); + d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); + l1 = sqrt(d + e + f); + d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); + e = yy * yy; + f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); + l2 = sqrt(d + e + f); + d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); + l3 = sqrt(d + e + f); + d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); + e = yy * yy; + f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); + l4 = sqrt(d + e + f); + mytheta1 = mytheta1 / 3.1415926 * 180; + mytheta2 = mytheta2 / 3.1415926 * 180; + if(abs(l1 - l) <= 0.0001) + { + theta6 = mytheta1; + } + else if(abs(l2 - l) <= 0.0001) + { + theta6 = -mytheta1; + } + else if(abs(l4 - l) <= 0.0001) + { + theta6 = mytheta2; + } + else + { + theta6 = -mytheta2; + } + Platform->theta[0] = theta1; + Platform->theta[1] = theta2; + Platform->theta[2] = theta3; + Platform->theta[3] = theta4; + Platform->theta[4] = theta5; + Platform->theta[5] = theta6; + theta0 = atan(lengthOfCardan / lengthOfSteelWheel) / 3.1415926 * 180; + Platform->theta_servo[0] = theta1 - theta0; + Platform->theta_servo[1] = theta2 - theta0; + Platform->theta_servo[2] = theta3 - theta0; + Platform->theta_servo[3] = theta4 - theta0; + Platform->theta_servo[4] = theta5 - theta0; + Platform->theta_servo[5] = theta6 - theta0; +} + + + +//******************************************** +//功能:角度制的三角函数 余弦 +//输入参数:angle:角度 +//输出参数:无 +//返回值:三角函数值 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +float cosd(float angle) +{ + return cos(angle/180*3.1415926); +} + +//******************************************** +//功能:角度制的三角函数 正弦 +//输入参数:angle:角度 +//输出参数:无 +//返回值:三角函数值 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +float sind(float angle) +{ + return sin(angle/180*3.1415926); +} + + +//******************************************** +//功能:在命令行打印矩阵 +//输入参数:A:要打印的矩阵 +//输出参数:无 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void PrintMatrix(MatrixType* A) +{ + int i, j; + int pos2 = 0; + printf("\r\nThe Matrix is:\r\n"); + for(i = 1; i <= A->Size[0]; i++) + { + for(j = 1; j <= A->Size[1]; j++) + { + pos2 = f2(i,j,A->Size[1]); + printf(" %f ", A->Elements[pos2]); + } + printf("\r\n"); + } +} +
diff -r 000000000000 -r 2b80f11eb1d3 StewartPlatform.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/StewartPlatform.h Wed Oct 11 07:05:25 2017 +0000 @@ -0,0 +1,151 @@ +#ifndef __STEWARTPLATFORM_H +#define __STEWARTPLATFORM_H + +#include <stdio.h> +#include <stdlib.h> +#include <math.h> + +#define f1(i) (i-1) +/* 把习惯的一阶矩阵的下标转化为C语言数组下标*/ + +#define f2(i,j,n) ((i-1)*(n)+j-1) +/* 把习惯的二阶矩阵的下标转化为C语言数组下标*/ + +//******************************************** +//矩阵数据结构体 +//******************************************** +typedef struct +{ + float Elements[50]; //矩阵元素得存储空间 + int Size[2]; //矩阵的行列数 +} MatrixType; //最大支持有50个元素的矩阵 + +//******************************************** +//动感平台数据结构体 +//******************************************** +typedef struct +{ + float topRadius; //平台结构尺寸参数 + float topInterval; + float bottomRadius; + float bottomInterval; + float lengthOfSteelWheel; + float lengthOfCardan; + float lengthOfBar; + float x; //上平台姿态参数 + float y; + float z; + float a; + float b; + float c; + float theta[6]; //角度 + float theta_servo[6]; //舵机角度 + float BarLength[6]; //上下平面对应的顶点之间的距离 + +} StewartPlatformType; + +//******************************************** +//功能:计算矩阵乘法 C=A*B +//输入参数:A、B:参加运算的矩阵 +//输出参数:C:运算结果 +//返回值:计算是否成功 成功返回0 否则返回1 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C); + +//******************************************** +//功能:计算矩阵转置 +//输入参数:A:被转置的矩阵 +//输出参数:B:转置后的矩阵 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void MatrixTransposition(MatrixType* A, MatrixType* B); + +//******************************************** +//功能:获得矩阵的子阵 +//输入参数:A:原矩阵 StartRow、StartColumn、EndRow、EndColumn:子阵起始元素 子阵终了元素 +//输出参数:B:子阵 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B); + +//******************************************** +//功能:填充矩阵 将一个矩阵填充到另一个矩阵中 +//输入参数:A:被填充的矩阵 Row、Column:矩阵填充的位置 B:要填充到被填充矩阵的矩阵 +//输出参数:A:被填充的矩阵 +//返回值:0 代表成功 1代表失败 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B); + +//******************************************** +//功能:指定动平台变换矩阵参数x,y,z,a,b,c,计算动平台上的点A在绝对坐标系下的坐标B A可以是多个点 一行一个点 +//输入参数:x,y,z,a,b,c:动平台变换矩阵参数 A:动平台上点的相对坐标 +//输出参数:B:点在绝对坐标系下的坐标 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B); + +//******************************************** +//功能:计算矩阵行向量所表示的坐标点之间的距离 +//输入参数:A, B:要计算距离的矩阵 A,B必须均为n*4的矩阵,维度相同 +//输出参数:C:包含距离值信息的列向量 +//返回值:0:计算成功 1:出错 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +int Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C); + + +//******************************************** +//功能:解析动感平台 +//输入参数:Platform:动感平台数据结构 包含各种输入输出 +//输出参数:无 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void CalStewartPlatform(StewartPlatformType* Platform); + + +//******************************************** +//功能:角度制的三角函数 余弦 +//输入参数:angle:角度 +//输出参数:无 +//返回值:三角函数值 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +float cosd(float angle); + +//******************************************** +//功能:角度制的三角函数 正弦 +//输入参数:angle:角度 +//输出参数:无 +//返回值:三角函数值 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +float sind(float angle); + + +//******************************************** +//功能:在命令行打印矩阵 +//输入参数:A:要打印的矩阵 +//输出参数:无 +//返回值:无 +//调用外部函数:无 +//作者:陈欢 h-che14@mails.tsinghua.edu.cn +//******************************************** +void PrintMatrix(MatrixType* A); + + +#endif
diff -r 000000000000 -r 2b80f11eb1d3 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 11 07:05:25 2017 +0000 @@ -0,0 +1,38 @@ +#include "mbed.h" +#include <stdio.h> +#include <stdlib.h> +#include <math.h> +#include "StewartPlatform.h" +DigitalOut myled(LED1); +Serial pc(SERIAL_TX, SERIAL_RX); +StewartPlatformType Platform; //动感平台数据结构体 + +int main() +{ + while(1) + { + myled = 1; // LED is ON + wait(0.2); // 200 ms + myled = 0; // LED is OFF + wait(1.0); // 1 sec + + + Platform.topRadius = 244.95; //平台参数初始化 + Platform.topInterval = 100; + Platform.bottomRadius = 332.54; + Platform.bottomInterval = 340; + Platform.lengthOfSteelWheel = 150; + Platform.lengthOfCardan = 150; + Platform.lengthOfBar = 368; + Platform.x = 25; //设定上平台姿态 + Platform.y = 31; + Platform.z = 278; + Platform.a = 15; + Platform.b = 12; + Platform.c = 22; + CalStewartPlatform(&Platform); //解析平台数据 + pc.printf("Angle: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.theta[0], Platform.theta[1], Platform.theta[2], Platform.theta[3], Platform.theta[4], Platform.theta[5]); + pc.printf("Servo Angle: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.theta_servo[0], Platform.theta_servo[1], Platform.theta_servo[2], Platform.theta_servo[3], Platform.theta_servo[4], Platform.theta_servo[5]); + pc.printf("BarLength: %.2f %.2f %.2f %.2f %.2f %.2f \r\n", Platform.BarLength[0], Platform.BarLength[1], Platform.BarLength[2], Platform.BarLength[3], Platform.BarLength[4], Platform.BarLength[5]); + } +}
diff -r 000000000000 -r 2b80f11eb1d3 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Oct 11 07:05:25 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/235179ab3f27 \ No newline at end of file