一种移动机器人位置和姿态同时规划方法

1.本发明属于机器人领域，具体涉及一种移动机器人位置和姿态同时规划方法。

背景技术：

2.目前，具有三自由度移动机器人的轨迹规划方法主要是对机器人的位置进行规划，很少同时考虑到移动机器人航向角的规划。具有三自由度移动机器人的高机动性和灵活性，当移动机器人的位置和姿态都有设定时，具有三自由度的移动机器人能实现位姿同时规划，不必要先移动后旋转或先旋转后移动。所以移动机器人的位置和姿态同时规划很有必要。
3.现有的位姿规划方法是使用两种规划方法把位置和姿态分开进行规划，然后统一两种轨迹运行时间，实现了位置和姿态同时开始启动，同时停止。但是现有的位姿同步规划方法需要建立两种规划方法中的参数之间的关系，如果两参数之间的关系建立的不准确会影响两条轨迹的同步性。如专利号为cn113103240a，cn113103241a中提供了机器人位姿规划方法，提供的方法先将位置和姿态分两种方法规划，再建立两规划参数关系实现位置和姿态轨迹的同步规划；cn113942016a中提供了一种机械臂末端运动位姿时间同步方法，分别采用梯形轨迹规划方法和“角-轴”法得到位置轨迹参数和姿态轨迹参数，还需要根据位置轨迹时间调整姿态轨迹；非专利文献《机械手空间圆弧位姿轨迹规划算法的实现》中采用齐次矩阵和四元数分别进行位置和姿态轨迹规划，且姿态轨迹c2连续。上述方法虽然实现了机器人位置和姿态同时规划，但是规划方法过程复杂，计算效率低，而且只能保证姿态c2连续，不能保证姿态的加速度光滑连续，这将影响移动机器人的跟踪精度。因此研究一种移动机器人位置和姿态轨迹同时规划方法且能保证移动机器人运动加速度连续光滑很重要。

技术实现要素：

