数学模型

2024-07-17 06:40| 来源: 网络整理| 查看: 265

写在前面

之前做了一个2022年Mathorcup数学建模挑战赛C题的比赛心得，上一篇文章主要讲了A*算法的改进以及A*算法如何在C题的第3问的应用。本文主要介绍C题的第2问，即三种泊车模型如何建立，因此部分并非我写，在比赛期间，我主要攻克的是第3问，因此，写这篇文章也花了我不少心思，重新看代码，跑代码，尽可能详细地讲清楚泊车模型地建立，希望能够帮到有需要的同学。

题目

先来看问题：

图4如下：

根据题目要求，我们要做出车辆从初始位置到10号垂直停车位，82号平行停车位以及31号倾斜停车位的轨迹图，加速度，加加速度，路径长度....等等。

在本文中，我们不考虑各种物理量的求解以及关于最小转弯半径等问题，我们仅考虑最重要的部分，也就是车辆到达停车位附近后，该如何停进停车位。

问题分析

在该问题中，我们要实现车辆停进垂直停车位，水平停车位，倾斜停车位，也就是说，我们要建立三种不同的泊车模型。

首先，我们先对数据做了预处理，即我们假设，每种停车方式，都能通过把方向盘打死（方向盘打死后就可以达到最小转弯半径，最容易停进停车位），一次性倒车入库。

一次性倒车入库的意思是：车辆只需要后退，不需要前进调整位置，在后退过程中可以更改方向盘方向。

我们通过查找文献，计算车辆的最小转弯半径以及最小通过圆，为什么要计算这两个值呢？

因为我们要粗略计算，一次性倒车入库时，我们会不会碰撞到边界，即车身转弯时所画出的环（这个环就是车身在转弯时所经过的地方，这个环内不允许有任何东西，有的话就会碰撞到）会不会与边界相交。

关于最小转弯半径的原理以及计算公式，最小通过圆概念的博客如下：

最小转弯半径的计算，最小通过圆，含图片

了解了最小转弯半径以及最小通过圆后，我们可以继续：

例如：

假设黑线是车辆方向盘打死的情况下，计算得到的最外层侧转弯半径以及最内侧转弯半径，则由这两个半径所构成的环已经碰撞到边界了，因此此时车辆是无法一次性倒车入库的。

（我们这里用最终状态来反推初始状态）

具体的计算是：

先计算最小转弯半径因为车辆转弯时，车身处处作同心圆的转弯，因此，车辆转弯时，转弯圆心固定，车身不同部分的转弯半径不同，通过最小转弯半径，计算最内侧的圆的半径以及最外侧的圆的半径现已知最内侧圆的半径，最外侧圆的半径，建立坐标系，原点选取在一个比较好计算的位置，这个随你建立好坐标系，将边界画出，用函数表示，因为边界皆为竖或横，用y=c，x=c即可不断调整圆心，看是否存在一个圆心，使得最外侧圆与最内侧圆所构成的环中不经过边界

很遗憾的是，通过计算，在三种泊车模型中，仅有垂直泊车是可以一次性倒入库的，平行泊车与倾斜泊车是无法一次性倒入库的，因此，平行泊车与倾斜泊车的复杂程度要远远高于垂直泊车。

因此我们必须想另一种方法来建立泊车模型：既然一次性倒车不行，那么只能多次，反复前进后退来改变车辆自身位置，慢慢将车辆挪进停车位。

我们同样用反溯法，从车辆已经停好的状态反推初始状态，将车辆从停车位中挪出即可。

接下来，我们仅以水平泊车模型作为例子，倾斜泊车模型与水平泊车相似，将碰撞边界以及车辆初始位置改动即可。垂直泊车模型不需要用该模型，只要确定转动圆心以及转动半径即可。

因此，同样以上图的水平停车位为例，其遍历步骤为：

