PlotClock“小贱钟”彻底研究——运笔算法选择

有了模拟器，就可以做一些针对性的试验了。

第一个比较感兴趣的问题是书写的控制算法，以及书写的效果。

因为之前制作模拟器，已经有了通过指标获得角度的算法。所以，首先能够想到的一个简单的方法，就是分别获取起止位置的角度差，然后控制摆臂从开始的角度运动到结束的角度，就可以完成书写的动作。
例如我们获得一个转动需要的角度

image.png

我们可以将它切分成很多的小的角度，这里示例切分为50份：

image.png
我们只要控制循环，每次转动一个小的角度，就可以大臣给我们的目标。假设我们需要画笔从A到B画一段线段。那么我们首先求出当画笔位于位置A时两个舵机的角度，假设为αA、θA。然后求出画笔在B点时两个舵机的角度，αB、θB。然后我们让两个舵机在相同的时间内，同步将角度分别从αA、θA转动到αB、θB，就应该能绘制出我们需要的线段AB了。

为了这个测试我们首先需要一个转换函数，将目标的坐标转换为两个舵机的角度。

具体的几何分析可以建之前的文章。因为实现的过程之中涉及到了具体的编程语言对最终结果的影响，所以我们需要使用具体的代码来进行讨论了。

//一级摆臂的长度
float L1 = 150; 
//二级摆臂的长度
float L2 = 190;
//持笔夹的长度
float L3 = 30;
//持笔夹与二级摆臂的夹角
float bata = 135;
//持笔夹端点T与左一级摆臂端点见的距离
float L4 = sqrt((sq(L3)+sq(L2))-(2*L3*L2*cos(radians(bata))));
//坐标系原点位置的X坐标
float origin_x = 250;
//坐标系原点位置的Y坐标
float origin_y = 500;
//右侧舵机转轴位置与原点的距离
float distance = 100;
//左一级摆臂与X轴的夹角
float alpha = 0;
//右一级摆臂与X周的夹角
float theta = 0;
//每次循环增加的步长
float step = 1;
//循环进行的次数
int count = 50;
//alpha角增加的步长
float alpha_step = 1;
//theta角增加的步长
float theta_step = 1;
//循环计数器
int counter = 0;

void setup()
{
  //设置窗口大小
  size(600,600);
//根据坐标计算alpha的初始角度
  alpha = degrees(point2alpha(from_x, from_y));
//根据坐标计算theta的初始角度
  theta = degrees(point2theta(from_x, from_y));
  
//根据坐标计算alpha的最终角度
  float alpha_to = degrees(point2alpha(to_x, to_y));
//根据坐标计算theta的最终角度
  float theta_to = degrees(point2theta(to_x, to_y));
  
//根据循环次数计算alpha、theta的步长
  alpha_step = (alpha_to-alpha)/count;
  theta_step = (theta_to-theta)/count;
//初始化循环计数器为0
  counter = 0;

  println("alpha:"+alpha+"  alpha_to:"+alpha_to+"   alpha_step:"+alpha_step+";  tehta:"+theta
      +"   theta_to:"+theta_to+"   theta_step:"+theta_step);
  
}

//根据持笔夹端点坐标计算alpha的角度
float point2alpha(float x, float y)
{
  //计算持笔夹端点t到原点o的距离to
  float to = sqrt(sq(x)+sq(y));
  //to与x轴的夹角
  float alpha2 = acos(x/to);
  //to与L1的夹角
  float alpha1 = acos((sq(L1)+sq(to)-sq(L4))/(2*L1*to));

  //alpha = 180-alpha1-alpha2
  return PI-alpha1-alpha2;
}

float point2theta(float x, float y)
{
  //计算L4与L2的夹角
  float temp_angle1 = acos((sq(L4)+sq(L2)-sq(L3))/(2*L4*L2));
  float to = sqrt(sq(x)+sq(y));
  //计算L4与L1的夹角
  float temp_angle2 = acos((sq(L4)+sq(L1)-sq(to))/(2*L4*L1));
  //计算L2与L1的夹角
  float temp_angle = temp_angle2-temp_angle1;
  //计算左右一级摆臂端点K与原点o的距离ko
  float ko = sqrt(sq(L2)+sq(L1)-2*L2*L1*cos(temp_angle));
  //计算L1与KO的夹角
  float gamma1 = acos((sq(L1)+sq(ko)-sq(L2))/(2*L1*ko));
  //计算ko与x轴的夹角
  float gamma = PI-gamma1-point2alpha(x,y);
  //计算左右一级摆臂端点K与右侧舵机转轴位置C的距离kc
  float kc = sqrt(sq(ko)+sq(distance)-2*ko*distance*cos(gamma));
  //计算kc与X轴的夹角
  float theta1 = acos((sq(distance)+sq(kc)-sq(ko))/(2*distance*kc));
  //计算有一级摆臂L1与kc的夹角
  float theta2 = acos((sq(kc)+sq(L1)-sq(L2))/(2*kc*L1));
  //theta= 180-theta1-theta2；
  return PI-theta1-theta2;
}

我们可以在我们的模拟器上进行验证。

drawline1.gif

设置的起始坐标是(0,170)，结束坐标是(150,170)目标是得到一条水平的直线，但是，应该是一条弧线。如下图：

image.png

为了验证他是否是个弧线，我从所有点的y坐标中，将最大最小值取出来。如果是水平的直线，最大最小值应该是同一个数。最终的结果如下：

