CN111290406B - 一种路径规划的方法、机器人及存储介质 - Google Patents

Abstract

本发明实施例涉及轨迹规划技术领域，公开了一种路径规划的方法、机器人及存储介质。本发明中路径规划的方法包括：根据机器人在指定方向的当前状态、指定方向的第一目标状态以及指定方向的第一控制参数，确定机器人的最优运动时长，最优运动时长是指机器人从当前状态变更为第一目标状态耗能最优情况下花费的运动时长；根据指定方向的当前状态、第一目标状态以及最优运动时长，确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹，指定方向的当前状态包括：机器人在指定方向的当前位姿、当前速度以及当前加速度，第一控制参数为加加速度。采用本实施方式，使得规划的运动轨迹更加合理，降低机器人根据运动轨迹移动所消耗的能量。

Description

一种路径规划的方法、机器人及存储介质

技术领域

本发明实施例涉及轨迹规划技术领域，特别涉及一种路径规划的方法、机器人及存储介质。

背景技术

随着计算机技术、传感器技术及人工智能的迅速发展，机器人自主导航技术也取得了很大进展，其中，机器人局部路径规划算法在机器人导航系统中占据重要地位。

发明人发现相关技术中至少存在如下问题：目前机器人的路径规划不合理，导致机器人功耗高的问题。

发明内容

本发明实施方式的目的在于提供一种路径规划的方法、机器人及存储介质，使得规划的运动轨迹更加合理，降低机器人根据运动轨迹移动所消耗的能量。

为解决上述技术问题，本发明的实施方式提供了一种路径规划的方法，包括：根据机器人在指定方向的当前状态、指定方向的第一目标状态以及指定方向的第一控制参数，确定机器人的最优运动时长，其中，最优运动时长是指机器人从当前状态变更为第一目标状态耗能最优情况下花费的运动时长；根据指定方向的当前状态、第一目标状态以及最优运动时长，确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹；其中，指定方向的当前状态包括：机器人在指定方向的当前位姿以及当前速度，第一目标状态包括：机器人在指定方向的目标位姿以及目标速度，第一控制参数为加速度；或者，指定方向的当前状态包括：机器人在指定方向的当前位姿、当前速度以及当前加速度，第一目标状态包括：机器人在指定方向的目标位姿、目标速度及目标加速度，第一控制参数为加加速度。

本发明的实施方式还提供了一种机器人，包括：至少一个处理器，以及，与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行上述的路径规划的方法。

本发明的实施方式还提供了一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时实现上述的路径规划的方法。

本发明实施方式相对于现有技术而言，通过机器人在指定方向的当前状态、第一目标状态以及指定方向的控制参数，确定该机器人的最优运动时长，由于该最优运动时长不是人为预先设定，而是在机器人耗能最优情况下确定，使得基于该最优运动时长、当前状态以及目标状态规划出的运动轨迹耗能最小，避免了因固定运动时长过大而导致机器人出现绕路、绕圈等问题，也避免因固定的运动时间过短，导致机器人需要在行进过程中提高速度，导致消耗的能量过多的问题；同时，基于当前状态、第一目标状态确定的最优运动时长，保证了机器人可以变更为设定的第一目标状态，以满足不同场景的需求；此外，机器人所处的当前状态可以包括当前的加速度、当前位姿或者包括当前加速度、当前位姿和当前速度，同理，第一目标状态可以包括目标加速度、目标位姿或者可以包括：目标加速度、目标位姿和目标速度，丰富了机器人所处状态的数据，进一步提高对不同场景需求的适用。

另外，指定方向的数量为多个，多个指定方向均与机器人底盘所在平面平行。指定方向包括多个，使得可以确定出不同指定方向上的运动轨迹，提高了机器人路径规划的灵活性。

另外，指定方向的数量为两个，其中，两个指定方向相互垂直。相互垂直的两个指定方向可以是机器人运动方向上分解出的两个方向，确定出每个指定方向的运动轨迹，便于后续规划机器人在运动方向上的运动轨迹。

另外，根据机器人在指定方向的当前状态、指定方向的第一目标状态以及指定方向的第一控制参数，确定机器人的最优运动时长，包括：将指定方向的当前状态、第一目标状态作为最优控制模型的系统状态量，将第一控制参数作为预设的最优控制模型的系统控制量，按照最优控制模型构建基于第一控制参数的代价函数；将代价函数最小情况下，代价函数中的时间值作为最优运动时长。将第一控制参数作为控制量，将当前状态和目标状态作为系统状态量，根据最优控制模型构建基于第一控制参数的代价函数，从而可以通过第一控制参数控制系统状态量，使得在代价函数最小时，可以确保机器人变更至第一目标状态。

