我想使用里程计系统通过每 10 毫秒查找机器人的位移来随时查找我的机器人所在的位置。 为此，我使用了一个垂直跟踪轮，用于跟踪机器人相对于机器人向前或向后移动的时间；一个水平跟踪轮，用于跟踪机器人相对于机器人左右移动的时间；以及一个方向传感器，用于跟踪机器人相对于机器人的左右移动。始终给出机器人的方向（方向传感器没有界限）。 我使用vertical_rotation_sensor.get_position() / 5729.57795131获取垂直跟踪轮移动了多少（转换为英寸），使用horizontal_rotation_sensor.get_position() / 5729.57795131获取水平跟踪轮移动了多少（转换为英寸），以及imu .get_rotation() * M_PI / 180 获取当前方向（以弧度为单位）。 垂直跟踪轮偏离中心 -10 英寸，水平偏离 4.21875 英寸。 我用C++。
我尝试通过用弧线计算轮子的运动来计算运动。 我预计数字会相差 +- 0.1 - 0.5 英寸。 当我向任何方向直线移动时，我的代码运行得很好，但是当我将机器人转动几英寸时。 例如，我向前行驶 24 英寸，我的机器人说我在 x = 24.03 和 y = 0.05 处，但当我转动一圈时，确保始终保持在圆的路径上，然后移回到起点，我得到了x = .49 且 y = -4.45。 这是我的代码。
我首先找到每个轮子和方向传感器的运动变化。 **这部分有效。 **
double total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
double total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
double theta = imu.get_rotation() * M_PI / 180;
//finds changes in the tracking wheel's movement and orientation of the robot
double delta_wheel_movement_vertical = total_wheel_movement_vertical - previous_total_wheel_movement_vertical;
double delta_wheel_movement_horizontal = total_wheel_movement_horizontal - previous_total_wheel_movement_horizontal;
double delta_theta = theta - previous_theta;
//updates previous values for the next calculations
previous_total_wheel_movement_vertical = total_wheel_movement_vertical;
previous_total_wheel_movement_horizontal = total_wheel_movement_horizontal;
previous_theta = theta;
然后，我假设机器人已沿弧线移动并使用每个轮子弧的弦长来计算相对于机器人的 x 和 y 运动，或者如果机器人尚未转动，我将运动计算为直线。
//calculates the chord length of the arc / the length of the movement of the tracking center
if (fabs(delta_theta) != 0) {
delta_local_y = 2 * sin(delta_theta / 2) * (delta_wheel_movement_vertical / delta_theta + tracking_wheel_offset_vertical);
delta_local_x = 2 * sin(delta_theta / 2) * (delta_wheel_movement_horizontal / delta_theta + tracking_wheel_offset_horizontal);
} else {
delta_local_y = delta_wheel_movement_vertical;
delta_local_x = delta_wheel_movement_horizontal;
}
然后，我计算弧的平均角度。 这将是我最终计算时使用的角度。
double average_theta = previous_theta - delta_theta / 2;
最后，我将圆弧的平均角度和每个圆弧的长度代入公式，找到两条垂直线的端点，并将其添加到起始全局位置以找到新的全局位置。
x += delta_local_y * cos(average_theta) - delta_local_x * sin(average_theta);
y += delta_local_y * sin(average_theta) + delta_local_x * cos(average_theta);
为了完整性，这是我的所有代码。
//sets tracking wheel constants
const double tracking_wheel_offset_vertical = -10; //offset of tracking wheels from the tracking center of the robot in inches
const double tracking_wheel_offset_horizontal = 4.21875; //offset of tracking wheels from the tracking center of the robot in inches
//sets autonomous starting values
double x = 0; //global x position
double y = 0; //global y position
double delta_local_y = 0; //vertical chord length of the arc / movement of the tracking center
double delta_local_x = 0; //horizontal chord length of the arc / movement of the tracking center
//function to update the robot's estimated position
void update_pose() {
//sets previous values to current values for intial calculations
double previous_total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
double previous_total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
double previous_theta = imu.get_rotation() * M_PI / 180;
while (true) {
//delays the start of each cycle to save computing resources
pros::delay(10);
//gets current values of the tracking wheel in inches and the orientation of the robot in radians
double total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
double total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
double theta = imu.get_rotation() * M_PI / 180;
//finds changes in the tracking wheel's movement and orientation of the robot
double delta_wheel_movement_vertical = total_wheel_movement_vertical - previous_total_wheel_movement_vertical;
double delta_wheel_movement_horizontal = total_wheel_movement_horizontal - previous_total_wheel_movement_horizontal;
double delta_theta = theta - previous_theta;
//updates previous values for the next calculations
previous_total_wheel_movement_vertical = total_wheel_movement_vertical;
previous_total_wheel_movement_horizontal = total_wheel_movement_horizontal;
previous_theta = theta;
//calculates the chord length of the arc / the length of the movement of the tracking center
if (fabs(delta_theta) != 0) {
delta_local_y = 2 * sin(delta_theta / 2) * (delta_wheel_movement_vertical / delta_theta + tracking_wheel_offset_vertical);
delta_local_x = 2 * sin(delta_theta / 2) * (delta_wheel_movement_horizontal / delta_theta + tracking_wheel_offset_horizontal);
} else {
delta_local_y = delta_wheel_movement_vertical;
delta_local_x = delta_wheel_movement_horizontal;
}
//calculates the angle of the chord of the arc / which direction the tracking center moved
double average_theta = previous_theta - delta_theta / 2;
//changes in the global position based on the chord's length and angle / the direction and distance the tracking center moved
x += delta_local_y * cos(average_theta) - delta_local_x * sin(average_theta);
y += delta_local_y * sin(average_theta) + delta_local_x * cos(average_theta);
//prints tracking and debuging values to screen
pros::lcd::print(0, "X: %f", x);
pros::lcd::print(1, "Y: %f", y);
pros::lcd::print(2, "Theta: %f", theta * 180 / M_PI);
pros::lcd::print(3, "Total Local X Movement: %f", total_wheel_movement_horizontal);
pros::lcd::print(4, "Total Local Y Movement: %f", total_wheel_movement_vertical);
}
}```
[1]: http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf
我发现问题在于我将偏移量配置到了机器人的一个角落，因为我误解了 E-Bots πlons 的位置跟踪论文中的一段（不知道如何添加链接）。我虽然证明了跟踪中心在哪里并不重要，但它证明了你只需要知道跟踪中心的垂直长度。 感谢马丁·布朗让我再次回顾这篇论文。