robot Inverse Kinematics

偏执类工业机器人逆运动学解数值解法程序主代码

%TA1400型机器人逆运动学求解
%算法思路来源于有卜王辉的论文——基于切断点自由度解耦的手腕偏置型6R机器人位置反解
%该方法是为了验证其算法的正确与否，所以编程实现
%该方法大致的思路是：用其中的一个关键表示其他关节，最后建立起一个只含有个一个未知...
 %数的关节的非线性方程
%用关节6的转动角表示其他的关节角，采用迭代的方式求得
%长度单位为mm
%2011.7.3
clc  %清理matlab输出窗口
clear all %清理空间所有变量
format long
%定义机器人定的机械常数
syms JointOne JointTwo JointThree JointFour JointFive JointSix %单位 度
syms Joint%6个关节的转角值
syms s1 s2 s3 s4 s5 s6 c1 c2 c3 c4 c5 c6
%s1对应sin（JointOne*pi/180）c1对应cos（JointOne*pi/180）

%机器人D-H参数
syms a1 a2 a3 a5%关节连杆长度
syms d4         %连杆距离
%设定机器人D-H参数
a1=150;%160;
a2=570;%560;
a3=200;%130;
a5=30;%40;
d4=640;%600;%单位mm
MechanicalConstant=[a1 a2 a3 a5 d4];
%机器人末端工具坐标系
syms nx ny nz
syms ox oy oz
syms ax ay az  %机器人末端工具坐标系的姿态
syms px py pz  %机器人末端工具坐标系的位置
%机器人末端工具坐标系位姿（数值型） 指定机器人末端工具坐标系位姿
%{
Te=[ -0.82629575 -0.47274236  0.30618621  658.53407;
      0.43118622 -0.18118622  0.88388350  0.0      ;
     -0.36237244  0.86237244  0.35355340  483.02788;
      0.0         0.0          0.0        1.0      ];
%}
Te=[ 0.008660254037844  -0.005000000000000   0.000000000000000   8.199999999999999;
     0.004330127018922   0.007500000000000  -0.005000000000000   0.000000000000000;
     0.002500000000000   0.004330127018922   0.008660254037844   7.700000000000000;
                     0                   0                   0   0.010000000000000;]*100;
%{
Te=[1     0     0   950;
    0    -1     0   0  ;
    0     0    -1  -640;
    0     0     0     1];
%}
%机器人末端工具坐标系字母与数值对应
nx=Te（11）;ox=Te（12）;ax=Te（13）;px=Te（14）;
ny=Te（21）;oy=Te（22）;ay=Te（23）;py=Te（24）;
nz=Te（31）;oz=Te（32）;az=Te（33）;pz=Te（34）;
%由于建立得到的关节6的等式是非线性一元多次等式
%第一步设法寻找到关节6的有根区间
%关节6的取值范围【-180~180】
syms JointSixLimUp      %关节6转动范围的上限
syms JointSixLimDown    %关节6转动范围的下限
syms StepSizeAngle %步长，用于搜索有根区间
syms NumberStep    %步数
%以机器人TA1400为例
JointSixLimUp=180;
JointSixLimDown=-180;
StepSizeAngle=1;%步长设置为2
NumberStep=fix（（JointSixLimUp-JointSixLimDown）/StepSizeAngle）;%向零方向取整
%%{
%对余数进行处理
if（JointSixLimDown+StepSizeAngle*NumberStep    NumberStep=NumberStep+1;
end
%%}
syms A B%Asin（JointFive）+Bcos（JointFive）=0
syms FunctionJointSix%关节6转角的方程式
syms JointSixRootXn %寻找的有根区间左端 
syms JointSixRootXnn%寻找的有根区间右端
syms FunctionJointSixRootXn %区间左端时，判断方程式的正负号
syms FunctionJointSixRootXnn%区间右端时，判断方程式的正负号，用于寻找有根区间
JointSixRootXn=JointSixLimDown; %#ok
JointSixRootXnn=JointSixLimDown;%赋初值

for i=0:1:NumberStep
    JointSix=JointSixLimDown+i*StepSizeAngle;%计算当前的关节6的转角
    JointSixRootXnn=JointSix;%更新有根区间上限
    %计算关节6的正余弦值
    s6=sin（JointSix*pi/180）;
    c6=cos（JointSix*pi/180）;
    %求解关节1的转动角
    JointOne=atan2（py-（ny*c6-oy*s6）*a5px-（nx*c6-ox*s6）*a5）*180/pi;
    A=（（py*ox-px*oy）*s6+（px*ny-py*nx）*c6）*d4;
    B=（px*ay-py*ax+a5*（（ox*ay-oy*ax）*s6+（ny*ax-nx*ay）*c6））*d4;
    %求解关节5的转动角
    %计算关节5的第一个解
    %JointFive=atan2（B-A）*180/pi-atan2（0+sqrt（A^2+B^2））*180/pi;
    JointFive=atan2（B-A）*180/pi-atan2（-0-sqrt（A^2+B^2））*180/pi;
    %{
    if

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       11394  2020-09-23 22:03  ╞½╓┤└α╗·╞≈╚╦─µ╜Γ╦π╖¿\InverseOfOffsetWristTA1400.m
     文件        6180  2020-09-23 22:03  ╞½╓┤└α╗·╞≈╚╦─µ╜Γ╦π╖¿\SymbolicComputationbased.m
     文件       10438  2020-09-23 22:03  ╞½╓┤└α╗·╞≈╚╦─µ╜Γ╦π╖¿\ZZInverseOfOffsetWristTA14001.m