Yanshee 传感器 6 让机器人摔倒后自动爬起
通过本课程，你将学习运动传感器（即 陀螺仪+加速度）的工作原理，然后使用它来做简单的姿态判断。我们机器人内置运动传感器，所以不需借助外接设备就可知晓自己的姿态（平躺、趴倒还是站立等）。当机器人知道自己的姿态后，就可以做一些事情。例如，在本课程中，我们通过编程让机器人发现自己处于摔倒状态后，就会自动爬起来。
自然界中人类或大多数动物们靠小脑和神经系统等来达到维持身体的平衡的感知目的，当一个人摔倒之前他能意识到自己的身体失去了平衡，并通过视觉、触觉、大脑神经系统等得知自己可能要摔倒的信息，最后保持身体平衡。而这种判断自己的身体是否摔倒的能力，机器人身上也想拥有，于是我们就有了姿态控制概念的产生。姿态控制目前已经被应用于诸多场合，比如：平衡车、无人机、机器人等等领域。而姿态控制的核心理念就是通过读取传感器的值来做相应的算法，进而实现对应的场景功能。于是陀螺仪就应用而生了。人们通常利用陀螺仪这种传感器的值来判断电子设备的位置姿态信息，进而获得可利用的交互信息来完成相应功能的控制。于是本节的重点就是介绍平衡传感器—陀螺仪等的原理和使用。我们将揭开这些重要设备的神秘面纱。而我们Yanshee机器人又是如何知道自己是不是摔倒的呢？让我们一起来学习一下吧。
陀螺仪是一种用来感測与维持方向的裝置，基于角动量守恒的理论设计出来的，它的测量物理量是偏转、倾斜时的转动角速度。 陀螺仪应用广泛，常见的应用有以下这些：
（1）动作感应的GUI：通过小幅度的倾斜，偏转手机，实现菜单，目录的选择和操作的执行。（比如前后倾斜手机，实现通讯录条目的上下滚动；左右倾斜手机，实现浏览页面的左右移动或者页面的放大或缩小。） （2）计算行走步数：手环、手机或者智能手表的计算每天行走的步数。 （3）拍照时的图像稳定，防止手的抖动对拍照质量的影响。在按下快门时，记录手的抖动动作，将手的抖动反馈给图像处理器，可以抓到更清晰稳定的图片 （4）GPS的惯性导航：当汽车行驶到隧道或城市高大建筑物附近，没有GPS讯号时，可以通过陀螺仪来测量汽车的偏航或直线运动位移，从而继续导航。 （5）通过动作感应控制游戏：这也是Steve重点介绍的，也是可以给APP开发者更多创新空间的地方。开发者可以通过陀螺仪对动作检测的结果（3D范围内手机的动作），去实现对游戏的操作。比如，把你的手机当作一个方向盘，你的手机屏幕上是一架飞行中的战斗机，只要你上下，左右地倾斜手机，飞机就可以做上下，左右的动作。 它们都需要通过陀螺仪来感知的，为了测量更加准确可能同时还使用加速度计对数据进行融合。
是速度变化量与发生这一变化所用时间的比值Δv/Δt，是描述物体速度变化快慢的物理量。
用来确定定点转动刚体位置的3个一组独立的角度参量，由章动角θ、旋进角（即进动角）ψ和自转角j组成，因欧拉首先提出而得名。
也叫做微电子机械系统、微系统、微机械等，指尺寸在几毫米乃至更小的高科技装置。MEMS是在微电子技术（半导体制造技术）基础上发展起来的，融合了光刻、腐蚀、薄膜、LIGA、硅微加工、非硅微加工和精密机械加工等技术制作的高科技电子机械器件。
我们Yanshee机器人内置的运动传感器型号是MPU9250，它内部集成了MPU6050和AK8963，其中MPU6050为6轴运动传感器（集成了3轴MEMS陀螺仪，3轴MEMS加速度计），AK8963为3轴电子罗盘（本课程未用到，有兴趣可以查资料了解）。
Yanshee机器人一台
树莓派Rasbian系统、linux系统、python环境
在一些对角度要求不高的场合，要得到角度，只用加速度计就可以了，比如做一个自平衡小车。可是要得到一个更加精确的角度，就需要用到陀螺仪了，比如四旋翼飞行器。陀螺仪测量的是角度的变化率，对这个变化率积分，就可以得到角度值。
首先我们知道重力加速度可以分解成x, y, z三个方向的分加速度。而加速度计可以测量某一时刻x, y, z三个方向的加速度值。利用各个方向的分量与重力加速度的比值来计算出小车大致的倾角。其实在自平衡小车在运动的时候，加速度计测出的结果并不是非常精确。因为大家在高中物理的时候都学过，物体时刻都会受到地球的万有引力作用产生一个向下的重力加速度，而小车在动态时，受电机的作用肯定有一个前进或者后退方向的作用力，而加速度计测出的结果是，重力加速度与小车运动加速度合成得到一个总的加速度在三个方向上的分量。这是加速度计测不准的一个原因。我们这里先理想化加速度计不受外界干扰。
下边我们就开始分析从加速度得到角度的方法。如下图，把加速度计平放，分别画出xyz轴的方向。这三个轴 就是我们后边分析所要用到的坐标系。
假设MPU6050安装在自平衡车上时也是这样的水平安装在小车底盘上的，假设两个车轮安装时车轴和y轴在一条直线上。那么小车摆动时，参考水平面就是桌面，并且车轴（y轴）与桌面始终是平行的，小车摆动和移动过程中y轴与桌面的夹角是不会发生变化的，一直是0度。发生变化的是x轴与桌面的夹角以及z轴与桌面的夹角，而且桌面与x轴z轴夹角变化度数是一样的。所以我们只需要计算出x轴和z轴中任意一个轴的夹角就可以反映出小车的倾斜的情况了。
为了方便分析，由于y轴与桌面夹角始终不变，我们从y轴的方向俯看下去，那么这个问题就会简化成只有x轴和z轴的二维关系。假设某一时刻小车上加速度计（MPU6050）处于如下状态，下图是我们看到简化后的模型。
在这个图中，y轴已经简化和坐标系的原点o重合在了一起。我们来看看如何计算出小车的倾斜角，也就是与桌面的夹角a。上图g是重力加速度，gx、gz分别是g在x轴和z轴的分量。 由于重力加速度是垂直于水平面的，得到： 角a+角b=90度 X轴与y轴是垂直关系，得到： 角c+角b=90度 于是轻松的就可以得出：角a=角c 根据力的分解，g、gx、gz三者构成一个长方形，根据平行四边形的原理可以得出：角c=角d 所以计算出角度d就等效于计算出了x轴与桌面的夹角a。前边已经说过gx是g在x轴的分量，那么根据正弦定理就可以得出： Sind=gx/g 所以：a = d = asin(gx/g）
陀螺仪读出的是角速度，角速度乘以时间，就是转过的角度。把每次计算出的角度做累加就会等到当前所在位置的角度。先看下图：
假设最初陀螺仪是与桌面平行，单片机每tms读一次陀螺仪的角速度，当读了三次角速度以后z轴转到上图的位置，则在这段时间中转过的角度为x： 角x=角1+角2+角3 假设从陀螺仪读出的角速度为w，那总角度为：X=(w1t1+w2t2+w3t3)/1000 假设经过n次，那么总的角度如下：X=(w1t1+w2t2+w3t3+…+wn*tn)/1000 实际上这就是一个积分过程，其实这种计算出来的角度也存在一定的误差，而且总的角度是经过多次相加得到的，这样误差就会越积累越大，最终导致计算出的角度与实际角度相差很大。我们可以使用比较常见的卡尔曼滤波（当然还有其他融合方式）把加速度计读出的角度结合在一起，使计算出的角度更准确。
MPU9250在机器人中的位置示意（上图）
MPU9250的X、Y、Z轴示意（上图） 我们使用的MPU9250内置有DMP姿态融合器，所以可以很方便的得到三维角度值。
基本数据读取，我们通过python编程对机器人陀螺仪传感器的欧拉角进行读取，执行相关代码：
#!/usr/bin/python
# _*_ coding: utf-8 -*-
import time
import RobotApi as api
api.ubtRobotInitialize()
#------------------------------Connect----------------------------------------
ret = api.ubtRobotConnect("SDK", "1", "127.0.0.1")
if (0 != ret):
print ("Can not connect to robot %s" % robotinfo.acName)
exit(1)
#---------------------------Read Sensor Value-------------------------------
gyro_size = 96
gyro_sensor = api.UBTEDU_ROBOTGYRO_SENSOR_T()
while True:
time.sleep(2)
ret = api.ubtReadSensorValue("gyro",gyro_sensor,gyro_size)
if ret != 0:
print("Can not read Sensor value. Error code: %d" % (ret))
else:
print("angle_x: %f" % (gyro_sensor.dEulerxValue))
print("angle_y: %f" % (gyro_sensor.dEuleryValue))
print("angle_z: %f" % (gyro_sensor.dEulerzValue))
#---------------------------Disconnect--------------------------------------
api.ubtRobotDisconnect("SDK","1","127.0.0.1")
api.ubtRobotDeinitialize()
将上面代码保存到query_gyro.py中，按照SDK使用说明，完成相应的安装步骤。
后仰时运行结果：
前趴时运行结果：
通过上面的测试，我们知道当机器人前趴时，角度约为0°左右，机器人后仰时角度约为180°左右，考虑到机器人摔倒时的自身结构差异或地面不平，我们简单认为：当X轴角度在-20°~20°时，机器人前趴摔倒；当X轴角度在<-160°或>160°, 机器人后仰摔倒。
当判断机器前趴摔倒时我们执行getup_in_front动作（内置动作），当机器人后仰摔倒时我们执行内置动作getup_in_back动作（内置动作），从而爬起来。
#!/usr/bin/python
# _*_ coding: utf-8 -*-
import time
import RobotApi as api
api.ubtRobotInitialize()
#------------------------------Connect----------------------------------------
ret = api.ubtRobotConnect("SDK", "1", "127.0.0.1")
if (0 != ret):
print ("Can not connect to robot %s" % robotinfo.acName)
exit(1)
#---------------------------Read Sensor Value-------------------------------
isInterrputed = 1
gyro_size = 96
gyro_sensor = api.UBTEDU_ROBOTGYRO_SENSOR_T()
while True:
time.sleep(2)
ret = api.ubtReadSensorValue("gyro",gyro_sensor,gyro_size)
if ret != 0:
print("Can not read Sensor value. Error code: %d" % (ret))
continue
else:
print("Read dEulerxValue : %f" % (gyro_sensor.dEulerxValue))
print("Read dEuleryValue : %f" % (gyro_sensor.dEuleryValue))
print("Read dEulerzValue : %f" % (gyro_sensor.dEulerzValue))
if gyro_sensor.dEulerxValue > 160 or gyro_sensor.dEulerxValue < -160:
print 'Detected fall backward, I am going to get up'
ret = api.ubtVoiceTTS(isInterrputed, '哎呀，不好意思后脑勺着地了。赶快爬起来。')
if ret != 0:
print("Can not play TTS voice. Error code: %d" % ret)
exit(3)
api.ubtStartRobotAction("getup_in_back", 1)
elif gyro_sensor.dEulerxValue > -20 and gyro_sensor.dEulerxValue < 20:
print 'Detected fall forward, I am going to get up'
ret = api.ubtVoiceTTS(isInterrputed, '哎呀，不好意思脸着地了，没有人看见吧。')
if ret != 0:
print("Can not play TTS voice. Error code: %d" % ret)
exit(3)
api.ubtStartRobotAction("getup_in_front", 1)
#---------------------------Disconnect--------------------------------------
api.ubtRobotDisconnect("SDK","1","127.0.0.1")
api.ubtRobotDeinitialize()
将上面代码保存到fall_manager.py中,运行后，将机器人放到，看它是不是能自动爬起来呢？
上面我们已经实现了摔倒后自己爬起来的功能，但有时候会存在一个问题：如果机器人是被拿起来过程中，某个阶段刚好处于摔倒时的姿态，它也会执行爬起动作，这样很容易夹伤手。 被拿起还是摔倒的区别在于速度，通常情况下自己摔倒的话速度大。MP9250可以检测三个值：陀螺仪、加速度、磁场。所以用陀螺仪值和加速度值都可以用来判断，这里就不讲了，有兴趣的话可以自己多测试两种情况下的差异，然后编程实现。