CN114509936B - 一种运动能力可在线学习的运动规划方法、装置和介质 - Google Patents

Abstract

本发明属于运动控制领域，涉及一种运动能力可在线学***台的运动学与动力学约束，初始化曲率‑速率函数；步骤S2：根据运动平台的实际运行轨迹与期望轨迹实时评估运动平台的轨迹跟踪能力；步骤S3：根据运动平台的跟踪能力实时更新曲率‑速率函数；步骤S4：运动平台的运动规划器根据更新后的曲率‑速率函数，得到运动平台的运动规划结果；步骤S5：运动平台的控制器根据运动规划结果得到运动命令，根据运动命令控制运动平台运动。本方法能够使运动平台在运动能力很好的条件下，自适应的提高运动效率；同时在运动能力不好的条件下，自适应的降低速度以提高跟踪精度。

Description

一种运动能力可在线学习的运动规划方法、装置和介质

技术领域

本发明属于运动控制领域，涉及一种运动能力可在线学习的运动规划方法、装置和介质。

背景技术

机器人等运动平台对运动效率、轨迹跟踪的精度与平稳性都有很高的要求，这要求运动平台能够最大限度的发挥其本身的运动能力，同时避免过于保守的或激进的运动规划结果造成的效率下降或轨迹跟踪精度与平稳性下降等问题。运动平台的运动能力受材质、安装精度、运动模型标定精度、运动环境以及运动平台的动力性能等因素的影响较大。因而，不同运动平台的运动能力可能存在很大差别，甚至同一个运动平台在不同时间与环境下的运动能力都可能存在很大差异。为了能够兼顾运动效率与轨迹跟踪精度，运动平台的运动规划需要考虑自身的运动能力。

公开号为CN108549328A的中国发明专利申请《自适应速度规划方法及***》公开了一种根据运动平台的限制条件（包括以下的一个或多个：各轴的最大转速、向心加速度-曲率约束速率、防止振动和减小冲击限定的速率、用户使用需求限定速率）创建速率约束曲线，并据此生成考虑运动平台能力的速度规划曲线方法及***。该专利需要人工预先设定好限制条件，并且设定的限制条件在运动平台运动过程中保持不变。这种方式在固定环境，并且运动平台的运动性能稳定时，能够最大限度的发挥运动平台的实际运动能力。当环境变化较大，或运动平台的运动性能受运动时间与动力（例如：电量）的影响较大时，则此方法具有较大局限。

公开号为CN112873208A的中国发明专利申请《一种抗噪与动力学约束的机器人实时运动规划方法及装置》公开了一种使运动平台在运行过程中能在线考虑跟踪误差方法，使运动平台在控制***存在噪声的情况下，仍能对期望轨迹的高精度跟踪。该专利针对特定数学模型的机器人设计考虑轨迹跟踪误差的加速度等式约束条件。轨迹跟踪误差信息是一个以轨迹跟踪误差为输入的PID控制器结构，该信息作为加速度的惩罚项，即：轨迹跟踪误差越大，加速度被约束为越小的值。这种方式仅利用历史轨迹跟踪误差信息在线调整当前一步运动规划的加速度约束条件，无法用于更普遍的、能在时间维度上得到最优运动的多步运动规划任务。因为，在未来的运动规划时间点还没有产生可以用于计算运动约束的轨迹跟踪误差信息。

发明内容

为了解决现有技术中存在的上述技术问题，本发明提出了一种运动能力可在线学***台运动的过程中根据实时评估的运动能力在线学***台的运动规划约束条件，从而得到兼顾运动效率与轨迹跟踪精度的运动规划结果，其具体技术方案如下：

一种运动能力可在线学习的运动规划方法，包括以下步骤：

步骤S1：根据运动平台的运动学与动力学约束，初始化曲率-速率函数，所述曲率-速率函数即：以曲率为自变量的速率约束函数；

步骤S2：根据运动平台的实际运行轨迹与期望轨迹实时评估运动平台的轨迹跟踪能力；

步骤S3：根据运动平台的跟踪能力实时更新曲率-速率函数；

步骤S4：运动平台的运动规划器根据更新后的曲率-速率函数，得到运动平台的运动规划结果；

步骤S5：运动平台的控制器根据运动规划结果得到运动命令，根据运动命令控制运动平台运动。

