数值法求四自由度机器人逆运动学

0、前记：数值法逆解

系统图如下：

1、定义系统几何参数

%杆长
syms   L1Y_s L2Y_s L3Y_s 
%角度
syms   theta1  %link1 
syms   theta2  %link2
syms   theta3  %link3
syms   theta4 %转盘
%末端位置
syms XE YE ZE

2、计算正运动学【给定角度计算末端位置】