技術標籤：ROS下UR機器人的軌跡規劃

0.前言

我今早陷入了沉思，我想不起自己為啥要寫一個逆運算包括今天這個笛卡爾空間位置和姿態插值的程式碼了，我在VS2019裡進行模擬也不現實，我想用matlab模擬那為啥不用
matlab語言寫，更何況我的畢設是在ros平臺下模擬的！
但是我後來頓悟了，我是為了瞭解笛卡爾空間軌跡規劃過程中的插值原理，尤其是四元數用於姿態的插值！我最終探討的問題是：
如何實驗笛卡爾空間下直線和圓弧的軌跡規劃
並且在關節空間保證速度、加速度、加加速度的光滑！
PS：如果你不在乎原理，建議直接開啟ROS 的 catkin_UR功能包進行使用！肯定比我寫的好~

1.摘要

本片文章主要內容是書寫笛卡爾空間的軌跡規劃（PTP），插值部分的程式碼，輸入為Ta，Tb，輸出為T1，T2 、、、T100，中間的插值矩陣，實現直線的插值。

2.數學基礎

2.1旋轉矩陣和四元數之間的換算
公式截圖來自部落格https://blog.csdn.net/M_try/article/details/82900500
（這是一篇旋轉變換表示換算很完整的部落格）

若已知旋轉矩陣R，求四元數[q0 q1 q2 q2]

則對應的四元數為：

這個來自我自己部落格嘻嘻

3程式碼

3.1第一部分是不加插值，只進行旋轉矩陣的四元數換算

//笛卡爾空間PTP軌跡規劃的位置和姿態插值程式碼編寫
#include<iostream>
#include"math.h"
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;
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;//姿態
}
void OuputT(double t[4][4])

{
    
    
    cout << "輸出矩陣T:   " << endl;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            cout << t[i][j] << "      ";
        }
        cout << endl;
    }
}
void OuputQuat(double quat[4])

{
    cout << "四元數quat：" << " [  " << quat[0] << "   " << quat[1] << "   " << quat[2] << "   " << quat[3] << "  ] " << endl;

   
}
void Quat_to_Dcm(double quat[4], double* T)
{
    double q0 = quat[0], q1 = quat[1],q2 = quat[2] ,q3 = quat[3];
    *T = q0*q0+q1*q1-q2*q2-q3*q3; T++;  //T00
    *T = 2*(q1*q2 - q0*q3); T++;  //T01
    *T = 2 * (q1 * q3 + q0 * q2); T++;//T02
    *T = 0; T++;//Px

    *T = 2 * (q1 * q2 +q0 * q3); T++;//ny  T10
    *T = q0 * q0 +-q1 * q1 + q2 * q2 - q3 * q3; T++;//Oy   T11
    *T = 2 * (q2 * q3 - q0 * q1); T++;//ay   T12
    *T = 0; T++;//py


    *T = 2 * (q1 * q3 - q0 * q2); T++;//nz  T20
    *T = 2 * (q3 * q2 + q0 * q1); T++;//oz   T21
    *T = q0 * q0 + -q1 * q1 - q2 * q2 +q3 * q3; T++;//az  T22
    *T = 0; T++;//Pz
    *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;//姿態
}
void Dcm_to_Quat(double T[4][4],double * Quat)
{
    double q0, q1, q2, q3;
    q0 = 0.5 * sqrt(1 + T[0][0] + T[1][1] + T[2][2]);
    q1 = (T[2][1] - T[1][2]) / (4 * q0);
    q2 = (T[0][2] - T[2][0]) / (4 * q0);
    q3 = (T[1][0] - T[0][1]) / (4 * q0);
    *Quat = q0; Quat++;  *Quat = q1; Quat++;  *Quat=q2; Quat++; *Quat = q3;


}
void Ctraj(double T1, double T2, int t)
{
    cout << endl;
}
int main()

{
    double quat[4];//quat為四元數
	double q[6] = { 0.1 ,0.2,0.3,0.4,0.5,0.6 };//q為關節角度
    double t[4][4];
    double* T, * Q,* Quat;
    T = &t[0][0];
    Q = &q[0];
    Quat = &quat[0];
    forward(Q, T);//手打矩陣T過於麻煩，所以我還是用正運動學根據q算出來的
    OuputT(t);//相對於上一篇部落格，這裡我把矩陣和陣列的列印都封裝為函數了
    Dcm_to_Quat(t, Quat);
    OuputQuat(quat);
    Quat_to_Dcm(quat,T);
    OuputT(t);
}

執行結果

輸出由q 進行正運動學結果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
四元數quat： [  0.459866   0.558768   -0.612823   0.317411  ]
輸出四元數計算得到的旋轉矩陣T:
0.0473957      -0.976785      -0.208915      0
-0.392918      0.174058      -0.90295        0
0.918351      0.124882      -0.375547        0
0      0      0      1

可以看出來位置矩陣我還沒加上 Px，Py，Pz位置是0，下邊插值演算法中會加上的哈~
3.2完整版插值程式碼

3.3matlab的模擬結果驗證