CN115495882B - 一种非平坦地形下的机器人运动基元库构建方法及装置 - Google Patents

Abstract

本发明公开了一种非平坦地形下的机器人运动基元库构建方法及装置，涉及机器人路径规划技术领域。包括：获取机器人的参数；将参数输入到构建好的非平坦地形运动基元库构建模型；根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库。本发明提供一种应用于非平坦地形下的机器人运动基元库构建方法，本发明生成在机器人容许爬坡角度范围内不同坡度下满足其运动学约束的运动基元，从而构建适用于非平坦地形下路径规划的运动基元库。本发明计算过程简单，可适用于差速转向、阿克曼转向和全向转向驱动的地面移动机器人。

Description

一种非平坦地形下的机器人运动基元库构建方法及装置

技术领域

本发明涉及机器人路径规划技术领域，特别是指一种非平坦地形下的机器人运动基元库构建方法及装置。

背景技术

路径规划问题可以抽象为给定机器人的模型、环境地图模型、起始状态和目标状态，利用相关的路径规划算法搜索获得一条无碰撞最优路径，它是实现机器人自主导航的基本前提。运动基元是指根据机器人自身约束条件在有限时间内生成的轨迹。作为机器人运动学约束的抽象表征，在路径搜索过程中运动基元定义了搜索状态空间中节点之间的连接关系，实时、高效、合理的运动基元不仅能极大地提升路径规划的效率，更能保证所生成路径的可执行性。

常见的运动基元多为4连接、8连接或者16连接的离散栅格连接，这种连接关系仅考虑了节点的位置信息，因此，采用这种运动基元所规划生成的路径点航向不连续，导致在机器人进行路径跟踪控制时容易发生陷入局部极小或者路径跟踪困难等问题。通过考虑机器人的位置、航向、速度和轨迹曲率等运动约束，构建机器人的运动学模型，进而离线采样生成机器人可执行的运动基元是目前较为常用的方法。这种运动基元确定了机器人在路径搜索过程中节点与节点之间更为平滑、合理和可执行的连接关系，但其仅适用于二维平坦地形环境，无法用于解决高低起伏的非平坦地形下的路径规划问题。

申请号为201610348356.3的中国专利公开了一种基于动态运动基元学习模型的移动机器人路径规划方法，该方法采用手柄控制机器人运动，记录机器人的运动轨迹作为动态运动基元模型的样本，通过建立动态运动基元模型，利用轨迹样本进行训练获得动态运动基元模型参数。然而，这种方法所述的运动基元生成过程复杂耗时，且受人为操作水平影响较大。申请号为201710437505.8的中国专利公开了一种基于运动基元库的带电作业机械臂自主避障路径规划方法，该方法建立机械臂完整运动学模型，分类设计运动基元库，之后为各类库设计相应运动基元作为库成员，这种方法并不适用于移动机器人。申请号为202010136047.6的中国专利公开了一种类人驾驶的运动基元库的构建方法，该方法通过采集在有人驾驶情况下的智能车辆位置、航向和速度等数据，按照航向变化过零点作为数据分割点，将轨迹数据进行分割，从而获得不同属性的运动基元集，这种方法仅适用于二维平坦环境，而所生成的运动基元具有一定的局限性，无法包含所有可执行的运动基元。

发明内容

本发明针对如何生成在机器人容许爬坡角度范围内不同坡度下满足其运动学约束的运动基元，从而构建适用于非平坦地形下路径规划的运动基元库的问题，提出了本发明。

为解决上述技术问题，本发明提供如下技术方案：

一方面，本发明提供了一种非平坦地形下的机器人运动基元库构建方法，该方法由电子设备实现，该方法包括：

S1、获取机器人的参数。

S2、将参数输入到构建好的非平坦地形运动基元库构建模型。

S3、根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库。

可选地，S3中的根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库包括：

S31、根据机器人的参数，计算得到机器人的爬坡角度范围。

