[发明专利]基于直行路口检测的移动机器人航迹推算修正系统及方法

权利要求书

1.一种移动机器人航迹推算修正方法，其特征在于，包括如下步骤：

1)启动移动机器人，获取里程计和惯导传感器的数据；

2)通过获取里程计和惯导传感器的数据，对移动机器人的行驶轨迹进行航迹推算；

3)通过相机对移动机器人前方环境进行数据采集，通过路口检测模块判断是否为斑马线，并把数据传送到修正模块，结合电子地图数据对移动机器人航迹推算获取的位置进行修正；

步骤2)中，所述航迹推算的具体步骤为：

21)以移动机器人的起点(x₀,y₀)为原点，设置局部坐标系，将GPS经纬度坐标和航迹推算坐标转换为局部坐标系坐标，并以正北方向作为Y轴正方向，正东方向作为X轴正方向；

22)通过里程计获取行驶距离，通过惯导传感器获取机器人的实时航向，采用通过三角函数法计算得到移动机器人的初始坐标(x_dr,y_dr)，具体公式如下：

xdr=x0+Σk=1nΔd cos[θk-1+Δθ/2-(θs+θ0)]ydr=y0+Σk=1nΔd sin[θk-1+Δθ/2-(θs+θ0)]]]>

其中：

Δd=Δdl+Δdr2]]>

Δθ=Δdr-Δdldwidth]]>

θ_k-1为惯导在k-1时刻输出的航向信息，Δd_r、Δd_l分别为里程计中左轮和右轮在采样时间内获得的里程偏差，d_width为机器人的轮距，θ_s、θ₀分别为惯导的安装偏差和零点输出偏差。