CN106525049A - 一种基于计算机视觉的四足机器人本体位姿跟踪方法 - Google Patents

Abstract

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

Description

一种基于计算机视觉的四足机器人本体位姿跟踪方法

技术领域

本发明涉及一种基于计算机视觉的四足机器人本体位姿跟踪方法，属于智能机器人的技术领域。

背景技术

四足仿生机器人被认为是最佳的足式机器人形式，四足步行机器人比两足步行仿人机器人承载能力强、稳定性好，具备更加良好的适应复杂路况环境的能力，同时相比于六足、八足步行机器人，四足机器人的电子机械结构设计简单、易于控制，使其在军事行动、反恐防暴以及工程建设等多种环境下具有广泛的应用前景。对四足机器人本体姿态进行的研究，为机器人规划协调层接下来的路径规划、步态生成、平衡控制等具有重要的工程价值及实际意义。

机器人位姿标定的传统算法主要有两种：1.运动学回路法，即在已知机器人初始位姿的情况下，通过测量装置获得机器人末时刻的位姿，然后通过求解机器人的运动学方程来获得机器人的运动参数；2.轴线测量法，通过将机器人的关节轴线抽象成一条空间中的直线，然后以该轴线为基础建立机器人的运动学模型，利用空间几何关系解出机器人运动模型中的相关参数。

机器人姿态估计的一般步骤：首先，建立四足机器人本体的几何结构模型，建立载体坐标系、地理坐标系、平台坐标系以及导航坐标系，通过四者之间的关系，推导出载体坐标与导航坐标之间的捷联转换矩阵。然后，根据四足机器人状态感知系统协调规划层的需求，结合机器人本体几何结构模型，对四足机器人本体姿态求解，通过四元数实时计算，得出机器人本体速度、位置以及姿态的实时计算模型，从而计算出机器人本体的速度、位置信息以及机器人本体航向角、俯仰角、翻滚角的姿态信息。

传统位姿估计需要用到加速度计来获得航向角、俯仰角、翻滚角的角加速度，但是随着时间的累积，由于加速度计本身的噪声影响带来的精度问题，误差会越来越大，而后续的累积误差修正算法则会影响机器的实时性，增加系统运算的时间，影响系统的性能。而采用更高精度的加速度计则会提高机器人的设计成本。

发明内容

针对现有技术的不足，本发明提供一种基于计算机视觉的四足机器人本体位姿跟踪方法。

发明概述：

四足机器人Viroscope是一个以液压为动力驱动的四足机器人，本发明为其在trot对角小跑步态时对其位姿进行实时追踪并向MCU传导更新，运行环境涵盖了除实验室外的旷野、山地以及夜间黑暗环境，实验运行轨迹为直线行走。

本发明的技术方案为：

一种基于计算机视觉的四足机器人本体位姿跟踪方法，包括步骤如下：

1)在四足机器人本体背面配置两个单目摄像头，1号摄像头和2号摄像头；1号摄像头指向正前方，2号摄像头指向侧方，两个单目摄像头方向满足严格正交；机器人采取双摄像头替换了传统设计中的高精度陀螺仪和加速度计，节省了机器人的生产成本。

2)图像采集：

将机器人步态设置为对角小跑步态，运动路线设置为直线行走；运动开始时开启1号摄像头和2号摄像头，每个摄像头每1S内拍摄N张图片；图像采集不采用录像的方式，而是使摄像头每1S内拍摄N张图片的方式，有效减少了图像的数量，节省了运算量，改善了系统的实时性。

3)图像匹配：

将两个单目摄像头采集到的第一幅图像分别设定为各自的参考帧图像，对参考帧图像和当前帧图像进行MSER检验，选取前C个区域进行区域标定，然后在标定的区域进行FAST特征点检测；在标定的区域进行FAST特征点检测，取代了传统算法中对整幅图像进行特征点检测的方法，使得提取出来的特征点更加稳定，性能优越，而且FAST角点提取速度非常快，而传统的特征点提取还要筛去特征点，筛去野点，防止误匹配。在标定区域进行FAST特征点检测大大节省了时间，增强了匹配的可靠性，减小了特征点之间的误匹配概率，提高了对四足机器人本体运动参数估计的精度。

4)计算四足机器人运动参数：

