1.摘要
文章的主要内容是,编写C++代码，实现六轴机械臂的正运动学运算（输入为关节角度，输出为T6 ）和逆运动学求解（输入为T6，输出为关节角度），这个代码是很基础的，可能十几年前网上就有成熟的了，当然你也可以用我下边粘贴的代码。
本文数学公式截图来自论文
[1]张付祥,赵阳.UR5机器人运动学及奇异性分析[J].河北科技大学学报,2019,40(01):51-59.
2.DH参数表
DH参数表基于ur10机器人
3.正运动学
①数学公式
②
#include<iostream>
using namespace std;
const double d1 = 0.1273;
const double a2 = 0.612;
const double a3 = 0.5723;
const double d4 = 0.163941;
const double d5 = 0.1157;
const double d6 = 0.0922;
const double ZERO_THRESH = 0.00000001;
int SIGN(double x)
{
return (x > 0) - (x < 0);
}
const double PI = 3.1415926;
void forward(const double_ q, double_ T) {
double s1 = sin(_q), c1 = cos(_q); q++;
double q23 = _q, q234 = _q, s2 = sin(_q), c2 = cos(_q); q++;
double s3 = sin(_q), c3 = cos(_q); q23 += _q; q234 += _q; q++;
double s4 = sin(_q), c4 = cos(_q); q234 += _q; q++;
double s5 = sin(_q), c5 = cos(_q); q++;
double s6 = sin(_q), c6 = cos(_q);
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
_T = c6 _ (s1 _ s5 + c234 _ c1 _ c5) - s234 _ c1 _ s6; T++; //nx
_T = -s6 _ (s1 _ s5 + c234 _ c1 _ c5) - s234 _ c1 _ c6; T++; //Ox
_T =-( c234 _ c1 _ s5 - c5 _ s1); T++;//ax
_T =- (d6 _ c234 _ c1 _ s5 - a3 _ c23 _ c1 - a2 _ c1 _ c2 - d6 _ c5 _ s1 - d5 _ s234 _ c1 - d4 _ s1); T++;//Px
_T = -c6 _ (c1 _ s5 - c234 _ c5 _ s1) - s234 _ s1 _ s6; T++;//ny
_T = s6 _ (c1 _ s5 - c234 _ c5 _ s1) - s234 _ c6 _ s1; T++;//Oy
_T = -(c1 _ c5 + c234 _ s1 _ s5); T++;//ay
_T = -(d6 _ (c1 _ c5 + c234 _ s1 _ s5) + d4 _ c1 - a3 _ c23 _ s1 - a2 _ c2 _ s1 - d5 _ s234 _ s1); T++;//py
_T = -(-c234 _ s6 - s234 _ c5 _ c6); T++;//nz
_T = -(s234 _ c5 _ s6 - c234 _ c6); T++;//oz
_T = -s234 _ s5; T++;//az
_T = d1 + a3 _ s23 + a2 _ s2 - d5 _ (c23 _ c4 - s23 _ s4) - d6 _ s5 _ (c23 _ s4 + s23 _ c4); T++;//Pz
_T = 0.0; T++; _T = 0.0; T++; _T = 0.0; T++; _T = 1.0;//姿态
}
int main()
{
double Q[6] = { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 };
double t[4][4];
double _ T,_q;
q = &Q[0];
T = &t[0][0];
forward(q, T);
//输出q
cout << “关节角度q: “ << “[“ ;
for (int i = 0; i < 6; i++) {cout << Q[i]<<“ “;}
cout << “]”<<endl;
//输出JZ
cout << “矩阵T: “ <<endl ;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout <<t[i][j]<<“ “; }
cout << endl;
}
return 0;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
运行结果：
关节角度q: [0.1 0.2 0.3 0.4 0.5 0.6 ]
矩阵T:
0.0473957 -0.976785 -0.208915 1.18382
-0.392918 0.174058 -0.90295 -0.127305
0.918351 0.124882 -0.375547 0.416715
0 0 0 1
- 1
- 2
- 3
- 4
- 5
- 6
4.逆运动学
这个我应该码出来的，但是我家好冷，我的手已经僵了，我现在感觉我敲机械键盘像是敲缝纫机，所以我明天会将这个文章更新（PS：我写的逆运动学代码，肯定会比matlab自带的好用哦！）
好久没更新的，我今天上号我发现我多了几个粉丝！啊我好激动，我的动力来了！！！！
以下为逆运动学计算代码
①数学公式
接下求第2、3、4关节的时候有点复杂了，你可要看好了
#include<iostream>
using namespace std;
const double d1 = 0.1273;
const double a2 = 0.612;
const double a3 = 0.5723;
const double d4 = 0.163941;
const double d5 = 0.1157;
const double d6 = 0.0922;
const double ZERO_THRESH = 0.00000001;
int SIGN(double x)
{
return (x > 0) - (x < 0);
}
const double PI = 3.1415926;
void forward(const double_ q, double_ T) {
double s1 = sin(_q), c1 = cos(_q); q++;
double q23 = _q, q234 = _q, s2 = sin(_q), c2 = cos(_q); q++;
double s3 = sin(_q), c3 = cos(_q); q23 += _q; q234 += _q; q++;
double s4 = sin(_q), c4 = cos(_q); q234 += _q; q++;
double s5 = sin(_q), c5 = cos(_q); q++;
double s6 = sin(_q), c6 = cos(_q);
double s23 = sin(q23), c23 = cos(q23);
double s234 = sin(q234), c234 = cos(q234);
_T = c6 _ (s1 _ s5 + c234 _ c1 _ c5) - s234 _ c1 _ s6; T++; //nx
_T = -s6 _ (s1 _ s5 + c234 _ c1 _ c5) - s234 _ c1 _ c6; T++; //Ox
_T =-( c234 _ c1 _ s5 - c5 _ s1); T++;//ax
_T =- (d6 _ c234 _ c1 _ s5 - a3 _ c23 _ c1 - a2 _ c1 _ c2 - d6 _ c5 _ s1 - d5 _ s234 _ c1 - d4 _ s1); T++;//Px
_T = -c6 _ (c1 _ s5 - c234 _ c5 _ s1) - s234 _ s1 _ s6; T++;//ny
_T = s6 _ (c1 _ s5 - c234 _ c5 _ s1) - s234 _ c6 _ s1; T++;//Oy
_T = -(c1 _ c5 + c234 _ s1 _ s5); T++;//ay
_T = -(d6 _ (c1 _ c5 + c234 _ s1 _ s5) + d4 _ c1 - a3 _ c23 _ s1 - a2 _ c2 _ s1 - d5 _ s234 _ s1); T++;//py
_T = -(-c234 _ s6 - s234 _ c5 _ c6); T++;//nz
_T = -(s234 _ c5 _ s6 - c234 _ c6); T++;//oz
_T = -s234 _ s5; T++;//az
_T = d1 + a3 _ s23 + a2 _ s2 - d5 _ (c23 _ c4 - s23 _ s4) - d6 _ s5 _ (c23 _ s4 + s23 _ c4); T++;//Pz
_T = 0.0; T++; _T = 0.0; T++; _T = 0.0; T++; _T = 1.0;//姿态
}
int inverse(const double_ T, double_ q_sols,double _ q) {
int num_sols = 0;
double T00 = _T; T++; double T01 = _T; T++; double T02 = _T; T++; double T03 = _T; T++;
double T10 = _T; T++; double T11 = _T; T++; double T12 = _T; T++; double T13 = _T; T++;
double T20 = _T; T++; double T21 = _T; T++; double T22 = _T; T++; double T23 = _T;
double qq1, qq2, qq3, qq4, qq5, qq6;
double q1[2];
{
double A = d6 _ T12 - T13;
double B = d6 _ T02 - T03;
double R = A _ A + B _ B;
if (fabs(A) < ZERO_THRESH) {
double div;
if (fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
div = -SIGN(d4) _ SIGN(B);
else
div = -d4 / B;
double arcsin = asin(div);
if (fabs(arcsin) < ZERO_THRESH)
arcsin = 0.0;
if (arcsin < 0.0)
q1[0] = arcsin + 2.0 _ PI;
else
q1[0] = arcsin;
q1[1] = PI - arcsin;
}
else if (fabs(B) < ZERO_THRESH) {
double div;
if (fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
div = SIGN(d4) _ SIGN(A);
else
div = d4 / A;
double arccos = acos(div);
q1[0] = arccos;
q1[1] = 2.0 _ PI - arccos;
}
else if (d4 _ d4 > R) {
return num_sols;
}
else {
double arccos = acos(d4 / sqrt(R));
double arctan = atan2(-B, A);
double pos = arccos + arctan;
double neg = -arccos + arctan;
if (fabs(pos) < ZERO_THRESH)
pos = 0.0;
if (fabs(neg) < ZERO_THRESH)
neg = 0.0;
if (pos >= 0.0)
q1[0] = pos;
else
q1[0] = 2.0 _ PI + pos;
if (neg >= 0.0)
q1[1] = neg;
else
q1[1] = 2.0 _ PI + neg;
}
qq1 = q1[1];
}
double q5[2][2];
{
for (int i = 0; i < 2; i++) {
double numer = (T03 _ sin(q1[i]) - T13 _ cos(q1[i]) - d4);
double div;
if (fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
div = SIGN(numer) _ SIGN(d6);
else
div = numer / d6;
double arccos = acos(div);
q5[i][0] = arccos;
q5[i][1] = 2.0 _ PI - arccos;
}
qq5 = q5[1][0];
}
{
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 2; j++) {
double c1 = cos(q1[i]), s1 = sin(q1[i]);
double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
double q6;
if (fabs(s5) < ZERO_THRESH)
q6 = 0;
else {
q6 = atan2(SIGN(s5) _ -(T01 _ s1 - T11 _ c1),
SIGN(s5) _ (T00 _ s1 - T10 _ c1));
if (fabs(q6) < ZERO_THRESH)
q6 = 0.0;
if (q6 < 0.0)
q6 += 2.0 _ PI;
}
double q2[2], q3[2], q4[2];
double c6 = cos(q6), s6 = sin(q6);
double x04x = -s5 _ (T02 _ c1 + T12 _ s1) - c5 _ (s6 _ (T01 _ c1 + T11 _ s1) - c6 _ (T00 _ c1 + T10 _ s1));
double x04y = c5 _ (T20 _ c6 - T21 _ s6) - T22 _ s5;
double p13x = d5 _ (s6 _ (T00 _ c1 + T10 _ s1) + c6 _ (T01 _ c1 + T11 _ s1)) - d6 _ (T02 _ c1 + T12 _ s1) +
T03 _ c1 + T13 _ s1;
double p13y = T23 - d1 - d6 _ T22 + d5 _ (T21 _ c6 + T20 _ s6);
double c3 = (p13x _ p13x + p13y _ p13y - a2 _ a2 - a3 _ a3) / (2.0 _ a2 _ a3);
if (fabs(fabs(c3) - 1.0) < ZERO_THRESH)
c3 = SIGN(c3);
else if (fabs(c3) > 1.0) {
// TODO NO SOLUTION
continue;
}
double arccos = acos(c3);
q3[0] = arccos;
q3[1] = 2.0 _ PI - arccos;
double denom = a2 _ a2 + a3 _ a3 + 2 _ a2 _ a3 _ c3;
double s3 = sin(arccos);
double A = (a2 + a3 _ c3), B = a3 _ s3;
q2[0] = atan2((A _ p13y - B _ p13x) / denom, (A _ p13x + B _ p13y) / denom);
q2[1] = atan2((A _ p13y + B _ p13x) / denom, (A _ p13x - B _ p13y) / denom);
double c23_0 = cos(q2[0] + q3[0]);
double s23_0 = sin(q2[0] + q3[0]);
double c23_1 = cos(q2[1] + q3[1]);
double s23_1 = sin(q2[1] + q3[1]);
q4[0] = atan2(c23_0 _ x04y - s23_0 _ x04x, x04x _ c23_0 + x04y _ s23_0);
q4[1] = atan2(c23_1 _ x04y - s23_1 _ x04x, x04x _ c23_1 + x04y _ s23_1);
qq6 = q6;
qq2 = q2[0];
qq3 = q3[0];
qq4 = q4[0];
num_sols++;
}
}
}
}
_q = qq1; q++; _q = qq2; q++; _q = qq3; q++; _q = qq4; q++; _q = qq5; q++; _q = qq6;
return num_sols;
}
int main()
{
double Q[6] = { 1, 0.2, 0.3, 0.4, 0.5, 0.8 };
double t[4][4];
double Qs[6] = { 0, 0, 0, 0, 0, 0 };
double _ T,_q,_qq;
q = &Q[0];
T = &t[0][0];
qq = &Qs[0];
double q_sols[48];
int num_sols;
//逆运动学
forward(q, T);
//输出初始的关节角度q
cout << “输出初始的关节角度q: “ << “[“ ;
for (int i = 0; i < 6; i++) {cout << Q[i]<<“ “;}
cout << “]”<<endl;
//输出由q 进行正运动学结果T
cout << “输出由q 进行正运动学结果T: “ <<endl ;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout <<t[i][j]<<“ “;
}
cout << endl;
}
num_sols = inverse(T, q_sols, qq);
//输出由T输入 进行逆运动学得到的结果qq
cout << “输出由T输入 进行逆运动学得到的结果qq: “ << “[“;
for (int i = 0; i < 6; i++) { cout << Qs[i] << “ “; }
cout << “]” << endl;
return 0;
}
结果：
输出初始的关节角度q: [1 0.2 0.3 0.4 0.5 0.8 ]
输出由q 进行正运动学结果T:
0.182807 -0.795703 0.577442 0.835595
-0.333502 -0.602703 -0.724931 0.848183
0.924856 -0.0600552 -0.375547 0.416715
0 0 0 1
输出由T输入 进行逆运动学得到的结果qq: [1 0.2 0.3 0.4 0.5 0.8 ]
- >
5预告
下一篇应该在明天或者后天：关于插值的c++代码/c++中姿态的图像表述
（先后顺序还没想好）
这篇文写了两天，所以中间有很多碎碎叨叨的话，我就不删了哦，代码运行有问题可以私戳我
我写的好认真，然后我发现有粉丝好开心，所以我就过分的设置成仅粉丝可见了，原谅我的虚荣，我的指针用的还不太熟，所以代码不够高级，请见谅~
评论（0）
您还未登录，请登录后发表或查看评论