车辆运动矢量向左，开始前进若碰到边界则停止运动，若无则继续前进车辆运动矢量向右，开始后退如碰到边界则停止运动，若无则继续后退在坐标轴中，以车辆右侧做一条直线，若右侧边界最突出点在直线下方，则退出循环，若在直线上方，则继续上述循环。模型建立给出下面模型所需要的各种全局变量： addpath lib; R=4.97; global car_Len car_Wid car_Weel_Den car_Cen_Cen; % 车辆参数 global car_alpha car_oil_ddt car_speed_dt car_speed_ddt car_FXP_speed bump_den bump_speed park_forward_Speed park_backward_Speed; global pos_Len pos_Wid pos_Head road_Wid car_Cen_Head; % 车位和道路参数 car_Len=4.9; % 车长 car_Wid=1.8; % 车宽 car_Weel_Den=1.7; % 轮宽 car_Cen_Cen=2.8; % 轴距 car_alpha=470/16/180*pi; % 最大转角 car_oil_ddt=3; % 油门加速度 car_speed_dt=-6; % 最大刹车减速度 car_speed_ddt=20; % 最大加加速度 car_FXP_speed=400/180*pi; % 方向盘转动角速度 bump_den=5; % 减速带距离 bump_speed=10; % 减速带速度 park_forward_Speed=20; % 汽车最大前进车速 park_backward_Speed=10; % 汽车最大倒车车速 pos_Len=5.4; % 车位长度 pos_Wid=2.4; % 车位宽度 pos_Head=0.2; % 车位头部 road_Wid=5.5; % 道路宽度 car_Cen_Head=(car_Len-car_Cen_Cen)/2; % 车辆控制中心至车尾距离 Dcar2wall=0.00; % 车尾与墙的间距建立地图模型

我们将地图纳入坐标轴中，以边界左侧顶点，即车位左侧最外点作为坐标原点(0,0)，根据题目给出的车位宽2.4米，长5.1米，各边界的函数，一共有6条边界，图中圆圈为原点：

给出上述图形的代码：

这段代码是用来记录边界信息，即碰撞边界：

%% 边界信息 function wall=getbound() global pos_Len pos_Wid road_Wid; wall.bound{1}.line=[-pos_Len,0; 0,0]; wall.bound{2}.line=[0,0; 0,-pos_Wid]; wall.bound{3}.line=[0, pos_Len; -pos_Wid,-pos_Wid]; wall.bound{4}.line=[pos_Len, pos_Len; -pos_Wid,0]; wall.bound{5}.line=[pos_Len, 2*pos_Len; 0,0]; wall.bound{6}.line=[-pos_Len, 2*pos_Len; road_Wid,road_Wid]; wall.num=6; end

这段代码是用来画车位的：

% 画车位 figure(1); hold on; box on; axis equal; plotbox([-pos_Len,-pos_Wid,pos_Len,pos_Wid]) plotbox([pos_Len,-pos_Wid,pos_Len,pos_Wid]) % rectangle('Position',[-pos_Len,-pos_Wid,pos_Len,pos_Wid],'Linewidth',2,'LineStyle','-','EdgeColor','k'); % rectangle('Position',[pos_Len,-pos_Wid,pos_Len,pos_Wid],'Linewidth',1,'LineStyle','-','EdgeColor','k'); plot([0,pos_Len],[-pos_Wid,-pos_Wid],'k-','Linewidth',1); plot([-pos_Len,2*pos_Len],[road_Wid,road_Wid],'k-','Linewidth',1); wall=getbound();

这段代码用来画盒子：

%% 画盒子 function h=plotbox(box) xdata = [box(1); box(1)+box(3); box(1)+box(3); box(1)]; ydata = [box(2); box(2); box(2)+box(4); box(2)+box(4)]; [h,g] = crosshatch_poly(xdata, ydata, 45, 0.25, 'hold', 1, ... 'edgestyle', '-', 'edgecolor', 'k', 'edgewidth', 1, ... 'linestyle', '--', 'linecolor', 'k', 'linewidth', 0.5); end 建立车辆模型

我们需要将车辆的所有信息都表示在车辆模型中，包括：车长，车宽，车轴长，车轴宽，我们在车辆前轮加入车轮方向，在车辆后轮加入一个车身方向以及车身方向的法向量，以便后续计算。

给出车辆模型的代码：

这段代码用于计算车辆信息，包括车轮方向向量，车身方向向量，车身方向的法向量，车后轮位置，车前轮位置等等

%% 计算车辆信息 function car=carinfo(car) % 已知车辆后轴中心和前进单位向量 car.back_cen car.body_e % 求车辆四个顶点的信息 global car_Cen_Head car_Wid car_Cen_Cen; car.head_cen=car.back_cen+car_Cen_Cen*car.body_e; car.body_w=[cos(pi/2), -sin(pi/2); sin(pi/2), cos(pi/2)]*car.body_e; % 车辆的左侧方向 car.weel_e=[cos(car.weel_alpha), -sin(car.weel_alpha); sin(car.weel_alpha), cos(car.weel_alpha)]*car.body_e; % 车辆的车轮方向 car.v1=-car_Cen_Head*car.body_e-car_Wid*car.body_w/2; car.v2=-car_Cen_Head*car.body_e+car_Wid*car.body_w/2; car.v3= car_Cen_Head*car.body_e+car_Wid*car.body_w/2; car.v4= car_Cen_Head*car.body_e-car_Wid*car.body_w/2; car.box=[car.back_cen+car.v1, car.back_cen+car.v2, car.head_cen+car.v3, car.head_cen+car.v4]; car=getcarbound(car); end

