%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