[发明专利]一种自平衡机器人的控制方法、系统及存储介质

权利要求书

1.一种自平衡机器人的控制方法，其特征在于，包括如下步骤：

S1，获取自平衡机器人的目标自平衡状态和当前自平衡状态；

S2，基于所述目标自平衡状态和所述当前自平衡状态，通过非线性函数确定提供给所述自平衡机器人的运动转矩以完成自平衡校准；

S3，响应于所述自平衡机器人的自平衡校准，提取所述自平衡机器人的视觉设备拍摄的双目图像中障碍物的特征点，采用跟随算法对所述双目图像中障碍物的特征点进行跟随；

S4，响应于跟随所述双目图像中障碍物的特征点，分别构建所述障碍物与所述自平衡机器人的三维模型；

S5，基于所述三维模型构建虚拟地图，获取所述障碍物与所述自平衡机器人的距离信息，并采用避障策略规划避障路径，所述避障策略具体为：

利用A*算法构建所述自平衡机器人最短避障路径，通过矢量共享方法求解所述自平衡机器人运动方向的偏移量，通过不断修正所述偏移量实时规划所述自平衡机器人的运动路径；

步骤S3中采用跟随算法对所述双目图像中障碍物的特征点进行跟随的具体方法为：

计算所述双目图像的金字塔，其中，对从视觉设备中获取到的双目图像做为零层图像，通过对零层图像进行高斯卷积，并对于所述零层图像为连续时间序列的图像进行采样得到第一层至顶层图像，其中，所述顶层图像即为所述双目图像分辨率最小的图像；设定每一层金字塔图像都有一个初始光流g和一个剩余光流d，从所述顶层图像往下开始逐层计算，假设有n层所述金字塔图像，所述顶层图像为第n-1层，则每层金字塔的初始光流计算如下：

其中，g_i为第i层的跟踪点的初始光流，g_i+1为第i+1层的跟踪点的初始光流，d_i+1为第i+1层的跟踪点的剩余光流，d_i+1为在g_i+1的基础上进行迭代优化以获取精确跟踪点。

2.根据权利要求1所述的自平衡机器人的控制方法，其特征在于，步骤S2中所述自平衡机器人采用的运动转矩的计算，所述自平衡机器人的控制结构为：

其中，τ为所述自平衡机器人的转矩；q为所述自平衡机器人的关节位置；M(q)为惯性矩阵；为所述自平衡机器人的关节速度；为关节速度误差；q_d为给定参考输入；分别为q_d的一阶、二阶时间导数；K_p为比例增益矩阵；K_D为微分增益矩；G(q)为重力向量；为离心力矩阵；F_v为正常数的对角矩阵，表示关节粘滞摩擦系数。

3.根据权利要求1所述的自平衡机器人的控制方法，其特征在于，步骤S3中提取所述自平衡机器人的视觉设备拍摄的双目图像中障碍物的特征点包括利用Harris角点特征提取方法、FAST角点特征提取方法或SURF算法。

4.根据权利要求1所述的自平衡机器人的控制方法，其特征在于，步骤S4中构建所述障碍物与所述自平衡机器人的三维模型的具体步骤为：

接收所述自平衡机器人的视觉设备拍摄的双目图像，对所述双目图像进行像素分割，获得所述双目图像的全部像素点分割结果；

响应于所述双目图像的全部像素点分割结果，通过SAD双目立体算法匹配得到深度图信息，运用坐标变换，将所述深度图信息统一转换至相机坐标系下，得到当前帧3D点云；

将所述相机坐标系下的当前帧3D点云统一至世界坐标系下，将所述世界坐标系下的当前帧3D点云插入世界坐标系下的全局地图中，构建所述障碍物的三维模型和机器人模型。

5.根据权利要求4所述的自平衡机器人的控制方法，其特征在于，步骤S5中基于所述三维模型构建虚拟地图的具体为使用opencv对所述三维模型实时构建虚拟地图，并将在所述相机坐标系中的坐标转化为机器人坐标系的坐标以获得所述自平衡机器人与所述障碍物的相对位置信息。