寸_铁

这个屌丝很懒，什么也没留下！

热门标签

Matlab机器人工具箱Robotics Toolbox逆运动学启发式求解方法（CCD）_robotics toolbox 逆运动学

作者：寸_铁 | 2024-07-15 07:45:47

踩

robotics toolbox 逆运动学

由于Robotics Toolbox的逆运动学求解是基于迭代的方法，导致经常出现收敛到局部最优解的情况，经常需要自己不断调整机器人的初始值以求能够达到全局最优解。上课时老师讲到逆运动学的启发式求解方法，在此总结一下CCD的具体实现过程。

注：本文不详细讲解机器人建模及运动学求解的基础知识，CSDN上有大量的优质文章已经进行了系统详细的阐述。

循环坐标下降（CCD）算法概述

CCD算法是各大启发类算法中的一类，最开始由Luenberger等人为解决高维函数优化问题提出，而后由Wang等人在机器人领域进行了推广，这是一种迭代启发式方法，适用于串联关节链的交互控制。CCD算法在每次迭代过程中，在当前点 $k$ 处沿一个坐标方向 $q$ 循环寻找函数 $f$ 的局部最小值，具体的数学描述会在下文详细讲解。Saha等人在文献中的研究表明，在逐一最小化坐标位置的情况下，CCD算法会收敛到函数的最小值。

二维平面机器人的求解

二维平面机器人的求解比较简单，且在各类CCD的论文中均有详细的说明，在此仅做简单介绍。

转动关节

以上图为例子， $\theta _i$ 为各关节与机器人末端和目标点（Target）之间的夹角，即为关节所需转动的角度。利用向量的点乘，很容易算得

$\theta _i=\arccos \left( \frac{P_e-P_{ji}}{\lVert P_e-P_{ji} \rVert}\cdot \frac{P_t-P_{ji}}{\lVert P_t-P_{ji} \rVert} \right)$

当然，上式只能算得角度的大小，并不能确定关节转动的方向，可用下式计算转动方向

$\vec{r}_i=\frac{P_e-P_{ji}}{\lVert P_e-P_{ji} \rVert}\times \frac{P_t-P_{ji}}{\lVert P_t-P_{ji} \rVert}$

下面给出二维CCD算法的具体例子

上图为单次循环的步骤，即依次移动各关节，使关节到机器人末端连线与关节到目标点的连线夹角为零。

移动关节

移动关节与转动关节类似，只不过变量由角度转变为长度，具体计算方法如下

$P_{j2}$ 即为一个移动关节，其移动长度 $d$ 可由下式确定

$d=\left( P_t-P_e \right) \cdot \frac{\left( P_e-P_{j2} \right)}{\lVert P_e-P_{j2} \rVert}$

其移动方向由符号决定

三维空间机器人的求解

三维建模我采用的是改进的D-H方法（Modified D-H method）。

在众多关于CCD算法实际应用的文章中，关于空间机器人的讲解还是比较少的，故而在此详细说明。

齐次坐标

齐次坐标是一种用于投影几何的坐标表示形式，类似于用于欧氏几何的笛卡尔坐标。齐次坐标除了用在机器人学中，也是计算机图形学的重要工具之一，由于它既能够用来区分向量和点，也更易于进行仿射变换，因此通过矩阵与向量相乘的一般运算可有效地实现坐标平移、旋转、缩放及透视投影。

一般情况下，点的齐次坐标表示为

$\boldsymbol{P}=\left[ \begin{matrix}{} a& b& c& \omega \\ \end{matrix} \right] ^T$

通常取 $\omega$ 为1，即点的齐次坐标的通式为

$\boldsymbol{P}=\left[ \begin{matrix}{} x& y& z& 1\\ \end{matrix} \right] ^T$

齐次坐标变换

坐标系 $\left\{ j \right\}$ 的相对于坐标系 $\left\{ i \right\}$ 的旋转矩阵 $_{i}^{j}R$ 与平移向量 $_{i}^{O_j}P$ 可构成一个 $4\times 4$ 的其次变换矩阵

$_{i}^{j}T=\left[ \begin{matrix}{} & _{i}^{j}R& & _{i}^{O_j}P\\ 0& 0& 0& \ 1\\ \end{matrix} \right] _{4\times 4}$