S32、获取平坦地形下运动基元的构建方法。

S33、根据爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库。

可选地，S31中的根据机器人的参数，计算得到机器人的爬坡角度范围包括：

S311、根据机器人的质量以及机器人轮子与地面之间的滚动阻力系数，计算机器人的最大驱动力，进而得到机器人的最大爬坡角度。

S312、根据机器人轮子与地面之间的滚动阻力系数，得到机器人的最小爬坡角度。

S313、根据最大爬坡角度以及最小爬坡角度，得到机器人的爬坡角度范围。

可选地，S32中的平坦地形下运动基元的构建方法包括：

S321、给定机器人的初始状态量以及终点状态量，以曲率能量函数作为优化目标，构建平坦地形下运动基元生成问题模型。

S322、根据直接配点法以及平坦地形下运动基元生成问题模型，得到直接配点法形式的运动基元生成问题模型。

S323、对直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元。

可选地，S323中对直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元包括：

采用数值求解工具库CasADi对直接配点法形式的运动基元生成问题模型进行求解，得到满足插值约束、配点约束以及不等式约束的弧长曲率函数，进而得到平坦地形下的运动基元。

可选地，S33中的根据爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库包括：

S331、根据爬坡角度范围以及机器人爬坡角度离散化分辨率，得到多个爬坡角度。

S332、根据多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元。

S333、根据多个爬坡角度下的非平坦地形运动基元，得到非平坦地形运动基元库。

可选地，S332中的根据多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元包括：

S3321、获取当终点状态量为以机器人初始位置为圆心、半径为r的圆周上的多个离散航向点时，平坦地形下的多个运动基元。

S3322、获取平坦地形下的多个运动基元中任一运动基元，分别在多个爬坡角度下对运动基元上的任一状态点进行矩阵变换，得到状态点在多个爬坡角度下对应的状态点。

S3323、根据状态点在多个爬坡角度下对应的状态点，得到多个爬坡角度下的非平坦地形运动基元。

可选地，S3322中的对运动基元上的任一状态点进行矩阵变换包括：

S33221、获取机器人坐标系以及预设参考坐标系；

S33222、根据机器人坐标系以及参考坐标系，得到坐标系变换矩阵；

S33223、根据坐标系变换矩阵，对运动基元上的任一状态点进行矩阵变换。

可选地，S1中的机器人包括差速转向机器人、阿克曼转向机器人以及全向转向驱动的机器人。

另一方面，本发明提供了一种非平坦地形下的机器人运动基元库构建装置，该装置应用于实现非平坦地形下的机器人运动基元库构建方法，该装置包括：

获取模块，用于获取机器人的参数。

输入模块，用于将参数输入到构建好的非平坦地形运动基元库构建模型。

输出模块，用于根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库。

可选地，输出模块，进一步用于：

S31、根据机器人的参数，计算得到机器人的爬坡角度范围。

S32、获取平坦地形下运动基元的构建方法。

S33、根据爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库。

可选地，输出模块，进一步用于：

S311、根据机器人的质量以及机器人轮子与地面之间的滚动阻力系数，计算机器人的最大驱动力，进而得到机器人的最大爬坡角度。

S312、根据机器人轮子与地面之间的滚动阻力系数，得到机器人的最小爬坡角度。

S313、根据最大爬坡角度以及最小爬坡角度，得到机器人的爬坡角度范围。

可选地，输出模块，进一步用于：

S321、给定机器人的初始状态量以及终点状态量，以曲率能量函数作为优化目标，构建平坦地形下运动基元生成问题模型。

S322、根据直接配点法以及平坦地形下运动基元生成问题模型，得到直接配点法形式的运动基元生成问题模型。

S323、对直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元。

可选地，输出模块，进一步用于：

采用数值求解工具库CasADi对直接配点法形式的运动基元生成问题模型进行求解，得到满足插值约束、配点约束以及不等式约束的弧长曲率函数，进而得到平坦地形下的运动基元。

