通用求根算法zeroin_Modern Robotics运动学数值解法及SVD算法（C matlab）

前言原著之前CSDN已经注销，新CSDN

Galaxy_Robot的博客_CSDN博客-机器人,C语言,我是谁?领域博主blog.csdn.net

这半个月的业余时间研究了机器人逆运动学的解析解法和迭代数值解法，并用程序实现。由于解析法只适合于特定结构的机器人，不具有通用性，因此这里不讨论解析解，只讨论迭代数值解法。本文相关的程序及研究素材都可以从github下载--链接。

数值逆运动学

机器人逆运动学的解析解虽然很优美，但是有时候获得解析解很困难，甚至以人类的智慧无法求得解析解，这时候就可以用迭代数值解法。即使存在解析解，数值解法也经常用来提高解的精度。例如PUMA类型的机器人，最后三个轴由于机械制造和装配误差，实际上可能没有相交于一点，肩关节的轴线也可能不垂直。这种情况下，不必抛弃任何可用的解析解，这些解可以作为迭代数值解法的初始值。

求解非线性方程的根有多种迭代方法，我们将使用一种非线性求根的基本方法，牛顿拉普森法。此外，在不存在精确解的情况下，我们需要寻找最接近的近似解，这就需要优化方法。因此，我们现在讨论非线性求根的牛顿拉普森方法，以及优化的一阶必要条件。

牛顿-拉普森方法

数值逆运动学算法

/*** @brief           Description: Algorithm module of robotics, according to the*                  book[modern robotics : mechanics, planning, and control].* @file:           RobotAlgorithmModule.h* @author:         Brian* @date:           2019/03/01 12:23* Copyright(c)     2019 Brian. All rights reserved.*                    * Contact          https://blog.csdn.net/Galaxy_Robot* @note:     * @warning:
*/#ifndef SVDPINV_H_
#define SVDPINV_H_
#ifdef __cplusplus
extern "C" {#endif/***@brief         Computes Matrix transpose.*@param[in]     a           a matrix.*@param[in]     m           rows of matrix a.*@param[in]     n           columns of matrix a.*@param[out]    b           transpose of matrix a.*@note:*@waring:*/void MatrixT(double *a, int m, int n, double *b);/***@brief         Computes matrix multiply.*@param[in]     a       First matrix.*@param[in]     m       rows of matrix a.*@param[in]     n       columns of matrix b.*@param[in]     b       Second matrix.*@param[in]     l       columns of matrix b.*@param[out]    c       result of a*b.*@note:*@waring:*/void MatrixMult(const double *a, int m, int n, const double *b, int l, double *c);/***@brief         Given the matrix a,this routine computes its singular value decomposition.*@param[in/out] a       input matrix a or output matrix u.*@param[in]     m       number of rows of matrix a.*@param[in]     n       number of columns of matrix a.*@param[in]     tol     any singular values less than a tolerance are treated as zero.*@param[out]    w       output vector w.*@param[out]    v       output matrix v.*@return        No return value.*@note:         input a must be a  m rows and n columns matrix,w is n-vector,v is a n rows and n columns matrix. *@waring:*/int svdcmp(double *a, int m, int n,double tol, double *w, double *v);/***@brief         Computes Moore-Penrose pseudoinverse by  Singular Value Decomposition (SVD) algorithm.*@param[in]     a           an arbitrary order matrix.*@param[in]     m           rows.*@param[in]     n           columns.*@param[in]     tol         any singular values less than a tolerance are treated as zero.*@param[out]    b           Moore-Penrose pseudoinverse of matrix a.*@return        0:success,Nonzero:failure.*@retval        0           success.*@retval        1           failure when use malloc to allocate memory.*@note:*@waring:*/int MatrixPinv(const double *a, int m, int n, double tol, double *b);#ifdef __cplusplus
}
#endif#endif//SVDPINV_H_

实现文件

