分为三步：

转换传感器数据

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

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

首先打开这个文件

首先在

取得转换矩阵

%% 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 %%

最后运行