可选地，输出模块，进一步用于：

S331、根据爬坡角度范围以及机器人爬坡角度离散化分辨率，得到多个爬坡角度。

S332、根据多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元。

S333、根据多个爬坡角度下的非平坦地形运动基元，得到非平坦地形运动基元库。

可选地，输出模块，进一步用于：

S3321、获取当终点状态量为以机器人初始位置为圆心、半径为r的圆周上的多个离散航向点时，平坦地形下的多个运动基元。

S3322、获取平坦地形下的多个运动基元中任一运动基元，分别在多个爬坡角度下对运动基元上的任一状态点进行矩阵变换，得到状态点在多个爬坡角度下对应的状态点。

S3323、根据状态点在多个爬坡角度下对应的状态点，得到多个爬坡角度下的非平坦地形运动基元。

可选地，输出模块，进一步用于：

S33221、获取机器人坐标系以及预设参考坐标系；

S33222、根据机器人坐标系以及参考坐标系，得到坐标系变换矩阵；

S33223、根据坐标系变换矩阵，对运动基元上的任一状态点进行矩阵变换。

可选地，机器人包括差速转向机器人、阿克曼转向机器人以及全向转向驱动的机器人。

一方面，提供了一种电子设备，所述电子设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述至少一条指令由所述处理器加载并执行以实现上述非平坦地形下的机器人运动基元库构建方法。

一方面，提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现上述非平坦地形下的机器人运动基元库构建方法。

本发明实施例提供的技术方案带来的有益效果至少包括：

上述方案中，提供一种应用于非平坦地形下的机器人运动基元库构建方法，该方法生成在机器人容许爬坡角度范围内不同坡度下满足其运动学约束的运动基元，从而构建适用于非平坦地形下路径规划的运动基元库。这种方法计算过程简单，可适用于差速转向、阿克曼转向和全向转向驱动的地面移动机器人。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例提供的非平坦地形下的机器人运动基元库构建方法流程示意图；

图2是本发明实施例提供的非平坦地形下机器人受力分析图；

图3是本发明实施例提供的机器人起始状态到终点状态之间的一条运动基元图；

图4是本发明实施例提供的参考坐标系与机器人坐标系之间的变换示意图；

图5是本发明实施例提供的一种非平坦地形下的机器人运动基元库构建方法计算流程图；

图6是本发明实施例提供的在坡度角为0°时生成的运动基元图；

图7是本发明实施例提供的在非平坦地形不同坡度下生成的机器人运动基元库图；

图8是本发明实施例提供的非平坦地形下的机器人运动基元库构建装置框图；

图9是本发明实施例提供的一种电子设备的结构示意图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

如图1所示，本发明实施例提供了一种非平坦地形下的机器人运动基元库构建方法，该方法可以由电子设备实现。如图1所示的非平坦地形下的机器人运动基元库构建方法流程图，该方法的处理流程可以包括如下的步骤：

S1、获取机器人的参数。

可选地，S1中的机器人可以包括差速转向机器人、阿克曼转向机器人以及全向转向驱动的机器人等。

S2、将参数输入到构建好的非平坦地形运动基元库构建模型。

S3、根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库。

可选地，S3中的根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库包括：

S31、根据机器人的参数，计算得到机器人的爬坡角度范围。

可选地，S31中的根据机器人的参数，计算得到机器人的爬坡角度范围包括：

S311、根据机器人的质量以及机器人轮子与地面之间的滚动阻力系数，计算机器人的最大驱动力，进而得到机器人的最大爬坡角度。

一种可行的实施方式中，假定机器人在整个运动周期保持恒定速度v，其在非平坦地形上主要受重力G和摩擦力F_f作用，如图2所示。从图2中的机器人受力分析可知，机器人的驱动力F可以表示为下式(1)：

F＝mg(μcosφ+sinφ) (1)

