APA(自动泊车辅助系统)： 航位推算算法

APA(自动泊车辅助系统)： 航位推算算法

航迹推算算法又叫做相对定位算法，主要是通过车轮脉冲传感器检测车辆每一时刻的运行里程，并结合车辆加速度、角速度等信息计算车辆相对上一时刻的位置，进而得到车辆每一时刻的位姿（即位置和姿态）。同时，也可知道，只有实时定位出车辆的位置，才可对车辆实时精确控制，如：路径规划、路径跟踪等。

一、航位推算算法

航位推算算法原理图

如上图“航位推算算法原理图”所示，假设车辆k与k+1两位置间的时间间隔很小，则在此微元时间内，车辆行驶的里程及车辆位姿计算可如下公式所示：

两位置间的行驶里程推导公式

上述公式是参考航位推算算法原理图所进行的推导，推导过程用到平面几何的知识，求弧长等知识，欢迎大家手动推导，以加深印象。

综上，在学术研究过程中，用上述公式毫无问题，且求解较为准确。但是，在实际的工程应用中，实际工况往往不是学术研究中的理想情况，实际工况如：车辆具有阿克曼角、路面波动、方向盘转角与转速等关系限制，因此在实际工程中用到上述公式时，应考虑阿克曼角（会产生一个沿车辆纵轴线的阿克曼偏移量）的影响

实际后轴中心的转弯半径可换算为如下公式所示：

考虑阿克曼偏移的后轴中心转弯半径

将考虑阿克曼偏移量的后轴转弯半径代入行驶里程推导公式，可得到更为精确的里程计算公式。

到这，基本的航位推算算法过程讲解几乎已经讲解完成，学术研究及工程应用过程还需根据实际工况或者着重研究的方向进行稍微改动，但整体原理不变。