[发明专利]机器人及其航迹设置方法和系统

权利要求书

1.一种机器人的航迹设置方法，其特征在于，包括：

预测姿态角步骤：采用粒子滤波算法预测机器人的姿态角，所述粒子滤波算法采用头脑风暴算法优化所述粒子滤波算法的权值公式的步长参数，所述姿态角包括偏航角、俯仰角及横滚角；

预测航迹坐标步骤：获取所述机器人的起始航迹点、当前时刻的姿态角及当前速度，以预测所述机器人在下一时刻的航迹坐标；其中，所述预测航迹坐标步骤采用改进的粒子滤波算法实现对所述机器人在下一时刻的航迹坐标进行预测；

所述采用头脑风暴算法优化所述粒子滤波算法的权值公式的步长参数的具体步骤如下：

S1：对所述步长参数St进行随机初始化操作，得到步长参数矩阵；

S2：采用聚类算法对所述步长参数矩阵进行聚类，得到多类参数矩阵，其中，每类的中心为所述参数矩阵的第一行；

S3：生成第一随机数，并与预设的第一超参数进行比较，如果所述第一随机数大于等于该第一超参数，则随机选择一类所述参数矩阵的中心，并生成第一新向量；

或者，

S4：生成第二随机数，并与预设的第二超参数进行比较，如果所述第二随机数大于等于该第二超参数，则随机选择一类参数矩阵的中心，并在该一类参数矩阵中随机选择一个元素以一个随机数代替所述元素，生成第二新向量；

或者，

S5：随机选择两类所述参数矩阵的中心，并通过交叉重组得到第三新向量；

S6：根据预设的判断指标和所述第一新向量、或所述第二新向量、或所述第三新向量，输出优化步长参数矩阵，或者返回步骤S2。

2.根据权利要求1所述的航迹设置方法，其特征在于，所述预测姿态角步骤具体包括：

在t时刻，根据所述姿态角的先验条件概率，产生N个随机样本，所述N个随机样本被称为粒子；

根据所述粒子相对于以往实际轨迹的左右位置，将所述粒子划分为左派粒子和右派粒子；

采用后验概率计算公式分别计算所述t时刻的所述左派粒子和所述右派粒子的权值，其中，所述后验概率计算公式为：π是圆周率，w(i)是第i个粒子在最后全部粒子线性叠加预测运算中的权重，R是激光雷达的标准误差值，Di是第i个粒子与所述激光雷达观察的航线的偏离值，St是t时刻的步长参数，所述St值采用头脑风暴算法进行优化；

根据所述左派粒子和所述右派粒子的权值，采用加权平均算法计算得出所述机器人在t+1时刻的姿态角。

3.根据权利要求2所述的航迹设置方法，其特征在于，所述机器人为海陆空三栖机器人，所述预测姿态角步骤之前还包括模态识别步骤：

识别所述机器人的当前行驶模态，所述行驶模态包括三种类型：第一模态、第二模态及第三模态；所述第一模态为海、陆及空三个模态；所述第二模态为海、陆、空、海陆、空海、陆空六个模态；所述第三模态为海、陆、空、海陆、空海、陆空、陆海、空陆、海空九个模态；

所述预测姿态角步骤包括：根据所述前行驶模态，调整所述权值公式的所述步长参数St，所述St值采用头脑风暴算法针对模态过渡总时长的1/k的片段时间序列进行优化，k＝4。

4.根据权利要求1或2所述的航迹设置方法，其特征在于，所述预测姿态角步骤还包括：

在下一个t时刻，根据所述姿态角的先验条件概率，重新产生N个随机粒子。

5.根据权利要求1或2所述的航迹设置方法，其特征在于，所述预测航迹坐标步骤之后还包括航迹优化步骤：

采用复数分数方差函数评价所述机器人的速度变化，所述复数分数方差函数计算公式为：m为分数，n为时刻数，ξ_i为各时刻速度值，为速度平均值，ξ_i为复数，虚数表示横向行驶速度，实数表示纵向行驶速度；

调整所述机器人的行驶速度符合最小评价函数，所述最小评价函数的定义为：使得所述复数分数方差函数的模最小。

6.根据权利要求5所述的航迹设置方法，其特征在于，所述航迹优化步骤之后还包括步骤：在所述机器人的实际行程超过预定的误差时，重新执行所述预测姿态角步骤、所述预测航迹坐标步骤及所述航迹优化步骤。