第三章 运动控制

import numpy as np

class DifferentialRobot:
    def __init__(self, wheel_base, wheel_radius):
        self.L = wheel_base      # 轮距
        self.R = wheel_radius    # 轮半径
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

    def forward_kinematics(self, v_left, v_right, dt):
        """正运动学"""
        # 计算机器人速度
        v = (v_left + v_right) / 2.0
        omega = (v_right - v_left) / self.L

        # 更新位姿
        self.x += v * np.cos(self.theta) * dt
        self.y += v * np.sin(self.theta) * dt
        self.theta += omega * dt

        return v, omega

    def inverse_kinematics(self, v, omega):
        """逆运动学"""
        v_left = v - omega * self.L / 2.0
        v_right = v + omega * self.L / 2.0
        return v_left, v_right

    def get_pose(self):
        return self.x, self.y, self.theta

# 使用示例
robot = DifferentialRobot(wheel_base=0.3, wheel_radius=0.05)

# 已知左右轮速,计算机器人速度
v, omega = robot.forward_kinematics(v_left=1.0, v_right=1.2, dt=0.1)
print(f"线速度: {v:.2f} m/s, 角速度: {omega:.2f} rad/s")

# 已知期望速度,计算左右轮速
v_left, v_right = robot.inverse_kinematics(v=1.0, omega=0.5)
print(f"左轮: {v_left:.2f} m/s, 右轮: {v_right:.2f} m/s")

def turning_radius(v, omega):
    """计算转弯半径"""
    if abs(omega) < 1e-6:
        return float('inf')  # 直线运动
    return abs(v / omega)

# 示例
R = turning_radius(v=1.0, omega=0.5)
print(f"转弯半径: {R:.2f} m")

class OmniThreeWheel:
    def __init__(self, wheel_radius, robot_radius):
        self.r = wheel_radius     # 轮半径
        self.L = robot_radius     # 机器人半径

    def inverse_kinematics(self, vx, vy, omega):
        """逆运动学: 机器人速度 → 轮速"""
        # 120度分布
        v1 = -0.5 * vx + 0.866 * vy + self.L * omega
        v2 = -0.5 * vx - 0.866 * vy + self.L * omega
        v3 = vx + self.L * omega

        # 转换为轮速(rad/s)
        w1 = v1 / self.r
        w2 = v2 / self.r
        w3 = v3 / self.r

        return w1, w2, w3

    def forward_kinematics(self, w1, w2, w3):
        """正运动学: 轮速 → 机器人速度"""
        # 轮速转线速度
        v1 = w1 * self.r
        v2 = w2 * self.r
        v3 = w3 * self.r

        # 解算机器人速度
        vx = (-v1 - v2 + 2 * v3) / 3.0
        vy = (v1 - v2) / 1.732
        omega = (v1 + v2 + v3) / (3.0 * self.L)

        return vx, vy, omega

# 使用示例
robot = OmniThreeWheel(wheel_radius=0.05, robot_radius=0.2)

# 期望向右前方移动
w1, w2, w3 = robot.inverse_kinematics(vx=0.5, vy=0.3, omega=0.0)
print(f"轮速: {w1:.2f}, {w2:.2f}, {w3:.2f} rad/s")

class MecanumWheel:
    def __init__(self, wheel_base_x, wheel_base_y, wheel_radius):
        self.lx = wheel_base_x / 2  # 半轮距(x方向)
        self.ly = wheel_base_y / 2  # 半轮距(y方向)
        self.r = wheel_radius

    def inverse_kinematics(self, vx, vy, omega):
        """X型配置麦轮逆运动学"""
        # 四个轮子: 左前、右前、左后、右后
        v_lf = vx - vy - (self.lx + self.ly) * omega
        v_rf = vx + vy + (self.lx + self.ly) * omega
        v_lr = vx + vy - (self.lx + self.ly) * omega
        v_rr = vx - vy + (self.lx + self.ly) * omega

        # 转换为轮速
        w_lf = v_lf / self.r
        w_rf = v_rf / self.r
        w_lr = v_lr / self.r
        w_rr = v_rr / self.r

        return w_lf, w_rf, w_lr, w_rr

    def forward_kinematics(self, w_lf, w_rf, w_lr, w_rr):
        """正运动学"""
        v_lf = w_lf * self.r
        v_rf = w_rf * self.r
        v_lr = w_lr * self.r
        v_rr = w_rr * self.r

        vx = (v_lf + v_rf + v_lr + v_rr) / 4.0
        vy = (-v_lf + v_rf + v_lr - v_rr) / 4.0
        omega = (-v_lf + v_rf - v_lr + v_rr) / (4.0 * (self.lx + self.ly))

        return vx, vy, omega

