机器人机械臂问题求教

用D-H参数做的，正运动学算过正确，也就是通过改变关节脚求出空间位置，完全合乎预期。即D-H参数是正确的，有4个关节，其方程如下所示
A1=[cos(r1) -sin(r1) 0 0;sin(r1) cos(r1) 0 0;0 0 1 0;0 0 0 1]* [1 0 0  0;0 1 0 0;0 0 1 0 ;0 0 0 1] * [1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1]
A2=[cos(r2) -sin(r2) 0 0;sin(r2) cos(r2) 0 0;0 0 1 0;0 0 0 1]* [1 0 0  a2;0 1 0 0;0 0 1 0;0 0 0 1] * [1 0 0 0;0 1  0 0;0 0 1 0;0 0 0 1]
A3=[cos(r3) -sin(r3) 0 0;sin(r3) cos(r3) 0 0;0 0 1 0;0 0 0 1]* [1 0 0  a3;0 1 0 0;0 0 1 0;0 0 0 1] * [1 0 0 0;0 1  0 0;0 0 1 0;0 0 0 1]
A4=[cos(r4) -sin(r4) 0 0;sin(r4) cos(r4) 0 0;0 0 1 0;0 0 0 1]* [1 0 0  a4;0 1 0 0;0 0 1 0;0 0 0 1] * [1 0 0 0;0 1  0 0;0 0 1 0;0 0 0 1]
现在在求逆运算，卡在求第二个关节的角度上，s234和c234求不出来，麻烦哪位这方面的大神出手相助。

我也在用单片机做机器人控制，可不可以交流一下，，，