[发明专利]自适应机器人的跑步机控制方法和系统

权利要求书

1.自适应机器人的跑步机控制方法，其特征在于，所述方法包括以下步骤：

S1：使足式机器人在跑步机的跑带上运动；

S2：通过步态采集模块采集足式机器人的多项运动数据；

S3：融合运动数据得到最优估计值；

S4：利用最优估计值计算足式机器人的速度和加速度，并构建出足式机器人实时姿态的三维姿态模型；

S5：根据速度、加速度和三维姿态模型数据，控制跑步机跑带速度和跑带倾斜角度，让跑带的速度自适应四足机器人的速度；

步骤S2所述步态采集模块通过激光雷达和深度相机分别采集雷达深度数据和相机深度数据；

步骤S3所述融合运动数据的方式为卡尔曼融合，所述卡尔曼融合的具体方法为：

设深度相机在时间0-N范围里所测得的k个目标点位置为Z_0，k＝{P₁，...，P_k}_N；其中N代表第N周期，P_k代表第k个目标点的位置，P＝(x²+y²+z²)^1/2，x、y、z分别为以深度相机为原点的目标点坐标值；并且深度相机的测量值遵循正态高斯变量，即有Z₀为某一时刻k个目标点的位置，表示正态分布的均值为μ₀，正态分布的方差为

首先初始化移动机器人的位置和误差协方差；在状态预测阶段，利用深度相机采集到机器人运动数据，并将此值作为当前时刻系统状态值，作为对下一时刻位姿的预测值；的协方差矩阵∑_0,k计算公式为：

表示在以深度相机测量值遵循的正态分布下第1周期的方差；

同理，设激光雷达所测量的深度信息值为Z_1，k＝{P’₁，...，P’_k}_N；其中N代表第N周期，P’_k代表第k个目标点的位置，P’＝(x’²+y’²+z’²)^1/2，x’、y’、z’分别为以激光雷达为原点的目标点坐标值；并且深度相机的测量值遵循正态高斯变量，即有其协方差矩阵如公式(2)；

将上面两组传感器的测量值进行卡尔曼融合，由上述式子Z_0，k＝{P₁，...，P_k}_N和Z_1，k＝{P’₁，...，P’_k}_N计算得出卡尔曼滤波中得观测矢量有

协方差为：

0_N表示N阶零矩阵

卡尔曼预测方程有A_k＝0_N和B_k＝I_N、I_N为N阶的单位矩阵，

卡尔曼更新方程有

∑_N|N＝(I-K_NH_k)∑_N|N-1 (9)

其中：

∑代表的是公式(1)和(2)的两组协方差的组合矩阵；

K_k代表的是卡尔曼增益系数；

∑_N|N代表的是方差矩阵更新方程；

代表预测观测值，也即公式(5)为预测方程；

∑N|N-1代表预测协方差；

A_k和B_k是系统参数，具体的，A_k状态转移矩阵，B_k为控制矩阵；

u_k表示跑带速度和跑带倾斜度的控制量；

所以最终得到了利用卡尔曼增益计算传感器数据融合得最优估计值为通过公式(8)计算得出