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

说明书

本发明设计一种利用移动机器人位置和速度信息观测姿态角的方法，解决测量移动机器人姿态角的问题。1)利用状态变换，将机器人运动学模型转换成一种特殊形式，消去模型方程中的三角函数；2)在新的状态方程下，基于机器人的位置和速度信息构造针对姿态角的观测器；3)对观测器动态方程进行累积求和运算，计算出机器人姿态角的观测值，解决机器人姿态角准确获取的问题。该方法可很好地估计出移动机器人的姿态角，无稳态误差，无漂移。

技术领域

本发明属于电子信息、机器人控制技术领域，涉及一种利用移动机器人的位置、线速度和角速度信息观测移动机器人姿态角的方法。

背景技术

移动机器人是工程应用、控制理论研究的重要对象，是包含环境感知、动态规划、运动控制等技术的综合系统。对移动机器人的研究是涉及模式识别，人工智能，自动控制等学科，体现出计算机技术和控制技术的结合，具有很广泛的应用价值。

移动机器人的控制可划分为点镇定、路径跟踪、轨迹跟踪这三类问题。解决移动机器人的控制问题需要获取移动机器人的位置、速度和姿态角等信息，其中，位置和速度是很容易得到的，在实际应用中有各种各样的方案，而对于移动机器人的姿态角常采用惯性导航模块或航迹推算算法获得。惯性导航模块因存在随机误差、零点漂移等问题，常需要花费大量时间调节参数、抑制零漂，而航迹推算算法的重大缺陷在于存在累积误差，上述两种方法均不能保证长时间稳定获取移动机器人姿态角。

发明内容

为了解决上述准确获取移动机器人姿态角的问题，本发明提出一种基于移动机器人位置、线速度和角速度信息的姿态角观测方法，实现对移动机器人姿态角的准确观测。

本发明包括以下内容：

(1)以移动机器人运动学模型为基础，进行状态变换，消去运动学模型中包含的三角函数；

(2)以变换后的模型方程为基础，分别考虑移动机器人运动线速度的正负情况，提出一种针对移动机器人的姿态角的观测方法。

附图说明

图1程序运行流程图

图2姿态角估计误差的变化情况

具体实施方式

本发明以移动机器人运动学模型为研究对象。

(1)模型描述及状态变换。移动机器人的一种运动学方程为

其中(x，y)为移动机器人在惯性坐标系下的坐标，θ为姿态角，(v，ω)分别为移动机器人的线速度和角速度。进行状态变换为

z₁＝x，z₂＝y，z₃＝coSθ，z₄＝sinθ <2>得到新状态的动态方程为

由<3>可知，新状态的动态方程不显含三角函数。

(2)状态观测器。结合式<3>可知，利用新状态动态方程的结构特点，考虑当机移动器人的线速度为非负的情况，得到针对(z₃，z₄)的观测方法为

其中为观测器辅助变量，为(z₃，z₄)的估计值，L为可调节控制参数，需满足L＞0。考虑移动机器人的线速度为负的情况，得到针对(z₃，z₄)的观测方法为

由<4><5>两式，得到移动机器人姿态角的观测值