另外，指定方向的当前状态包括：机器人在指定方向的当前位姿、当前速度以及当前加速度，第一目标状态包括：机器人在指定方向的目标位姿、目标速度及目标加速度，第一控制参数为加加速度；按照最优控制模型构建基于第一控制参数的代价函数，包括：对所述最优控制模型进行变换，得到所述代价函数；

最优模型f_s表示为：

其中，s包括机器人在指定方向的当前状态和第一目标状态，s表示系统状态量，u用于表示系统控制量，j表示第一控制参数，v表示机器人的速度，a表示机器人的加速度；最优控制模型的哈密尔顿函数表示为：

H表示哈密尔顿函数，λ为未知系数，T表示最优运动时长；代价函数J表示为：

其中，

β和γ为代价函数中的系数。

另外，根据指定方向的当前状态、第一目标状态以及最优运动时长，确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹，包括：根据指定方向的当前状态、第一目标状态构建包含未知系数的运动方程；根据最优运动时长，确定运动方程中的未知系数，得到指定方向的运动方程；根据预设的采样点数，对最优运动时长采样，得到多个采样点；根据多个采样点和确定的指定方向的运动方程，得到指定方向的运动轨迹。通过指定方向的当前状态、第一目标状态可以构建包含未知系统的运动方程，通过确定的最优运动时长，确定未知系数，使得可以得到准确的指定方向的运动方程，根据该指定方向的运动方程以及最优运动时长，可以准确确定出指定方向的运动轨迹。

