-1

//这是一个寻找机械臂inv运动学的程序。我的怀疑不在kine:-)。我的程序执行但总是打印“无法实现的位置”，即使对于已知值，它也是如此。在这里，我使用弧度表示角度，因为这就是 c 所理解的。请告诉我如何遇到这个，甚至告诉我更好的方法来实现这个。谢谢。// 现在，这个编辑过的程序可以工作，但是速度很慢.. 有人可以建议一种优化它的方法吗？

#include<stdio.h>
#include<math.h>
#include<conio.h>
int main()
{
    float a, b, c, d;
    int a1, b1, c1, d1;
    int x = 0, y = 0, z = 0;
    int x1 = 0, y1 = 0, z1 = 0;
    int i = 0, j = 0;
    printf("Enter X value: \n");
    scanf("%d", &x);
    printf("Enter Y value: \n");
    scanf("%d", &y);
    printf("Enter Z value: \n");
    scanf("%d", &z);
    for (a1 = -100 ; a1 <= 100 ; a1++)
    {
            printf("%d \t", j++);
            for (b1 = 0; b1 <= 180; b1++)
        {
            for (c1 = -100; c1 <= 100; c1++)
            {
                              for (d1 = -45; d1 <= 45; d1++)
                      {
                       a = a1 * 0.0174532925;
                       b = b1 * 0.0174532925;
                       c = c1 * 0.0174532925;
                       d = d1 * 0.0174532925;
                               x1 = (11*cos(d+c+b+a)+11*cos(d+c+b-a)+12*cos(c+b+a)+12*cos(c+b-a)+9*cos(b+a)+9*cos(b-a))/2;
                               if(x1 == x)
                               {
                                           y1 =(11*sin(d+c+b+a)-11*sin(d+c+b-a)+12*sin(c+b+a)-12*sin(c+b-a)+9*sin(b+a)-9*sin(b-a))/2;
                                           if(y1 == y)
                                           {
                                                 z1 = 11*sin(d+c+b) + 12*sin(c+b) + 9*sin(b);
                                                 if(z1 == z)
                                                 {
                              i = 1;
                              goto status;
                                                 }
                                            }
                                  }
                }
            }
        }
    }
    status:
    if(i == 0)
    printf("*****Positon unacheivable*****");
    else
        printf(" The joint angles for the desired positon are \n %d \t %d \t %d \t %d \n", a1, b1, c1, d1);
        getch();

}

4

3 回答 3

1

首先或第二次阅读此内容，但请阅读： 每位计算机科学家应了解的浮点运算知识

0.02 在大多数计算机上无法准确表示。它将大致表示。在您的循环中，每次添加大约 0.02 时，您的舍入误差就会越来越大。

于 2012-08-28T04:48:15.557 回答
0

这是一个简单的爬山算法。它从 a=b=c=d=0 开始，测量到所需 x、y、z 的距离，然后对 a、b、c、d 进行小的随机调整，并保留那些提高距离的调整。

它将永远运行，除非在极少数情况下找到完全匹配；由您决定停止条件。如果需要，您还可以对随机性设置一些界限，以防止它选择超出允许范围的 a、b、c、d 值，因为我懒得把它放进去。

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

static double getdist(double x, double y, double z,
                      double a, double b, double c, double d)
{
        double x1, y1, z1;

        x1 = (11*cos(d+c+b+a)+11*cos(d+c+b-a)+12*cos(c+b+a)+12*cos(c+b-a)+9*cos(b+a)+9*cos(b-a))/2;
        y1 = (11*sin(d+c+b+a)-11*sin(d+c+b-a)+12*sin(c+b+a)-12*sin(c+b-a)+9*sin(b+a)-9*sin(b-a))/2;
        z1 = 11*sin(d+c+b) + 12*sin(c+b) + 9*sin(b);

        return (x1-x)*(x1-x) + (y1-y)*(y1-y) + (z1-z)*(z1-z);
}

int main(int argc, char **argv)
{
    char *end;
    double x, y, z, a, b, c, d, dist, best;

    if(argc != 4) {
        fprintf(stderr, "Usage: %s x y z\n", argv[0]);
        return EXIT_FAILURE;
    }

    x = strtod(argv[1], &end);
    if(*end || !*argv[1]) {
        fprintf(stderr, "x value %s is not a number\n", argv[1]);
        return EXIT_FAILURE;
    }
    y = strtod(argv[2], &end);
    if(*end || !*argv[2]) {
        fprintf(stderr, "y value %s is not a number\n", argv[2]);
        return EXIT_FAILURE;
    }
    z = strtod(argv[3], &end);
    if(*end || !*argv[3]) {
        fprintf(stderr, "z value %s is not a number\n", argv[3]);
        return EXIT_FAILURE;
    }

    a = b = c = d = 0;
    best = getdist(x,y,z,a,b,c,d);
    printf("a=%f b=%f c=%f d=%f (dist=%f)\n", a, b, c, d, sqrt(best));
    srand(time(0));

    while(best) {
        double save_a, save_b, save_c, save_d;

        save_a = a;
        save_b = b;
        save_c = c;
        save_d = d;

        a += rand()*1./RAND_MAX*.02 - .01;
        b += rand()*1./RAND_MAX*.02 - .01;
        c += rand()*1./RAND_MAX*.02 - .01;
        d += rand()*1./RAND_MAX*.02 - .01;

        dist = getdist(x,y,z,a,b,c,d);

        if(dist < best) {
            best = dist;
            printf("a=%f b=%f c=%f d=%f (dist=%f)\n", a, b, c, d, sqrt(best));
        } else {
            a = save_a;
            b = save_b;
            c = save_c;
            d = save_d;
        }
    }
    return 0;
}
于 2012-08-28T07:08:22.840 回答
0

您要求速度优化：

你有

for (a...)
   for (b..)
      for (c...)
         for (d...)
            ...
            11*cos(d+c+b+a) + 9*cos(b+a) + 12*cos(c+b+a)
            ...

更好的是

for (a...)
   for (b..)
      mybpa = 9*cos(b+a)
      for (c...)
         mycpbpa = 12*cos(c+b+a)
         for (d...)
            mydpcpbpa = 11*cos(d+c+b+a)
            ...
            mydpcpbpa + mycpbpa + mybpa
            ...

所以你尽早做耗时的计算。当它们达到该水平时，您应该计算所有这些值，它们在下一个循环中不会再改变。你甚至应该9*cos(b+a)+9*cos(b-a)在它们没有改变时立即进行小计

于 2012-08-28T06:57:33.653 回答