​matlab怎么实现小车避让障碍物

分为三步：

转换传感器数据

将传感器数据从小车坐标系转换到世界坐标系

通过转换好的传感器数据求解避让运动参数

首先打开这个文件

首先在

取得转换矩阵

%% START CODE BLOCK %%

R = [cos(theta) -sin(theta) x; sin(theta) cos(theta) y; 0 0 1];

%% END CODE BLOCK %%

在

实现转换传感器数据、将传感器数据从小车坐标系转换到世界坐标系

%% START CODE BLOCK %%

% Apply the transformation to robot frame.

ir_distances_rf = zeros(3,n_sensors);

for i=1:n_sensors

x_s = obj.sensor_placement(1,i);

y_s = obj.sensor_placement(2,i);

theta_s = obj.sensor_placement(3,i);

R = obj.get_transformation_matrix(x_s,y_s,theta_s);

ir_distances_rf(:,i) = R*[ir_distances(i); 0; 1];

end

% Apply the transformation to world frame.

[x,y,theta] = state_estimate.unpack();

R = obj.get_transformation_matrix(x,y,theta);

ir_distances_wf = R*ir_distances_rf;

ir_distances_wf = ir_distances_wf(1:2,:);

%% END CODE BLOCK %%

然后在

实现通过转换好的传感器数据求解避让运动参数

%% START CODE BLOCK %%

% Compute the heading vector

n_sensors = length(robot.ir_array);

sensor_gains = [1 1 0.5 1 1];

u_i = (ir_distances_wf-repmat([x;y],1,n_sensors))*diag(sensor_gains);

u_ao = sum(u_i,2);

% Compute the heading and error for the PID controller

theta_ao = atan2(u_ao(2),u_ao(1));

e_k = theta_ao-theta;

e_k = atan2(sin(e_k),cos(e_k));

%% END CODE BLOCK %%

最后运行