Four-Wheel Independent Drive Kinematics

1        Front
2          ↑
3    1 ─────── 2
4    │    •    │  ← Center of rotation
5    │         │
6    4 ─────── 3
7    
8    Wheels: 1=FL, 2=FR, 3=RR, 4=RL

 1import numpy as np
 2
 3class FourWheelDrive:
 4    def __init__(self, wheelbase=0.5, track_width=0.4, wheel_radius=0.1):
 5        """
 6        Args:
 7            wheelbase: Distance between front and rear axles (m)
 8            track_width: Distance between left and right wheels (m)
 9            wheel_radius: Radius of wheels (m)
10        """
11        self.L = wheelbase
12        self.W = track_width
13        self.r = wheel_radius
14        
15        # State: [x, y, theta]
16        self.state = np.array([0.0, 0.0, 0.0])
17    
18    def forward_kinematics(self, omega1, omega2, omega3, omega4, dt):
19        """
20        Update robot pose from wheel velocities
21        
22        Args:
23            omega1, omega2, omega3, omega4: Wheel angular velocities (rad/s)
24                1=FL, 2=FR, 3=RR, 4=RL
25            dt: Time step (s)
26        
27        Returns:
28            Updated pose [x, y, theta]
29        """
30        x, y, theta = self.state
31        
32        # Average left and right velocities
33        omega_left = (omega1 + omega4) / 2
34        omega_right = (omega2 + omega3) / 2
35        
36        # Robot velocities
37        v = self.r * (omega_right + omega_left) / 2
38        omega = self.r * (omega_right - omega_left) / self.W
39        
40        # Update pose
41        if abs(omega) < 1e-6:
42            # Straight line motion
43            x += v * np.cos(theta) * dt
44            y += v * np.sin(theta) * dt
45        else:
46            # Arc motion
47            R = v / omega
48            x += R * (np.sin(theta + omega * dt) - np.sin(theta))
49            y += R * (-np.cos(theta + omega * dt) + np.cos(theta))
50            theta += omega * dt
51        
52        # Normalize angle
53        theta = np.arctan2(np.sin(theta), np.cos(theta))
54        
55        self.state = np.array([x, y, theta])
56        return self.state.copy()
57    
58    def get_velocity(self, omega1, omega2, omega3, omega4):
59        """Get robot linear and angular velocity"""
60        omega_left = (omega1 + omega4) / 2
61        omega_right = (omega2 + omega3) / 2
62        
63        v = self.r * (omega_right + omega_left) / 2
64        omega = self.r * (omega_right - omega_left) / self.W
65        
66        return v, omega
67
68# Example
69robot = FourWheelDrive(wheelbase=0.5, track_width=0.4, wheel_radius=0.1)
70
71# All wheels same speed → straight line
72for _ in range(100):
73    robot.forward_kinematics(5.0, 5.0, 5.0, 5.0, dt=0.01)
74
75print(f"After straight motion: {robot.state}")

 1def inverse_kinematics(self, v_desired, omega_desired):
 2    """
 3    Compute wheel velocities for desired motion
 4    
 5    Args:
 6        v_desired: Desired linear velocity (m/s)
 7        omega_desired: Desired angular velocity (rad/s)
 8    
 9    Returns:
10        (omega1, omega2, omega3, omega4) wheel velocities
11    """
12    omega_left = (v_desired - omega_desired * self.W / 2) / self.r
13    omega_right = (v_desired + omega_desired * self.W / 2) / self.r
14    
15    # Distribute to all four wheels
16    omega1 = omega_left   # FL
17    omega2 = omega_right  # FR
18    omega3 = omega_right  # RR
19    omega4 = omega_left   # RL
20    
21    return omega1, omega2, omega3, omega4
22
23# Add to FourWheelDrive class
24FourWheelDrive.inverse_kinematics = inverse_kinematics
25
26# Example: Move forward at 0.5 m/s
27robot = FourWheelDrive()
28w1, w2, w3, w4 = robot.inverse_kinematics(v_desired=0.5, omega_desired=0)
29print(f"Straight: ω₁={w1:.2f}, ω₂={w2:.2f}, ω₃={w3:.2f}, ω₄={w4:.2f}")
30
31# Turn in place
32w1, w2, w3, w4 = robot.inverse_kinematics(v_desired=0, omega_desired=1.0)
33print(f"Rotation: ω₁={w1:.2f}, ω₂={w2:.2f}, ω₃={w3:.2f}, ω₄={w4:.2f}")

 1def check_wheel_slip(self, omega1, omega2, omega3, omega4, threshold=2.0):
 2    """
 3    Check for potential wheel slip
 4    
 5    Returns:
 6        (has_slip, slip_info)
 7    """
 8    # Check front wheels
 9    front_diff = abs(omega1 - omega2)
10    
11    # Check rear wheels
12    rear_diff = abs(omega4 - omega3)
13    
14    # Check left wheels
15    left_diff = abs(omega1 - omega4)
16    
17    # Check right wheels
18    right_diff = abs(omega2 - omega3)
19    
20    slip_info = {
21        'front_diff': front_diff,
22        'rear_diff': rear_diff,
23        'left_diff': left_diff,
24        'right_diff': right_diff
25    }
26    
27    has_slip = (front_diff > threshold or rear_diff > threshold or
28                left_diff > threshold or right_diff > threshold)
29    
30    return has_slip, slip_info
31
32# Add to class
33FourWheelDrive.check_wheel_slip = check_wheel_slip

 1def synchronized_control(self, v_desired, omega_desired, max_accel=1.0, dt=0.01):
 2    """
 3    Smooth wheel velocity changes to prevent slip
 4    
 5    Args:
 6        max_accel: Maximum acceleration (rad/s²)
 7    """
 8    target_w1, target_w2, target_w3, target_w4 = self.inverse_kinematics(
 9        v_desired, omega_desired
10    )
11    
12    # Gradually adjust to target (simple rate limiting)
13    # In practice, use PID controllers for each wheel
14    
15    return target_w1, target_w2, target_w3, target_w4

 1def terrain_adaptive_control(self, terrain_type='flat'):
 2    """
 3    Adjust control parameters based on terrain
 4    
 5    Args:
 6        terrain_type: 'flat', 'rough', 'slippery', 'incline'
 7    """
 8    if terrain_type == 'rough':
 9        # Increase individual wheel control
10        return {'independent': True, 'slip_threshold': 3.0}
11    elif terrain_type == 'slippery':
12        # Reduce acceleration, increase monitoring
13        return {'max_accel': 0.5, 'slip_threshold': 1.0}
14    elif terrain_type == 'incline':
15        # Increase rear wheel power
16        return {'rear_bias': 1.2}
17    else:
18        return {'independent': False, 'slip_threshold': 2.0}