iterati.m

来自「三臂机器人仿真程序」· M 代码 · 共 52 行

function [w1,t]=iterati(w1,p,l)

%Diese Programm beechnet den naechsten Iterationswert fuer
%die Winkelwerte (Vektor w1). Ist diese Berechnung nicht moeglich,
%d.h. die Funktionalmatrix ist nicht invertierbar, so wird der
%Kontrollparameter t=0 zurueckgegeben, sonst t=1.

%Aufstellen der Funktionalmatrix D
 D=ones(3);
 D(1,1)=-l(1,1)*sin(w1(1,1))+l(2,1)*sin(w1(1,1)+w1(2,1))-...
         l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1));
 D(1,2)= l(2,1)*sin(w1(1,1)+w1(2,1))-...
         l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1));
 D(1,3)=-l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1));
 D(2,1)= l(1,1)*cos(w1(1,1))-l(2,1)*cos(w1(1,1)+w1(2,1))+...
         l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1));
 D(2,2)=-l(2,1)*cos(w1(1,1)+w1(2,1))+...
         l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1));
 D(2,3)= l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1));

%Falls die Determinante zu D=0 ausfaellt (if), wird der
%Parameter t=0 gesetzt, ansonsten wird mit der Berechnung
%der naechsten Iterationswerte fuer die Winkel (Vektor w1)
%fortgefahren.
 if det(D)==0
   t=0;
 else
   t=1;
  %Berechnung der Inversen Matrix I zu D
   E=eye(3,3);
   I=D\E;
  %Berechnung des Funktionswertes an der Stelle w1_n
    f(1,1)=l(1,1)*cos(w1(1,1))-l(2,1)*cos(w1(1,1)+w1(2,1))+...
           l(3,1)*cos(w1(1,1)+w1(2,1)+w1(3,1))-p(1,1);
    f(2,1)=l(1,1)*sin(w1(1,1))-l(2,1)*sin(w1(1,1)+w1(2,1))+...
           l(3,1)*sin(w1(1,1)+w1(2,1)+w1(3,1))-p(2,1);
    f(3,1)=w1(1,1)+w1(2,1)+w1(3,1)-2*pi-(p(3,1)*pi)/180;
  %Berechnung und Rueckgabe der neuen Winkelwerte w1_n+1
    w1=w1-I*f;
 end                %Ende der if-Struktur