机器人本体运动包括水平横向位移Δx、水平纵向位移Δy、竖直位移Δh、俯仰角θ、翻滚角γ和航向角ψ；仰角θ向上为正值，向下为负值，定义域为[-90°～+90°]；翻滚角γ右倾为正值，左倾为负值，定义域为[-180°～+180°]；航向角ψ，定义域为[0°～360°]；其中，航向角ψ由加速度计测得；

统计匹配点，对1号摄像头采集的图像用最小二乘法求解上述超定方程得到翻滚角γ，对2号摄像头采集的图像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(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₀)为机器人初始位姿坐标。至此，四足机器人本体位置姿态信息全部更新完毕；

优选的，针对旷野和夜间环境，选择捷联惯导作为姿态求解的备选系统；定义四元数Q表征机器人当前状态的姿态向量：

其中，

其中，φ表示等效角坐标系的旋转角度；

实时计算四元数Q；输入信号为陀螺仪的数字输出信号其中i为x、y或z；b为机器人躯干坐标系，ω_ib表示在机器人躯干坐标系中，四足机器人饶x,y或z轴的角速度；然后采用二阶龙格库塔法计算：

其中，

K1、K2、Y为中间变量，T为运动过程中的更新周期；q(t)为四元数对时间t时刻的函数；q(t+T)为四元数对时间t+T时刻的函数；ω_i为四足机器人饶x,y或z轴的角速度； 所述陀螺仪设置在机器人躯干中。

由此可得欧拉参数表示的等效旋转矩阵：

根据机器人本体旋转轴为一般轴的等效旋转矩阵：

其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；

可得：

其中，r_ab为机器人本体旋转轴为一般轴的等效旋转矩阵的第a行第b列元素，a＝1,2,3；b＝1,2,3；

由旋转矩阵中可得航向角ψ，俯仰角θ和翻滚角γ；

针对旷野和夜间等视觉不良的环境，我们通过接连惯导作为四足机器人姿态的备选系统对机器人本体姿态进行求解，避免了极端情况下视觉算法失效带来的机器人姿态追踪失败的问题。

由于在某些特殊地形与极端条件下，(比如像旷野与夜间)基于计算机视觉的位姿跟踪无法正常运行，此时将传统捷联式惯导系统作为位姿检测备选方案，大大提高了四足机器人对环境的适应能力和对极端情况的应变能力，可以在多种环境下使用。

优选的，N的取值范围为5～10；C的取值范围为2～4。

优选的，在所述步骤3)和步骤4)之间还包括图像误匹配的步骤，具体方法如下：

计算第j副图像帧与参考帧图像的匹配分数

其中，S_j(m,n)为参考帧图像在坐标(m,n)处的灰度值，T(m,n)为当前帧图像在坐标(m,n)处的灰度值，坐标(m,n)所处的坐标系为图像坐标系，图像左上角起点为原点，x轴为图像的水平轴，y轴为图像的竖直轴，M是指图像的横坐标最大索引值，即图像的宽度，N是指图像的纵坐标的最大索引值，即图像的长度；

设定第一阈值M₁和第二阈值M₂，当匹配分数低于M₁，则判定图像匹配失败，丢弃当前帧图像，如果M₂幅图像后依旧为图像匹配失败状态，则选择当前帧图像作为参考帧图像。

随着四足机器人运动过程的推进，场景最终发生大幅度转换，与参考帧图像公共部分逐渐变小直至消失，这时会导致图像匹配失败；采取上述方法解决了场景切换带来的匹配失败的问题，使得机器人即使发生大幅度的转向运动该算法依旧可以保持优良的性能。

进一步优选的，M₁的取值范围为10～20；M₂的取值范围为3～5。

优选的，所述四足机器人为Viroscope。

本发明的有益效果为：

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

2.本发明所述基于计算机视觉的四足机器人本体位姿跟踪方法，舍弃了传统算法带来的累积误差，算法并未涉及陀螺仪与加速度计的使用，无需根据地磁进行误差补偿，免除了传统捷联式惯导的累积误差和地磁影响；同时在6帧/s的图像采集标准下，位姿跟踪精度达到亚像素精度，比传统算法具有更加精确的机器人本体位姿反馈；

3.本发明所述基于计算机视觉的四足机器人本体位姿跟踪方法，与传统算法逆求解运动方程相比，耗时更短，速度更快，满足机器人运动过程中的实时性要求；硬件上，节省了两个高精度加速度计，有效节省了机器人制造的成本。

附图说明

图1为本发明所述四足机器人Viroscope的概念图；

图2为本发明所述基于计算机视觉的四足机器人本体位姿跟踪方法的流程图；