/*** @brief           Description: Algorithm module of robotics, according to the*                  book[modern robotics : mechanics, planning, and control].* @file:           RobotAlgorithmModule.c* @author:         Brian* @date:           2019/03/01 12:23* Copyright(c)     2019 Brian. All rights reserved.*                    * Contact          https://blog.csdn.net/Galaxy_Robot* @note:     * @warning:
*/
#include "RobotAlgorithmModule.h"
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
#define MAX(a,b) (a>b?a:b)
#define MIN(a,b) (a<b?a:b)/**
*@brief         Computes (a^2+b^2)^(1/2),
*@param[in]     a
*@param[in]     b
*@note:
*@waring:
*/
double pythag(const double a, const double b)
{double absa, absb;absa = fabs(a);absb = fabs(b);if (absa > absb) return absa*sqrt(1.0 + (absb / absa)*(absb / absa));else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0 + (absa / absb)*(absa / absb)));}
/**
*@brief         Given the matrix a,this routine computes its singular value decomposition.
*@param[in/out] a       input matrix a or output matrix u.
*@param[in]     m       number of rows of matrix a.
*@param[in]     n       number of columns of matrix a.
*@param[in]     tol     any singular values less than a tolerance are treated as zero.
*@param[out]    w       output vector w.
*@param[out]    v       output matrix v.
*@return        No return value.
*@note:         input a must be a  m rows and n columns matrix,w is n-vector,v is a n rows and n columns matrix.
*@waring:
*/
int svdcmp(double *a, int m, int n, double tol, double *w, double *v)
{int flag, i, its, j, jj, k, l, nm;double anorm, c, f, g, h, s, scale, x, y, z;double *rv1 = (double *)malloc(n * sizeof(double));if (rv1==NULL){return 1;}g = scale = anorm = 0.0;for (i=0;i<n;i++)//Householder reduction to bidiagonal form.{l = i + 2;rv1[i] = scale*g;g = s = scale = 0.0;if (i<m){for (k = i; k < m; k++)scale += fabs(a[k*n + i]);if (scale!=0.0){for (k = i; k < m;k++){a[k*n + i] /= scale;s += a[k*n + i] * a[k*n + i];}f = a[i*n + i];g = -SIGN(sqrt(s), f);h = f*g - s;a[i*n + i] = f - g;for (j=l-1;j<n;j++){for (s = 0.0, k = i; k < m;k++){s += a[k*n + i] * a[k*n + j];}f = s / h;for (k = i; k < m;k++){a[k*n + j] += f*a[k*n + i];}}for (k = i; k < m;k++){a[k*n + i] *= scale;}}}w[i] = scale*g;g = s = scale = 0.0;if (i+1 <= m && (i+1)!=n){for (k=l-1;k<n;k++){scale += fabs(a[i*n + k]);}if (scale!=0.0){for (k=l-1;k<n;k++){a[i*n + k] /= scale;s += a[i*n + k] * a[i*n + k];}f = a[i*n + l - 1];g = -SIGN(sqrt(s), f);h = f*g - s;a[i*n + l - 1] = f - g;for (k=l-1;k<n;k++){rv1[k] = a[i*n + k] / h;}for (j = l - 1; j < m; j++){for (s = 0.0, k = l - 1; k < n; k++){s += a[j*n + k] * a[i*n + k];}for (k = l - 1; k < n; k++){a[j*n + k] += s*rv1[k];}}for (k=l-1;k<n;k++){a[i*n + k] *= scale;}}}anorm = MAX(anorm, (fabs(w[i]) + fabs(rv1[i])));}for (i=n-1;i>=0;i--)//Accumulation of right-hand transformations.{if (i<n-1){if (g!=0.0){for (j = l;j<n;j++)//Double division to avoid possible underflow.{v[j*n + i] = (a[i*n + j] / a[i*n + l]) / g;}for (j = l;j<n;j++){for (s = 0.0, k = l;k<n;k++){s += a[i*n + k] * v[k*n + j];}for (k = l;k<n;k++){v[k*n + j] += s*v[k*n + i];}}}for (j = l;j<n;j++){v[i*n + j] = v[j*n + i] = 0.0;}}v[i*n + i] = 1.0;g = rv1[i];l = i;}for (i=MIN(m,n)-1;i>=0;i--)//Accumulation of left-hand transformations.{l = i + 1;g = w[i];for (j = l;j<n;j++){a[i*n + j] = 0.0;}if (g!=0.0){g = 1.0 / g;for (j = l;j<n;j++){for (s = 0.0, k = l; k < m;k++){s += a[k*n + i] * a[k*n + j];}f = (s / a[i*n + i])*g;for (k = i; k < m;k++){a[k*n + j] += f*a[k*n + i];}}for (j = i; j < m;j++){a[j*n + i] *= g;}}else{for (j = i; j < m;j++){a[j*n + i] = 0.0;}}++a[i*n + i];}for (k=n-1;k>=0;k--){/* Diagonalization of the bidiagonal form: Loop oversingular values, and over allowed iterations. */for (its=0;its<30;its++){flag = 1;for (l = k;l>=0;l--)//Test for splitting.{nm = l - 1;     if (l==0 || fabs(rv1[l])<= tol*anorm){flag = 0;break;}if (fabs(w[nm]) <=tol*anorm)break;}if (flag)//Cancellation of rv1[l], if l > 0.{c = 0.0;s = 1.0;for (i=l;i<k+1;i++){f = s*rv1[i];rv1[i] = c*rv1[i];if (fabs(f) <= tol*anorm)break;g = w[i];h = pythag(f, g);w[i] = h;h = 1.0 / h;c = g*h;s = -f*h;for (j = 0; j < m;j++){y = a[j*n + nm];z = a[j*n + i];a[j*n + nm] = y*c + z*s;a[j*n + i] = z*c - y*s;}}}z = w[k];if (l==k)//Convergence{if (z<0.0)//Singular value is made nonnegative..{w[k] = -z;for (j=0;j<n;j++){v[j*n + k] = -v[j*n + k];}}break;}//if (its==29)//{//  printf("no convergence in 30 svdcmp iterationsn");//}x = w[l]; //Shift from bottom 2-by-2 minor.nm = k - 1;y = w[nm];g = rv1[nm];h = rv1[k];f = ((y - z)*(y + z) + (g - h)*(g + h)) / (2.0*h*y);g = pythag(f, 1.0);f = ((x - z)*(x + z) + h*((y / (f + SIGN(g, f))) - h)) / x;c = s = 1.0;for (j = l;j<=nm;j++)//Next QR transformation:{i = j + 1;g = rv1[i];y = w[i];h = s*g;g = c*g;z = pythag(f, h);rv1[j] = z;c = f / z;s = h / z;f = x*c + g*s;g = g*c - x*s;h = y*s;y *= c;for (jj=0;jj<n;jj++){x = v[jj*n + j];z = v[jj*n + i];v[jj*n + j] = x*c + z*s;v[jj*n + i] = z*c - x*s;}z = pythag(f, h);w[j] = z;if (z)//Rotation can be arbitrary if z = 0.{z = 1.0 / z;c = f*z;s = h*z;}f = c*g + s*y;x = c*y - s*g;for (jj = 0; jj < m;jj++){y = a[jj*n + j];z = a[jj*n + i];a[jj*n + j] = y*c + z*s;a[jj*n + i] = z*c - y*s;}}rv1[l] = 0.0;rv1[k] = f;w[k] = x;}}free(rv1);return 0;
}/**
*@brief         Computes Moore-Penrose pseudoinverse by  Singular Value Decomposition (SVD) algorithm.
*@param[in]     a           an arbitrary order matrix.
*@param[in]     m           rows.
*@param[in]     n           columns.
*@param[in]     tol         any singular values less than a tolerance are treated as zero.
*@param[out]    b           Moore-Penrose pseudoinverse of matrix a.
*@return        0:success,Nonzero:failure.
*@retval        0           success.
*@retval        1           failure when use malloc to allocate memory.
*@note:
*@waring:
*/
int MatrixPinv(const double *a, int m, int n, double tol, double *b)
{int i, j;double *u = (double *)malloc(m*n * sizeof(double));if (u == NULL ){return 1;}double *w = (double *)malloc(n * sizeof(double));if (w == NULL){free(u);return 1;}double *v = (double *)malloc(n*n * sizeof(double));if (v == NULL){free(u);free(w);return 1;}double *vw = (double *)malloc(m*n * sizeof(double));if (vw == NULL){free(u);free(w);free(v);return 1;}double *uT = (double *)malloc(n*n * sizeof(double));if (uT == NULL){free(u);free(w);free(v);free(vw);return 1;}memcpy(u, a, m*n * sizeof(double));svdcmp(u, m, n, tol,w, v);for (i = 0; i < n; i++){if (w[i] <= tol){w[i] = 0;}else{w[i] = 1.0 / w[i];}}for (i = 0; i < m; i++){for (j = 0; j < n; j++){vw[i*n + j] = v[i*n + j] * w[j];}}MatrixT(u, n, n, uT);MatrixMult(vw, m, n, uT, n, b);free(u);free(w);free(v);free(vw);free(uT);return 0;
}

