python实现两轮小车差分驱动在直角坐标系内x、y轴坐标与theta角角度反馈

最新推荐文章于 2023-04-19 11:09:55 发布

__Bolide__

最新推荐文章于 2023-04-19 11:09:55 发布

阅读量193

点赞数 1

文章标签： python 开发语言自动化

本文链接：https://blog.csdn.net/m0_46195532/article/details/129952426

版权

该代码定义了一个名为Drive的类，用于模拟不同情况下的小车运动：直线运动、原地自旋和圆弧运动。diffdrive方法根据左轮和右轮的速度、时间和轮距计算小车的新位置和角度。在给定示例中，小车在1秒后的位置和角度被打印出来。

摘要由CSDN通过智能技术生成

import math


class Drive:
    """
        case1：v_l = v_r, V = v_r = v_l, R = ∞，小车做直线运动
        case2：v_l = -v_r, V = 0, R = 0, 小车原地自旋
        case3：左右轮速度不等，V = (v_r + v_l) / 2, R = l * (v_l + v_r) / (v_l - v_r), 小车做圆弧运动
    """
    x_old = float
    y_old = float
    theta_old = float

    def __init__(self, x_o: float, y_o: float, theta_o: float):
        self.x_old = x_o
        self.y_old = y_o
        self.theta_old = theta_o
        print('初始参数：\n初始x轴坐标：%.5f\n初始y轴坐标：%.5f\n初始theta角：%.5f\n' % (
            self.x_old, self.y_old, self.theta_old))

    def diffdrive(self, v_l: float, v_r: float, t: float, wheelbase: float = 1.0) -> tuple:
        """
        :param wheelbase:两轮间距
        :param v_l: 左轮速度
        :param v_r: 邮轮速度
        :param t: 行驶时间
        :return: 小车运动结束后的x轴坐标x_new、y轴坐标y_new、车头与x轴夹角theta_new
        """
        x = self.x_old  # 小车初始
        y = self.y_old  # 小车初始y轴坐标
        theta = self.theta_old  # 小车车头朝向与x轴的初始夹角
        theta_change = 0
        v_gap = v_l - v_r
        v_sum = v_l + v_r
        if v_l == v_r:  # case1：直线
            v = v_r
            v_x = math.cos(theta) * v  # x轴方向速度分量
            v_y = math.sin(theta) * v  # y轴方向速度分量
            x_new = x + v_x * t
            y_new = y + v_y * t
        elif v_l == -v_r:  # case2：原地转
            x_new = x  # x轴坐标不变
            y_new = y  # y轴坐标不变
        else:
            theta_change = v_gap / wheelbase
            x_new = math.cos(theta_change) * v_sum / 2
            y_new = math.sin(theta_change) * v_sum / 2
        theta_new = theta + theta_change
        if theta_new >= 2 * math.pi:
            theta_new -= 2 * math.pi

        return x_new, y_new, theta_new


if __name__ == '__main__':
    drive = Drive(1, 2, math.pi / 2)
    x_n, y_n, theta_n = drive.diffdrive(1, 1, 1)
    print('运动结束后参数：\n结束x轴坐标：%.5f\n结束y轴坐标：%.5f\n结束theta角：%.5f\n' % (x_n, y_n, theta_n))