其次变换矩阵 $_{i}^{j}T$ 的含义是坐标系 $\left\{ j \right\}$ 相对于坐标系 $\left\{ i \right\}$ 的位姿，既表示了坐标系 $\left\{ j \right\}$ 相对于坐标系 $\left\{ i \right\}$ 的位置，也表示了坐标系 $\left\{ j \right\}$ 相对于坐标系 $\left\{ i \right\}$ 的姿态。

D-H矩阵

使用改进的D-H方法进行建模时，坐标系 $\left\{ i \right\}$ 相对于坐标系 $\left\{ i-1 \right\}$ 的转换矩阵为

$_{i-1}^{i}T=\left[ \begin{matrix} \cos \theta _i& -\sin \theta _i& 0& a_{i-1}\\ \sin \theta _i\cos \alpha _{i-1}& \cos \theta _i\cos \alpha _{i-1}& -\sin \alpha _{i-1}& -\sin \alpha _{i-1}d_i\\ \sin \theta _i\sin \alpha _{i-1}& \cos \theta _i\sin \alpha _{i-1}& \cos \alpha _{i-1}& \cos \alpha _{i-1}d_i\\ 0& 0& 0& 1\\ \end{matrix} \right]$

机器人的运动学建模

对于具有 $n$ 个自由度的机器人，在建立其D-H坐标系并确定相邻坐标系之间的D-H参数后，即可获得 $n$ 个D-H矩阵，将所有矩阵按顺序连乘，即可得到该机器人的运动学模型，6自由度机器人的运动学模型如下

$_{0}^{6}T=_{0}^{1}T\cdot _{1}^{2}T\cdot _{2}^{3}T\cdot _{3}^{4}T\cdot _{4}^{5}T\cdot _{5}^{6}T$

点的坐标变换

使用齐次坐标变换矩阵进行点的坐标变换公式如下

$^iP=_{i}^{j}T^jP$

其中， $_{i}^{j}T$ 为坐标系 $\left\{ j \right\}$ 相对于坐标系 $\left\{ i \right\}$ 的姿态， $^jP$ 和 $^iP$ 分别是点在 $\left\{ j \right\}$ 和 $\left\{ i \right\}$ 坐标系下的齐次坐标。

三维CCD算法

三维与二维的主要区别在于：三维空间下，机械臂的旋转平面可能与目标点不在同一平面。

本文采用的方法是，将目标点和机器人末端点均变换到需要调整的关节坐标系下，由改进的D-H坐标系的建立方法可知，z轴即为旋转轴，通过简单的几何知识，要使两线夹角最小（因为不在同一平面故而无法达到0），只需将两点投影到xoy平面内，两点与原点的连线夹角即为旋转角，旋转方向可用二维平面的方法，即通过叉乘求得。

根据改进D-H建模规则，从 $z$ 轴正方向看逆时针旋转 $\theta$ 为正，因此，当叉乘向量指向 $z$ 轴正方向时，将关节对应的角度加上 $\theta$ 即可。

本文以PUMA560机器人为例。

代码：

PUMA560的基本参数：


function PUMA560=robot(t1,t2,t3,t4,t5,t6)
L1=Link([t1,0,0,0,0],'modified');
L2=Link([t2,149.09,0,-pi/2,0],'modified');
L3=Link([t3,0,431.8,0,0],'modified');
L4=Link([t4,433.07,20.32,-pi/2,0],'modified');
L5=Link([t5,0,0,pi/2,0],'modified');
L6=Link([t6,56.25,0,-pi/2,0],'modified');
PUMA560=SerialLink([L1 L2 L3 L4 L5 L6],'name','Puma560');
end

计算D-H矩阵：


function T=matrix_T(theta,d,a,alpha)
% theta:     弧度
% d,a,alpha: 其余参数
T=[cos(theta),          -sin(theta),            0,          a;
   sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d;
   sin(theta)*sin(alpha),cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d;
   0,                    0,                     0,          1];
end

计算各关节的变换矩阵：


