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

最新推荐文章于 2022-04-01 16:39:55 发布

李建标

最新推荐文章于 2022-04-01 16:39:55 发布

阅读量1.1k

点赞数

文章标签：通用求根算法zeroin

本文链接：https://blog.csdn.net/weixin_30878051/article/details/113038591

版权

本文探讨了通用求根算法Zeroin在现代机器人运动学中的应用，特别是在数值逆运动学的解决过程中。通过牛顿-拉普森方法，介绍了求解非线性方程根的迭代过程。文中提供了2R机械臂和UR3机械臂的逆解示例，详细展示了从初始状态螺旋轴表示、轨迹点描述到逆解关节位置和运动仿真的步骤，强调了数值解法在实际机器人控制中的重要性和灵活性。

摘要由CSDN通过智能技术生成

前言原著之前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];