式中，m为机器人的质量，μ为轮子与地面之间的滚动阻力系数，g为重力加速度。

进一步地，在机器人爬坡运动过程中，由于受输出驱动力限制而导致其无法越过大坡度地形。假定机器人的最大驱动力为F_max，根据公式(1)可求解机器人能越过的最大倾斜角为下式(2)：

式中，F_max＝P_max/v，P_max是机器人驱动电机的最大输出功率。

S312、根据机器人轮子与地面之间的滚动阻力系数，得到机器人的最小爬坡角度。

一种可行的实施方式中，在机器人下坡运动过程中存在一个临界状态，即由重力引起的驱动力和摩擦力平衡，这时地形的坡度可表示为下式(3)：

φ_min＝-tan^-1(μ) (3)

通过以上分析可知，当地形坡度φ＜φ_min时，机器人将自主开始下坡运动而不需要额外的驱动力，这种不受控的情况是极其危险的。

S313、根据最大爬坡角度以及最小爬坡角度，得到机器人的爬坡角度范围。

一种可行的实施方式中，综合分析机器人在非平坦地形下上、下坡时的受力情况，可得其爬坡角度范围为φ＝[φ_minφ_max]。

S32、获取平坦地形下运动基元的构建方法。

可选地，S32中的平坦地形下运动基元的构建方法包括：

S321、给定机器人的初始状态量以及终点状态量，以曲率能量函数作为优化目标，构建平坦地形下运动基元生成问题模型。

一种可行的实施方式中，在平坦地形条件下，这时地形坡度为0°，机器人在z轴方向无位移，即z＝0。对于非完整性约束的机器人，在Frenet坐标系中将横向运动和纵向运动解耦进行分别描述，则机器人的纵向运动描述为一维的非线性方程，如下式(4)：

s＝s(t) (4)

式中，s为机器人纵向运动路径长度，即运动曲线弧长，t为机器人运动时间。以v表示机器人的纵向运动速度，则弧长与纵向速度的关系如下式(5)：

ds＝vdt (5)

因此，以弧长s作为变量表示的机器人状态χ(s)＝(x(s),y(s),z(s),θ(s))在笛卡尔坐标系中的姿态表示为下式(6)～(9)：

z(s)＝0 (8)

式中，κ(s)是弧长曲率函数。

从公式(6)～(9)可知，当弧长曲率函数κ(s)确定后，可获得该弧长上机器人的姿态信息，如下式(10)：

以三阶多项式螺旋曲线表示的弧长曲率函数κ(s)为下式(11)：

κ(s)＝a₀+a₁s+a₂s²+a₃s³ (11)

公式(11)可同时满足机器人边界约束中的位置、航向和曲率条件，同时保持着较小的参数空间维度，在自由度和对运动的表达性方面满足要求。

进一步地，在地形坡度为0°时，给定机器人起始状态量χ₀＝(x₀,y₀,z₀,θ₀)和终点状态量χ_g＝(x_g,y_g,z_g,θ_g)，以曲率能量函数作为优化目标，运动基元生成问题表示为下式(12)：

式中，‖κ(s)‖是路径曲率的标量，s_f为机器人到达终点状态量χ_g的弧长，κ_max为机器人的最大转弯曲率。

通过最小化目标函数J，在满足起始状态量和终点状态量的边界约束的条件下，求解满足曲率约束的运动基元。上述优化目标包含终点位置约束，而位置表达式是Fresnel积分，没有解析表达式，采用下述复化Simpson公式近似该积分，关于x坐标的积分形式如下式(13)：

式中，a、b分别为积分的下边界与上边界，N为积分到终点位置的采样份数。

同理可求出y(s)。

S322、根据直接配点法以及平坦地形下运动基元生成问题模型，得到直接配点法形式的运动基元生成问题模型。