4.为了弥补现有技术的不足，本发明提供一种移动机器人位置和姿态同时规划方法的技术方案。
5.为实现以上发明目的，本发明采用的技术方案包括：
6.一种移动机器人位置和姿态同时规划方法，主要用于具有三个自由度的移动机器人的位置和姿态轨迹同步规划，包括以下步骤：
7.步骤一，从路径规划模块中获取已规划出移动机器人的位姿点pi＝(x,y,θ),i＝0,1,
…
,n,n≥5。
8.步骤二，为了满足移动机器人的始末速度和加速度为0，可使始末的路径点重复四次输出给轨迹规划模块。例如，路径规划模块规划的位姿点p0,p1,
…
,pn,为满足移动机器人运动要求，输入给轨迹规划模块的位姿点序列为p0,p0,p0,p0,p1,
…
,p
n-1
,pn,pn,pn,pn。
9.步骤三，采用基于李李代数表示的de casteljau算法生成bezier曲线插值算法保证生成的移动机器人位置和姿态轨迹规划的同步性。将轨迹曲线求导后得出的速度曲线作为移动机器人的控制输入，移动机器人能实现既平移又旋转的运动，bezier曲线生成的
运动轨迹可保证移动机器人的线速度，角速度的光滑连续变化，提高了移动机器人运动的平稳性。
10.所述基于李李代数表示的de casteljau算法生成bezier曲线插值算法为：
11.用旋量坐标q＝[v
x,vy,vz,wx,wy,wz,
]表示移动机器人在全局坐标系的位姿，其中v
x
,vy,wz分别用x,y,θ代替。给定若干个移动机器人的旋量坐标q1,q2,
…
,qn∈se(3),对于每一个t∈[0,1],相邻两个李代数元素和之间的李代数为：
[0012][0013]
其中，
[0014]
经过n-1次递归，得出在每一个t∈[0,1]时刻bezier位姿轨迹曲线上的位姿点形成的正交矩阵
[0015]
本发明中基于李李代数的de casteljau算法能拟合给定的一系列位姿点，得出通过起点和终点的bezier轨迹曲线，其算法特殊之处在于它能同步拟合给定的移动机器人的位置和航向角，不需要将位置和航向角分开规划再统一时间，而且算法简单。
[0016]
所述基于李李代数表示的de casteljau算法生成bezier曲线插值算法为：
[0017]
每一个t∈[0,1]时刻对应的正交矩阵通过对数映射得出位姿点的旋量坐标所有移动机器人位姿点形成一条c3连续的bezier曲线。
[0018]
通过在相邻位姿点间采用差分算法得出一条连续光滑的移动机器人速度曲线，并能满足移动机器人起始和终止状态速度为零。
[0019]
同样地，通过在相邻速度点采用差分算法得出一条连续光滑的移动机器人加速度曲线保持连续光滑性，提高了移动机器人的平稳性。
[0020]
移动机器人的加速度曲线通过在相邻速度点间采用差分算法得出的，加速度曲线连续光滑性，提高了移动机器人的平稳性。
[0021]
步骤四，通过轮式里程计实时获取移动机器人的航向角，将规划的笛卡尔空间速度映射到机器人工作空间作为控制输入，实现移动机器人位置和姿态同步变化，移动机器人的输入可由下式给出：
[0022][0023]
其中,等式左边为移动机器人的速度输入，等式右边r(θ)是笛卡尔坐标系向移动机器人坐标系映射的旋转矩阵，[v
x v
y w]
t
是规划出的移动机器人的速度。
[0024]
只要将t时刻规划的速度[v
x
,vy,ω]左乘此刻笛卡尔坐标系向机器人坐标系映射的旋转矩r(θ)作为移动机器人的输入，就能实现移动机器人既平移又旋转运动。
[0025]
与现有技术相比，本发明的有益效果在于：
[0026]
本发明适用于三自由度移动机器人的位姿规划，基于李李代数表示的decasteljau算法生成bezier曲线算法使用同一个参数t,用李代数表示设定的位姿点，通过多次李代数之间的递归和李李代数之间的映射，得到每个t时刻对应的移动机器人位
置和姿态，该方法使用同一个参数同时规划移动机器人的位置和姿态，不需要用两个参数分别规划，再建立两个参数之间的关系，能获得c3连续的移动机器人的位姿轨迹，保证了移动机器人速度和加速度连续光滑，有效提高了移动机器人运行的平稳性。
附图说明
[0027]
图1为本发明一种移动机器人位置和姿态同时规划方法流程图；
[0028]
图2为本发明方法规划的移动机器人的位姿轨迹图；
[0029]
图3为本发明方法规划的移动机器人速度曲线图；
[0030]
图4为本发明方法规划的移动机器人加速度曲线图；
[0031]
图5为本发明方法应用在移动机器人实际运动轨迹图；
[0032]
图2中原点表示给定移动机器人的位置，星号点表示移动机器人实际位置，粗箭头代表给定移动机器人的航向角，细箭头代表移动机器人实际的航向角。
具体实施方式
[0033]
在本发明的描述中，需要理解的是，术语“一端”、“另一端”、“外侧”、“上”、“内侧”、“水平”、“同轴”、“中央”、“端部”、“长度”、“外端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0034]
下面结合附图对本发明作进一步说明。
[0035]
本发明所提出的一种移动机器人位置和姿态同时规划方法如图1所示，该方法的工作流程如下：
[0036]
步骤一，在路径规划模块中获取已规划好的位姿点pi＝(x,y,θ),i＝0,1,
…
,n,n≥5，其中移动机器人的位置用x,y表示，朝向用θ表示。
[0037]
步骤二，为满足移动机器人的运动状态要求，需整理移动机器人的位姿点序列。
[0038]
步骤三，采用基于李李代数表示的de casteljau算法生成bezier曲线表示下连续位姿轨迹，使用差分方法获得移动机器人位姿轨迹速度曲线。
[0039]
步骤四，将笛卡尔空间规划的速度映射到移动机器人工作空间中作为输入速度以控制机器人在位置和姿态上同步运动变化。
[0040]
从图2，图3，图4所示的移动机器人的位姿轨迹曲线，速度曲线和加速度曲线可知，本发明移动机器人位姿轨迹同步规划方法简单高效，有效提高移动机器人运行的平稳性。
[0041]
从路径规划模块中获取到移动机器人在笛卡尔坐标系中的位姿点后，为了满足移动机器人的始末速度和加速度为0，可使始末的路径点重复四次输出给轨迹规划模块。例如，路径规划模块规划的位姿点列表为：
[0042]
[0043][0044]
为满足移动机器人运动要求，输入给轨迹规划模块的位姿点序列为：
[0045]
序列xyθp0000p1000p2000p3000p40.805p51.20.59p61.5115p71.81.217p82.51.522p92.51.522p
10
2.51.522p
11
2.51.522
[0046]
所述的基于李李代数表示的de casteljau算法生成bezier曲线插值算法为：用旋量坐标q＝[v
x,vy,vz,wx,wy,wz,
]表示移动机器人在笛卡尔坐标系的位姿，其中v
x
,vy,wz用x,y,θ代替。给定若干个移动机器人的旋量坐标q1,q2,
…
,q
12
∈se(3),如图2中所示，移动机器人在笛卡尔坐标系中的位置用圆点表示，移动机器人在笛卡尔坐标系中的航向角用粗箭头表示。对于每一个t∈[0,1],相邻两个李代数元素和之间的李代数为：
[0047][0048]
其中，
[0049]
经过11次递归，得出在每一个t∈[0,1]时刻bezier位姿轨迹曲线上的位姿点形成的正交矩阵
[0050]
所述的基于李李代数表示的de casteljau算法生成bezier曲线插值算法为：每一个t∈[0,1]时刻对应的正交矩阵通过对数映射得出位姿点的旋量坐标移动机器人的位姿点形成了位姿轨迹曲线如图2所示，该位姿轨迹曲线具有c3连续性。星点表示移动机器人在笛卡尔坐标系中的位置，细箭头表示移动机器人在笛卡尔坐标系中的航向角。
[0051]
移动机器人的速度曲线通过在相邻位姿点间采用差分算法得出。在t1时刻，移动机器人沿笛卡尔坐标系x轴运动速度根据旋量坐标和的第一列一次差分得出，移动机器人沿笛卡尔坐标系y轴运动速度根据旋量坐标和的第二列一次差分得出和移动机器人沿笛卡尔坐标系绕z轴转动的速度根据旋量坐标和的第一列一次差分得出，移动机器人的位姿轨迹是c3连续的bezier曲线，通过差分算法得出的速度曲线连续光滑，其速度曲线如图3所示，能够使移动机器人位置和姿态同时运动变化，且满足在起始和终止时刻移动机器人速度为零。
[0052]
同样地，移动机器人的加速度曲线是在相邻速度点间采用差分算法得出的，在t1时刻，移动机器人沿笛卡尔坐标系x轴运动加速度根据旋量坐标和的第一列二次差分得出，移动机器人沿笛卡尔坐标系y轴运动加速度根据旋量坐标和的第二列二次差分得出和移动机器人沿笛卡尔坐标系绕z轴转动的速度根据旋量坐标和的第一列二次差分得出，各方向的加速度曲线光滑连续，对移动机器人能满足移动机器人起点和终点加速度为零要求，其加速度曲线如图4所示，能够提高移动机器人的平稳性。
[0053]
进一步地，移动机器人的输入可由下式给出：
[0054][0055]
其中等式左边为移动机器人的速度输入，等式右边r(θ)是笛卡尔坐标系向移动机器人坐标系映射的旋转矩阵，其中航向角是从轮式里程计实时获取的，[v
x v
y w]
t
是规划出的移动机器人的速度。
[0056]
依次按照上述步骤进行规划移动机器人位置和姿态，具有三自由度的移动机器人的实际运动轨迹如图5所示，移动机器人的位置和姿态同时启动，同时停止，而且位姿轨迹光滑连续，有效提高了位姿轨迹的平稳性。因存在地面不平整或轮子打滑等原因，允许位置误差在
±
0.10m，姿态误差在
±5°
，实际运动轨迹的误差在允许误差范围内。
[0057]
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神及范围的情况下，能够做出其他改进和变形以其他的具体形式实现本发明。因此，应当将实施例看作是示范例，而且没有限制性，本发明的范围由所附权利要求而不是上述说明限定。
[0058]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

技术特征：

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,θ代替，给定若干个移动机器人的旋量坐标q1,q2,
…
,q
n
∈se(3),对于每一个t∈[0,1],相邻两个李代数元素和之间的李代数为：其中，经过n-1次递归，得出在每一个t∈[0,1]时刻bezier位姿轨迹曲线上的位姿点形成的正交矩阵5.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤三中bezier曲线的插值算法为：每一个t∈[0,1]时刻对应的正交矩阵通过对数映射得出位姿点的旋量坐标所有移动机器人位姿点形成一条c3连续bezier曲线。6.根据权利要求1所述的一种移动机器人位置和姿态同时规划方法，其特征在于，所述步骤四具体包括：通过轮式里程计实时获取移动机器人的航向角，将规划的笛卡尔空间速度映射到机器人工作空间作为控制输入，实现移动机器人位置和姿态同步变化。

技术总结

本发明属于机器人领域，具体涉及本发明公开了一种移动机器人位置和姿态同时规划方法，包括在路径规划模块中获取规划好的位姿点P

技术研发人员：

李齐 吕冰海 郑天江 邵兵兵 蒋德鑫

受保护的技术使用者：

浙江工业大学

技术研发日：

2022.07.21

技术公布日：

2022/10/11