我面临的问题是尝试计算具有推算功能的机器人的x和y位置。

从编码器中读取数据,并使机器人的车轮正确旋转。机器人有3个轮子,其中3个可以控制,如图所示:



计算这些轮子旋转的距离以及这些距离的方向机器人已经过测试并且可以正常工作。以下是该标题的代码:

void MotorHandler::UpdateHeading(Position& currentPosition, double leftMotorDistance, double rightMotorDistance)
{
   currentPosition.heading = (double)((int)(((rightMotorDistance - leftMotorDistance) / (2 * PI * AXLE_LENGTH)) * FULL_ROTATION_DEGREES) % 360);
}


其中axis_length是两个车轮之间的长度,full_rotation_degree是360。当我尝试在任何给定时刻计算机器人的x和y时,都会得到奇怪的结果。以下是计算x和y的代码:

void MotorHandler::CalculateCurrentPosition(Position& currentPosition)
{
double leftMotorDistance = (motors[0]->GetRotation()/FULL_ROTATION_PULSES) * WHEEL_CIRCUMFERENCE;
double rightMotorDistance = (motors[1]->GetRotation()/FULL_ROTATION_PULSES) * WHEEL_CIRCUMFERENCE;

double headingToRadians = (currentPosition.heading * PI) / 180;

if (fabs(leftMotorDistance - rightMotorDistance) < 1.0e-6)
{
    currentPosition.X = leftMotorDistance * cos(headingToRadians);
    currentPosition.Y = rightMotorDistance * sin(headingToRadians);
}
else
{
    double wd = (rightMotorDistance - leftMotorDistance) / AXLE_LENGTH;
    double expr = (AXLE_LENGTH * (rightMotorDistance + leftMotorDistance)) / (2 * (rightMotorDistance - leftMotorDistance));

    currentPosition.X = expr * sin(wd + headingToRadians) - expr * sin(headingToRadians);
    currentPosition.Y = -expr * cos(wd + headingToRadians) + expr *  cos(headingToRadians);
    UpdateHeading(currentPosition, leftMotorDistance, rightMotorDistance);
}
}


其中起始x,y和航向均为0。如上所述,计算机器人航向正常,这意味着车轮的距离也可以正确计算并且可以正常工作。

使用的链接: archive / fall11 / cos495 / COS495-Lecture5-Odometry.pdf
http://rossum.sourceforge.net/papers/DiffSteer/计算差动驱动机器人的位置

Ty!

评论

乍看之下,我想说的是,为了进行推算,您需要在某个位置累积值,否则您将只计算与上一个时间步的相对位置。
无需查看上一步与现在之间的区别,我只是从头开始查看所有内容并进行分配。车轮的旋转不断增加,因此机器人也行进了距离。

#1 楼

这是我在这里写的更长答案的另一个方面。简单地说,您的问题是这样的轮式机器人是不完整的,这意味着您可以使用绝对编码器计数并获得有效的位置和航向结果。

您有两个控制轴:左轮和右轮;并且您具有三个自由度:向左/向右平移,向前/向后平移和旋转。

我将重申另一个答案中给出的示例。考虑:


向前移动10米并旋转180度,= OR =

向前旋转10度。

这些方案如何在您的代码中起作用?看来您的航向可能是正确计算的,但是您的位置仅基于当前航向。在上述两种情况下,机器人都会旋转180度后结束。在第二种情况下,您的代码应该给出正确的答案,因为旋转之前没有平移。然后,随着机器人的旋转,您的代码将使用当前航向来索引行进的距离,因此,看起来好像您的机器人正在快速刻划半径10米的圆。

同样,如另一个答案所述,解决方案是按照提到的@ 50k4进行操作,并累积当前位置作为每个样本的差分量,而不仅仅是查看总数。

评论


$ \ begingroup $
是的,这是问题所在。将增量距离与绝对航向结合使用可以解决问题。而不是原始示例代码中所示的绝对距离。
$ \ endgroup $
–Mathijs F
17年12月19日在16:43

$ \ begingroup $
@MathijsF-很高兴您已解决问题!
$ \ endgroup $
–卡盘
17年12月19日在18:56