|
| 1 | +import cvxpy as cp |
| 2 | +import numpy as np |
| 3 | +import matplotlib.pyplot as plt |
| 4 | + |
| 5 | + |
| 6 | +def solve_car_control(x_final, L=0.1, N=50, h=0.1, gamma=10): |
| 7 | + """ |
| 8 | + Solve the nonlinear optimal control problem for car trajectory planning. |
| 9 | + |
| 10 | + Parameters: |
| 11 | + - x_final: tuple (p1, p2, theta) for final position and orientation |
| 12 | + - L: wheelbase length |
| 13 | + - N: number of time steps |
| 14 | + - h: time step size |
| 15 | + - gamma: weight for control smoothness term |
| 16 | + |
| 17 | + Returns: |
| 18 | + - x_opt: optimal states (N+1 x 3) |
| 19 | + - u_opt: optimal controls (N x 2) |
| 20 | + """ |
| 21 | + |
| 22 | + # Variables |
| 23 | + # States: x[k] = [p1(k), p2(k), theta(k)] |
| 24 | + x = cp.Variable((N+1, 3)) |
| 25 | + # Controls: u[k] = [s(k), phi(k)] |
| 26 | + u = cp.Variable((N, 2)) |
| 27 | + |
| 28 | + # Initial state (starting at origin with zero orientation) |
| 29 | + x_init = np.array([0, 0, 0]) |
| 30 | + |
| 31 | + # Objective function |
| 32 | + objective = 0 |
| 33 | + |
| 34 | + # Sum of squared control inputs |
| 35 | + for k in range(N): |
| 36 | + objective += cp.sum_squares(u[k, :]) |
| 37 | + |
| 38 | + # Control smoothness term |
| 39 | + for k in range(N-1): |
| 40 | + objective += gamma * cp.sum_squares(u[k+1, :] - u[k, :]) |
| 41 | + |
| 42 | + # Constraints |
| 43 | + constraints = [] |
| 44 | + |
| 45 | + # Initial state constraint |
| 46 | + constraints.append(x[0, :] == x_init) |
| 47 | + |
| 48 | + # Dynamics constraints |
| 49 | + # Note: We're pretending cp.sin, cp.cos, and cp.tan exist as atoms |
| 50 | + for k in range(N): |
| 51 | + # x[k+1] = f(x[k], u[k]) |
| 52 | + # where f(x, u) = x + h * [u[0]*cos(x[2]), u[0]*sin(x[2]), u[0]*tan(u[1])/L] |
| 53 | + |
| 54 | + # Position dynamics |
| 55 | + constraints.append(x[k+1, 0] == x[k, 0] + h * u[k, 0] * cp.cos(x[k, 2])) |
| 56 | + constraints.append(x[k+1, 1] == x[k, 1] + h * u[k, 0] * cp.sin(x[k, 2])) |
| 57 | + |
| 58 | + # Orientation dynamics |
| 59 | + constraints.append(x[k+1, 2] == x[k, 2] + h * (u[k, 0] / L) * cp.tan(u[k, 1])) |
| 60 | + |
| 61 | + # Final state constraint |
| 62 | + constraints.append(x[N, :] == x_final) |
| 63 | + |
| 64 | + # Steering angle limits (optional but realistic) |
| 65 | + # Assuming max steering angle of 45 degrees |
| 66 | + max_steering = np.pi / 4 |
| 67 | + constraints.append(u[:, 1] >= -max_steering) |
| 68 | + constraints.append(u[:, 1] <= max_steering) |
| 69 | + |
| 70 | + # Create and solve the problem |
| 71 | + problem = cp.Problem(cp.Minimize(objective), constraints) |
| 72 | + |
| 73 | + # Solve using an appropriate solver |
| 74 | + # For nonlinear problems, we might need special solver options |
| 75 | + problem.solve(solver=cp.SCS, verbose=True) |
| 76 | + |
| 77 | + # Extract solution |
| 78 | + x_opt = x.value |
| 79 | + u_opt = u.value |
| 80 | + |
| 81 | + return x_opt, u_opt |
| 82 | + |
| 83 | + |
| 84 | +def plot_trajectory(x_opt, u_opt, L, h, title="Car Trajectory"): |
| 85 | + """ |
| 86 | + Plot the car trajectory with orientation indicators. |
| 87 | + """ |
| 88 | + fig, ax = plt.subplots(1, 1, figsize=(8, 8)) |
| 89 | + |
| 90 | + # Plot trajectory |
| 91 | + ax.plot(x_opt[:, 0], x_opt[:, 1], 'b-', linewidth=2, label='Trajectory') |
| 92 | + |
| 93 | + # Plot car position and orientation at several time steps |
| 94 | + car_length = L |
| 95 | + car_width = L * 0.6 |
| 96 | + |
| 97 | + # Select time steps to show car outline (every 5th step) |
| 98 | + steps_to_show = range(0, len(x_opt), 5) |
| 99 | + |
| 100 | + for k in steps_to_show: |
| 101 | + p1, p2, theta = x_opt[k] |
| 102 | + |
| 103 | + # Car outline (simplified rectangle) |
| 104 | + # Front of car |
| 105 | + front_x = p1 + (car_length/2) * np.cos(theta) |
| 106 | + front_y = p2 + (car_length/2) * np.sin(theta) |
| 107 | + |
| 108 | + # Rear of car |
| 109 | + rear_x = p1 - (car_length/2) * np.cos(theta) |
| 110 | + rear_y = p2 - (car_length/2) * np.sin(theta) |
| 111 | + |
| 112 | + # Draw car as a line with orientation |
| 113 | + ax.plot([rear_x, front_x], [rear_y, front_y], 'k-', linewidth=3, alpha=0.5) |
| 114 | + |
| 115 | + # Draw steering angle indicator if not at final position |
| 116 | + if k < len(u_opt): |
| 117 | + phi = u_opt[k, 1] |
| 118 | + # Steering direction from front of car |
| 119 | + steer_x = front_x + (car_length/3) * np.cos(theta + phi) |
| 120 | + steer_y = front_y + (car_length/3) * np.sin(theta + phi) |
| 121 | + ax.plot([front_x, steer_x], [front_y, steer_y], 'r-', linewidth=2, alpha=0.7) |
| 122 | + |
| 123 | + # Mark start and end points |
| 124 | + ax.plot(x_opt[0, 0], x_opt[0, 1], 'go', markersize=10, label='Start') |
| 125 | + ax.plot(x_opt[-1, 0], x_opt[-1, 1], 'ro', markersize=10, label='Goal') |
| 126 | + |
| 127 | + ax.set_xlabel('p1') |
| 128 | + ax.set_ylabel('p2') |
| 129 | + ax.set_title(title) |
| 130 | + ax.legend() |
| 131 | + ax.grid(True, alpha=0.3) |
| 132 | + ax.axis('equal') |
| 133 | + |
| 134 | + return fig, ax |
| 135 | + |
| 136 | + |
| 137 | +# Example usage |
| 138 | +if __name__ == "__main__": |
| 139 | + # Test cases from the figure |
| 140 | + test_cases = [ |
| 141 | + ((0, 1, 0), "Move forward to (0, 1)"), |
| 142 | + ((0, 1, np.pi/2), "Move to (0, 1) and turn 90°"), |
| 143 | + ((0, 0.5, 0), "Move forward to (0, 0.5)"), |
| 144 | + ((0.5, 0.5, -np.pi/2), "Move to (0.5, 0.5) and turn -90°") |
| 145 | + ] |
| 146 | + |
| 147 | + # Solve for each test case |
| 148 | + for x_final, description in test_cases: |
| 149 | + print(f"\nSolving for: {description}") |
| 150 | + print(f"Target state: p1={x_final[0]}, p2={x_final[1]}, theta={x_final[2]:.2f}") |
| 151 | + |
| 152 | + try: |
| 153 | + x_opt, u_opt = solve_car_control(x_final) |
| 154 | + |
| 155 | + if x_opt is not None and u_opt is not None: |
| 156 | + print("Optimization successful!") |
| 157 | + print(f"Final position: p1={x_opt[-1, 0]:.3f}, p2={x_opt[-1, 1]:.3f}, theta={x_opt[-1, 2]:.3f}") |
| 158 | + |
| 159 | + # Plot the trajectory |
| 160 | + fig, ax = plot_trajectory(x_opt, u_opt, L=0.1, h=0.1, title=description) |
| 161 | + plt.show() |
| 162 | + else: |
| 163 | + print("Optimization failed!") |
| 164 | + |
| 165 | + except Exception as e: |
| 166 | + print(f"Error: {e}") |
| 167 | + |
| 168 | + # Additional analysis: plot control inputs |
| 169 | + if x_opt is not None and u_opt is not None: |
| 170 | + fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6)) |
| 171 | + |
| 172 | + time_steps = np.arange(len(u_opt)) * 0.1 # h = 0.1 |
| 173 | + |
| 174 | + ax1.plot(time_steps, u_opt[:, 0], 'b-', linewidth=2) |
| 175 | + ax1.set_ylabel('Speed s(t)') |
| 176 | + ax1.set_xlabel('Time') |
| 177 | + ax1.grid(True, alpha=0.3) |
| 178 | + |
| 179 | + ax2.plot(time_steps, u_opt[:, 1], 'r-', linewidth=2) |
| 180 | + ax2.set_ylabel('Steering angle φ(t)') |
| 181 | + ax2.set_xlabel('Time') |
| 182 | + ax2.grid(True, alpha=0.3) |
| 183 | + |
| 184 | + plt.tight_layout() |
| 185 | + plt.show() |
0 commit comments