/*** @brief           Description: Test Demos for robot algorithm modules.* @file:           TestDemo.c* @author:         Brian* @date:           2019/03/01 12:23* Copyright(c)     2019 Brian. All rights reserved.*                    * Contact          https://blog.csdn.net/Galaxy_Robot* @note:     * @warning:
*/
#include <stdio.h>
#include <math.h>
#include "RobotAlgorithmModule.h"
void test_svdcmp();
void test_MatrixPinv();
int main()
{//void test_svdcmp();test_MatrixPinv();return 0;
}void test_svdcmp()
{double a[4][4] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};double w[4], v[4][4];double tol = 2.2e-15;svdcmp((double *)a, 4, 4, tol,w, (double *)v);int i, j,k;printf("svdcmp,u:n");for (i = 0; i < 4; i++){for (j = 0; j < 4; j++){printf("%lf   ", a[i][j]);}printf("n");}printf("w:n");for (i = 0; i < 1; i++){for (j = 0; j < 4; j++){printf("%lf   ", w[j]);}printf("n");}printf("v:n");for (i = 0; i < 4; i++){for (j = 0; j < 4; j++){printf("%lf   ", v[i][j]);}printf("n");}double vT[4][4];//验证分解是否正确MatrixT((double *)v, 4, 4, (double *)vT);double tmp[4][4];for (i=0;i<4;i++){for (j=0;j<4;j++){tmp[i][j]=0;for (k=0;k<4;k++){tmp[i][j] = tmp[i][j] +w[k] *a[i][k] * vT[k][j];}}}printf("uwvT:n");for (i = 0; i < 4; i++){for (j = 0; j < 4; j++){printf("%lf   ", tmp[i][j]);}printf("n");}return;
}void test_MatrixPinv()
{double a[4][4] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};double b[4][4];double tol = 2.22e-15;MatrixPinv((double *)a, 4, 4, tol,(double *)b);printf("MatrixPinv(a):n");int i, j;for (i = 0; i < 4; i++){for (j = 0; j < 4; j++){printf("%lf  ", b[i][j]);}printf("n");}return;
}