一种可行的实施方式中，直接配点法是轨迹生成问题中常用的数值方法，通过将连续曲线离散为有限序列，把问题(12)转化为非线性规划问题。首先对运动基元离散化，将状态量表示为一系列关于离散弧长的值，即配置点，如下式(14)：

进一步地，在获取配置点后，为得到连续的轨迹，对曲线采用Hermite-Simpson配点法进行插值，设τ＝s-s_k，h_k≡s_k+1-s_k，则在Frenet坐标系中弧长的状态量χ(s)可以表示为下式(15)：

将问题(12)表示为直接配点法的形式，如下式(16)：

式中，第一个等式约束是插值约束，第二个约束为配点约束，不等式约束为整个路径中的曲率约束。

S323、对直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元。

可选地，S323中对直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元包括：

采用数值求解工具库CasADi对直接配点法形式的运动基元生成问题模型进行求解，得到满足插值约束、配点约束以及不等式约束的弧长曲率函数，进而得到平坦地形下的运动基元。

一种可行的实施方式中，建立完整的直接配点法形式的问题模型后，采用数值求解工具库CasADi，迭代求解出满足约束的曲率κ(s)的表达式，从而获得机器人起始状态到终点状态之间的一条运动基元，如图3所示，图中箭头表示机器人状态点的航向变化。

S33、根据爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库。

可选地，S33中的根据爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库包括：

S331、根据爬坡角度范围以及机器人爬坡角度离散化分辨率，得到多个爬坡角度。

一种可行的实施方式中，在非平坦地形下，机器人的爬坡角度范围为φ＝[φ_minφ_max]，为了生成不同爬坡度下的运动基元，对机器人的爬坡角度φ进行离散化，坡度角分辨率为slop_res。

S332、根据多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元。

可选地，S332中的根据多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元包括：

S3321、获取当终点状态量为以机器人初始位置为圆心、半径为r的圆周上的多个离散航向点时，平坦地形下的多个运动基元。

一种可行的实施方式中，假定机器人的航向角θ取值范围为[0 2π]，航向角分辨率为heading_res。

考虑机器人在水平面上，即坡度角φ＝0。给定机器人的初始状态为χ₀＝(0,0,0,0)，而终点状态χ_i＝(x_i,y_i,z_i,θ_i)分别选择以机器人初始位置为圆心、半径为r的圆周上的离散航向点，其坐标表示为下式(17)：

式中，i＝0,1,…,2π/heading_res。

因此，给定机器人的初始状态χ₀＝(0,0,0,0)及不同的终点状态χ_i＝(x_i,y_i,z_i,θ_i)，可以按照公式(10)和(16)生成机器人在坡度角φ＝0时的所有运动基元。

S3322、获取平坦地形下的多个运动基元中任一运动基元，分别在多个爬坡角度下对运动基元上的任一状态点进行矩阵变换，得到状态点在多个爬坡角度下对应的状态点。

可选地，S3322中的对运动基元上的任一状态点进行矩阵变换包括：

S33221、获取机器人坐标系以及预设参考坐标系；

S33222、根据机器人坐标系以及参考坐标系，得到坐标系变换矩阵；

S33223、根据坐标系变换矩阵，对运动基元上的任一状态点进行矩阵变换。

一种可行的实施方式中，利用机器人在坡度角φ＝0时生成的运动基元，变换生成在不同坡度角下的运动基元。假设参考坐标系为x_gy_gz_g，它与坡度角φ＝0时的机器人坐标系x₀y₀z₀重合，而坡度角为φ_j时的机器人坐标系x_jy_jz_j可看作是绕着参考坐标系x_gy_gz_g的x_g轴旋转了角度φ_j，如图4所示。因此，机器人坐标系x_jy_jz_j和参考坐标系x_gy_gz_g之间的变换矩阵为下式(18)：

因此，对于在坡度角φ＝0时生成的运动基元上任一状态点χ_m＝(x_m,y_m,z_m,θ_m)，可通过公式(18)的变换矩阵求解在坡度角φ＝φ_j时对应的状态点χ_n＝(x_n,y_n,z_n,θ_n)，具体为下式(19)：