function T_joint=matrix_joint(theta,d,a,alpha)
% input theta,d,a,alpha: 向量
% return T:              六个关节对于基本坐标系（0系）的变换矩阵
for num_i=1:6
    if 1==num_i
        T_joint=matrix_T(theta(num_i),d(num_i),a(num_i),alpha(num_i));
        continue
    end
    T_joint(:,:,num_i)=T_joint(:,:,num_i-1)*matrix_T(theta(num_i),d(num_i),a(num_i),alpha(num_i));
end
end

主函数：


clc,clear;
tic
PUMA560=robot(pi/2,0,-pi/2,0,0,0);
 
target_1=[400,200,0,1];%-------------------- 目标点1
target_2=[500,-100,200,1];%----------------- 目标点2
theta=[pi/2,0,-pi/2,0,0,0];%---------------- theta为变量
d=[0,149.09,0,433.07,0,56.25];
a=[0,0,431.8,20.32,0,0];
alpha=[0,-pi/2,0,-pi/2,pi/2,-pi/2];
% 误差
err=1000;
% 最大迭代次数
N_max=100;
N=0;
filename='demo1.gif';
while ~(err<=10^-2 || N>=N_max)
    N=N+1;
    pause(0.5)
    PUMA560.plot(theta)
    f = getframe(gcf);  
    imind = frame2im(f);
    [imind,cm] = rgb2ind(imind,256);
    % imwrite时要保证文件已存在
    if N == 1
        imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.1);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.1);
    end
    % 仅进行位置变换，故只需移动前五个关节(逆向变换)
    for joint=1:5
        % 注意进行theta的更新
        T_joint=matrix_joint(theta,d,a,alpha);
        [x,y,z]=coordinate_ground(T_joint);
        % 获取末端(6)关节坐标
        p_6_ground=[x(end),y(end),z(end),1];
        p_6_j=inv(T_joint(:,:,joint))*p_6_ground.';
        p_target_j=inv(T_joint(:,:,joint))*target_1.';
        % 计算夹角
        x1=p_6_j(1);
        y1=p_6_j(2);
        x2=p_target_j(1);
        y2=p_target_j(2);
        target_theta=acos((x1*x2+y1*y2)/(sqrt(x1^2+y1^2)*sqrt(x2^2+y2^2)));
        % 判断旋转方向(利用叉乘)
        if (x1*y2-x2*y1)<=0
            theta(joint)=theta(joint)-target_theta;
        else
            theta(joint)=theta(joint)+target_theta;
        end
    end
    err=sqrt((p_6_ground(1)-target_1(1))^2+(p_6_ground(2)-target_1(2))^2);
end
 
filename='demo2.gif';
err=1000;N=0;
while ~(err<=10^-2 || N>=N_max)
    N=N+1;
    pause(0.5)
    PUMA560.plot(theta)
    f = getframe(gcf);  
    imind = frame2im(f);
    [imind,cm] = rgb2ind(imind,256);
    % imwrite时要保证文件已存在
    if N == 1
        imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.1);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.1);
    end
    % 仅进行位置变换，故只需移动前五个关节(逆向变换)
    for joint=1:5
        % 注意进行theta的更新
        T_joint=matrix_joint(theta,d,a,alpha);
        [x,y,z]=coordinate_ground(T_joint);
        % 获取末端(6)关节坐标
        p_6_ground=[x(end),y(end),z(end),1];
        p_6_j=inv(T_joint(:,:,joint))*p_6_ground.';
        p_target_j=inv(T_joint(:,:,joint))*target_2.';
        % 计算夹角
        x1=p_6_j(1);
        y1=p_6_j(2);
        x2=p_target_j(1);
        y2=p_target_j(2);
        target_theta=acos((x1*x2+y1*y2)/(sqrt(x1^2+y1^2)*sqrt(x2^2+y2^2)));
        % 判断旋转方向(利用叉乘)
        if (x1*y2-x2*y1)<=0
            theta(joint)=theta(joint)-target_theta;
        else
            theta(joint)=theta(joint)+target_theta;
        end
    end
    err=sqrt((p_6_ground(1)-target_2(1))^2+(p_6_ground(2)-target_2(2))^2);
end
 
toc

结果展示：

机器人三维模型：

从初始点移动到位置1：

从位置1移动到位置2：

结束

第一次写，如有错误烦请予以纠正。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/寸_铁/article/detail/828338