轨迹中最小的Y坐标是：169.99994
轨迹中最大的Y坐标是：175.7333

我们可以看到最小值与我们设置的170很接近了，但是最大值有5.7333的偏差。

我分析误差应该与计算的精度有关，第一个想到的方法就是增加精度，将float修改为double提高计算精度。但是因为计算过程中使用到了radians函数，将角度转换为弧度。而radians函数接受的是float类型的参数，因此不能使用这个方法。

再想到一个方法，将计算过程进行内联，也就是将所有的计算过程合并到一行代码里，通过减少中间结果来提高进度。可以看到计算alpha的过程中我们使用了两个中间变量来保存两个角度alpha1和alpha2。最终输出：PI-(alpha1+alpha2)使用以下的三角函数公式可以将这个过程内联。

image.png

通过这个过程，我们可以减少一次弧度的计算，预期由此能够提高精度。
根据以上的思路，我们对代码进行改造：

float point2alpha(float x, float y)
{
  //alpha = 180-alpha1-alpha2
  
  float to = sqrt(sq(x)+sq(y));
  float v1 = x/to;
  float v2 = (sq(L1)+sq(to)-sq(L4))/(2*L1*to);
  float alpha2 = acos(v1);
  float alpha1 = acos(v2);
  float alpha12 = acos(v1*v2-sqrt(1-sq(v1)*sqrt(1-sq(v2))));
  
  
  println("v1:"+v1+"  v2:"+v2+"  alpha1:"+alpha1+"  alpha2:"+alpha2+"  alpha1+alpha2:"+(alpha1+alpha2) +"  alpha12:"+alpha12);
  return PI-alpha12;
}

这里特别将两种算法的结果(alpha1+alpha2)以及alpha12进行了输出方便比较，最终的到一下的结果：

v1:0.0 v2:0.12429381 alpha1:1.4461802 alpha2:1.5707964 alpha1+alpha2:3.0169766 alpha12:3.1415927

可以看到最终的结果相差0.13，两种算法的结果是不同的。使用新算法得到的图形如下：

image.png

这个结果，误差反而更大了，轨迹的终点甚至偏离了预订的坐标。使用内联方式计算的角度，偏差更大了。理论上来说他们的结果不应该偏差这么大的。

为了弄清楚这个问题，我反复确认了三角函数的公式以及对应的代码没有问题。再次尝试调整精度，因为processing中acos和sq函数只接受float数据，所以决定使用java再验证一次。
以下是java代码：

public class math_angle {
    double L1 = 150;
    double L2 = 190;
    double L3 = 30;
    double bata = 135;
    double L4 = Math.sqrt((Math.pow(L3,2)+Math.pow(L2,2))-(2*L3*L2*Math.cos(Math.toRadians(bata))));

    public static void main(String[] args) {
        math_angle ma = new math_angle();
        ma.point2alpha(0,170);

    }

    private void point2alpha(double x, double y){
        double to = Math.sqrt(Math.pow(x,2)+Math.pow(y,2));
        double v1 = x/to;
        double v2 = (Math.pow(L1,2)+Math.pow(to,2)-Math.pow(L4,2))/(2*L1*to);
        double alpha2 = Math.acos(v1);
        double alpha1 = Math.acos(v2);
        double alpha12 = Math.acos(v1*v2-Math.sqrt(1-Math.pow(v1,2))*Math.sqrt(1-Math.pow(v2,2)));
        System.out.println("v1:"+v1+"  v2:"+v2+"  alpha1:"+alpha1+"  alpha2:"+alpha2+"  alph1+alpha2:"+(alpha1+alpha2)
                +"  alpha12:"+alpha12);
    }
}

完全复刻了processing中的算法，只是将数据类型由float改为了double。使用同样的输入(0,170)得到以下的结果：

v1:0.0 v2:0.12429377832300716 alpha1:1.4461802683167755 alpha2:1.5707963267948966 alph1+alpha2:3.016976595111672 alpha12:3.016976595111672

可以看到在double精度下，内联算法与中间值算法的结果是一样的，三角函数公式并没有问题，而float的精度下，内联算法是有问题的。float精度下，接近正确值的是非内联的算法。因此想要通过内联的方式提高进度的方法在这里是不适用的。

通过观察画图的轨迹，我们可以发现，起点和终点是没有偏离的。因为这两个点是由目的坐标直接计算得到的。由此可以想到一种微分的思路，将画一条长线段，拆分为画很多条收尾相接的小线段。

于是，修改原来的算法，将需要移动的距离切分为小段，分段计算需要转动的的角度。

  if(counter<count){
    //tx为新的目标x指标，因为画的是水平的直线，先简化只求x坐标，Y做表固定。step为增加的步长
    tx += step;
    //对于每个步长的移动，每次重新计算角度
    alpha = degrees(point2alpha(tx,170));
    theta = degrees(point2theta(tx,170));
    counter++;
  }else{
    noLoop();
  }

运行新的代码得到一下的结果：

image.png

可以看到虽然得到了一条平直的带锯齿的线段，在考察轨迹的最大最小Y坐标：

轨迹最小Y坐标值：169.99982
轨迹最大Y坐标值：170.00009
已经可以算是条比较理想的平直线段了。

通过以上的比较，可以确定使用微分线段的方法来控制书写的轨迹是比较恰当的。