[发明专利]一种人体关节角度解算方法及装置

摘要

本发明涉及一种基于四元数的人体关节角度解算方法及装置。所述方法包括：获取第一采集点的初始四元数和第二采集点的初始四元数；利用所述第一采集点的初始四元数和所述第二采集点的初始四元数计算两者的初始偏置四元数；获取每个采集周期内第一采集点的当前四元数和第二采集点的当前四元数；利用所述第一采集点的当前四元数和所述第二采集点的当前四元数计算两者的绝对偏置四元数；计算当前时刻下所述第一采集点和所述第二采集点的相对偏置四元数；根据上述相对偏置四元数解算右臂肘关节在三轴方向上的相对转角。所述装置基于上述方法实现。本发明可以利用两个采集点的四元数解算到人体关节在三轴方向上相对于初始位置的实时角度变化，实现基于人体姿态的机器人随动控制。

主权项

1.一种人体关节角度解算方法，其特征在于，包括：获取人体关节的第一采集点的初始四元数和与所述第一采集点相邻的第二采集点的初始四元数；利用所述第一采集点的初始四元数和所述第二采集点的初始四元数计算两者的初始偏置四元数；获取每个采集周期内所述第一采集点的当前四元数和所述第二采集点的当前四元数；利用所述第一采集点的当前四元数和所述第二采集点的当前四元数计算两者的绝对偏置四元数；计算当前时刻下所述第一采集点和所述第二采集点的相对偏置四元数；根据上述相对偏置四元数解算人体关节在三轴方向上的相对转角；其中，所述初始偏置四元数采用以下公式表示：所述绝对偏置四元数采用以下公式表示：式中，T＝q₁₀²+q₁₁²+q₁₂²+q₁₃²；所述相对偏置四元数表示以下公式表示：在上述公式中，T＝q₁₀²+q₁₁²+q₁₂²+q₁₃²；Δq'₀、Δq'₁、Δq'₂和Δq'₃分别为第一采样点与第二采样点初始偏置中的第一元数、第二元数、第三元数和第四元数，Δq₀、Δq₁、Δq₂和Δq₃分别为第一采样点与第二采样点绝对偏置中的第一元数、第二元数、第三元数和第四元数；q'₁₀、q'₁₁、q'₁₂和q'₁₃分别为初始情况下第一采集点中第一元数、第二元数、第三元数和第四元数，q'₂₀、q'₂₁、q'₂₂和q'₂₃分别为初始情况下第二采集点中第一元数、第二元数、第三元数和第四元数，T'为初始情况下第一采集点中第一元数、第二元数、第三元数和第四元数的平方和；q₁₀、q₁₁、q₁₂和q₁₃分别为采集周期内第一采集点中第一元数、第二元数、第三元数和第四元数，q₂₀、q₂₁、q₂₂和q₂₃分别为采集周期内第二采集点中第一元数、第二元数、第三元数和第四元数，T为采集周期内第一采集点中第一元数、第二元数、第三元数和第四元数的平方和，q'₁为第一采集点的初始四元数，q'₂为第二采样点的初始四元数，q₁为任意一个采集周期内第一采集点的当前四元数，q₂为任意一个采集周期内第二采集点的当前四元数。