[发明专利]一种胶囊机器人滚转偏移角估计方法

权利要求书

1.一种胶囊机器人滚转偏移角估计方法，其特征在于，包括以下步骤：

步骤一、标定相机正向等效点；

具体为：找出相机输出正置图像的方向，记为相机正向，将相机安装进胶囊机器人时，如果相机中心与胶囊中心不重合，使相机正向与周向横截面上相机中心指向胶囊机器人中心的方向相同；如果相机中心与胶囊中心重合，则相机正向随意；然后，记录相机正向在胶囊机器人壳体的交点位置，此交点沿胶囊轴线方向在胶囊中心横截面A上的投影点记为相机正向等效点；

步骤二、在胶囊机器人的外壳沿周向设置多个力传感器，确保力传感器在胶囊机器人横截面的相邻两个投影点与横截面圆心所构成的圆心角不小于90°；

步骤三、通过力传感器测得胶囊机器人外壳所受力，并将各力感应点沿胶囊轴线投影在胶囊中心横截面A上；

步骤四、根据投影在平面A上的感应力确定等效力感应点；

具体为：若力感应点中只有一个感应力为有效力，则该力感应点沿胶囊轴线方向在胶囊中心横截面A上的投影点记为等效力感应点；若力感应点中有两个以上的感应力为有效力，首先把所有有效力感应点向A平面投影，再确定这些有效力投影点所覆盖的胶囊壳体部分圆弧，以距离该圆弧中心最近的投影点记为等效力感应点，所述有效力指不小于设定阈值的感应力；

步骤五、计算等效力感应点与相机正向等效点之间的圆心角，即为胶囊机器人滚转偏移角。