# 使用示例
robot = MecanumWheel(wheel_base_x=0.4, wheel_base_y=0.3, wheel_radius=0.05)

# 横向移动(麦轮特色)
w_lf, w_rf, w_lr, w_rr = robot.inverse_kinematics(vx=0, vy=0.5, omega=0)
print(f"横向移动轮速: {w_lf:.2f}, {w_rf:.2f}, {w_lr:.2f}, {w_rr:.2f}")

import numpy as np

def dh_transform(theta, d, a, alpha):
    """D-H变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)

    T = np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d   ],
        [0,   0,      0,     1   ]
    ])
    return T

class RobotArm2DOF:
    """2自由度平面机械臂"""
    def __init__(self, L1, L2):
        self.L1 = L1  # 第一段长度
        self.L2 = L2  # 第二段长度

    def forward_kinematics(self, theta1, theta2):
        """正运动学: 关节角 → 末端位置"""
        x = self.L1 * np.cos(theta1) + self.L2 * np.cos(theta1 + theta2)
        y = self.L1 * np.sin(theta1) + self.L2 * np.sin(theta1 + theta2)
        return x, y

    def inverse_kinematics(self, x, y):
        """逆运动学: 末端位置 → 关节角"""
        # 余弦定理求theta2
        c2 = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)

        if abs(c2) > 1:
            raise ValueError("目标点超出工作空间")

        # 两种解(肘向上/肘向下)
        theta2_1 = np.arccos(c2)
        theta2_2 = -np.arccos(c2)

        # 对应的theta1
        k1 = self.L1 + self.L2 * np.cos(theta2_1)
        k2 = self.L2 * np.sin(theta2_1)
        theta1_1 = np.arctan2(y, x) - np.arctan2(k2, k1)

        k1 = self.L1 + self.L2 * np.cos(theta2_2)
        k2 = self.L2 * np.sin(theta2_2)
        theta1_2 = np.arctan2(y, x) - np.arctan2(k2, k1)

        return [(theta1_1, theta2_1), (theta1_2, theta2_2)]

    def jacobian(self, theta1, theta2):
        """雅可比矩阵"""
        J = np.array([
            [-self.L1*np.sin(theta1) - self.L2*np.sin(theta1+theta2),
             -self.L2*np.sin(theta1+theta2)],
            [self.L1*np.cos(theta1) + self.L2*np.cos(theta1+theta2),
             self.L2*np.cos(theta1+theta2)]
        ])
        return J

# 使用示例
arm = RobotArm2DOF(L1=1.0, L2=0.8)

# 正运动学
x, y = arm.forward_kinematics(theta1=np.pi/4, theta2=np.pi/6)
print(f"末端位置: ({x:.2f}, {y:.2f})")

# 逆运动学
solutions = arm.inverse_kinematics(x=1.2, y=0.8)
for i, (t1, t2) in enumerate(solutions):
    print(f"解{i+1}: theta1={np.degrees(t1):.2f}°, theta2={np.degrees(t2):.2f}°")

class PIDController:
    def __init__(self, Kp, Ki, Kd, output_limits=None):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.output_limits = output_limits

        self.integral = 0.0
        self.last_error = 0.0

    def update(self, error, dt):
        """计算PID输出"""
        # 比例项
        P = self.Kp * error

        # 积分项
        self.integral += error * dt
        I = self.Ki * self.integral

        # 微分项
        D = self.Kd * (error - self.last_error) / dt if dt > 0 else 0

        # 总输出
        output = P + I + D

        # 限幅
        if self.output_limits:
            output = np.clip(output, self.output_limits[0], self.output_limits[1])

            # 抗积分饱和
            if output >= self.output_limits[1] or output <= self.output_limits[0]:
                self.integral -= error * dt  # 回退积分

        self.last_error = error
        return output

    def reset(self):
        """重置控制器"""
        self.integral = 0.0
        self.last_error = 0.0

# 使用示例:电机速度控制
pid = PIDController(Kp=1.0, Ki=0.1, Kd=0.05, output_limits=(-255, 255))

target_speed = 1.0  # m/s
current_speed = 0.0
dt = 0.01  # 10ms

for _ in range(100):
    error = target_speed - current_speed
    pwm = pid.update(error, dt)

    # 模拟电机响应
    current_speed += pwm * 0.001

    print(f"Speed: {current_speed:.3f}, PWM: {pwm:.1f}")

def tune_pid_manually():
    """
    调参步骤:
    1. 设置Ki=0, Kd=0, 调节Kp
       - Kp过小: 响应慢
       - Kp过大: 震荡
       - 目标: 快速响应但略有超调

    2. 增加Ki消除稳态误差
       - Ki过小: 残余误差
       - Ki过大: 震荡、积分饱和
       - 目标: 稳态误差为0

    3. 增加Kd抑制超调
       - Kd过小: 超调大
       - Kd过大: 对噪声敏感
       - 目标: 减小超调,平滑响应
    """
    pass

# 典型值参考
# 电机速度控制: Kp=1.0, Ki=0.1, Kd=0.05
# 位置控制: Kp=5.0, Ki=0.5, Kd=0.2
# 温度控制: Kp=2.0, Ki=0.1, Kd=0.0

import heapq
import numpy as np

class AStarPlanner:
    def __init__(self, grid_map):
        """
        grid_map: 0=可通行, 1=障碍物
        """
        self.grid = np.array(grid_map)
        self.rows, self.cols = self.grid.shape

    def heuristic(self, node, goal):
        """启发式函数(曼哈顿距离)"""
        return abs(node[0] - goal[0]) + abs(node[1] - goal[1])

    def get_neighbors(self, node):
        """获取邻居节点(8方向)"""
        neighbors = []
        directions = [
            (-1, 0), (1, 0), (0, -1), (0, 1),  # 上下左右
            (-1, -1), (-1, 1), (1, -1), (1, 1)  # 对角线
        ]

        for dx, dy in directions:
            x, y = node[0] + dx, node[1] + dy

            if 0 <= x < self.rows and 0 <= y < self.cols:
                if self.grid[x, y] == 0:  # 可通行
                    cost = 1.414 if dx != 0 and dy != 0 else 1.0  # 对角线代价
                    neighbors.append(((x, y), cost))

        return neighbors

    def plan(self, start, goal):
        """A*路径规划"""
        # 初始化
        open_set = []
        heapq.heappush(open_set, (0, start))

        came_from = {}
        g_score = {start: 0}
        f_score = {start: self.heuristic(start, goal)}

        while open_set:
            current = heapq.heappop(open_set)[1]

            if current == goal:
                # 重建路径
                path = []
                while current in came_from:
                    path.append(current)
                    current = came_from[current]
                path.append(start)
                return path[::-1]

            for neighbor, cost in self.get_neighbors(current):
                tentative_g = g_score[current] + cost

                if neighbor not in g_score or tentative_g < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g
                    f_score[neighbor] = tentative_g + self.heuristic(neighbor, goal)
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))

        return None  # 无路径

# 使用示例
grid_map = [
    [0, 0, 0, 1, 0],
    [0, 1, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 1, 0],
    [0, 0, 0, 0, 0]
]

planner = AStarPlanner(grid_map)
path = planner.plan(start=(0, 0), goal=(4, 4))
print("Path:", path)

class RRTPlanner:
    def __init__(self, start, goal, obstacles, bounds, step_size=0.5, max_iter=1000):
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.obstacles = obstacles  # 障碍物列表
        self.bounds = bounds  # (x_min, x_max, y_min, y_max)
        self.step_size = step_size
        self.max_iter = max_iter

        self.tree = {tuple(self.start): None}

    def random_point(self):
        """生成随机点"""
        x = np.random.uniform(self.bounds[0], self.bounds[1])
        y = np.random.uniform(self.bounds[2], self.bounds[3])
        return np.array([x, y])

    def nearest_node(self, point):
        """找最近节点"""
        nodes = np.array(list(self.tree.keys()))
        distances = np.linalg.norm(nodes - point, axis=1)
        return tuple(nodes[np.argmin(distances)])

    def steer(self, from_node, to_point):
        """从from_node向to_point扩展step_size"""
        direction = to_point - np.array(from_node)
        distance = np.linalg.norm(direction)

        if distance < self.step_size:
            return to_point

        return np.array(from_node) + direction / distance * self.step_size

    def is_collision_free(self, point):
        """检查是否无碰撞"""
        for obs_center, obs_radius in self.obstacles:
            if np.linalg.norm(point - np.array(obs_center)) < obs_radius:
                return False
        return True

    def plan(self):
        """RRT规划"""
        for i in range(self.max_iter):
            # 以一定概率采样目标点
            if np.random.random() < 0.1:
                rand_point = self.goal
            else:
                rand_point = self.random_point()

            # 找最近节点
            nearest = self.nearest_node(rand_point)

            # 扩展
            new_point = self.steer(nearest, rand_point)

            # 碰撞检测
            if self.is_collision_free(new_point):
                self.tree[tuple(new_point)] = nearest

                # 检查是否到达目标
                if np.linalg.norm(new_point - self.goal) < self.step_size:
                    self.tree[tuple(self.goal)] = tuple(new_point)
                    return self.extract_path()

        return None

    def extract_path(self):
        """提取路径"""
        path = [tuple(self.goal)]
        current = tuple(self.goal)

        while current != tuple(self.start):
            current = self.tree[current]
            path.append(current)

        return path[::-1]

# 使用示例
obstacles = [
    ([2, 2], 0.5),  # 圆心(2,2), 半径0.5
    ([3, 4], 0.7)
]

planner = RRTPlanner(
    start=(0, 0),
    goal=(5, 5),
    obstacles=obstacles,
    bounds=(0, 5, 0, 5),
    step_size=0.3
)

path = planner.plan()
print("RRT Path:", path)

class DWA:
    def __init__(self, config):
        # 机器人参数
        self.max_speed = config['max_speed']
        self.min_speed = config['min_speed']
        self.max_yaw_rate = config['max_yaw_rate']
        self.max_accel = config['max_accel']
        self.max_dyaw_rate = config['max_dyaw_rate']

        # DWA参数
        self.v_resolution = config['v_resolution']
        self.yaw_rate_resolution = config['yaw_rate_resolution']
        self.dt = config['dt']
        self.predict_time = config['predict_time']

        # 评价函数权重
        self.heading_weight = config['heading_weight']
        self.clearance_weight = config['clearance_weight']
        self.velocity_weight = config['velocity_weight']

    def calc_dynamic_window(self, v, yaw_rate):
        """计算动态窗口"""
        # 基于速度限制的窗口
        Vs = [self.min_speed, self.max_speed,
              -self.max_yaw_rate, self.max_yaw_rate]

        # 基于加速度限制的窗口
        Vd = [v - self.max_accel * self.dt,
              v + self.max_accel * self.dt,
              yaw_rate - self.max_dyaw_rate * self.dt,
              yaw_rate + self.max_dyaw_rate * self.dt]

        # 交集
        dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
              max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]

        return dw

    def predict_trajectory(self, x, y, yaw, v, yaw_rate):
        """预测轨迹"""
        trajectory = [[x, y, yaw, v, yaw_rate]]

        time = 0
        while time <= self.predict_time:
            x += v * np.cos(yaw) * self.dt
            y += v * np.sin(yaw) * self.dt
            yaw += yaw_rate * self.dt
            trajectory.append([x, y, yaw, v, yaw_rate])
            time += self.dt

        return trajectory

    def calc_heading_eval(self, trajectory, goal):
        """航向评价"""
        dx = goal[0] - trajectory[-1][0]
        dy = goal[1] - trajectory[-1][1]
        goal_theta = np.arctan2(dy, dx)
        cost = abs(goal_theta - trajectory[-1][2])
        return np.pi - cost  # 转换为评分(越大越好)

    def calc_clearance_eval(self, trajectory, obstacles):
        """障碍物距离评价"""
        min_dist = float('inf')

        for point in trajectory:
            for obs in obstacles:
                dist = np.hypot(point[0] - obs[0], point[1] - obs[1])
                if dist < min_dist:
                    min_dist = dist

        return min_dist

    def calc_velocity_eval(self, trajectory):
        """速度评价"""
        return trajectory[-1][3]  # 期望快速行驶

    def plan(self, x, y, yaw, v, yaw_rate, goal, obstacles):
        """DWA规划"""
        dw = self.calc_dynamic_window(v, yaw_rate)

        best_v, best_yaw_rate = 0, 0
        best_score = -float('inf')
        best_trajectory = None

        # 遍历速度空间
        for test_v in np.arange(dw[0], dw[1], self.v_resolution):
            for test_yaw_rate in np.arange(dw[2], dw[3], self.yaw_rate_resolution):
                # 预测轨迹
                trajectory = self.predict_trajectory(x, y, yaw, test_v, test_yaw_rate)

                # 评价
                heading = self.calc_heading_eval(trajectory, goal)
                clearance = self.calc_clearance_eval(trajectory, obstacles)
                velocity = self.calc_velocity_eval(trajectory)

                # 综合评分
                score = (self.heading_weight * heading +
                        self.clearance_weight * clearance +
                        self.velocity_weight * velocity)

                # 选择最优
                if score > best_score:
                    best_score = score
                    best_v = test_v
                    best_yaw_rate = test_yaw_rate
                    best_trajectory = trajectory

        return best_v, best_yaw_rate, best_trajectory

# 使用示例
config = {
    'max_speed': 1.0,
    'min_speed': 0.0,
    'max_yaw_rate': 40.0 * np.pi / 180.0,
    'max_accel': 0.2,
    'max_dyaw_rate': 40.0 * np.pi / 180.0,
    'v_resolution': 0.01,
    'yaw_rate_resolution': 0.1 * np.pi / 180.0,
    'dt': 0.1,
    'predict_time': 3.0,
    'heading_weight': 1.0,
    'clearance_weight': 2.0,
    'velocity_weight': 1.0
}

dwa = DWA(config)
v, yaw_rate, trajectory = dwa.plan(
    x=0, y=0, yaw=0, v=0.5, yaw_rate=0,
    goal=(10, 10),
    obstacles=[(5, 5), (6, 6)]
)
print(f"最优速度: {v:.2f} m/s, 角速度: {yaw_rate:.2f} rad/s")

class PurePursuit:
    def __init__(self, lookahead_distance, wheelbase):
        self.ld = lookahead_distance  # 前瞻距离
        self.L = wheelbase            # 轮距

    def find_target_point(self, path, current_pos):
        """找前瞻点"""
        min_dist = float('inf')
        target_idx = 0

        for i, point in enumerate(path):
            dist = np.hypot(point[0] - current_pos[0],
                          point[1] - current_pos[1])

            if abs(dist - self.ld) < min_dist:
                min_dist = abs(dist - self.ld)
                target_idx = i

        return path[target_idx]

    def calc_steering(self, current_pos, current_yaw, target_point):
        """计算转向角"""
        dx = target_point[0] - current_pos[0]
        dy = target_point[1] - current_pos[1]

        # 前瞻点在车体坐标系下的位置
        alpha = np.arctan2(dy, dx) - current_yaw

        # Pure Pursuit公式
        steering = np.arctan2(2.0 * self.L * np.sin(alpha), self.ld)

        return steering

# 使用示例
pp = PurePursuit(lookahead_distance=2.0, wheelbase=0.3)

path = [(0, 0), (1, 0), (2, 0.5), (3, 1), (4, 1.5)]
current_pos = (0.5, 0.1)
current_yaw = 0.1

target = pp.find_target_point(path, current_pos)
steering = pp.calc_steering(current_pos, current_yaw, target)
print(f"转向角: {np.degrees(steering):.2f}°")

# 左右轮速相反,大小相等
v_left = -0.5  # m/s
v_right = 0.5  # m/s

# 角速度: ω = (v_right - v_left) / L
# 线速度: v = (v_left + v_right) / 2 = 0

# 转弯半径R, 线速度v, 轮距L
R = 2.0  # 转弯半径
v = 1.0  # 线速度
L = 0.3  # 轮距

# 角速度
omega = v / R

# 左右轮速
v_left = v - omega * L / 2
v_right = v + omega * L / 2

# 例: R=2m, v=1m/s, L=0.3m
# omega = 0.5 rad/s
# v_left = 0.925 m/s
# v_right = 1.075 m/s

output = Kp*error + Ki*integral + Kd*derivative

if output > limit:
    output = limit
    integral -= error * dt  # 回退

if abs(error) > threshold:
    # 误差大时不积分
    I = 0
else:
    integral += error * dt
    I = Ki * integral

integral += error * dt
integral = np.clip(integral, -integral_limit, integral_limit)

if abs(error) > threshold:
    Ki_effective = 0
else:
    Ki_effective = Ki * (1 - abs(error)/threshold)

# Dijkstra
f = g  # 只考虑已走距离

# A*
f = g + h  # 已走距离 + 启发式估计

# 启发式函数h(n)要求:
# 1. h(n)必须可容(admissible): h(n) <= 实际代价
# 2. h(n)一致(consistent): h(n) <= cost(n,n') + h(n')

def select_nearest_solution(solutions, current_joints):
    """选择与当前关节角最接近的解"""
    min_dist = float('inf')
    best_sol = None

    for sol in solutions:
        dist = sum((s - c)**2 for s, c in zip(sol, current_joints))
        if dist < min_dist:
            min_dist = dist
            best_sol = sol

    return best_sol

def is_valid_solution(joints, limits):
    """检查是否在关节限位内"""
    for joint, (min_limit, max_limit) in zip(joints, limits):
        if not (min_limit <= joint <= max_limit):
            return False
    return True

def check_singularity(joints):
    """检查雅可比矩阵行列式"""
    J = calculate_jacobian(joints)
    det = np.linalg.det(J)
    return abs(det) > threshold  # 远离奇异

def energy_cost(joints):
    """关节角度平方和最小"""
    return sum(j**2 for j in joints)

def s_curve_profile(t, t_total, v_max):
    """S型速度曲线"""
    if t < t_total / 3:
        # 加速阶段
        v = v_max * (3 * (t / t_total)**2 - 2 * (t / t_total)**3)
    elif t < 2 * t_total / 3:
        # 匀速阶段
        v = v_max
    else:
        # 减速阶段
        t_decel = t - 2 * t_total / 3
        v = v_max * (1 - 3 * (t_decel / (t_total/3))**2
                     + 2 * (t_decel / (t_total/3))**3)
    return v

class TrapezoidalProfile:
    def __init__(self, v_max, accel):
        self.v_max = v_max
        self.accel = accel

    def generate(self, distance):
        # 加速时间
        t_accel = self.v_max / self.accel

        # 加速距离
        d_accel = 0.5 * self.accel * t_accel**2

        if 2 * d_accel > distance:
            # 无匀速段
            v_peak = np.sqrt(self.accel * distance)
            return self.triangular_profile(distance, v_peak)
        else:
            # 有匀速段
            return self.trapezoidal_profile(distance, d_accel, t_accel)

class LowPassFilter:
    def __init__(self, alpha):
        self.alpha = alpha  # 0-1, 越小越平滑
        self.value = 0

    def update(self, new_value):
        self.value = self.alpha * new_value + (1 - self.alpha) * self.value
        return self.value

# 使用
lpf = LowPassFilter(alpha=0.1)
smoothed_speed = lpf.update(target_speed)