一般机器人逆运动学数值解法实现

有了前面实现的计算伪逆的算法，那么接下来就可以基于旋量理论和牛顿拉普算法，实现一般机器人逆运动学数值解法。基于前面数值逆运动算法的分析，下面使用C语言实现，主要有两个函数，其中的依赖函数可以在以前的博客找到。代码不断更新迭代，完整的最新源码参见github--链接。

IKinBodyNR(JointNum, (double *)Blist, M, T, thetalist0, eomg, ev, maxiter, thetalist);
IKinSpaceNR(JointNum, (double *)Slist, M, T, thetalist0, eomg, ev, maxiter, thetalist);

这两个函数的参数区别在于输入的螺旋轴单位矢量Blist是在末端物体坐标系下的表达，Slist是在固定坐标系下表达，其他参数含义相同。内部实现的区别在于IKinBodyNR内部使用物体坐标系下的雅可比，IKinSpaceNR内部使用固定坐标系下的雅可比.

函数声明

/*** @brief           Description: Algorithm module of robotics, according to the*                  book[modern robotics : mechanics, planning, and control].* @file:           RobotAlgorithmModule.h* @author:         Brian* @date:           2019/03/01 12:23* Copyright(c)     2019 Brian. All rights reserved.*                    * Contact          https://blog.csdn.net/Galaxy_Robot* @note:     * @warning:
*/
#ifndef RobotALGORITHMMODULE_H_
#define RobotALGORITHMMODULE_H_
#ifdef __cplusplus
extern "C" {#endif#define         DEBUGMODE           1           //打印调试信息#define         PI                  3.14159265358979323846//if the norm of vector is near zero(< 1.0E-6),regard as zero.#define         ZERO_VALUE          1.0E-6      /***@brief         Description:use an iterative Newton-Raphson root-finding method.*@param[in]     JointNum        Number of joints.*@param[in]     Blist           The joint screw axes in the Body frame when the manipulator*                               is at the home position, in the format of a matrix with the*                               screw axes as the columns.*@param[in]     M               The home configuration of the end-effector.*@param[in]     T               The desired end-effector configuration Tsd.*@param[in]     thetalist0      An initial guess of joint angles that are close to satisfying Tsd.*@param[in]     eomg            A small positive tolerance on the end-effector orientation error.*                               The returned joint angles must give an end-effector orientation error less than eomg.*@param[in]     ev              A small positive tolerance on the end-effector linear position error. The returned*                               joint angles must give an end-effector position error less than ev.*@param[in]     maxiter         The maximum number of iterations before the algorithm is terminated.*@param[out]    thetalist       Joint angles that achieve T within the specified tolerances.*@return        0:success,Nonzero:failure*@retval        0               success.*@retval        1               The maximum number of iterations reach before the algorithm is terminated.*@retval        2               failure to allocate memory for Jacobian matrix.*@note:*@waring:*/int IKinBodyNR(int JointNum, double *Blist, double M[4][4], double  T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist);/***@brief         Description:use an iterative Newton-Raphson root-finding method.*@param[in]     JointNum        Number of joints.*@param[in]     Slist           The joint screw axes in the space frame when the manipulator*                               is at the home position, in the format of a matrix with the*                               screw axes as the columns.*@param[in]     M               The home configuration of the end-effector.*@param[in]     T               The desired end-effector configuration Tsd.*@param[in]     thetalist0      An initial guess of joint angles that are close to satisfying Tsd.*@param[in]     eomg            A small positive tolerance on the end-effector orientation error.*                               The returned joint angles must give an end-effector orientation error less than eomg.*@param[in]     ev              A small positive tolerance on the end-effector linear position error. The returned*                               joint angles must give an end-effector position error less than ev.*@param[in]     maxiter         The maximum number of iterations before the algorithm is terminated.*@param[out]    thetalist       Joint angles that achieve T within the specified tolerances.*@return        0:success,Nonzero:failure*@retval        0*@note:*@waring:*/int IKinSpaceNR(int JointNum, double *Slist, double M[4][4], double T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist);#ifdef __cplusplus
}
#endif#endif//RobotALGORITHMMODULE_H_

