6軸機器人DH建模线梗、仿真、正逆解代碼

一.模型

二.MDH模型

使用修改DH模型怠益、改進DH模型與標(biāo)準(zhǔn)DH模型主要區(qū)別在于末端仪搔，改進DH并沒有建到工具坐標(biāo)系，這里將4蜻牢、5僻造、6軸的坐標(biāo)系都建在同一點，因此機器人連桿的參數(shù)只需要4個值a1孩饼、a2髓削、a3、d4镀娶。a2立膛、d4分別代表大臂與小臂的長度。要完全得到末端或者工具尖點的位姿還需要再右成一個工具坐標(biāo)系6Te

  • DH圖


    1604040111741.jpg

  • DH參數(shù)表

θ d a α
0 0 0 0
0 0 109 -90
0 0 250.5 0
0 239.5 115.5 -90
0 0 0 90
0 0 0 -90

  • MATLAB仿真效果


    image.png

  • MATLAB代碼

clear;
clc;
%建立機器人模型
%       theta    d           a        alpha     offset
ML1=Link([0       0           0           0        0    ],'modified'); 
ML2=Link([0       0           109       -pi/2      0    ],'modified');
ML3=Link([0       0           250.5       0        0    ],'modified');
ML4=Link([0       239.5       115.5     -pi/2      0    ],'modified');
ML5=Link([0       0           0          pi/2      0    ],'modified');
ML6=Link([0       0           0         -pi/2      0    ],'modified');
modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','6axis');
teach(modrobot)
  • 一般6軸機器人初始狀態(tài)為大臂垂直與地面梯码，主要是通過給2軸一個初始角度-90°來實現(xiàn)宝泵，下圖第一個模型為各軸0°時的姿態(tài)，第二圖模型為一般回零后機器人保持的姿態(tài)轩娶。
    1

    2

三.6軸機器人運動學(xué)正解

  • 代碼
#include <Eigen/Dense>
/* 正解函數(shù)
* @joint 6軸各關(guān)節(jié)值結(jié)構(gòu)體儿奶，單位弧度
* @return 返回一個4*4的位姿矩陣
*/
Eigen::Matrix4d IFKinematics::fKine(const Joint & joint) 
{
    double j1 = joint.j1,
           j2 = joint.j2,
           j3 = joint.j3,
           j4 = joint.j4,
           j5 = joint.j5,
           j6 = joint.j6;

    double s1 = sin(j1), c1 = cos(j1),
           s2 = sin(j2), c2 = cos(j2),
           s3 = sin(j3), c3 = cos(j3),
           s4 = sin(j4), c4 = cos(j4),
           s5 = sin(j5), c5 = cos(j5),
           s6 = sin(j6), c6 = cos(j6);

    Eigen::Matrix4d T01, T12, T23, T34, T45, T56, T0_6;
    T01 << c1,    0, -s1, a1 * c1,
           s1,    0,  c1, a1 * s1,
            0,   -1,   0,       0,
            0,    0,   0,       1;
    T12 << c2,  -s2,   0, a2 * c2,
           s2,   c2,   0, a2 * s2,
            0,    0,   1,       0,
            0,    0,   0,       1;
    T23 << c3,    0, -s3, a3 * c3,
           s3,    0,  c3, a3 * s3,
            0,   -1,   0,       0,
            0,    0,   0,       1;
    T34 << c4,    0,  s4,       0,
           s4,    0, -c4,       0,
            0,    1,   0,      d4,
            0,    0,   0,       1;
    T45 << c5,    0, -s5,       0,
           s5,    0,  c5,       0,
            0,   -1,   0,       0,
            0,    0,   0,       1;
    T56 << c6,  -s6,   0,       0,
           s6,   c6,   0,       0,
            0,    0,   1,       0,
            0,    0,   0,       1;
    T0_6 = T01 * T12 * T23 * T34 * T45 * T56;
    return T0_6;
}

四.6軸機器人運動學(xué)逆解

  • 代碼
#include <Eigen/Dense>
#include <math.h>

#define EPSINON 1e-4  // 精度，小于它就認(rèn)為是0
#define ALLOW_MAX_DIFF (1 / 180 * pi)  /* 距離上一軸角度的最大允許偏差 */

