[发明专利]一种基于机器人位置和速度信息的姿态角观测方法

权利要求书

1.一种用于移动机器人姿态角的观测方法，其特征在于将所述移动机器人的运动学方程进行坐标变换，得到新状态的动态方程，通过所述新状态的动态方程，获取所述移动机器人的线速度为非负值时的第一状态观测器，以及所述移动机器人的线速度为负值时的第二状态观测器；根据所述第一状态观测器和所述第二状态观测器获取所述移动机器人的姿态角；

其中，所述新状态的动态方程为：(z₁,z₂)为移动机器人在惯性坐标系下的坐标，(z₃,z₄)分别为姿态角的余弦、正弦值，v为线速度，ω为角速度；

当所述移动机器人的所述线速度为非负值时，获取所述移动机器人的所述第一状态观测器为：

其中为辅助变量，为(z₃,z₄)的估计值，L为可调节控制参数，L＞0；

当所述移动机器人的所述线速度为负值时，获取所述移动机器人的所述第二状态观测器为：

根据所述第一状态观测器和所述第二状态观测器，获取所述移动机器人的所述姿态角，表达式为：

2.根据权利要求1所述的移动机器人姿态角观测方法，其特征在于通过判断移动机器人运动线速度的正负，分别采用两种状态观测器观测移动机器人姿态角的正弦、余弦值，并利用arctan2函数得到移动机器人姿态角的观测值

上式中，(z₁,z₂)为移动机器人在惯性坐标系下的坐标，为姿态角的余弦、正弦估计值，为辅助变量，是的导数，(v,ω)分别为移动机器人的线速度和角速度，L为控制参数，为姿态角的观测值。