因此，根据公式(19)可以将坡度角φ＝0时生成的运动基元上的状态点转换为坡度角φ＝φ_j时对应的状态点，即生成坡度角φ＝φ_j时的运动基元。

S3323、根据状态点在多个爬坡角度下对应的状态点，得到多个爬坡角度下的非平坦地形运动基元。

S333、根据多个爬坡角度下的非平坦地形运动基元，得到非平坦地形运动基元库。

一种可行的实施方式中，根据机器人的爬坡角度范围为φ＝[φ_minφ_max]和坡度角分辨率slop_res，对于离散化后的每一个机器人坡度角，按照公式(19)生成相应坡度下的运动基元，这些不同坡度下的运动基元构成了机器人在非平坦地形下的运动基元库。

本发明所提出的一种非平坦地形下的机器人运动基元库构建方法的计算流程如图5所示，以差速转向驱动机器人为例说明具体实施过程。在平坦地形下，即坡度角φ＝0时，以(0,0,0,0)作为差速转向机器人的初始状态，而终点状态选择以机器人初始位置为圆心、半径为2m的圆周上的离散点，航向角分辨率为π/8，所生成的运动基元如图6所示。

假定差速转向机器人的爬坡度为[-π/6,π/6]，坡度角分辨率为π/18，则根据机器人在坡度角φ＝0时的运动基元可以转换生成在不同坡度下的运动基元，主要包括坡度角φ分别在-π/6、-π/9、-π/18、0、π/18、π/9和π/6等7个坡度下的运动基元。这些不同坡度下的运动基元构成了差速转向驱动机器人的运动基元库，如图7所示。

本发明实施例中，提供一种应用于非平坦地形下的机器人运动基元库构建方法，该方法生成在机器人容许爬坡角度范围内不同坡度下满足其运动学约束的运动基元，从而构建适用于非平坦地形下路径规划的运动基元库。这种方法计算过程简单，可适用于差速转向、阿克曼转向和全向转向驱动的地面移动机器人。

如图8所示，本发明实施例提供了一种非平坦地形下的机器人运动基元库构建装置800，该装置800应用于实现非平坦地形下的机器人运动基元库构建方法，该装置800包括：

获取模块810，用于获取机器人的参数。

输入模块820，用于将参数输入到构建好的非平坦地形运动基元库构建模型。

输出模块830，用于根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库。

可选地，输出模块830，进一步用于：

S31、根据机器人的参数，计算得到机器人的爬坡角度范围。

S32、获取平坦地形下运动基元的构建方法。

S33、根据爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库。

可选地，输出模块830，进一步用于：

S311、根据机器人的质量以及机器人轮子与地面之间的滚动阻力系数，计算机器人的最大驱动力，进而得到机器人的最大爬坡角度。

S312、根据机器人轮子与地面之间的滚动阻力系数，得到机器人的最小爬坡角度。

S313、根据最大爬坡角度以及最小爬坡角度，得到机器人的爬坡角度范围。

可选地，输出模块830，进一步用于：

S321、给定机器人的初始状态量以及终点状态量，以曲率能量函数作为优化目标，构建平坦地形下运动基元生成问题模型。

S322、根据直接配点法以及平坦地形下运动基元生成问题模型，得到直接配点法形式的运动基元生成问题模型。

S323、对直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元。

可选地，输出模块830，进一步用于：

采用数值求解工具库CasADi对直接配点法形式的运动基元生成问题模型进行求解，得到满足插值约束、配点约束以及不等式约束的弧长曲率函数，进而得到平坦地形下的运动基元。

可选地，输出模块830，进一步用于：

S331、根据爬坡角度范围以及机器人爬坡角度离散化分辨率，得到多个爬坡角度。

S332、根据多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元。