/* 各軸運動角度范圍 */
const double MAX_JOINTS[6] = { 90, 30, 90, 135, 135, 180}; 
const double MIN_JOINTS[6] = { -90, -135, -135, -135, -135, -180};
const double pi = 3.1415926

/* 角度值轉(zhuǎn)弧度值 */
inline double toRadian(double degree)
{
    return degree * pi / 180;
}

/* 逆解函數(shù)
* @Joint 6軸各關(guān)節(jié)值結(jié)構(gòu)體鳄抒，單位弧度
* @lastJoints 上一次計算的關(guān)節(jié)值結(jié)構(gòu)體
* @pose 要解逆解的位姿矩陣
*/
Joint IFKinematics::sixJointsIK(Eigen::Matrix4d pose, Joint lastJoints) 
{
    double nx = pose(0, 0), ox = pose(0, 1), ax = pose(0, 2),
           ny = pose(1, 0), oy = pose(1, 1), ay = pose(1, 2),
           nz = pose(2, 0), oz = pose(2, 1), az = pose(2, 2),
           px = pose(0 ,3), py = pose(1, 3), pz = pose(2, 3);

    /** joint 1 */
    double j1[2] = {0, 0};
    j1[0] = -atan2(-py, px);
    j1[1] = -atan2(-py, px) + pi;

    /** joint 3 */
    double j3[4] = {0, 0, 0, 0};
    /* temp, x, y, z, A are temp valiables */
    double temp[2], A[2];
    double x = a2 * d4,
           y = a2 * a3,
           z = a2 * a2 + a3 * a3 + d4 * d4;
    for (int i = 0; i < 4; i+=2) {
        temp[i/2] = a1 - px * cos(j1[i/2]) - py * sin(j1[i/2]);
        A[i/2] = z - pz * pz - temp[i/2] * temp[i/2];
        j3[i]   = -atan2(-y, x) + atan2(A[i/2],sqrt( (4 * x * x + 4 * y * y - A[i/2] * A[i/2])));
        j3[i+1] = -atan2(-y, x) + atan2(A[i/2],-sqrt((4 * x * x + 4 * y * y - A[i/2] * A[i/2])));
    }


    /** joint 2 */
    double j2[4] = {0, 0, 0, 0};
    double m[4]  = {0, 0, 0, 0};
    double n[4]  = {0, 0, 0, 0};
    for (int i = 0; i < 4; ++i) {
        m[i] = a2 + a3 * cos(j3[i]) - d4 * sin(j3[i]);
        n[i] = a3 * sin(j3[i]) + d4 * cos(j3[i]);
        j2[i] = atan2(-pz * m[i] + n[i] * temp[i/2],-pz * n[i] - m[i] * temp[i/2]);
    }


    /** joint 5 */
    double j5[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    for (int i = 0; i < 4; ++i) {
        m[i] = (ax * cos(j1[i / 2]) * cos(j2[i]) + ay * sin(j1[i / 2]) * cos(j2[i]) + az * -1 * sin(j2[i]));
        n[i] = -(ax * cos(j1[i / 2]) * sin(j2[i]) + ay * sin(j1[i / 2]) * sin(j2[i]) - az * -1 * cos(j2[i]));
    }
    for (int i = 0; i < 8; ) {
        x =  n[i/2] * cos(j3[i/2]) - m[i/2] * sin(j3[i/2]);
        y = sqrt(pow((ay * cos(j1[i / 4]) - ax * sin(j1[i / 4])),2) + pow((m[i/2] * cos(j3[i/2]) + n[i/2] * sin(j3[i/2])),2));
        j5[i++] = atan2( y, x);
        j5[i++] = atan2(-y, x);
    }


    /** joint 4 */
    double j4[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    for (int i = 0; i < 8; ++i) {
        if (fabs(sin(j5[i])) >= EPSINON) {
            y = ((ay*cos(j1[i/4])-ax*sin(j1[i/4]))) / sin(j5[i]);
            x= (-m[i/2]*cos(j3[i/2])-n[i/2]*sin(j3[i/2]))/(sin(j5[i]));
            j4[i] = atan2(y , x);
        } else {
            j4[i] = 0;
        }
    }


    /** joint 6 */
    double j6[8] = {0, 0, 0, 0, 0, 0, 0 ,0};
    for (int i = 0; i < 8; ++i) {
        double u = nx * sin(j1[i/4]) - ny * cos(j1[i/4]);
        double v = ox * sin(j1[i/4]) - oy * cos(j1[i/4]);
        j6[i] = atan2(cos(j4[i]) * u - cos(j5[i]) * sin(j4[i]) * v, cos(j4[i]) * v + cos(j5[i]) * sin(j4[i]) * u);
    }

    /** 8組逆解結(jié)果 */
    double ans[8][6];
    for (int i = 0; i < 8; ++i) {
        ans[i][0] = j1[i / 4];
        ans[i][1] = j2[i / 2];
        ans[i][2] = j3[i / 2];
        ans[i][3] = j4[i];
        ans[i][4] = j5[i];
        ans[i][5] = j6[i];
    }

    /** 在選擇8組解中的最優(yōu)解闯捎，主要根據(jù)三個原則：連續(xù)原則、限位原則许溅、最短路徑原則 瓤鼻，連續(xù)原則暫時不使用 */
    double last[6] = {lastJoints.j1, lastJoints.j2, lastJoints.j3, lastJoints.j4, lastJoints.j5, lastJoints.j5};
    bool isOutLimit[8] = {false, false, false, false, false, false, false, false};
    //bool isContinuity[8] = {true, true, true, true, true, true, true, true};

    /** 排除掉超過運動范圍的解 */
    for (int i = 0; i < 8; ++i) {
        for (int j = 0; j < 6; ++j) {
            if (ans[i][j] < toRadian(MIN_JOINTS[j]) || ans[i][j] > toRadian(MAX_JOINTS[j])) {
                isOutLimit[i] = true;
                break;
            }
        }
    }

    /** 運動連續(xù)：排除掉離上一軸角度過大的解，需要設(shè)定最大允許偏差值 ALLOW_MAX_DIFF */
//    for (int i = 0; i < 8; ++i) {
//        for (int j = 0; j < 6; ++j) {
//            if (abs(ans[i][j] - last[j]) > ALLOW_MAX_DIFF) {
//                isContinuity[i] = false;
//                break;
//            }
//        }
//    }

    /** 最短路徑原則：選出距離上一次姿態(tài)各軸所需最小的運動量贤重，給各軸分配一個權(quán)重茬祷，使盡量動小軸或姿態(tài)軸 */
    double weight[6] = {0.2, 0.25, 0.2, 0.15, 0.12, 0.08};
    //double weight[6] = {1, 1, 1, 1, 1, 1};  // 各軸相同權(quán)重
    double sum[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    double minSum = INT_MAX;
    int opt = -1;
    for (int i = 0; i < 8; ++i) {
        if (isOutLimit[i]) {
            //printf("第%d解超限\n", i + 1);
            continue;
        }
        for (int j = 0; j < 6; ++j) {
            sum[i] += fabs(ans[i][j] - last[j]) * weight[j];
        }
        if (sum[i] < minSum) {
            minSum = sum[i];
            opt = i;
        }
    }

    /** 最優(yōu)解 */
    Joint res;
    if (opt >= 0) {
        /// 因為4軸和6軸平行,因此4軸轉(zhuǎn)-x°和6軸轉(zhuǎn)x°效果相同,因此如果遇到這種情況就將他們矯正
        if (-ans[opt][3] - ans[opt][5] <= 1e-2) {
            ans[opt][3] = 0;
            ans[opt][5] = 0;
        }
        res = Joint(ans[opt][0], ans[opt][1], ans[opt][2],
                    ans[opt][3], ans[opt][4], ans[opt][5]);
    } else {
        printf("無合適逆解\n");
        res =  lastJoints;
    }
    return res;
}
最后編輯于 ：
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
  • 人面猴
    序言：七十年代末，一起剝皮案震驚了整個濱河市并蝗，隨后出現(xiàn)的幾起案子祭犯，更是在濱河造成了極大的恐慌，老刑警劉巖滚停，帶你破解...
    沈念sama閱讀 211,817評論 6贊 492
  • 死咒
    序言：濱河連續(xù)發(fā)生了三起死亡事件沃粗，死亡現(xiàn)場離奇詭異，居然都是意外死亡铐刘，警方通過查閱死者的電腦和手機陪每，發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 90,329評論 3贊 385
  • 救了他兩次的神仙讓他今天三更去死
    文/潘曉璐 我一進店門，熙熙樓的掌柜王于貴愁眉苦臉地迎上來，“玉大人檩禾，你說我怎么就攤上這事挂签。” “怎么了盼产？”我有些...
    開封第一講書人閱讀 157,354評論 0贊 348
  • 道士緝兇錄：失蹤的賣姜人
    文/不壞的土叔 我叫張陵饵婆，是天一觀的道長。 經(jīng)常有香客問我戏售，道長侨核，這世上最難降的妖魔是什么？ 我笑而不...
    開封第一講書人閱讀 56,498評論 1贊 284
  • ?港島之戀（遺憾婚禮）
    正文 為了忘掉前任灌灾，我火速辦了婚禮搓译，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘锋喜。我一直安慰自己些己，他們只是感情好，可當(dāng)我...
    茶點故事閱讀 65,600評論 6贊 386
  • 惡毒庶女頂嫁案：這布局不是一般人想出來的
    文/花漫 我一把揭開白布嘿般。 她就那樣靜靜地躺著段标，像睡著了一般。 火紅的嫁衣襯著肌膚如雪炉奴。 梳的紋絲不亂的頭發(fā)上逼庞，一...
    開封第一講書人閱讀 49,829評論 1贊 290
  • 城市分裂傳說
    那天，我揣著相機與錄音瞻赶，去河邊找鬼赛糟。 笑死，一個胖子當(dāng)著我的面吹牛共耍，可吹牛的內(nèi)容都是我干的虑灰。 我是一名探鬼主播吨瞎，決...
    沈念sama閱讀 38,979評論 3贊 408
  • 雙鴛鴦連環(huán)套：你想象不到人心有多黑
    文/蒼蘭香墨 我猛地睜開眼痹兜，長吁一口氣：“原來是場噩夢啊……” “哼！你這毒婦竟也來了颤诀？” 一聲冷哼從身側(cè)響起字旭，我...
    開封第一講書人閱讀 37,722評論 0贊 266
  • 萬榮殺人案實錄
    序言：老撾萬榮一對情侶失蹤，失蹤者是張志新（化名）和其女友劉穎崖叫，沒想到半個月后遗淳，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
    沈念sama閱讀 44,189評論 1贊 303
  • ?護林員之死
    正文 獨居荒郊野嶺守林人離奇死亡心傀，尸身上長有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點故事閱讀 36,519評論 2贊 327
  • ?白月光啟示錄
    正文 我和宋清朗相戀三年屈暗，在試婚紗的時候發(fā)現(xiàn)自己被綠了。 大學(xué)時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片。...
    茶點故事閱讀 38,654評論 1贊 340
  • 活死人
    序言：一個原本活蹦亂跳的男人離奇死亡养叛，死狀恐怖种呐，靈堂內(nèi)的尸體忽然破棺而出，到底是詐尸還是另有隱情弃甥，我是刑警寧澤爽室，帶...
    沈念sama閱讀 34,329評論 4贊 330
  • ?日本核電站爆炸內(nèi)幕
    正文 年R本政府宣布，位于F島的核電站淆攻，受9級特大地震影響阔墩，放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜瓶珊，卻給世界環(huán)境...
    茶點故事閱讀 39,940評論 3贊 313
  • 男人毒藥：我在死后第九天來索命
    文/蒙蒙 一啸箫、第九天 我趴在偏房一處隱蔽的房頂上張望。 院中可真熱鬧伞芹，春花似錦筐高、人聲如沸。這莊子的主人今日做“春日...
    開封第一講書人閱讀 30,762評論 0贊 21
  • 一樁弒父案柑土，背后竟有這般陰謀
    文/蒼蘭香墨 我抬頭看了看天上的太陽。三九已至绊汹，卻和暖如春稽屏，著一層夾襖步出監(jiān)牢的瞬間，已是汗流浹背西乖。 一陣腳步聲響...
    開封第一講書人閱讀 31,993評論 1贊 266
  • 情欲美人皮
    我被黑心中介騙來泰國打工狐榔， 沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留，地道東北人获雕。 一個月前我還...
    沈念sama閱讀 46,382評論 2贊 360
  • 代替公主和親
    正文 我出身青樓薄腻，卻偏偏與公主長得像，于是被迫代替她去往敵國和親届案。 傳聞我的和親對象是個殘疾皇子庵楷，可洞房花燭夜當(dāng)晚...
    茶點故事閱讀 43,543評論 2贊 349