最近在做一个四自由度的机械臂，要实现的功能是，通过输入XYZ三轴的坐标值，让机械臂自动导航到坐标位置。
原理一句话可以概括：输入三个坐标值，通过计算得到底部步进电机的旋转角度和剩余三个舵机的旋转角度。
![我使用的四轴的机械臂]()
底座是步进电机，往上依次是三个舵机，用的是180度的舵机，因此有一些限制，到底有什么限制自己想象。每个舵机连接一个连杆，从下到上依次称为舵机5 ，4 ，3 ，由它们控制的杆称为L1,L2,L3。从图片可以看出来，三个舵机是完全在一个竖直平面上的，这个平面与底座平面垂直，那么就以底座平面为基准，建立X ,Y轴， Z轴就垂直于底座向上。
现在可以思考一下如何导航到空间中的任何一个点？空间中任何一个点都可以用两个角度表示，一个叫**横偏角**，另外一个叫**俯仰角**，任何三维坐标都可以用这两个角表示。
想象一下，第一步，底座步进电机可以让三舵机机械臂平面旋转任意角度，这个角度就是上面说的横偏角，第二步，三舵机的三杆通过运动可以让机械臂头部到达它们所在平面的任何一个位置（力所能及的位置，不对 ，是臂所能及！），这个目标位置与XOY平面的夹角就是上面说的俯仰角，那么就达到了目的，现在空间中整个球体的任何位置都可以通过 1 底座步进电机旋转 一个横偏角度 2 三杆运动之后一个头部与XOY平面俯仰角。道理就是这么个道理。
现在，已知条件是：目标点的X，Y，Z值，还有机械臂L1， L2， L3，的长度，通过上面的分析知道，第一步要做的就是让步进电机转过一个横偏角，计算这个横偏角很简单，这个角就是：目标点在XOY平面上的投影点与X轴的夹角对吧，现在这个变量名为
target_angle.Last_AngleStep（我定义了一个结构体，里面包含最终计算出来的三舵机角度以及步进电机横偏角）
那么
target_angle.Last_AngleStep =atan(Y/X)180/3.141593;
X， Y就是你输入的目标点的XY坐标
因为math.h库里面的三角函数输出的是弧度值，所以要180/3.141593转换成角度
第一步，我们已经转到了目标点所在的竖直平面。
接下来第二步，我们所有的计算都是基于接下来的这个竖直平面了，所以我们可以在这个竖直平面再建立一个平面坐标系，坐标系的横轴X2就是竖直平面与原空间坐标系XOY平面的交线，坐标系的纵轴Z2就是原空间坐标系的Z轴，我们称它为第二坐标系，现在我们要在第二坐标系中找到我们的目标点Target(X2,Z2)
Z2不用计算，就是输入的原空间坐标系Z坐标
X2怎么办？X2不就等于XX+YY然后再开方吗？就是个直角三角形
X2=sqrt(pow(X,2)+pow(Y,2));
现在，第二坐标系中目标点的横纵坐标也已经确定了，接下来的问题就是，如何规划三杆的角度，让它们合理的到达点（X2,Z2）呢？
如图 我们如果要到达正方体的B点，那么 横偏角就是0度了，X2=X；Z2=Z;
从o点开始从下往上，依次是L1，L2，L3，现在把这个平面拿出来如下图所示
可以看出来，俯仰角就是图中的角1，有人肯定有疑问，为什么不让L1直接指向B点呢，那样不就少了个角2吗？岂不是计算更方便？为什么要多此一举，多出来个角2呢？当然可以直接指向B点，但是如果L1直接指向B点的话，L2与L3就会挤得特别近，如果舵机是不太好的舵机，那么你计算出来的舵机角度，实际上舵机是无法到达那样的一个角度，在图中这种情况是可以直接指向B点的，但是如果B点的位置离O点特别近呢？如果L5<L1，这时候就必须用到角2了，那么，问题又来了，角2的大小是如何确定的？或者说角2的范围怎么确定？
目标点B的位置是不变的，O点位置是不变的，现在让L1从角1开始逆时针旋转，一直旋转，直到L2,L3被拉直成一条杆，此时，就是角2的最大角度，现在来计算角2的最大值：
杆L1，杆（L2+L3）,OB的长度L5在此时组成一个三角形，L1,L2,L3,L5长度都是已知的，
通过余弦定理直接计算出角2的最大值就可以了
cos(x)=(aa+bb-cc)/(2ab);
a, b是角x的临边，c是对边
上面是余弦定理，贴心不？？这么难的公式都给你们贴了出来哈哈哈哈哈哈哈
就这个高中用了三年的公式我还真给忘得一干二净，每次用都要上网搜一哈。。。
通过x=acos((aa+bb-cc)/(2a*b))就可以计算出x的值
同样要注意，acos函数输出的仍然是弧度值，要换算单位的。
此时角2的最大值已经计算完成，那么具体角2要偏移多少就根据你的实际情况而定了，我是偏移了（角2）/2的大小。
既然角2已经确定下来，那么L4的大小如何计算？同样是余弦定理，已知L1,L5和角2，求对边L4也很容易，此时整个机械臂骨架已经是确定的了，要求L2相对L1的偏移角度（舵机4角度）以及L3相对L2的偏移角度（舵机3角度）也是容易的事
，同样都是用余弦定理。
`/*************************************
函数功能：输入一个空间坐标，将机械臂头部自动导航到坐标处(由于舵机是180度舵机，因此存在盲区)
****************************/
double x2=0,z2=0; //第二坐标系的坐标值
float Angle_TargetToX2=0;
float Length_4_1=0;
float L1=10,L2=10,L3=14;
void Machine_AutoNavigation(float x,float y,float z)
{
float Max_OffsetAngleOf_DuoJi5_To_Aim=0;//舵机5的杆与X2轴的角度 偏移值
float Length_DuoJi5ToAim=0;//舵机5到目标点的距离
//判断xyz的有效性
if(pow(L1+L2+L3,2)<pow(x,2)+pow(y,2)+pow(z,2))//超出范围直接退出
return;
//由第一坐标系的点计算得到第二坐标系的点
x2=sqrt(xx+yy);
z2=z;
Length_DuoJi5ToAim=sqrt(z2z2+xx+yy);
if((L2+L3-L1)<Length_DuoJi5ToAim)//两边之差要小于第三边
Max_OffsetAngleOf_DuoJi5_To_Aim=180/3.141593acos((L1L1+Length_DuoJi5ToAimLength_DuoJi5ToAim-(L2+L3)(L2+L3))/(2L1Length_DuoJi5ToAim));
else
Max_OffsetAngleOf_DuoJi5_To_Aim=180;
//求步进电机需要转动的角度
if(x==0)
{
if(y>0)
target_angle.Last_AngleStep=90;
else if(y<0)
target_angle.Last_AngleStep=-90;
}
else
{
target_angle.Last_AngleStep=atan(y/x)*180/3.141593;
if(x<0)
target_angle.Last_AngleStep+=180;
}
//求目标点到x2轴的角度
Angle_TargetToX2=atan(z2/x2)*180/3.141593;
//求舵机5的目标角度
target_angle.duoji5=180-(Angle_TargetToX2+Max_OffsetAngleOf_DuoJi5_To_Aim/2);//求舵机4与舵机1之间的长度
Length_4_1=sqrt( sqrt(x2*x2+z2*z2) * sqrt(x2*x2+z2*z2)+L1*L1- cos(Max_OffsetAngleOf_DuoJi5_To_Aim/2*3.141592/180) *2*L1*sqrt(x2*x2+z2*z2) );
//求舵机4的目标角度
target_angle.duoji4=(180-90-Max_OffsetAngleOf_DuoJi5_To_Aim/2-180/3.141592*acos( ( Length_4_1*Length_4_1+ sqrt(x2*x2+z2*z2) * sqrt(x2*x2+z2*z2)-L1*L1 )/(2*sqrt(x2*x2+z2*z2)*Length_4_1) ))+180/3.141592*acos((L2*L2+Length_4_1*Length_4_1-L3*L3)/(2*L2*Length_4_1));
//求舵机3的角度
target_angle.duoji3=180/3.141592*acos((L2*L2+L3*L3-(Length_4_1*Length_4_1))/(2*L2*L3))-25;
//if()
//DuoJi_Angle_Control(5,Angle_DuoJi5);
//DuoJi_Angle_Control(4,Angle_DuoJi4);
//DuoJi_Angle_Control(3,Angle_DuoJi3);}`
以上就是我的代码，看不看的懂都没关系，因为写的乱的很，你肯定看不懂，但上面已经讲的很明白了，自己动手写一个是没有问题的。