实现代码如下

/*** @brief           Description: Algorithm module of robotics, according to the*                  book[modern robotics : mechanics, planning, and control].* @file:           RobotAlgorithmModule.c* @author:         Brian* @date:           2019/03/01 12:23* Copyright(c)     2019 Brian. All rights reserved.*                    * Contact          https://blog.csdn.net/Galaxy_Robot* @note:     * @warning:
*/#include "RobotAlgorithmModule.h"
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>/**
*@brief         Description:use an iterative Newton-Raphson root-finding method.
*@param[in]     JointNum        Number of joints.
*@param[in]     Blist           The joint screw axes in the Body frame when the manipulator
*                               is at the home position, in the format of a matrix with the
*                               screw axes as the columns.
*@param[in]     M               The home configuration of the end-effector.
*@param[in]     T               The desired end-effector configuration Tsd.
*@param[in]     thetalist0      An initial guess of joint angles that are close to satisfying Tsd.
*@param[in]     eomg            A small positive tolerance on the end-effector orientation error.
*                               The returned joint angles must give an end-effector orientation error less than eomg.
*@param[in]     ev              A small positive tolerance on the end-effector linear position error. The returned
*                               joint angles must give an end-effector position error less than ev.
*@param[in]     maxiter         The maximum number of iterations before the algorithm is terminated.
*@param[out]    thetalist       Joint angles that achieve T within the specified tolerances.
*@return        0:success,Nonzero:failure
*@retval        0               success.
*@retval        1               The maximum number of iterations reach before the algorithm is terminated.
*@retval        2               failure to allocate memory for Jacobian matrix.
*@note:
*@waring:
*/
int IKinBodyNR(int JointNum, double *Blist, double M[4][4], double T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist)
{int i, j;double Tsb[4][4];double iTsb[4][4];double Tbd[4][4];double se3Mat[4][4];double Vb[6];int ErrFlag;double *Jb = (double *)malloc(JointNum * 6 * sizeof(double));if (Jb == NULL){return 2;}double *pJb = (double *)malloc(JointNum * 6 * sizeof(double));if (pJb == NULL){free(Jb);return 2;}double *dtheta = (double *)malloc(JointNum * sizeof(double));if (dtheta == NULL){free(Jb);free(pJb);return 2;}MatrixCopy(thetalist0, JointNum, 1, thetalist);FKinBody(M, JointNum, Blist, thetalist, Tsb);TransInv(Tsb, iTsb);Matrix4Mult(iTsb,T,Tbd);MatrixLog6(Tbd, se3Mat);se3ToVec(se3Mat, Vb);ErrFlag = VecNorm2(3, &Vb[0]) > eomg || VecNorm2(3, &Vb[3]) > ev;i = 0;#if(DEBUGMODE)///Debug///printf("iteration:%4d  thetalist:  ", i);int k;for (k = 0; k < JointNum; k++){printf("%4.6lft", thetalist[k]);}printf("n");#endifwhile (ErrFlag && i<maxiter){JacobianBody(JointNum, Blist, thetalist, Jb);MatrixPinv(Jb, 6, JointNum, 2.2E-15, pJb);MatrixMult(pJb, JointNum,6 , Vb, 1, dtheta);for (j=0;j<JointNum;j++){thetalist[j] = thetalist[j] + dtheta[j];}i++;FKinBody(M, JointNum, Blist, thetalist, Tsb);TransInv(Tsb, iTsb);Matrix4Mult(iTsb, T, Tbd);MatrixLog6(Tbd, se3Mat);se3ToVec(se3Mat, Vb);ErrFlag = VecNorm2(3, &Vb[0]) > eomg || VecNorm2(3, &Vb[3]) > ev;
#if (DEBUGMODE)///DEBUG///printf("iteration:%4d  thetalist:  ", i);for (k = 0; k < JointNum; k++){printf("%4.6lft", thetalist[k]);}printf("n");//
#endif}free(Jb);free(pJb);free(dtheta);return ErrFlag;
}/**
*@brief         Description:use an iterative Newton-Raphson root-finding method.
*@param[in]     JointNum        Number of joints.
*@param[in]     Slist           The joint screw axes in the space frame when the manipulator
*                               is at the home position, in the format of a matrix with the
*                               screw axes as the columns.
*@param[in]     M               The home configuration of the end-effector.
*@param[in]     T               The desired end-effector configuration Tsd.
*@param[in]     thetalist0      An initial guess of joint angles that are close to satisfying Tsd.
*@param[in]     eomg            A small positive tolerance on the end-effector orientation error.
*                               The returned joint angles must give an end-effector orientation error less than eomg.
*@param[in]     ev              A small positive tolerance on the end-effector linear position error. The returned
*                               joint angles must give an end-effector position error less than ev.
*@param[in]     maxiter         The maximum number of iterations before the algorithm is terminated.
*@param[out]    thetalist       Joint angles that achieve T within the specified tolerances.
*@return        0:success,Nonzero:failure
*@retval        0
*@note:
*@waring:
*/
int IKinSpaceNR(int JointNum, double *Slist, double M[4][4], double T[4][4], double *thetalist0, double eomg, double ev, int maxiter,double *thetalist)
{int i, j;double Tsb[4][4];double iTsb[4][4];double Tbd[4][4];double AdT[6][6];double se3Mat[4][4];double Vb[6];double Vs[6];int ErrFlag;double *Js = (double *)malloc(JointNum * 6 * sizeof(double));if (Js == NULL){return 2;}double *pJs = (double *)malloc(JointNum * 6 * sizeof(double));if (pJs == NULL){free(Js);return 2;}double *dtheta = (double *)malloc(JointNum * sizeof(double));if (dtheta == NULL){free(Js);free(pJs);return 2;}MatrixCopy(thetalist0, JointNum, 1, thetalist);FKinSpace(M, JointNum, Slist, thetalist, Tsb);TransInv(Tsb, iTsb);Matrix4Mult(iTsb, T, Tbd);MatrixLog6(Tbd, se3Mat);se3ToVec(se3Mat, Vb);Adjoint(Tsb, AdT);MatrixMult((double *)AdT, 6, 6, Vb, 1, Vs);ErrFlag = VecNorm2(3, &Vs[0]) > eomg || VecNorm2(3, &Vs[3]) > ev;i = 0;#if(DEBUGMODE)///Debug///printf("iteration:%4d  thetalist:  ", i);int k;for (k = 0; k < JointNum; k++){printf("%4.6lft", thetalist[k]);}printf("n");#endifwhile (ErrFlag && i < maxiter){JacobianSpace(JointNum, Slist, thetalist, Js);MatrixPinv(Js, 6, JointNum, 2.2E-15, pJs);MatrixMult(pJs, JointNum, 6, Vs, 1, dtheta);for (j = 0; j < JointNum; j++){thetalist[j] = thetalist[j] + dtheta[j];}i++;FKinSpace(M, JointNum, Slist, thetalist, Tsb);TransInv(Tsb, iTsb);Matrix4Mult(iTsb, T, Tbd);MatrixLog6(Tbd, se3Mat);se3ToVec(se3Mat, Vb);Adjoint(Tsb, AdT);MatrixMult((double *)AdT, 6, 6, Vb, 1, Vs);ErrFlag = VecNorm2(3, &Vs[0]) > eomg || VecNorm2(3, &Vs[3]) > ev;
#if (DEBUGMODE)///DEBUG///printf("iteration:%4d  thetalist:  ", i);for (k = 0; k < JointNum; k++){printf("%4.6lft", thetalist[k]);}printf("n");//
#endif}free(Js);free(pJs);free(dtheta);return ErrFlag;
}