S333、根据多个爬坡角度下的非平坦地形运动基元，得到非平坦地形运动基元库。

可选地，输出模块830，进一步用于：

S3321、获取当终点状态量为以机器人初始位置为圆心、半径为r的圆周上的多个离散航向点时，平坦地形下的多个运动基元。

S3322、获取平坦地形下的多个运动基元中任一运动基元，分别在多个爬坡角度下对运动基元上的任一状态点进行矩阵变换，得到状态点在多个爬坡角度下对应的状态点。

S3323、根据状态点在多个爬坡角度下对应的状态点，得到多个爬坡角度下的非平坦地形运动基元。

可选地，输出模块830，进一步用于：

S33221、获取机器人坐标系以及预设参考坐标系；

S33222、根据机器人坐标系以及参考坐标系，得到坐标系变换矩阵；

S33223、根据坐标系变换矩阵，对运动基元上的任一状态点进行矩阵变换。

可选地，机器人包括差速转向机器人、阿克曼转向机器人以及全向转向驱动的机器人。

本发明实施例中，提供一种应用于非平坦地形下的机器人运动基元库构建方法，该方法生成在机器人容许爬坡角度范围内不同坡度下满足其运动学约束的运动基元，从而构建适用于非平坦地形下路径规划的运动基元库。这种方法计算过程简单，可适用于差速转向、阿克曼转向和全向转向驱动的地面移动机器人。

图9是本发明实施例提供的一种电子设备900的结构示意图，该电子设备900可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(centralprocessing units，CPU)901和一个或一个以上的存储器902，其中，存储器902中存储有至少一条指令，至少一条指令由处理器901加载并执行以实现下述非平坦地形下的机器人运动基元库构建方法：

S1、获取机器人的参数。

S2、将参数输入到构建好的非平坦地形运动基元库构建模型。

S3、根据参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库。

在示例性实施例中，还提供了一种计算机可读存储介质，例如包括指令的存储器，上述指令可由终端中的处理器执行以完成上述非平坦地形下的机器人运动基元库构建法。例如，计算机可读存储介质可以是ROM、随机存取存储器(RAM)、CD-ROM、磁带、软盘和光数据存储设备等。

本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种非平坦地形下的机器人运动基元库构建方法，其特征在于，所述方法包括：

S1、获取机器人的参数；

S2、将所述参数输入到构建好的非平坦地形运动基元库构建模型；

S3、根据所述参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库；

所述S3中的根据所述参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库包括：

S31、根据所述机器人的参数，计算得到机器人的爬坡角度范围；

S32、获取平坦地形下运动基元的构建方法；

S33、根据所述爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库；

所述S31中的根据所述机器人的参数，计算得到机器人的爬坡角度范围包括：

S311、根据机器人的质量以及机器人轮子与地面之间的滚动阻力系数，计算机器人的最大驱动力，进而得到机器人的最大爬坡角度；

S312、根据机器人轮子与地面之间的滚动阻力系数，得到机器人的最小爬坡角度；

S313、根据所述最大爬坡角度以及最小爬坡角度，得到机器人的爬坡角度范围；

所述S32中的平坦地形下运动基元的构建方法包括：

S321、给定机器人的初始状态量以及终点状态量，以曲率能量函数作为优化目标，构建平坦地形下运动基元生成问题模型；

S322、根据直接配点法以及所述平坦地形下运动基元生成问题模型，得到直接配点法形式的运动基元生成问题模型；

S323、对所述直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元；

所述S33中的根据所述爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库包括：

S331、根据所述爬坡角度范围以及机器人爬坡角度离散化分辨率，得到多个爬坡角度；

S332、根据所述多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元；

S333、根据所述多个爬坡角度下的非平坦地形运动基元，得到非平坦地形运动基元库；

所述S332中的根据所述多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元包括：

S3321、获取当终点状态量为以机器人初始位置为圆心、半径为r的圆周上的多个离散航向点时，平坦地形下的多个运动基元；

