[发明专利]一种基于计算机视觉的四足机器人本体位姿跟踪方法

摘要

本发明涉及一种基于计算机视觉的四足机器人本体位姿跟踪方法。本发明所述基于计算机视觉的四足机器人本体位姿跟踪方法，通过给四足机器人本体配置两个摄像头，可以实时获取机器人的翻滚角以及俯仰角，通过图像匹配可以达到亚像素级别的精度，优化了传统算法中的精确度问题，同时算法具有较高的鲁棒性，对噪声具有较强的抵抗能力，优化了传统算法带来的累计误差问题。

主权项

1.一种基于计算机视觉的四足机器人本体位姿跟踪方法，其特征在于，包括步骤如下：1)在四足机器人本体背面配置两个单目摄像头，1号摄像头和2号摄像头；1号摄像头指向正前方，2号摄像头指向侧方，两个单目摄像头方向满足严格正交；2)图像采集：将机器人步态设置为对角小跑步态，运动路线设置为直线行走；运动开始时开启1号摄像头和2号摄像头，每个摄像头每1S内拍摄N张图片；3)图像匹配：将两个单目摄像头采集到的第一幅图像分别设定为各自的参考帧图像，对参考帧图像和当前帧图像进行MSER检验，选取前C个区域进行区域标定，然后在标定的区域进行FAST特征点检测；4)计算四足机器人运动参数：机器人本体运动包括水平横向位移Δx、水平纵向位移Δy、竖直位移Δh、俯仰角θ、翻滚角γ和航向角ψ；俯仰角θ向上为正值，向下为负值，定义域为[‑90°～+90°]；翻滚角γ右倾为正值，左倾为负值，定义域为[‑180°～+180°]；航向角ψ，定义域为[0°～360°]；统计匹配点，对2号摄像头采集的图像用最小二乘法求解上述超定方程得到翻滚角γ，对1号摄像头采集的图像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(x₁,y₁)为1号摄像头采集的参考帧图像的特征点坐标，(x′₁，y′₁)为1号摄像头采集的当前帧图像的特征点坐标；(x₂，y₂)为2号摄像头采集的参考帧图像的特征点坐标，(x′₂，y′₂)为2号摄像头采集的当前帧图像的特征点坐标；坐标系为图像坐标系，x轴为图像的水平轴，y轴为图像的竖直轴，坐标系原点为图像左上角第一点；T_x1为1号摄像头帧间图像的水平位移，T_y1为1号摄像头帧间图像的垂直位移；T_x2为2号摄像头帧间图像的水平位移，T_y2为2号摄像头帧间图像的垂直位移；5)分别测量1号摄像头距离障碍物的距离l₁，2号摄像头距离障碍物的距离l₂，求解四足机器人本体运动位移；运算上述方程，更新机器人运动估计参数，其中，(x₀,y₀)为机器人初始位姿坐标。