这段代码用于计算车辆边界，即车的长方形的框框

%% 计算车辆边界 function car=getcarbound(car) car.bound{1}.line=[car.box(:,1:2)]; car.bound{2}.line=[car.box(:,2:3)]; car.bound{3}.line=[car.box(:,3:4)]; car.bound{4}.line=[car.box(:,[4,1])]; end

这段代码用于画汽车：

%% 画汽车 function h=plotcar(car) h{1}=plot(car.back_cen(1),car.back_cen(2),'ro'); h{2}=plot(car.head_cen(1),car.head_cen(2),'r>'); h{3}=patch(car.box(1,:),car.box(2,:),'-','EdgeColor','k','FaceColor','non'); h{4}=plot([car.back_cen(1),car.back_cen(1)+car.body_e(1)],[car.back_cen(2),car.back_cen(2)+car.body_e(2)],'k-'); h{5}=plot([car.back_cen(1),car.back_cen(1)+car.body_w(1)],[car.back_cen(2),car.back_cen(2)+car.body_w(2)],'m-'); h{6}=plot([car.head_cen(1),car.head_cen(1)+car.weel_e(1)],[car.head_cen(2),car.head_cen(2)+car.weel_e(2)],'b-'); end

这段代码用于清除汽车，因为我们画图用getframe()函数，是将多个图片堆叠在一起的，如果不清楚每一次的车辆，则会将每一张图片的汽车都画上去。（若想看汽车的运动轨迹，车身运动时所占用的范围，就不需要清除汽车）

%% 清除汽车 function clearcar(h) for i=1:length(h) delete(h{i}); end end

现在我们将地图和车辆都画好了，下面我们写碰撞检测以及遍历的代码

碰撞检测以及遍历

下面的代码为碰撞检测，即判断车辆的边界与碰撞边界是否发生重合，在坐标系中，边界的碰撞可以比较两直线函数的大小即可

