让机器人指向正确的方向

Question

我正在使用无线电从基站指挥机器人。基站使用机器人上的 AR 标签（使用 openCV）从头顶摄像头获取 location/orientation 信息。此外，base 会根据位置信息计算机器人到达目标所需的路径（A*，每个网格在相机中为 30 x 30 像素）。我的机器人只能转left/right（在它的中心点）然后走forward/backward。机器人包括一个带有两个 Lego NXT 电机的 Arduino Uno。

我使用以下代码让机器人指向正确的方向。然而，当机器人接近它应该行进的角度时，它并没有停止前进，而是试图无限地固定它的方向。

    def correctOrientation(self, rx: int, ry: int):
        #returns direction robot needs to point.
        direction = self.getDirection((self.sx, self.sy), (rx, ry))
        #method to stop robot.
        self.comms.stop()

        anglediff = (self.angle - direction + 180 + 360) % 360 - 180

        while not (abs(anglediff) < 15):
            #Decides which way to turn.
            if self.isTurnLeft(self.angle, direction):
                self.comms.turnLeft()
            else:
                self.comms.turnRight()
            #Put sleeps because there is a delay in camera feed. Allows it to get the location right
            time.sleep(0.3)
            self.comms.stop()
            #Updates position
            self.getPos()
            time.sleep(1)
            #Calculates orientation of robot and updates it
            self.angle = self.calcOrientation()
            anglediff = (self.angle - direction + 180 + 360) % 360 - 180
            print(anglediff)
            time.sleep(1)

我用到的辅助函数。我通过使用机器人上已知的两个点并在这两点之间画一条线来计算机器人的方向。因此，直线与方向平行。

    def isTurnLeft(self, angle, touchAngle):
        diff = touchAngle - angle
        if diff < 0:
            diff += 360
        if diff < 180:
           return False
        else:
            return True

    def calcOrientation(self) -> float:
        return self.getDirection(self.marker[0], self.marker[3])

    def getDirection(self, source: Tuple[int], target: Tuple[int]) -> float :
        return (math.degrees(math.atan2(target[1] - source[1], target[0] - source[0]))+360)%360

我搞不懂我的代码是不是逻辑有问题。如果是这样，我该怎么办？如果代码没问题，问题出在系统的 delay/setup 上，我还有什么其他方法可以控制机器人？

感谢您的帮助。

Answer 1

通过更改AR标签识别的图片库解决了该问题。我们为一个机器人使用了 2 个标签。它明显更慢并且容易检测到两个标签。将其更新为只有一个标签。此外，从基于角度的计算切换到基于矢量的计算，这更容易理解。

让机器人指向正确的方向

Getting Robot to Point to Correct Direction

python

robotics

opencv

arduino

path-finding