牛顿迭代求解串联机器人运动学正解

%%牛顿_拉夫森
clc
clear all;
syms alpha beta z;
%输入初值
x0（1）=input（‘迭代初值alpha=‘）*pi/180;
x0（2）=input（‘初值beta=‘）*pi/180;
x0（3）=input（‘初值z=‘）;
%输入原始长度
L1=input（‘驱动1 L1=‘）;
L2=input（‘驱动2 L2=‘）;
L3=input（‘驱动3 L3=‘）;
Alpha=x0（1）;
Beta=x0（2）;
Z=x0（3）;
w=1;%迭代次数
while（1）
f1=L1-（（765*cos（beta）-237*sin（alpha）*sin（beta）-800）^2+（-237*cos（alpha）+280）^2+（-765*sin（beta）-237*sin（alpha）*cos（beta）+z）^2）^（1/2）;
f2=L2-（（765*cos（beta）+237*sin（alpha）*sin（beta）-800）^2+（237*cos（alpha）-280）^2+（-765*sin（beta）+237*sin（alpha）*cos（beta）+z）^2）^（1/2）;
f3=L3-（（-765*cos（beta）+800）^2+（765*sin（beta）+z）^2）^（1/2）;
f=[-f1;-f2;-f3];
ff1（1）=diff（f1alpha）
ff1（2）=diff（f1beta）;
ff1（3）=diff（f1z）;
ff2（1）=diff（f2alpha）;
ff2（2）=diff（f2beta）;
ff2（3）=diff（f2z）;
ff3（1）=diff（f3alpha）;
ff3（2）=diff（f3beta）;
ff3（3）=diff（f3z）;
ff1（1）=subs（ff1（1）{alphabetaz}{AlphaBetaZ}）
ff1（2）=subs（ff1（2）{alphabetaz}{AlphaBetaZ}）;
ff1（3）=subs（ff1（3）{alphabetaz}{AlphaBetaZ}）;
ff2（1）=subs（ff2（1）{alphabetaz}{AlphaBetaZ}）;
ff2（2）=subs（ff2（2）{alphabetaz}{Al