进一步的，所述步骤S1具体为：

根据运动平台的运动学与动力学约束，初始化曲率-速率函数为V₀(k)=sqrt(a/|k|)，a是与运动平台的动力学约束相关的系数，k∈A，A表示运动平台的曲率约束，A=[K_min,K_max]，K_min和K_max分别表示运动平台的最小曲率和最大曲率。

进一步的，所述步骤S2具体为：

根据运动平台的实际运行轨迹r={q₁，q₂，…，q_N}， q₁~q_N为实际运行轨迹点，N为记录的历史轨迹步长，以及其期望轨迹rd={p₁，p₂，…，p_M}，p₁~p_M为期望运行轨迹点，M为期望轨迹的步长，从期望轨迹rd中找到距离t时刻运动平台的实际运行轨迹点q_t位置最近的轨迹点p_nearest，并根据q_t和p_nearest计算出跟踪误差e_t；再根据跟踪误差e_t评估出运动平台的轨迹跟踪能力u_t，其中u_t<0，u_t越大表示运动平台的轨迹跟踪能力越强，反之越差。

进一步的，所述轨迹跟踪能力u_t的表达式为：

u_t=-(kPe_t+kI∑^t _i=0e_i+kD(e_t-e_t-1))，其中，kP，kI，kD为权值。

进一步的，所述跟踪误差e_t的具体表达式为：e_t=c_p

e_position+c_h

e_heading，其中c_p与c_h为权值；e_position为跟踪位置误差，通过所述的p_nearest与所述的q_t计算欧氏距离得到；e_heading为跟踪角度误差，通过表达式|h_nearest-h_t|计算得到，h_nearest为轨迹点p_nearest的航向角，h_t为运动平台轨迹点p_t的航向角。

进一步的，所述步骤S3具体为：

若u_t<u_a1，则V_t(k)=V_t-1(k)+b₁|ut|G₁(||k-k_t||)；

若u_t>u_a2，则V_t(k)=V_t-1(k)+b₂|ut|G₂(||k-k_t||)；

若u_a1≤u_t≤u_a2，则V_t(k)=V_t-1(k)；

其中，u_a1为设置的运动平台允许的轨迹跟踪能力范围的下界，u_a2为设置的允许的轨迹跟踪能力范围的上界，b₁，b₂为学***台最近的轨迹点的曲率。

进一步的，所述函数G₁满足G₁<0，使更新得到的V_t(k)满足在k<0时为非递增函数，在k>0时为非递减函数，且V_t(k)<V₀(k)，对任意k∈A，

G₁（k，k_t）=-exp{-||k-k_t||²/(2sigma1²)}，其中exp为指数函数，sigma1为参数；

所述函数G₂满足G₂>0，使所述的更新后的V_t(k)满足在k<0时为非递增函数，在k>0时为非递减函数，且V_t(k)<V₀(k) ，对任意k∈A，一个优选的G₂（k，k_t）= exp{-||k-k_t||²/(2sigma2²)}，其中exp为指数函数，sigma2为参数。

进一步的，所述步骤S4具体为：

根据时刻t更新得到的曲率-速率函数V_t(k)构建曲率-速率约束项v≤V(k)，v为任意时刻的期望速率，V(k)表示任意时刻的曲率-速率函数；

在运动规划器中根据所述的曲率-速率约束项，得到满足该曲率-速率约束项的期望轨迹序列与期望速率序列，即运动规划结果。

一种运动能力可在线学习的运动规划装置，包括一个或多个处理器，用于实现所述的一种运动能力可在线学习的运动规划方法。

一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的一种运动能力可在线学习的运动规划方法。

与现有技术相比，本发明的方法不需要人工预先设定好限制条件，仅利用运动平台的运动学与动力学约束初始化曲率-速率函数，并根据实时评估的运动能力（由轨迹跟踪能力表示）在线更新曲率-速率函数，使运动平台得到兼顾运动效率与轨迹跟踪精度的多步运动规划结果，能够使运动平台在运动能力很好的条件下，自适应的提高运动效率，同时在运动能力不好的条件下，自适应的降低速度以提高跟踪精度，且本发明的方法并不局限于特定模型的运动平台，而是适用于所有需要高效并精准完成运动的运动平台。

附图说明

图1是本发明实施例中的运动能力可在线学习的运动规划方法的流程示意图；

