1.完整项目描述和程序获取
>面包多安全交易平台:https://mbd.pub/o/bread/Y5mampdp
>如果链接失效，可以直接打开本站店铺搜索相关店铺：
>如果链接失效，程序调试报错或者项目合作也可以加微信或者QQ联系。
2.部分仿真图预览
3.算法概述
根据车轮轮速信号和转向盘转角信号,基于改进无迹卡尔曼滤波(UKF)理论设计了自动泊车车辆位姿估计算法,首先基于阿克曼转向原理建立泊车运动学方程并推导出状态方程和测量方程,随后添加常值噪声统计估计器,最后通过联邦滤波结构输出结果.仿真和硬件在环试验结果表明,本文提出的算法在X、Y方向上得到的估计值与理论值偏差范围均在可接受范围内,相比于其他滤波算法,能够更好地描述泊车过程中车辆的运动轨迹,提高定位精度,且对噪声变化具有自适应能力.
UKF（Unscented Kalman Filter)，中文释义是无损卡尔曼滤波、无迹卡尔曼滤波或者去芳香卡尔曼滤波。是无损变换(UT) 和标准Kalman滤波体系的结合，通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系。
与EKF（扩展卡尔曼滤波）不同,UKF是通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系,而不是像EKF那样,通过线性化非线性函数实现递推滤波。目标跟踪有两个理论基础,即数据关联和卡尔曼滤波技术 . 由于在实际的目标跟踪中,跟踪系统的状态模型和量测模型多是非线性的,因此采用非线性滤波的方法.
4.部分源码
x2=x1+2*r*cos(pi+t1);
y2=y1+2*r*sin(pi+t1);
fai2=-fai;
for i=t1:-ds:(t1-t2)
x=x2+r*cos(i);
y=y2+r*sin(i);
theta=(pi/2)+i;
if(theta-(pi/2)<=0)
xs=x;
ys=y;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%终点偏角为负，因此下面一段为矫正让其直行
for j=ys:-2.5*ds:yd
x=xs;
y=j;
theta=pi/2;%theta0为起始点车身偏角
jiao1=atan((width/2)/(long-houxuan));
jiao2=jiao1;
jiao3=atan((width/2)/houxuan);
jiao4=jiao3;
jiao1=theta-jiao1;
jiao2=theta+jiao2;
jiao3=theta+pi-jiao3;
jiao4=theta+pi+jiao4;
r1=sqrt((width/2)^2+(long-houxuan)^2);%以下描述车身的四个端点
r2=sqrt((width/2)^2+houxuan^2);
youqianx=x+r1*cos(jiao1);
youqiany=y+r1*sin(jiao1);
zuoqianx=x+r1*cos(jiao2);
zuoqiany=y+r1*sin(jiao2);
zuohoux=x+r2*cos(jiao3);
zuohouy=y+r2*sin(jiao3);
youhoux=x+r2*cos(jiao4);
youhouy=y+r2*sin(jiao4);
l1=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-r');
l2=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-r');
l3=plot([zuohoux,youhoux],[zuohouy,youhouy],'-r');
l4=plot([youhoux,youqianx],[youhouy,youqiany],'-r');
jiao5=atan((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子
jiao6=jiao5;
jiao5=theta-jiao5;
jiao6=theta+jiao6;
jiao7=theta+(pi/2);
jiao8=theta-(pi/2);
jiao9=theta;
jiaoa=theta;
r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);
r4=width/2;
yqianlunzx=x+r3*cos(jiao5);
yqianlunzy=y+r3*sin(jiao5);
yqianlunqx=yqianlunzx+lunjin*cos(jiao9);
yqianlunqy=yqianlunzy+lunjin*sin(jiao9);
yqianlunhx=yqianlunzx-lunjin*cos(jiao9);
yqianlunhy=yqianlunzy-lunjin*sin(jiao9);
l5=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');
zqianlunzx=x+r3*cos(jiao6);
zqianlunzy=y+r3*sin(jiao6);
zqianlunqx=zqianlunzx+lunjin*cos(jiao9);
zqianlunqy=zqianlunzy+lunjin*sin(jiao9);
zqianlunhx=zqianlunzx-lunjin*cos(jiao9);
zqianlunhy=zqianlunzy-lunjin*sin(jiao9);
l6=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');
zhoulunzx=x+r4*cos(jiao7);
zhoulunzy=y+r4*sin(jiao7);
zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);
zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);
zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);
zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);
l7=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');
end
A110