Example1: 2R机械臂逆解

该例子调用数值逆解算法IKinBodyNR求解的程序如下

/*** @brief           Description: Test Demos for robot algorithm modules.* @file:           TestDemo.c* @author:         Brian* @date:           2019/03/01 12:23* Copyright(c)     2019 Brian. All rights reserved.*                    * Contact          https://blog.csdn.net/Galaxy_Robot* @note:     * @warning:
*/
#include <stdio.h>
#include <math.h>
#include "RobotAlgorithmModule.h"void test_IKinBodyNR();
int main()
{test_IKinBodyNR();return 0;
}
void test_IKinBodyNR()
{int JointNum = 2;double Blist[6][2] = {0,0,0,0,1,1,0,0,2,1,0,0,};double M[4][4] ={1,0,0,2,0,1,0,0,0,0,1,0,0,0,0,1};double T[4][4] = {-0.5,-0.866,0,0.366,0.866,-0.5,0,1.366,0,0,1,0,0,0,0,1};double thetalist0[2] = { 0,0.5 };double eomg = 0.001;double ev = 0.0001;double thetalist[2] = { 0 };int maxiter = 20;int ret=IKinBodyNR(JointNum, (double *)Blist, M, T, thetalist0, eomg, ev, maxiter, thetalist);if (ret){printf("IKinBody error %dn", ret);return;}printf("solution thetalist:n");int i;for (i=0;i<JointNum;i++){printf("%lf  ", thetalist[i]);}printf("n");return;
}