图2是本发明实施例中的运动能力可在线学习的运动规划方法的数据流图；

图3a是本发明实施例中的根据函数G₁满足G₁<0时的运动能力在线更新曲率-速率函数的示意图；

图3b是本发明实施例中的根据函数G₂满足G₂>0时的运动能力在线更新曲率-速率函数的示意图；

图4是本发明实施例中的运动能力可在线学习的运动规划装置结构示意图。

具体实施方式

为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。

本实施例以无人驾驶汽车作为本发明中所述的运动平台的一个特例对本发明技术进行说明，应当理解，此处所提供的实施例仅仅以解释发明，并不用于限定本发明。

如图1和图2所示，本发明的一种运动能力可在线学***台的运动学与动力学约束，初始化曲率-速率函数；然后，根据运动平台的实际轨迹与期望轨迹在线评估运动平台的轨迹跟踪能力，并根据所述的运动平台的轨迹跟踪能力在线更新曲率-速率函数；最后，运动平台的运动规划器以更新后的曲率-速率函数作为约束条件，得到考虑轨迹跟踪能力的运动规划结果。

更加具体的，包括以下步骤：

步骤S1：根据运动平台的运动学与动力学约束初始化曲率-速率函数；

本发明实施例根据无人驾驶汽车的运动学与动力学约束，初始化以曲率为自变量的速率约束函数，即初始化曲率-速率函数；

所述曲率-速率函数包括所述无人驾驶汽车的在跟踪不同曲率的运动路径点的最优速率，所述的曲率-速率函数在曲率小于零时为非递减函数，在曲率大于等于零时为非递增函数；

具体的，根据无人驾驶汽车的曲率约束，确定所述的曲率-速率函数的定义域A=[K_min, K_max]，其中，A表示曲率约束，K_min和K_max分别表示运动平台的最小和最大曲率；

根据无人驾驶汽车的动力学约束，初始化所述的曲率-速率函数的映射关系为V₀，V₀(k)=sqrt(a/|k|)，a是与以无人驾驶汽车作为本实施例运动平台的动力学约束相关的系数，一个优选的V₀(k)=sqrt(9.8f/|k|)，其中对任意k∈A，9.8为近似的重力加速度值，f是运动平台无人驾驶汽车与路面的附着系数。

步骤S2：根据运动平台的实际运行轨迹与期望轨迹实时评估运动平台的轨迹跟踪能力；

根据无人驾驶汽车行驶的实际行驶轨迹与期望轨迹即参考轨迹，实时评估其轨迹跟踪能力；

无人驾驶汽车行驶的实际行驶轨迹r由一系列历史实际行驶轨迹点{q₁，q₂，…，q_N}表示，其中，历史行驶轨迹点q_i包含无人驾驶汽车的空间位置、航向角、曲率等信息，q_i∈{q₁，q₂，…，q_N}，N为记录的历史轨迹步长，一个优选的N=1，表示仅记录当前无人驾驶汽车的轨迹点q_t；

所述的期望轨迹rd由点序列{p₁，p₂，…，p_M}表示，其中，p_i包含无人驾驶汽车的空间位置、航向角、曲率等信息，p_i∈{p₁，p₂，…，p_M}，M为期望轨迹的步长；

从期望轨迹点序列{p₁，p₂，…，p_M}中，找到距离轨迹点q_t位置最近的轨迹点p_nearest；

根据所述的p_nearest与所述的q_t计算跟踪位置误差e_position，一个优选的位置误差为p_nearest与q_t的欧氏距离；

根据所述的p_nearest与所述的q_t计算跟踪角度误差e_heading，一个优选的角度误差为|h_nearest-h_t|，其中，h_nearest为轨迹点p_nearest的航向角，h_t为无人驾驶汽车当前轨迹点p_t的航向角；

根据所述的跟踪位置误差e_position与跟踪角度误差e_heading得到跟踪误差e_t，一个优选的计算形式为e_t=c_p

e_position+c_h

e_heading，其中c_p与c_h为权值。

根据所述的跟踪误差e_t计算无人驾驶汽车（运动平台）的轨迹跟踪能力，记为u_t，其中u_t<0，u_t越大表示运动平台的轨迹跟踪能力越强，反之越差，一个优选的计算公式为u_t=-(kPe_t+kI∑^t _i=0e_i+kD(e_t-e_t-1))，其中，kP，kI，kD为权值。