S3322、获取所述平坦地形下的多个运动基元中任一运动基元，分别在多个爬坡角度下对所述运动基元上的任一状态点进行矩阵变换，得到所述状态点在多个爬坡角度下对应的状态点；

S3323、根据所述状态点在多个爬坡角度下对应的状态点，得到多个爬坡角度下的非平坦地形运动基元。

2.根据权利要求1所述的方法，其特征在于，所述S323中对所述直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元包括：

采用数值求解工具库CasADi对所述直接配点法形式的运动基元生成问题模型进行求解，得到满足插值约束、配点约束以及不等式约束的弧长曲率函数，进而得到平坦地形下的运动基元。

3.根据权利要求1所述的方法，其特征在于，所述S3322中的对所述运动基元上的任一状态点进行矩阵变换包括：

S33221、获取机器人坐标系以及预设参考坐标系；

S33222、根据所述机器人坐标系以及参考坐标系，得到坐标系变换矩阵；

S33223、根据所述坐标系变换矩阵，对所述运动基元上的任一状态点进行矩阵变换。

4.根据权利要求1所述的方法，其特征在于，所述S1中的机器人包括差速转向机器人、阿克曼转向机器人以及全向转向驱动的机器人。

5.一种非平坦地形下的机器人运动基元库构建装置，其特征在于，所述装置包括：

获取模块，用于获取机器人的参数；

输入模块，用于将所述参数输入到构建好的非平坦地形运动基元库构建模型；

输出模块，用于根据所述参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库；

所述根据所述参数以及非平坦地形运动基元库构建模型，得到非平坦地形下的机器人运动基元库包括：

S31、根据所述机器人的参数，计算得到机器人的爬坡角度范围；

S32、获取平坦地形下运动基元的构建方法；

S33、根据所述爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库；

所述S31中的根据所述机器人的参数，计算得到机器人的爬坡角度范围包括：

S311、根据机器人的质量以及机器人轮子与地面之间的滚动阻力系数，计算机器人的最大驱动力，进而得到机器人的最大爬坡角度；

S312、根据机器人轮子与地面之间的滚动阻力系数，得到机器人的最小爬坡角度；

S313、根据所述最大爬坡角度以及最小爬坡角度，得到机器人的爬坡角度范围；

所述S32中的平坦地形下运动基元的构建方法包括：

S321、给定机器人的初始状态量以及终点状态量，以曲率能量函数作为优化目标，构建平坦地形下运动基元生成问题模型；

S322、根据直接配点法以及所述平坦地形下运动基元生成问题模型，得到直接配点法形式的运动基元生成问题模型；

S323、对所述直接配点法形式的运动基元生成问题模型进行求解，得到平坦地形下的运动基元；

所述S33中的根据所述爬坡角度范围以及平坦地形下运动基元的构建方法，构建多个爬坡角度下的非平坦地形运动基元，进而得到非平坦地形下的机器人运动基元库包括：

S331、根据所述爬坡角度范围以及机器人爬坡角度离散化分辨率，得到多个爬坡角度；

S332、根据所述多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元；

S333、根据所述多个爬坡角度下的非平坦地形运动基元，得到非平坦地形运动基元库；

所述S332中的根据所述多个爬坡角度以及平坦地形下运动基元的构建方法，得到多个爬坡角度下的非平坦地形运动基元包括：

S3321、获取当终点状态量为以机器人初始位置为圆心、半径为r的圆周上的多个离散航向点时，平坦地形下的多个运动基元；

S3322、获取所述平坦地形下的多个运动基元中任一运动基元，分别在多个爬坡角度下对所述运动基元上的任一状态点进行矩阵变换，得到所述状态点在多个爬坡角度下对应的状态点；

S3323、根据所述状态点在多个爬坡角度下对应的状态点，得到多个爬坡角度下的非平坦地形运动基元。