结果输出如下图，经过4次迭代就可以求得期望位姿（构型）对应的关节角度分别为0.523589rad(30度),1.570829rad(90度)。这与手工计算的值也是一致的。

Example2: UR3机械臂逆解

如图所示是UR3机械臂的自由度分布和尺寸。

UR3尺寸示意图：

下面把前面实现的算法应用在六轴UR3机械臂的逆解。我们分为以下几步来做

首先是在测量机械臂在初始状态时各个轴线在基座坐标系的表达，
其次是建立任务坐标系，工具坐标系等，确定运动轨迹的描述，
然后把轨迹的数据输入到逆解算法，计算出各个关节的位置，
根据逆解的关节数据进行运动仿真，看末端是否走出了期望的轨迹。

（1）初始状态螺旋轴表示

（2）轨迹点描述

UR3初始状态示意图：

（3）逆解关节位置

我们调用IKinSpaceNR()函数来逆解关节，还需要把上面的参数和坐标转换一下，使得与逆解函数接受的参数含义一致。

用matlab处理一下，

clc
clear
%Slist
w1=[0;0;1];
w2=[0;1;0];
w3=[0;1;0];
w4=[0;1;0];
w5=[0;0;1];
w6=[0;1;0];
q1=[0;0;0];
q2=[0;0;151.900];
q3=[0;0;395.55];
q4=[213;0;395.55];
q5=[213;110.4;478.95];
q6=[213;110.4;478.95];
S1=[w1;cross(-w1,q1)];
S2=[w2;cross(-w2,q2)];
S3=[w3;cross(-w3,q3)];
S4=[w4;cross(-w4,q4)];
S5=[w5;cross(-w5,q5)];
S6=[w6;cross(-w6,q6)];
Slist=[S1,S2,S3,S4,S5,S6];
%初始构型
M=[1,0 , 0 ,213;0 ,1 ,0 ,267.8;0,0,1,478.95;0,0,0,1];
% M=[1,0 , 0 ,327.5853;
%     0 ,1 ,0 ,332.8;
%     0,0,1,364.9;
%     0,0,0,1];
%期望构型
TwA=[cos(pi/2),-sin(pi/2),0,125;sin(pi/2),cos(pi/2),0,75;0,0,1,60;0,0,0,1];
TwB=[1,0,0,85;0,cos(-pi/2),-sin(-pi/2),75;0,sin(-pi/2),cos(-pi/2),100;0,0,0,1];
TwC=[1,0,0,65;0,cos(-pi/2),-sin(-pi/2),75;0,sin(-pi/2),cos(-pi/2),100;0,0,0,1];%基坐标系{s}到任务坐标系{w}的变换Tsw=[1,0,0,-75;0,1,0,300;0,0,1,100;0,0,0,1];TsA= Tsw*TwA;TsB=Tsw*TwB;TsC=Tsw*TwC;%参数结果
display(Slist);
display(M);
display(TsA);
display(TsB);
display(TsC);