步骤S3：根据运动平台的跟踪能力实时更新曲率-速率函数；

根据所述的无人驾驶汽车的轨迹跟踪能力实时更新曲率-速率函数；

假设无人驾驶汽车在当前时刻的轨迹跟踪能力为u_t；

若u_t<u_a1，则V_t(k)=V_t-1(k)+b₁|ut|G₁(||k-k_t||)；

若u_t>u_a2，则V_t(k)=V_t-1(k)+b₂|ut|G₂(||k-k_t||)；

若u_a1≤u_t≤u_a2，则V_t(k)=V_t-1(k)；

其中，u_a1为设置的无人驾驶汽车允许的跟踪能力范围的下界，u_a2为设置的允许的跟踪能力范围的上界，b₁，b₂为学习率，V_t(k)为时刻t更新得到的曲率-速率函数，V_t-1(k)为时刻t-1的曲率-速率函数，G₁与G₂是以||k-k_t||为自变量的函数，A为所述的曲率-速率函数的定义域，k_t为期望轨迹中离当前无人驾驶汽车最近的轨迹点的曲率；

所述的函数G₁满足G₁<0，使所述的更新后的V_t(k)满足在k<0时为非递增函数，在k>0时为非递减函数，且V_t(k)<V₀(k)，对任意k∈A，一个优选的G₁（k，k_t）=-exp{-||k-k_t||²/(2sigma1²)}，其中exp为指数函数，sigma1为参数，此优选G₁对V_t的更新示意图参见图3a，说明G₁会在无人驾驶汽车跟踪能力差（u_t<u_a1）时适当降低所在曲率对应的速率值，以提高之后的轨迹跟踪精度；

所述的函数G₂满足G₂>0，使所述的更新后的V_t(k)满足在k<0时为非递增函数，在k>0时为非递减函数，且V_t(k)<V₀(k) ，对任意k∈A，一个优选的G₂（k，k_t）= exp{-||k-k_t||²/(2sigma2²)}，其中exp为指数函数，sigma2为参数，此优选G₂对V_t的更新示意图参见图3b，说明G₂会在无人驾驶汽车跟踪能力非常好（u_t>u_a2）时适当提高所在曲率对应的速度值，以提高之后的运动效率。

步骤S4：运动平台的运动规划器根据更新后的曲率-速率函数，得到运动平台的运动规划结果；

无人驾驶汽车的运动规划器以所述的更新后的曲率-速率函数作为约束条件，得到考虑轨迹跟踪能力的运动规划结果；

根据时刻t更新得到的曲率-速率函数V_t(k)构建曲率-速率约束项v≤V(k)，v为任意时刻的期望速率，V(k)表示任意时刻的曲率-速率函数；

在运动规划器中根据所述的曲率-速率约束项，得到满足该曲率-速率约束项的期望轨迹序列与期望速率序列；

所述的期望轨迹序列由点序列{p₁，p₂，…，p_M}表示，其中的p_i包含该轨迹点点在空间坐标系中的位置，姿态，曲率等信息；

所述的期望速率序列由速率序列{v₁，v₂，…，v_M}表示。

其中，M为期望步长即规划步长。

步骤S5：运动平台的控制器根据运动规划结果得到运动命令，根据运动命令控制运动平台运动；

无人驾驶汽车的控制器根据所述的期望轨迹与期望速率结果得到运动指令，控制运动无人驾驶汽车即平台运动一个时间步长。

最后，重复执行步骤S2，在无人驾驶汽车运动的过程中根据实时评估的跟踪能力在线学习一个曲率-速率函数，实时调整其运动规划约束条件，从而得到兼顾运动效率与轨迹跟踪精度的运动规划结果。

与前述一种运动能力可在线学习的运动规划方法的实施例相对应，本发明还提供了一种运动能力可在线学习的运动规划装置的实施例。

参见图4，本发明实施例提供的一种运动能力可在线学习的运动规划装置，包括一个或多个处理器，用于实现上述实施例中的一种运动能力可在线学习的运动规划方法。

本发明一种运动能力可在线学习的运动规划装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图4所示，为本发明一种运动能力可在线学习的运动规划装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图4所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。

上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的一种运动能力可在线学习的运动规划方法。

