1.算法描述
根据车轮轮速信号和转向盘转角信号,基于改进无迹卡尔曼滤波(UKF)理论设计了自动泊车车辆位姿估计算法,首先基于阿克曼转向原理建立泊车运动学方程并推导出状态方程和测量方程,随后添加常值噪声统计估计器,最后通过联邦滤波结构输出结果.仿真和硬件在环试验结果表明,本文提出的算法在X、Y方向上得到的估计值与理论值偏差范围均在可接受范围内,相比于其他滤波算法,能够更好地描述泊车过程中车辆的运动轨迹,提高定位精度,且对噪声变化具有自适应能力.
UKF（Unscented Kalman Filter)，中文释义是无损卡尔曼滤波、无迹卡尔曼滤波或者去芳香卡尔曼滤波。是无损变换(UT) 和标准Kalman滤波体系的结合，通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系。
与EKF（扩展卡尔曼滤波）不同,UKF是通过无损变换使非线性系统方程适用于线性假设下的标准Kalman滤波体系,而不是像EKF那样,通过线性化非线性函数实现递推滤波。目标跟踪有两个理论基础,即数据关联和卡尔曼滤波技术 . 由于在实际的目标跟踪中,跟踪系统的状态模型和量测模型多是非线性的,因此采用非线性滤波的方法.
（1）对于某个系统，你拥有准确的数学模型（状态方程和观测方程），也就是说，给出这个系统的输入，你必然能算出这个系统的输出；但是，在现实生活中往往拿到的第一手测试数据并不是你想要的最直观的数据（而且数据还有噪声），这时候使用UKF可以依靠相对准确的数值模型和结合测试数据来得到你想要的信息。
（2）数值模型的参数相对不准确，但实测数据可信度更好，这时候使用UKF可以依靠相对准确的实测数据和结合数值模型来还原较真实的参数。
2.仿真效果预览
matlab2022a仿真结果如下：
3.MATLAB核心程序 `x2=x1+2rcos(pi+t1);
y2=y1+2rsin(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');
yhoulunzx=x+r4*cos(jiao8);
yhoulunzy=y+r4*sin(jiao8);
yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);
yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);
yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);
yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);
l8=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');
pause(0.1);
delete(l1);
delete(l2);
delete(l3);
delete(l4);
delete(l5);
delete(l6);
delete(l7);
delete(l8);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
break;
end
A110`
