[发明专利]轨迹跟踪方法、装置

权利要求书

1.一种轨迹跟踪方法，其特征在于，包括：

获取目标设备的当前位置；

判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小；

若判断出所述当前位置与所述预定位置的距离大于所述预设距离阀值，则根据所述目标设备的当前位置和所述预定位置，生成控制指令，其中，所述控制指令包括有以下信息：所述目标设备移动的角速度和线速度，所述控制指令用于控制所述目标设备根据所述角速度和所述线速度，从所述当前位置移动至所述预定位置；

其中，根据所述目标设备的当前位置和所述预定位置，生成控制指令，包括：

根据所述当前位置的位置变量和方向变量，以及所述预定位置的位置变量和方向变量，确定所述目标设备从所述当前位置移动至所述预定位置的导航向量；

归一化处理所述导航向量，得到归一化处理后的导航向量；

获取所述归一化处理后的导航向量的角度值；

根据所述导航向量和所述归一化处理后的导航向量的角度值，得到所述控制指令；

其中，通过如下公式计算得到所述目标设备从所述当前位置移动至所述预定位置的导航向量：

预设所述当前位置为s_t＝[x_t，y_t，θ_t)，预设所述预定位置为s^tar＝[x^tar，y^tar，θ^tar]，预设所述预定位置的目标姿态向量p＝[p_n，p_m]^T＝[cos(θ^tar)，sin(θ^tar)]^T；

则，

其中，x_t，y_t为所述当前位置的位置变量，θ_t为所述当前位置的方向变量，x^tar，y^tar为所述预定位置的位置变量，θ^tar为所述预定位置的方向变量，F为导航向量，n表示目标姿态向量p的横坐标，m表示目标姿态向量p的纵坐标。

2.根据权利要求1所述的方法，其特征在于，在判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之后，所述方法还包括：

若判断出所述当前位置与所述预定位置的距离小于等于所述预设距离阀值，则确定所述目标轨迹的下一预定位置，其中，所述下一预定位置与所述当前位置的距离大于所述预设距离阀值，所述下一预定位置为所述预定位置的下一个位置；

根据所述目标设备的当前位置和所述下一预定位置生成所述控制指令，其中，所述控制指令用于控制所述目标设备根据所述角速度和所述线速度，从所述当前位置移动至所述下一预定位置。

3.根据权利要求1所述的方法，其特征在于，获取目标设备的当前位置，包括：

获取所述目标设备的位置变量和方向变量；

根据所述位置变量和所述方向变量，确定所述目标设备的当前位置。

4.根据权利要求3所述的方法，其特征在于，在所述预定位置为所述目标轨迹的初始轨迹点的情况下，在判断所述当前位置与预先设定的目标轨迹中预定位置的距离与预设距离阀值的大小之前，所述方法还包括：

将所述目标轨迹的初始轨迹点确定为所述预定位置；

获取所述预定位置的位置变量和方向变量，其中，根据所述目标设备的位置变量和方向变量，以及所述预定位置的位置变量和方向变量，确定所述当前位置与预先设定的目标轨迹中预定位置的距离。

5.根据权利要求4所述的方法，其特征在于，通过如下方式归一化处理所述导航向量，得到归一化处理后的导航向量：

其中，Fⁿ为所述归一化处理后的导航向量。

6.根据权利要求5所述的方法，其特征在于，通过如下公式计算得到所述归一化处理后的导航向量的角度值：

其中，φ为所述归一化处理后的导航向量的角度值。