图3为FAST特征点检测的效果图；

图4为对参考帧图像和当前帧图像进行MSER检验的效果图；

图5为机器人姿态解算示意图；

图6为机器人的运动轨迹图；

图7等效角坐标系。

具体实施方式

下面结合实施例和说明书附图对本发明做进一步说明，但不限于此。

实施例1

如图1-2所示。

一种基于计算机视觉的四足机器人本体位姿跟踪方法，包括步骤如下：

1)在Viroscope四足机器人本体背面配置两个单目摄像头，1号摄像头和2号摄像头；1号摄像头指向正前方，2号摄像头指向侧方，两个单目摄像头方向满足严格正交；

2)图像采集：

将机器人步态设置为对角小跑步态，运动路线设置为直线行走；运动开始时开启1号摄像头和2号摄像头，每个摄像头每1S内拍摄5张图片；

3)图像匹配：

将两个单目摄像头采集到的第一幅图像分别设定为各自的参考帧图像，对参考帧图像和当前帧图像进行MSER检验，选取前4个区域进行区域标定，然后在标定的区域进行FAST特征点检测；在标定的区域进行FAST特征点检测，取代了传统算法中对整幅图像进行特征点检测的方法，使得提取出来的特征点更加稳定，性能优越，而且FAST角点提取速度非常快，而传统的特征点提取还要筛去特征点，筛去野点，防止误匹配。在标定区域进行FAST特征点检测大大节省了时间，增强了匹配的可靠性，减小了特征点之间的误匹配概率，提高了对四足机器人本体运动参数估计的精度。如图3、4所示。

4)计算四足机器人运动参数：

机器人本体运动包括水平横向位移Δx、水平纵向位移Δy、竖直位移Δh、俯仰角θ、翻滚角γ和航向角ψ；仰角θ向上为正值，向下为负值，定义域为[-90°～+90°]；翻滚角γ右倾为正值，左倾为负值，定义域为[-180°～+180°]；航向角ψ，定义域为[0°～360°]；其中，航向角ψ由加速度计测得；

统计匹配点，对1号摄像头采集的图像用最小二乘法求解上述超定方程得到翻滚角γ，对2号摄像头采集的图像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(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₂，求解四足机器人本体运动位移；如图5所示。

运算上述方程，更新机器人运动估计参数，其中，(x₀,y₀)为机器人初始位姿坐标。至此，四足机器人本体位置姿态信息全部更新完毕；机器人本体运动轨迹见图6。

实施例2

如实施例1所述的基于计算机视觉的四足机器人本体位姿跟踪方法，所不同的是，针对旷野和夜间环境，选择捷联惯导作为姿态求解的备选系统；定义四元数Q表征机器人当前状态的姿态向量：

其中，

其中，φ表示等效角坐标系的旋转角度；如图7所示。

实时计算四元数Q；输入信号为陀螺仪的数字输出信号其中i为x、y或z；b为机器人躯干坐标系，ω_ib表示在机器人躯干坐标系中，四足机器人饶x,y或z轴的角速度；然后采用二阶龙格库塔法计算：

其中，

K1、K2、Y为中间变量，T为运动过程中的更新周期；q(t)为四元数对时间t时刻的函数；q(t+T)为四元数对时间t+T时刻的函数；ω_i为四足机器人饶x,y或z轴的角速度； 所述陀螺仪设置在机器人躯干中。

由此可得欧拉参数表示的等效旋转矩阵：

根据机器人本体旋转轴为一般轴的等效旋转矩阵：

其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；

可得：

其中，r_ab为机器人本体旋转轴为一般轴的等效旋转矩阵的第a行第b列元素，a＝1,2,3；b＝1,2,3；

由旋转矩阵中可得航向角ψ，俯仰角θ和翻滚角γ；

θ＝arctan2(r₂₁/cγ，r₁₁/cγ)

。

γ＝arctan2(r₃₂/cγ，r₃₃/cγ)

针对旷野和夜间等视觉不良的环境，我们通过接连惯导作为四足机器人姿态的备选系统对机器人本体姿态进行求解，避免了极端情况下视觉算法失效带来的机器人姿态追踪失败的问题。

由于在某些特殊地形与极端条件下，(比如像旷野与夜间)基于计算机视觉的位姿跟踪无法正常运行，此时将传统捷联式惯导系统作为位姿检测备选方案，大大提高了四足机器人对环境的适应能力和对极端情况的应变能力，可以在多种环境下使用。

实施例3

如实施例1所述的基于计算机视觉的四足机器人本体位姿跟踪方法，所不同的是，在所述步骤3)和步骤4)之间还包括图像误匹配的步骤，具体方法如下：

