[发明专利]一种移动机器人位置和姿态同时规划方法

权利要求书

1.一种移动机器人位置和姿态同时规划方法，其特征在于，包括以下步骤：

步骤一，在路径规划模块中获取规划好的位姿点P_i＝(x,y,θ),i＝0,1,…,n,n≥5，其中移动机器人的位置用x,y表示，朝向用θ表示；

步骤二，整理移动机器人的位姿点序列，以满足移动机器人的运动状态要求；

步骤三，用基于李群李代数表示的De Casteljau算法生成Bezier曲线表示下的连续位姿轨迹，使用差分方法获得移动机器人位姿轨迹速度曲线；

步骤四，将笛卡尔空间规划的速度映射到移动机器人工作空间中作为速度以输入控制机器人在位置和姿态上同步运动变化。

2.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤一具体包括：从路径规划模块中获取移动机器人已规划的起点，终点及其中间点的位姿序列P_i＝(x,y,θ),i＝0,1,…,n,n≥5。

3.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤二具体包括：使始末的路径点重复四次输出给轨迹规划模块，以满足移动机器人的始末速度和加速度为零的要求。

4.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤三中基于李群李代数示的De Casteljau算法生成Bezier曲线表示下的连续位姿轨迹包括：

用旋量坐标Q＝[v_x,v_y,v_z,w_x,w_y,w_z,]，其中V_x,V_y,W_z分别用x,y,θ代替，给定若干个移动机器人的旋量坐标Q₁,Q₂,…,Q_n∈se(3),对于每一个t∈[0,1],相邻两个李代数元素和之间的李代数为：

其中，

经过n-1次递归，得出在每一个t∈[0,1]时刻Bezier位姿轨迹曲线上的位姿点形成的正交矩阵

5.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤三中Bezier曲线的插值算法为：

每一个t∈[0,1]时刻对应的正交矩阵通过对数映射得出位姿点的旋量坐标所有移动机器人位姿点形成一条C3连续Bezier曲线。

6.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤四具体包括：通过轮式里程计实时获取移动机器人的航向角，将规划的笛卡尔空间速度映射到机器人工作空间作为控制输入，实现移动机器人位置和姿态同步变化。