3 Star 6 Fork 1

涂志明 / UGV-Path-Tracking-Algorithm

加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目,私有仓库也完全免费 :)
免费加入
该仓库未声明开源许可证文件(LICENSE),使用请关注具体项目描述及其代码上游依赖。
克隆/下载
plot_car.m 2.16 KB
一键复制 编辑 原始数据 按行查看 历史
NUDT_UGV_GROUP 提交于 2020-05-02 17:17 . First Version
function plot_car(VehicleParams, Vehicle_State, steer_state)
CoG = [Vehicle_State(1);Vehicle_State(2)];
yaw = Vehicle_State(3);
delta = steer_state;
wheel_base_front = VehicleParams.wheel_base_front;
wheel_base_rear = VehicleParams.wheel_base_rear;
front_axle = VehicleParams.front_axle;
rear_axle = VehicleParams.rear_axle;
longitudinal_axle = VehicleParams.longitudinal_axle;
outline = VehicleParams.Outline;
tire_fl = VehicleParams.Tire_fl;
tire_fr = VehicleParams.Tire_fr;
tire_rl = VehicleParams.Tire_rl;
tire_rr = VehicleParams.Tire_rr;
rotA = [cos(yaw) -sin(yaw);sin(yaw) cos(yaw);];
rotB = [cos(delta) -sin(delta);sin(delta) cos(delta);];
tire_fl = rotB*tire_fl; tire_fl(1,:)=tire_fl(1,:)+wheel_base_front;
tire_fr = rotB*tire_fr; tire_fr(1,:)=tire_fr(1,:)+wheel_base_front;
front_axle = rotB*front_axle; front_axle(1,:)=front_axle(1,:)+wheel_base_front;
tire_rl(1,:) = tire_rl(1,:)-wheel_base_rear;
tire_rr(1,:) = tire_rr(1,:)-wheel_base_rear;
rear_axle(1,:)=rear_axle(1,:)-wheel_base_rear;
outline = rotA*outline;
outline = outline + repmat(CoG,1,5);
tire_fl = rotA*tire_fl + repmat(CoG,1,5);
tire_fr = rotA*tire_fr + repmat(CoG,1,5);
tire_rl = rotA*tire_rl + repmat(CoG,1,5);
tire_rr = rotA*tire_rr + repmat(CoG,1,5);
front_axle = rotA*front_axle + CoG;
rear_axle = rotA*rear_axle + CoG;
longitudinal_axle = rotA*longitudinal_axle + CoG;
plot(outline(1,:),outline(2,:),'Color','black','LineWidth',1.5);
plot(tire_fl(1,:),tire_fl(2,:),'Color','blue','LineWidth',1.2);
plot(tire_fr(1,:),tire_fr(2,:),'Color','blue','LineWidth',1.2);
plot(tire_rl(1,:),tire_rl(2,:),'Color','blue','LineWidth',1.2);
plot(tire_rr(1,:),tire_rr(2,:),'Color','blue','LineWidth',1.2);
plot(front_axle(1,:),front_axle(2,:),'Color','black','LineWidth',1.5);
plot(rear_axle(1,:),rear_axle(2,:),'Color','black','LineWidth',1.5);
plot(longitudinal_axle(1,:),longitudinal_axle(2,:),'Color','black','LineWidth',1.5);
end
Matlab
1
https://gitee.com/zhi_ming_tu/UGV-Path-Tracking-Algorithm.git
git@gitee.com:zhi_ming_tu/UGV-Path-Tracking-Algorithm.git
zhi_ming_tu
UGV-Path-Tracking-Algorithm
UGV-Path-Tracking-Algorithm
master

搜索帮助