%% 碰撞检测 function [flag,car]=test(car,wal) car=move_step(car); car=carinfo(car); car=getcarbound(car); flag=0; for i=1:4 f=@(x,y) (y-car.bound{i}.line(2,1))*(car.bound{i}.line(1,2)-car.bound{i}.line(1,1))-... (x-car.bound{i}.line(1,1))*(car.bound{i}.line(2,2)-car.bound{i}.line(2,1)); for j=1:wal.num g=@(x,y) (y-wal.bound{j}.line(2,1))*(wal.bound{j}.line(1,2)-wal.bound{j}.line(1,1))-... (x-wal.bound{j}.line(1,1))*(wal.bound{j}.line(2,2)-wal.bound{j}.line(2,1)); if f(wal.bound{j}.line(1,1),wal.bound{j}.line(2,1))*f(wal.bound{j}.line(1,2),wal.bound{j}.line(2,2))0) % break; % end F(i)=getframe; % disp('forward'); % disp(car.weel_alpha/pi*180); % plot(car.head_cen,'.'); % plot(car.back_cen,'.'); else car.back=-1*car.back; car.weel_alpha=-1*car.weel_alpha; car=carinfo(car); i1=i1+1; % plot(car.head_cen,'-'); % plot(car.back_cen,'-'); % disp('backward'); % disp(car.weel_alpha/pi*180); % dot1=[dots1;car.back_cen(1),car.back_cen(2)]; % dots1=[dots1;dot1]; end dot=[car.back_cen(1),car.back_cen(2)]; dots=[dots;dot]; end car.weel_alpha=-470/16/180*pi; for i=1:200 i2=i2+1; [flag,trycar]=test(car,wall); if flag==0 car=trycar; clearcar(h); car=carinfo(car); h=plotcar(car); F(i)=getframe; if(car.body_e(2)0) % break; i3=i3+1; car.weel_e=car.body_e; car.weel_alpha=0; clearcar(h); h=plotcar(car); car.back=1; fstraight=@(x)((x-car.back_cen(1))/(car.head_cen(1)-car.back_cen(1))*... (car.head_cen(2)-car.back_cen(2))+car.head_cen(2)); y1=@(x)(-(car.box(1)-car.box(7))/(car.box(2)-car.box(8)))*(x-car.back_cen(1))+car.back_cen(2); end % d1=sqrt((car.back_cen(1)-5.4)^2+car.back_cen(2)^2) % y1(5.4); % if(y1(5.4)>0) % break; % end F(i)=getframe; % disp('forward'); % disp(car.weel_alpha/pi*180); % plot(car.head_cen,'.'); % plot(car.back_cen,'.'); else car.back=-1*car.back; car.weel_alpha=-1*car.weel_alpha; car=carinfo(car); i1=i1+1; % plot(car.head_cen,'-'); % plot(car.back_cen,'-'); % disp('backward'); % disp(car.weel_alpha/pi*180); % dot1=[dots1;car.back_cen(1),car.back_cen(2)]; % dots1=[dots1;dot1]; end dot=[car.back_cen(1),car.back_cen(2)]; dots=[dots;dot]; end car.weel_alpha=-470/16/180*pi; for i=1:200 i2=i2+1; [flag,trycar]=test(car,wall); if flag==0 car=trycar; clearcar(h); car=carinfo(car); h=plotcar(car); F(i)=getframe; if(car.body_e(2) 1) x = x'; end if (size(y,1) > 1) y = y'; end % if the background is set, then fill, and set the edge correctly if (exist('backgroundcolor', 'var')) h = fill(x,y,backgroundcolor); if (edgewidth > 0) set(h, 'LineWidth', edgewidth, 'EdgeColor', edgecolor, 'LineStyle', edgestyle); else set(h, 'EdgeColor', backgroundcolor); end end % plot edges if needed for i=1:N if (edgewidth > 0 & ~exist('backgroundcolor', 'var')) % only need to draw edges if width is > 0 and haven't already done so with fill j = mod(i, N)+1; h(i) = plot([x(i) x(j)], [y(i) y(j)], ... 'color', edgecolor, 'linestyle', edgestyle, 'linewidth', edgewidth); end end % now find the range for the lines to plot c = [cosd(lineangle), sind(lineangle)]; % normal to the lines v = [sind(lineangle), -cosd(lineangle)]; % direction of lines obj = c * [x; y]; [mx, kmx] = max(obj, [], 2); [mn, kmn] = min(obj, [], 2); % plot(x(kmx), y(kmx), 'r*'); % plot(x(kmn), y(kmn), 'ro'); distance = sqrt( (x(kmx)-x(kmn)).^2 + (y(kmx)-y(kmn)).^2 ); % find a line describing each edge for i=1:N j = mod(i, N)+1; if (abs(x(j) - x(i)) > 1.0e-12) % find the slope and intersept slope(i) = (y(j) - y(i)) / (x(j) - x(i)); y_int(i) = y(i) - slope(i)*x(i); else % the line is vertical slope(i) = Inf; y_int(i) = NaN; end end % now draw lines clipping them at points that are on the edge of the polygon g = []; % find a slightly larger polygon centroid_x = mean(x); centroid_y = mean(y); epsilon = 0.001; x_dash = (1+epsilon) * (x - centroid_x) + centroid_x; y_dash = (1+epsilon) * (y - centroid_y) + centroid_y; % fill(x_dash, y_dash, 'g'); for m=0:linegap:distance counter = ceil(m/linegap)+1; sigma = [x(kmn), y(kmn)] + m*c; % plot(sigma(1), sigma(2), '+', 'color', linecolor); % for each line, look where it intersepts the edge of polygon (if it does) for i=1:N % find the intercept with this line, and the relevant edge if (isinf(slope(i))) if (abs(v(1)) > 1.0e-12) t = (x(i) - sigma(1)) / v(1); x_i(i) = x(i); y_i(i) = sigma(2) + t * v(2); else x_i(i) = NaN; y_i(i) = NaN; end else if (abs(v(2) - slope(i)*v(1)) > 1.0e-12) t = (slope(i) * sigma(1) - sigma(2) + y_int(i)) / ( v(2) - slope(i)*v(1)); x_i(i) = sigma(1) + t * v(1); y_i(i) = sigma(2) + t * v(2); else x_i(i) = NaN; y_i(i) = NaN; end end end k = find(inpolygon(x_i, y_i, x_dash, y_dash)); if (length(k) == 2) g(counter) = plot(x_i(k), y_i(k), ... 'color', linecolor, 'linestyle', linestyle, 'linewidth', linewidth); elseif (length(k) < 2) % don't plot because we have no clear line elseif (length(k) > 2) % find two unique points d = [x_i(k)', y_i(k)']; d = round(100*d)/100; d = unique(d, 'rows'); g(counter) = plot(d(:,1), d(:,2), ... 'color', linecolor, 'linestyle', linestyle, 'linewidth', linewidth); hold on; end end

若你的matlab版本在2018及以上，则将两段代码分别复制创建成两个m文件，运行第一个代码即可。

如果你看到这里，真的非常感谢！希望我的博客能给你提供一点小小的帮助！

【本文地址】

公司简介

联系我们

今日新闻

推荐新闻

专题文章