计算第j副图像帧与参考帧图像的匹配分数

其中，S_j(m,n)为参考帧图像在坐标(m,n)处的灰度值，T(m,n)为当前帧图像在坐标(m,n)处的灰度值，坐标(m,n)所处的坐标系为图像坐标系，图像左上角起点为原点，x轴为图像的水平轴，y轴为图像的竖直轴，M是指图像的横坐标最大索引值，即图像的宽度，N是指图像的纵坐标的最大索引值，即图像的长度；

设定第一阈值M₁和第二阈值M₂，M₁＝20，M₂＝5；当匹配分数低于M₁，则判定图像匹配失败，丢弃当前帧图像，如果M₂幅图像后依旧为图像匹配失败状态，则选择当前帧图像作为参考帧图像。

随着四足机器人运动过程的推进，场景最终发生大幅度转换，与参考帧图像公共部分逐渐变小直至消失，这时会导致图像匹配失败；采取上述方法解决了场景切换带来的匹配失败的问题，使得机器人即使发生大幅度的转向运动该算法依旧可以保持优良的性能。

Claims (6)

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°]；

$\left[\begin{array}{c}{x}_1^{\′}\\ y_1^{\′}\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\θ}& -sin\mathrm{\θ}\\ sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]-\left[\begin{array}{c}{T}_{x1}\\ T_{y1}\end{array}\right]$

$\left[\begin{array}{c}{x}_2^{\′}\\ y_2^{\′}\end{array}\right]=\left[\begin{array}{cc}cos\mathrm{\γ}& -sin\mathrm{\γ}\\ sin\mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]-\left[\begin{array}{c}{T}_{x2}\\ T_{y2}\end{array}\right]$

统计匹配点，对1号摄像头采集的图像用最小二乘法求解上述超定方程得到翻滚角γ，对2号摄像头采集的图像用最小二乘法求解上述超定方程得到俯仰角θ；其中，(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₂，求解四足机器人本体运动位移；

$\mathrm{\Δ}x=x_0+T_{x1}-\frac{l_1}{tan\mathrm{\ψ}}$

$\mathrm{\Δ}y=y_0+T_{y1}-\frac{l_1}{\tan \mathrm{\γ}}$

$\mathrm{\Δ}h=x_0+T_{x2}-\frac{l_2}{tan\mathrm{\θ}}$

运算上述方程，更新机器人运动估计参数，其中，(x₀,y₀)为机器人初始位姿坐标。

2.根据权利要求1所述的基于计算机视觉的四足机器人本体位姿跟踪方法，其特征在于，针对旷野和夜间环境，选择捷联惯导作为姿态求解的备选系统；定义四元数Q表征机器人当前状态的姿态向量：

$Q=\[{\stackrel{\→}{q}}_1,{\stackrel{\→}{q}}_2,{\stackrel{\→}{q}}_3,{\stackrel{\→}{q}}_4\]=q_1\stackrel{\→}{i}+q_2\stackrel{\→}{j}+q_3\stackrel{\→}{k}+q_4$

其中，

$q_1=sin\frac{\mathrm{\φ}}{2}cos\mathrm{\θ}$

$q_2=sin\frac{\mathrm{\φ}}{2}cos\mathrm{\γ}$

$q_3=sin\frac{\mathrm{\φ}}{2}cos\mathrm{\ψ}$

$q_4=cos\frac{\mathrm{\φ}}{2}$

其中，φ表示等效角坐标系的旋转角度；

实时计算四元数Q；输入信号为陀螺仪的数字输出信号其中i为x、y或z；b为机器人躯干坐标系，ω_ib表示在机器人躯干坐标系中，四足机器人饶x,y或z轴的角速度；然后采用二阶龙格库塔法计算：

$\left\{\begin{array}{c}{K}_1={\mathrm{\Ω}}_b\left(t\right)q\left(t\right)\\ Y=q\left(t\right)+{\mathrm{T\Ω}}_b\left(t\right)q\left(t\right)\\ K_2={\mathrm{\Ω}}_b(t+T)Y\\ q(t+T)=q\left(t\right)+(T/2)\×(K_1+K_2)\end{array}\right.$

其中，

${\mathrm{\Ω}}_b=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]$

$Q=cos\[\frac{\mathrm{\θ}}{2}\]+\hat{n}\·\sin \[\frac{\mathrm{\θ}}{2}\]$

K1、K2、Y为中间变量，T为运动过程中的更新周期；q(t)为四元数对时间t时刻的函数；q(t+T)为四元数对时间t+T时刻的函数；ω_i为四足机器人饶x,y或z轴的角速度；

由此可得欧拉参数表示的等效旋转矩阵：

$R_q=\left[\begin{array}{ccc}1-q_2^2-q_3^2& 2(q_1{q}_2-q_3{q}_4)& 2(q_1{q}_3+q_2{q}_4)\\ 2(q_1{q}_2+q_3{q}_4)& 1-q_1^2-q_3^2& 2(q_2{q}_3-q_1{q}_4)\\ 2(q_1{q}_3-q_2{q}_4)& 2(q_2{q}_3+q_1{q}_4)& 1-q_1^2-q_2^2\end{array}\right]$

根据机器人本体旋转轴为一般轴的等效旋转矩阵：

$R_q\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}i\·i\·v\mathrm{\θ}+c\mathrm{\θ}& i\·j\·v\mathrm{\θ}-k\·s\mathrm{\θ}& i\·k\·v\mathrm{\θ}+j\·s\mathrm{\θ}\\ i\·j\·v\mathrm{\θ}+k\·s\mathrm{\θ}& j\·j\·v\mathrm{\θ}+c\mathrm{\θ}& j\·k\·v\mathrm{\θ}+i\·s\mathrm{\θ}\\ i\·k\·v\mathrm{\θ}+j\·s\mathrm{\θ}& j\·k\·v\mathrm{\θ}+i\·s\mathrm{\θ}& k\·k\·v\mathrm{\θ}+c\mathrm{\θ}\end{array}\right]$