另外，指定方向的当前状态包括：机器人在指定方向的当前位姿、当前速度以及当前加速度，指定方向的运动方程表示为：f_y(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵；

a₀～a₅是该5次多项式中的系数，t表示运动时长，其中，a₀＝y₀，

a₃＝10(y_T-y₀)T^-3-(6y_0d+4y_Td)T^-2+(0.5*y_Tdd-1.5y_0dd)T^-1，

a₄＝15(y₀-y_T)T^-4-(8y_0d+7y_Td)T^-3+(1.5*y_0dd-y_Tdd)T^-2，

a₅＝6(y_T-y₀)T^-5-(3y_0d+3y_Td)T^-4+(0.5*y_Tdd-0.5y_0dd)T^-3，

y₀表示机器人当前的位置、y_0d表示机器人当前的速度，y_0dd表示机器人当前的加速度，y_T表示机器人的目标位置、y_Td表示机器人的目标速度，y_Tdd表示目标的加速度。

另外，在确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹之后，该方法还包括：根据机器人从指定方向的当前状态变更为第一目标状态的运动轨迹，确定机器人运动至指定位置的路径，指定位置是指根据每个第一目标状态构建的二维坐标位置。在确定了指定方向的运动轨迹后，还可以确定达到指定位置的路径，使得规划处的路径更加完善。

另外，根据机器人从指定方向的当前状态变更为第一目标状态的运动轨迹，确定机器人运动至指定位置的路径，包括：获取机器人从每个指定方向的当前状态变更为第一目标状态的运动轨迹；拟合每个指定方向的运动轨迹，确定机器人运动至指定位置的路径。

另外，在确定机器人运动至指定位置的路径之后，方法还包括：存储指定位置的路径；从存储的不同指定位置的路径中，选取轨迹评分最高的路径作为机器人的最优运动路径。存储指定位置的路径，从不同指定位置的路径中选取最优运动路径，使得可以较为准确地的确定出最优运动路径。

另外，根据预设的采样点数，对最优运动时长采样，得到多个采样点，包括：将最优运动时长与预设的采样点数之间的比值作为采样的时间步进值；按照时间步进值对最优时长进行采样，得到多个采样点。通过平均该最优运动时长的方式获得采样的时间步长，使得可以获得较为规则的运动轨迹。

附图说明

一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1是根据本发明第一实施例提供的一种现有方式规划的路径效果图；

图2是根据本发明第一实施例提供的一种路径规划的方法的具体流程图；

图3是根据本发明第一实施例中两个指定方向的一种示意图；

图4是根据本发明第二实施例中提供一种路径规划的方法的具体流程图；

图5是根据本发明第三实施例中一种机器人的具体结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请所要求保护的技术方案。

发明人发现目前针对机器人的自主导航技术中包括全局规划路径规划和局部路径规划，局部路径规划是对分段后的全局路径的坐标进行局部重新规划。目前的局部路径规划方法中，通常基于目标位置和固定运动时长进行路径规划，或者基于设定的仿真目标位置、速度和固定运动时长进行路径规划，但是，这种固定运动时长路径规划的方法，往往会出现运动时长过大导致规划的路径有绕圈和绕路等问题，例如，如图1所示，从A点到B点，仿真时间为4秒，由于设定了目标速度Vt，而A点到B点的路径较短，为了规划出路径，则会出现如图1所示的路径，可见这样的路径是不合理的，若机器人按照该路径运动，会导致不必要的能量消耗。

本发明的第一实施方式涉及一种路径规划的方法。该方法应用于各种类型的机器人，如：轮式移动机器人(即无人驾驶汽车)、客户类机器人、教学机器人等；该路径规划的方法的具体流程如图2所示。

步骤101：根据机器人在指定方向的当前状态、指定方向的第一目标状态以及指定方向的第一控制参数，确定机器人的最优运动时长；最优运动时长是指机器人从当前状态变更为第一目标状态耗能最优情况下花费的运动时长。

具体的说，指定方向的当前状态包括：机器人在指定方向的当前位姿以及当前速度，第一目标状态包括：机器人在指定方向的目标位姿以及目标速度，第一控制参数为加速度；或者，指定方向的当前状态包括：机器人在指定方向的当前位姿、当前速度以及当前加速度，第一目标状态包括：机器人在指定方向的目标位姿、目标速度及目标加速度，第一控制参数为加加速度。

在一个例子中，指定方向的数量为多个，多个指定方向均为与机器人底盘所在平面平行。指定方向的数量可以根据实际需要设定，指定方向也可以根据适用场景的需要进行设定，例如，指定方向的数量可以为两个，且两个指定方向相互垂直，如图3所示的X方向和Y方向；机器人在X方向的当前状态可以表示为：当前位姿x₀、当前速度v_0d和当前加速度v_0dd；第一控制参数表示为：加加速度j₀。

在一个例子中，针对每个指定方向进行如下处理：将指定方向的当前状态、第一目标状态作为预设的最优控制模型的系统状态量，将第一控制参数作为最优控制模型的系统控制量，按照最优控制模型构建基于第一控制参数的代价函数；将代价函数最小情况下，代价函数中的时间值作为最优运动时长。

具体的说，本示例中以三自由度的机器人为例进行说明，三自由度的机器人是指沿如图3所示的X方向运动，沿Y方向运动以及原地旋转运动，其中，图3中的R1为机器人底盘。获取机器人在指定方向的当前状态、指定方向的第一目标状态以及指定方向的第一控制参数，其中，本示例中，机器人的状态包括指定方向的位姿、速度和加速度，第一控制参数为加加速度；本示例中包括两个指定方向，分别为如图3所示的X方向和Y方向，那么X方向的当前状态包括：在X方向的当前位姿、X方向的当前速度、X方向的当前加速度；Y方向的当前状态包括：Y方向的当前位姿、Y方向的当前速度以及Y方向的当前加加速度。同理，每个指定方向有各自对应的第一目标状态和第一控制参数。

下面以任一指定方向为例，介绍确定该指定方向的最优运动时长的过程。

本示例中，可以根据彭特里亚金(Pontryain)的极小值原理，设最优控制模型的系统的协态为λ＝(λ1,λ2,λ3)，定义哈密尔顿函数(Hamiltonian function)，通过定义系统控制量和系统状态量，得到最优控制模型；其中，j代表加加速度，s(p,v,a)代表机器人在指定方向的所处状态(包括当前状态和第一目标状态)，作为系统状态量，即指定方向的位姿、速度和加速度，u为系统的控制量，即j＝u；最优系统模型可以如公式(1)所示，则该最优系统模型的哈密尔顿函数可以如公式(2)所示：

其中，f_s表示最优控制模型。

其中，λ为未知系数，T表示为最优运动时长。

下面介绍计算未知系数的过程：

根据极小值原理，λ为当s与u取得最优值时，函数H对s求偏导时关于λ的微分方程的解，即：

求解该微分方程，为方便后续运算，得到如公式(4)所示的微分方程的一组解：

由于最优运动时长为机器人从指定方向的当前状态变更为第一目标状态耗能最优情况下的运动时长，则可以通过最小化j实现，故将λ带入H函数中，并对j(即u)求偏导，可得公式(5)：

另该偏导为0，即可接的最优的控制量u*(或j*)：

由于系统状态量s(p,v,a)为控制量u＝j的不同阶的积分，且需要满足s*(0)＝s(0)＝(y₀,y_0d,y_0dd)，故最优的系统状态量可通过最优系统控制量积分得到，如公式(7)所示：

其中，y₀,y_0d,y_0dd分别为指定方向的当前位姿、当前速度以及当前加速度。

带入s*(T)＝s(T)，整理可得：

其中，y_T,y_Td,y_Tdd分别表示为第一目标状态中的位姿、速度以及加速度。

得到

β和γ，表示如公式(9)所示：

将

β和γ带回可求得u*(t),s*(t)和λ(t)。

由于j(t)即为u*(t)，故可构建关于加加速度的代价函数，该代价函数如公式(10)所示：

由于J的表达式较为复杂，为求使得代价函数J最小时的T，可采用基于优化的方式，例如莱文贝格(Levenberg-Marquardt，简称“LM”)算法进行求解；具体的求解过程为数学方式，此处将不再赘述。求解得到的T为最优运动时长。

可以理解的是，其他指定方向的运动轨迹的确定方式与上述过程类似，此处将不再赘述。

步骤102：根据指定方向的当前状态、第一目标状态以及最优运动时长，确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹。

在一个例子中，根据指定方向的当前状态、第一目标状态构建包含未知系数的运动方程；根据最优运动时长，确定运动方程中的未知系数，得到指定方向的运动方程；根据预设的采样点数，离散最优运动时长，得到多个采样点；根据多个采样点和确定的指定方向的运动方程，得到指定方向的运动轨迹。

具体的说，本示例中还是以步骤101中的指定方向为例，介绍确定运动轨迹的过程。

机器人在Y方向的当前状态包括：当前的位置y₀、当前的速度y_0d、当前的加速度y_0dd；机器人在纵轴方向的目标状态包括：目标位置y_T、目标速度y_Td、目标的加速度y_Tdd。根据机器人的状态中包含的参数的种类数目，构建以时间为自变量的运动方程，例如，本示例中机器人的状态中包含3个参数，那么可以构建以时间为自变量的5次多项式；如公式(11)所示：

f_y(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ 公式(11)；

其中，a₀～a₅是该5次多项式中的系数。

下面根据该纵轴方向的当前状态、纵轴方向的目标状态以及在步骤101中确定最优运动时长T，求解a₀～a₅。

对公式(11)求一阶导数以及二阶导数，得到t关于速度与加速度的方程，如公式(12)和公式(13)所示：

f'_y(t)＝a₁+2a₂t+3a₃t²+4a₄t³+5a₅t⁴ 公式(12)；

f”_y(t)＝2a₂+6a₃t+12a₄t²+10a₅t³ 公式(13)；

当t＝0时，即机器人处于该纵轴方向的当前状态，将当前状态带入公式(11)～公式(13)中，可得：

a₀＝y₀ 公式(14)；

当t＝T时，即机器人在T时刻的状态，带入公式(11)～公式(13)；

y_T＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ 公式(17)；

y_Td＝a₁+2a₂T+3a₃T²+4a₄T³+5a₅T⁴ 公式(18)；

y_Tdd＝2a₂+6a₃T+12a₄T²+20a₅T³ 公式(19)；

带入a₀、a₁以及a₂，可解得a₃、a₄和a₅，a₃～a₅的表达式如下：

a₃＝10(y_T-y₀)T^-3-(6y_0d+4y_Td)T^-2+(0.5*y_Tdd-1.5y_0dd)T^-1 公式(20)；

a₄＝15(y₀-y_T)T^-4-(8y_0d+7y_Td)T^-3+(1.5*y_0dd-y_Tdd)T^-2 公式(21)；

a₅＝6(y_T-y₀)T^-5-(3y_0d+3y_Td)T^-4+(0.5*y_Tdd-0.5y_0dd)T^-3 公式(22)；

此时，解得公式(11)中的5个系数a₀～a₅。

在确定了公式(11)的运动方程后，可以根据最优运动时长以及预设的采样点数n，确定采样的时间步进值，时间步进值可以是：最优运动时长与预设的采样点数之间的比值

根据该时间步进值，对该最优运动时长T采样，得到多个采样点；并根据每个采样点tn和该运动方程，计算出机器人在每个采样点的状态，即坐标、速度、加速度。

采样点表示如公式(23)所示

其中，T_step表示采样的时间步进值，n表示预设的采样点数。

f_y(t_n)＝a₀+a₁t_n+a₂t_n ²+a₃t_n ³+a₄t_n ⁴+a₅t_n ⁵ 公式(24)；

f'_y(t_n)＝a₁+2a₂t_n+3a₃t_n ²+4a₄t_n ³+5a₅t_n ⁴ 公式(25)；

f”_y(t_n)＝2a₂+6a₃t_n+12a₄t_n ²+10a₅t_n ³ 公式(26)；

将采样点带入公式(24)至公式(26)，即可确定出每个采样点的状态。根据每个采样点即可得到在该指定方向的运动轨迹。

需要说明的是，指定方向的当前状态包括：机器人在指定方向的当前位姿以及当前速度，第一目标状态包括：机器人在指定方向的目标位姿以及目标速度，第一控制参数为加速度。那么可以根据指定方向的当前状态、第一目标状态构建包含位置系数的运动方程，该运动方程可以是以时间为自变量的4次多项式，例如，

f_y(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴ 公式(27)；

其中，a₀～a₄是该4次多项式中的系数。

该4次多项式中的系数的求解过程与公式(11)的求解类似，即对公式(27)进行一阶求导即可求解出a₀～a₄，求解的过程此处将不再赘述。

本发明实施方式相对于现有技术而言，通过机器人在指定方向的当前状态、第一目标状态以及指定方向的控制参数，确定该机器人的最优运动时长，由于该最优运动时长不是人为预先设定，而是在机器人耗能最优情况下确定，使得基于该最优运动时长、当前状态以及目标状态规划出的运动轨迹耗能最小，避免了因固定运动时长过大而导致机器人出现绕路、绕圈等问题，也避免因固定的运动时间过短，导致机器人需要在行进过程中提高速度，导致消耗的能量过多的问题；同时，基于当前状态、第一目标状态确定的最优运动时长，保证了机器人可以变更为设定的第一目标状态，以满足不同场景的需求。

本发明的第二实施方式涉及一种路径规划的方法。第二实施方式是对第一实施方式的进一步改进，主要改进之处在于：在本发明第二实施方式中，在确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹之后，该路径规划的方法还包括：确定机器人运动至指定位置的路径。该方法的具体流程如图4所示。

步骤201：根据机器人在指定方向的当前状态、指定方向的第一目标状态以及指定方向的第一控制参数，确定机器人的最优运动时长。

步骤202：根据指定方向的当前状态、第一目标状态以及最优运动时长，确定机器人从指定方向的当前状态变更为第一目标状态的运动轨迹。

步骤203：根据机器人从指定方向的当前状态变更为第一目标状态的运动轨迹，确定机器人运动至指定位置的路径，指定位置是指根据第一目标状态构建的二维坐标位置。

具体的说，指定位置可以是机器人坐标系下的一个二维坐标位置，可以根据第一目标状态，确定该指定位置，例如，第一目标状态中包含了X方向的目标位姿，那么可以设定一个Y方向的坐标位姿，构成该指定位置。若有多个指定方向，则可以根据每个指定方向的第一目标状态，构建指定位置，例如，有两个指定方向，分别为如图3所示的X方向和Y方向，那么该指定位置可以是包含该X坐标和Y坐标的位置，如指定位置Q(x_d,y_d)。

在一个例子中，指定方向的个数为1个，根据该第一目标状态，确定在机器人坐标系下，该第一目标状态中的目标位姿的坐标，并预先设定机器人坐标系下指定位置中的另一个坐标。例如，指定方向为如图3所示的Y方向，第一目标状态中的目标位姿为y_d，预先设定X方向上坐标为x_d，构建指定位置为Q(x_d,y_d)。在X方向的时间可以采用Y向运动的最优运动之间，X方向的加速度、速度没有限制，可以沿用机器人在X方向当前的速度和加速度。根据指定方向的运动轨迹中的采样点，即可确定出每个采样点的二维坐标位置，进而得到机器人达到指定位置的路径。

在另一个例子中，若指定方向的数量为多个，该指定位置是根据每个第一目标状态构建的二维坐标位置。根据机器人从指定方向的当前状态变更为第一目标状态的运动轨迹，确定机器人运动至指定位置的路径的过程为：获取机器人从每个指定方向的当前状态变更为第一目标状态的运动轨迹；拟合每个指定方向的运动轨迹，确定机器人运动至指定位置的路径。

具体的说，若有多个指定方向，则可以拟合每个指定方向确定的运动轨迹，确定机器人运动至指定位置的路径。例如，两个指定方向分别为如图3所示的X方向和Y方向，X方向和Y方向各自对应的第一目标状态中的目标位姿分别为x_d，y_d，构建指定位置为Q(x_d,y_d)。拟合每个指定方向的运动轨迹，得到机器人达到指定位置的路径。

在一个例子中，在确定机器人运动至指定位置的路径之后，方法还包括：存储该指定位置的路径，从存储的不同指定位置的路径中，选取轨迹评分最高的路径作为机器人的最优运动路径。

具体的说，每个指定方向可以有多个第一目标状态，故而可以有多个指定位置，得到存储的不同指定位置的路径，对每条路径进行轨迹评分，并存入路径列表中，其中，打分函数可以采用公式(28)：

其中，cost表示路径得分，pathDis表示目标路径与确定的路径的最短距离；goalDis表示目标路径终端与确定的路径的最短距离；occdis表确定的路径与障碍物的最短距离、vel表示当前机器人的线速度；angle Wei ght、path Wei ght、goal Wei ght、occWei ght、vel Wei ght和angle Wei ght表示各项的权重；cos t_pathDis、cos t_goalDis、cost_occDis、cos t_vel cos t_angle分别表示各项的得分；(angle_globle-angle_planner)表示目标路径终点与确定的路径终点的姿态角偏差。

最后从存储的路径列表中，选出轨迹评分最高的路径作为最优路径。

本实施方式中提供的路径规划的方法，在确定了指定方向的运动轨迹后，还可以确定达到指定位置，完善路径规划；此外还可以得到不同指定位置的路径，并从中选取轨迹评分最高的路径作为最优路径，使得得到的路径更加合理。

上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包括相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。

本发明第三实施方式涉及一种机器人，该机器人30的具体结构如图5所示，包括：至少一个处理器301，以及，与至少一个处理器301通信连接的存储器302；其中，存储器302存储有可被至少一个处理器301执行的指令，指令被至少一个处理器301执行，以使至少一个处理器301能够执行上述的路径规划的方法。

其中，存储器302和处理器301采用总线方式连接，总线可以包括任意数量的互联的总线和桥，总线将一个或多个处理器301和存储器302的各种电路链接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路链接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件，也可以是多个元件，比如多个接收器和发送器，提供用于在传输介质上与各种其他装置通信的单元。经处理器301处理的数据通过天线在无线介质上进行传输，进一步，天线还接收数据并将数据传送给处理器301。

处理器301负责管理总线和通常的处理，还可以提供各种功能，包括定时，外围接口，电压调节、电源管理以及其他控制功能。而存储器可以被用于存储处理器在执行操作时所使用的数据。

本发明第四实施方式涉及一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时实现上述的路径规划的方法。

本领域技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得一个设备(可以是单片机，芯片等)或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (12)

1.一种路径规划的方法，其特征在于，包括：

将机器人在指定方向的当前状态、所述指定方向的第一目标状态作为预设的最优控制模型的系统状态量，将所述指定方向的第一控制参数作为所述最优控制模型的系统控制量，按照所述最优控制模型构建基于所述第一控制参数的代价函数；

将所述代价函数最小情况下，所述代价函数中的时间值作为机器人的最优运动时长，其中，所述最优运动时长是指所述机器人从所述当前状态变更为所述第一目标状态耗能最优情况下花费的运动时长；

根据所述指定方向的当前状态、所述第一目标状态以及所述最优运动时长，确定所述机器人从所述指定方向的当前状态变更为所述第一目标状态的运动轨迹；

其中，所述指定方向的当前状态包括：所述机器人在所述指定方向的当前位姿以及当前速度，所述第一目标状态包括：所述机器人在所述指定方向的目标位姿以及目标速度，所述第一控制参数为加速度；

或者，

所述指定方向的当前状态包括：所述机器人在所述指定方向的当前位姿、当前速度以及当前加速度，所述第一目标状态包括：所述机器人在所述指定方向的目标位姿、目标速度及目标加速度，所述第一控制参数为加速度。

2.根据权利要求1所述的路径规划的方法，其特征在于，所述指定方向的数量为多个，多个所述指定方向均与所述机器人的底盘所在平面平行。

3.根据权利要求2所述的路径规划的方法，其特征在于，所述指定方向的数量为两个，其中，两个所述指定方向相互垂直。

4.根据权利要求1所述的路径规划的方法，其特征在于，所述指定方向的当前状态包括：所述机器人在所述指定方向的当前位姿、当前速度以及当前加速度，所述第一目标状态包括：所述机器人在所述指定方向的目标位姿、目标速度及目标加速度，所述第一控制参数为加速度；

所述按照所述最优控制模型构建基于所述第一控制参数的代价函数，包括：

对所述最优控制模型进行变换，得到所述代价函数；

所述最优控制模型

表示为：

，其中，

包括机器人在指定方向的当前状态和所述第一目标状态，

表示所述系统状态量，

用于表示所述系统控制量，

表示第一控制参数，

表示机器人的速度，

表示机器人的加速度；

所述最优控制模型的哈密尔顿函数表示为：

，

所述H表示哈密尔顿函数，

为未知系数，T表示最优运动时长；

所述代价函数

表示为：

；

其中，

和

为所述代价函数中的系数。

5.根据权利要求1至4中任一项所述的路径规划的方法，其特征在于，所述根据所述指定方向的当前状态、所述第一目标状态以及所述最优运动时长，确定所述机器人从所述指定方向的当前状态变更为所述第一目标状态的运动轨迹，包括：

根据所述指定方向的当前状态、所述第一目标状态构建包含未知系数的运动方程；

根据所述最优运动时长，确定所述运动方程中的未知系数，得到所述指定方向的运动方程；

根据预设的采样点数，对所述最优运动时长采样，得到多个采样点；

根据多个所述采样点和所述指定方向的运动方程，得到所述指定方向的运动轨迹。

6.根据权利要求5所述的路径规划的方法，其特征在于，所述指定方向的当前状态包括：所述机器人在所述指定方向的当前位姿、当前速度以及当前加速度，所述指定方向的运动方程表示为：

；

~

是该5次多项式中的系数，t表示运动时长，其中，

，

，

，

，

，

，

表示所述机器人当前的位置、

表示所述机器人当前的速度，

表示所述机器人当前的加速度，

表示所述机器人的目标位置、

表示所述机器人的目标速度，

表示所述目标的加速度。

7.根据权利要求2至4中任一项所述的路径规划的方法，其特征在于，在所述确定所述机器人从所述指定方向的当前状态变更为所述第一目标状态的运动轨迹之后，所述方法还包括：

根据所述机器人从所述指定方向的当前状态变更为所述第一目标状态的运动轨迹，确定所述机器人运动至指定位置的路径，所述指定位置是指根据每个所述第一目标状态构建的二维坐标位置。

8.根据权利要求7所述的路径规划的方法，其特征在于，根据所述机器人从所述指定方向的当前状态变更为所述第一目标状态的运动轨迹，确定所述机器人运动至指定位置的路径，包括：

获取机器人从每个所述指定方向的当前状态变更为所述第一目标状态的运动轨迹；

拟合每个所述指定方向的运动轨迹，确定所述机器人运动至所述指定位置的路径。

9.根据权利要求8所述的路径规划的方法，其特征在于，在所述确定所述机器人运动至所述指定位置的路径之后，所述方法还包括：

存储所述指定位置的路径；

从存储的不同指定位置的路径中，选取轨迹评分最高的路径作为所述机器人的最优运动路径。

10.根据权利要求5所述的路径规划的方法，其特征在于，所述根据预设的采样点数，对所述最优运动时长采样，得到多个采样点，包括：

将所述最优运动时长与所述预设的采样点数之间的比值作为采样的时间步进值；

按照所述时间步进值对所述最优运动时长进行采样，得到多个采样点。

11.一种机器人，其特征在于，包括：

至少一个处理器，以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1至10任一所述的路径规划的方法。

12.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至10中任一项所述的路径规划的方法。

Images