所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是风力发电机的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（Smart Media Card，SMC）、SD卡、闪存卡（Flash Card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。

以上所述，仅为本发明的优选实施案例，并非对本发明做任何形式上的限制。虽然前文对本发明的实施过程进行了详细说明，对于熟悉本领域的人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明精神和原则之内所做修改、同等替换等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种运动能力可在线学习的运动规划方法，其特征在于，包括以下步骤：

步骤S1：根据运动平台的运动学与动力学约束，初始化曲率-速率函数，所述曲率-速率函数即：以曲率为自变量的速率约束函数，具体为：

根据运动平台的运动学与动力学约束，初始化曲率-速率函数为V₀(k)=sqrt(a/|k|)，a是与运动平台的动力学约束相关的系数，k∈A，A表示运动平台的曲率约束，A=[K_min, K_max]，K_min和K_max分别表示运动平台的最小曲率和最大曲率；

步骤S2：根据运动平台的实际运行轨迹与期望轨迹实时评估运动平台的轨迹跟踪能力，具体为：

根据运动平台的实际运行轨迹r={q₁，q₂，…，q_N}， q₁~q_N为实际运行轨迹点，N为记录的历史轨迹步长，以及其期望轨迹rd={p₁，p₂，…，p_M}，p₁~p_M为期望运行轨迹点，M为期望轨迹的步长，从期望轨迹rd中找到距离t时刻运动平台的实际运行轨迹点q_t位置最近的轨迹点p_nearest，并根据q_t和p_nearest计算出跟踪误差e_t；再根据跟踪误差e_t评估出运动平台的轨迹跟踪能力u_t，其中u_t<0，u_t越大表示运动平台的轨迹跟踪能力越强，反之越差；

步骤S3：根据运动平台的跟踪能力实时更新曲率-速率函数，具体为：

若u_t<u_a1，则V_t(k)=V_t-1(k)+b₁|u_t|G₁(||k-k_t||)；

若u_t>u_a2，则V_t(k)=V_t-1(k)+b₂|u_t|G₂(||k-k_t||)；

若u_a1≤u_t≤u_a2，则V_t(k)=V_t-1(k)；

其中，u_a1为设置的运动平台允许的轨迹跟踪能力范围的下界，u_a2为设置的允许的轨迹跟踪能力范围的上界，b₁，b₂为学***台最近的轨迹点的曲率；

步骤S4：运动平台的运动规划器根据更新后的曲率-速率函数，得到运动平台的运动规划结果，具体为：

根据时刻t更新得到的曲率-速率函数V_t(k)构建曲率-速率约束项v≤V(k)，v为任意时刻的期望速率，V(k)表示任意时刻的曲率-速率函数；

在运动规划器中根据所述的曲率-速率约束项，得到满足该曲率-速率约束项的期望轨迹序列与期望速率序列，即运动规划结果；

步骤S5：运动平台的控制器根据运动规划结果得到运动命令，根据运动命令控制运动平台运动。

2.如权利要求1所述的一种运动能力可在线学习的运动规划方法，其特征在于，所述轨迹跟踪能力u_t的表达式为：u_t=-(kPe_t+kI∑^t _i=0e_i+kD(e_t-e_t-1))，其中，kP，kI，kD为权值。

3.如权利要求1所述的一种运动能力可在线学***台轨迹点p_t的航向角。

4.如权利要求1所述的一种运动能力可在线学习的运动规划方法，其特征在于，所述函数G₁满足G₁<0，使更新得到的V_t(k)满足在k<0时为非递增函数，在k>0时为非递减函数，且V_t(k)<V₀(k)，对任意k∈A，G₁（k，k_t）=-exp{-||k-k_t||²/(2sigma1²)}，其中exp为指数函数，sigma1为参数；

所述函数G₂满足G₂>0，使所述的更新后的V_t(k)满足在k<0时为非递增函数，在k>0时为非递减函数，且V_t(k)<V₀(k) ，对任意k∈A，G₂（k，k_t）= exp{-||k-k_t||²/(2sigma2²)}，其中exp为指数函数，sigma2为参数。

5.一种运动能力可在线学习的运动规划装置，其特征在于，包括一个或多个处理器，用于实现权利要求1-4中任一项所述的一种运动能力可在线学习的运动规划方法。

6.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-4中任一项所述的一种运动能力可在线学习的运动规划方法。

Images