其中，cθ＝cosθ,sθ＝sinθ,vθ＝1-cosθ；

可得：

$q_1=\frac{r_{32}-r_{23}}{4q_4}$

$q_2=\frac{r_{13}-r_{31}}{4q_4}$

$q_1=\frac{r_{21}-r_{12}}{4q_4}$

$q_4=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}$

其中，r_ab为机器人本体旋转轴为一般轴的等效旋转矩阵的第a行第b列元素，a＝1,2,3；b＝1,2,3；

由旋转矩阵中可得航向角ψ，俯仰角θ和翻滚角γ；

$\begin{array}{c}\mathrm{\ψ}=\arctan 2(-r_{31},\sqrt{{r_{11}}^2+{r_{21}}^2})\\ \mathrm{\θ}=\arctan 2(r_{21}/c\mathrm{\γ},r_{11}/c\mathrm{\γ})\\ \mathrm{\γ}=\arctan 2(r_{32}/c\mathrm{\γ},r_{33}/c\mathrm{\γ})\end{array}.$

3.根据权利要求1所述的基于计算机视觉的四足机器人本体位姿跟踪方法，其特征在于，N的取值范围为5～10；C的取值范围为2～4。

4.根据权利要求1所述的基于计算机视觉的四足机器人本体位姿跟踪方法，其特征在于，在所述步骤3)和步骤4)之间还包括图像误匹配的步骤，具体方法如下：

计算第j副图像帧与参考帧图像的匹配分数

$D\left(j\right)={\mathrm{\Σ}}_{m=1}^M{\mathrm{\Σ}}_{n=1}^N\[S_i(m,n)*T(m,n)\]$

其中，S_j(m,n)为参考帧图像在坐标(m,n)处的灰度值，T(m,n)为当前帧图像在坐标(m,n)处的灰度值，坐标(m,n)所处的坐标系为图像坐标系，图像左上角起点为原点，x轴为图像的水平轴，y轴为图像的竖直轴，M是指图像的横坐标最大索引值，即图像的宽度，N是指图像的纵坐标的最大索引值，即图像的长度；

设定第一阈值M₁和第二阈值M₂，当匹配分数低于M₁，则判定图像匹配失败，丢弃当前帧图像，如果M₂幅图像后依旧为图像匹配失败状态，则选择当前帧图像作为参考帧图像。

5.根据权利要求4所述的基于计算机视觉的四足机器人本体位姿跟踪方法，其特征在于，M₁的取值范围为10～20；M₂的取值范围为3～5。

6.根据权利要求1所述的基于计算机视觉的四足机器人本体位姿跟踪方法，其特征在于，所述四足机器人为Viroscope。