[发明专利]机器人定向方法、系统与计算机可读存储介质

摘要

本发明公开了机器人定向方法、系统与计算机可读存储介质，机器人定向方法包含以下步骤：初始化、采样点自增、获取偏航速率、先验估计、判断是否进行视觉校正、后验估计以及输出定向结果；机器人定向系统由偏航速率检测单元、地面图像采集单元以及数据处理单元组成，其中偏航速率检测单元是可选的；计算机可读存储介质存储有定向程序，用于实现定向方法。相比于已有技术，本发明具有成本低、精度高、抗干扰性好、检测快、隐私入侵度低等优点。

主权项

1.一种机器人定向方法，其特征在于，在机器人工作区域地面配置有平行的辅助线，所述定向方法包括以下步骤：S101：初始化：自动地将采样点序号t设置为0，人工地将采样间隔T设置为一个大于0的实数，人工地将视觉校正间隔参数N设置为一个大于0的整数，人工地将航向角后验估计值设置为一个大于等于0小于2π的实数，人工地设置偏航速率检测模式M：如果没有可以检测偏航速率的传感器，则令M＝0，如果有可以检测偏航速率的传感器，则令M＝1；S102：令t自增1；S103：获取偏航速率：获取第t个采样点的偏航速率测量值δt，具体如下：如果M＝0，则令δt＝0，如果M＝1，则将δt设置为第t个采样点的偏航速率传感器的输出值或者多个偏航速率传感器的输出融合值；S104：先验估计：计算第t个采样点的航向角先验估计值方式如下：S105：判断是否进行视觉校正：设置第t个采样点的视觉校正触发标志位λt，令S106：后验估计：如果λ_t＝0，则令如果λ_t＝1，则从地面图像采集单元读取地面图像，并获取地面图像中辅助线的参数其中i＝1,2,···,l_t，l_t为第t个采样点的地面图像中辅助线的数量，表示第t个采样点获取的地面图像的图像坐标系UV的原点到图像中第i条辅助线的垂线长度，表示第t个采样点获取的地面图像的图像坐标系U轴到上述垂线的角度，μ与ν分别为地面图像的宽度与高度；从辅助线上任取两点与其中(u,v)表示图像坐标系UV下的坐标点，带入到透视变换函数中，可以得到与变换后的坐标点，分别为：其中，与分别为与映射到真实坐标系下的坐标，进而，可以计算出辅助线的在图像坐标系UV下的参数映射到真实坐标系下的参数计算方式如下：然后，计算的均值即接着，获取第t个采样点的视觉航向角测量集合Θt，计算方式如下：其中，φ为辅助线角度；最后，计算第t个采样点的航向角后验估计值方式如下：其中，w∈[0,1)为融合权值，与为Θ_t中的元素，abs(·)为计算绝对值函数；S107：重复步骤S102至S106，输出每个采样点的航向角后验估计值，即定向结果。