各参数结果如下

Slist =0         0         0         0         0         00    1.0000    1.0000    1.0000         0    1.00001.0000         0         0         0    1.0000         00 -151.9000 -395.5500 -395.5500  110.4000 -478.95000         0         0         0 -213.0000         00         0         0  213.0000         0  213.0000M =1.0000         0         0  213.00000    1.0000         0  267.80000         0    1.0000  478.95000         0         0    1.0000TsA =0.0000   -1.0000         0   50.00001.0000    0.0000         0  375.00000         0    1.0000  160.00000         0         0    1.0000TsB =1.0000         0         0   10.00000    0.0000    1.0000  375.00000   -1.0000    0.0000  200.00000         0         0    1.0000
TsC =1.0000         0         0  -10.00000    0.0000    1.0000  375.00000   -1.0000    0.0000  200.00000         0         0    1.0000

设置误差姿态误差eomg = 1.0E-4，位置误差 ev = 1.0E-3，调用逆解函数，计算得到ABC这3点对应的3组关节变量如下

A点对应的关节量迭代过程及结果

B点对应的关节量迭代过程及结果

C点对应的关节量迭代过程及结果

（4）运动仿真

使用3D模型进行运动仿真，从初始位置运动到A点

从初始位置运动到B点

从初始位置运动到C点

参考文献

Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017