室内定位之行人航位推算（PDR）
室内定位之行人航位推算PDR
- 内容简介
- 惯性导航概述
- PDR算法概述
- 安卓手机的传感器使用
- 方向传感器
- 陀螺仪传感器
- 磁场传感器
- 重力传感器
- 线性加速度传感器
- 行人行位推算
- MainActivity
- Location
- StepDetectionHandler
- DeviceAttitudeHandler
- StepPositioningHandler
- 展望
内容简介
最近在做室内定位的项目，看到网上有关于PDR的介绍，自己动手实现了一下，特此记录。
本篇文章最终是利用手机的方向传感器和加速度传感器实现行人的室内定位。只是初步实现，使用华为荣耀8手机，方向传感器的误差20°左右，加速度传感器用来计步，不是很准，作者测试过程发现10步可能会对3步左右，相关参数等待有心的你去找出来吧。
本文主要参考了一下文章。
1.惯性室内导航入门到精通系列文章，主要介绍了PDR的相关概念、算法和部分源码，但可惜作者没有给出PDR的完整项目源码，（或者我没有找到，希望原文作者原谅）
链接：https://blog.csdn.net/qq_35651984/article/details/82845525
2.SensorManager传感器使用小结，主要介绍了安卓手机的相关传感器的使用方法，并给出了完整的项目源码。
链接：https://blog.csdn.net/b2569449528/article/details/79643158
3.Android 传感器开发详解，详细介绍了安卓手机各种传感器的使用及数据的物理含义的介绍
链接：https://blog.csdn.net/Airsaid/article/details/52902299
4.PDR，GitHub上一个开源的PDR项目，但是由于手机版本的问题，不能直接使用，我也是基于这个代码进行修改。
链接：https://github.com/aureliencousin/PDR
惯性导航概述
惯性导航是一种不借助外力（接收本体之外的信号）的自主性导航。你只要知道自己的初始位置，知道自己的初始朝向，知道自己每一时刻如何改变了朝向，知道自己每一时刻相对朝向是怎样走的，这样就能得到每时每刻自己的位置。
PDR算法概述
PDR算法能在无信标条件下，通过加速度三轴加速度值和方向角感知行人在行进过程中的加速度和方向角，并利用这些数据对行走路线进行相对定位，从而达到对行人进行定位跟踪的目的。其中主要涉及的过程有步态检测和方向计算。PDR原理图如下：
已知初始位置为（E0,N0）步长为d，航向角为 θ \theta θ，则下一步的位置为
{ E k = E 0 + ∑ n = 1 k d n sin θ n N k = N 0 + ∑ n = 1 k d n cos θ n \left\{\begin{array}{l}{E_{k}=E_{0}+\sum_{n=1}^{k} d_{n} \sin \theta_{n}} \\ {N_{k}=N_{0}+\sum_{n=1}^{k} d_{n} \cos \theta_{n}}\end{array}\right. {Ek=E0+∑n=1kdnsinθnNk=N0+∑n=1kdncosθn
在进行PDR算法时候，需要知道步长以及方向。通过方向传感器获得手机的方向，通过加速度传感器进行计步，有些手机已经有计步传感器，只需要直接调用即可，由于本人测试的手机并没有，所以这种方法暂且不表，如果你的手机可以直接调用计步传感的话，相信聪明如你，一定有办法的。
安卓手机的传感器使用
SensorManager是Android手机传感器的管理器类。
手机中常用的传感器
在Android2.3 gingerbread系统中，google提供了11种传感器供应用层使用。
#define SENSOR_TYPE_ACCELEROMETER 1 //加速度
#define SENSOR_TYPE_MAGNETIC_FIELD 2 //磁力
#define SENSOR_TYPE_ORIENTATION 3 //方向
#define SENSOR_TYPE_GYROSCOPE 4 //陀螺仪
#define SENSOR_TYPE_LIGHT 5 //光线感应
#define SENSOR_TYPE_PRESSURE 6 //压力
#define SENSOR_TYPE_TEMPERATURE 7 //温度
#define SENSOR_TYPE_PROXIMITY 8 //接近
#define SENSOR_TYPE_GRAVITY 9 //重力
#define SENSOR_TYPE_LINEAR_ACCELERATION 10 //线性加速度
#define SENSOR_TYPE_ROTATION_VECTOR 11 //旋转矢量
通过getSystemService方法获得SensorManager对象。而
// 获取传感器管理对象
SensorManager mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
通过SensorManager对象又可以得到指定的系统传感器，值得说明的是使用系统传感器服务不需要任何权限。
Sensor gSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);//重力传感器
Sensor mSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);//加速度传感器
Sensor oSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ORIENTATION);//方向传感器
Sensor gySensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);//陀螺仪传感器
在onResume()方法中监听传感器传回的数据：
@Override
protected void onResume() {
super.onResume();
// 为加速度传感器注册监听器
mSensorManager.registerListener(new SensorEventListener() {
// 当传感器的值改变的时候回调该方法
@Override
public void onSensorChanged(SensorEvent event) {
}
// 当传感器精度发生改变时回调该方法
@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
}
}, mSensor, SensorManager.SENSOR_DELAY_GAME);
}
其中，registerListener(SensorEventListener listener, Sensor sensor,int samplingPeriodUs)的三个参数说明如下：
listener：监听传感器时间的监听器，该监听器需要实现SensorEventListener接口。
sensor：传感器对象。
samplingPeriodUs：指定获取传感器频率，一共有如下几种：
- SensorManager.SENSOR_DELAY_FASTEST：最快，延迟最小，同时也最消耗资源，一般只有特别依赖传感器的应用使用该频率，否则不推荐。
- SensorManager.SENSOR_DELAY_GAME：适合游戏的频率，一般有实时性要求的应用适合使用这种频率。
- SensorManager.SENSOR_DELAY_NORMAL：正常频率，一般对实时性要求不高的应用适合使用这种频率。
- SensorManager.SENSOR_DELAY_UI：适合普通应用的频率，这种模式比较省电，而且系统开销小，但延迟大，因此只适合普通小程序使用。
并在onStop()方法中取消注册
@Override
protected void onStop() {
super.onStop();
// 取消监听
mSensorManager.unregisterListener(this);
}
下面一个列子，演示了完整的监听加速度传感器的开发，并将结果显示到屏幕上，布局文件只是从上向下顺序的几个TextView，无须额外的权限。
public class MainActivity extends AppCompatActivity implements SensorEventListener{
private SensorManager mSensorManager;
private TextView mTxtValue1;
private TextView mTxtValue2;
private TextView mTxtValue3;
private TextView mTxtValue4;
private TextView mTxtValue5;
private TextView mTxtValue6;
private TextView mTxtValue7;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
mTxtValue1 = (TextView) findViewById(R.id.txt_value1);
mTxtValue2 = (TextView) findViewById(R.id.txt_value2);
mTxtValue3 = (TextView) findViewById(R.id.txt_value3);
mTxtValue4 = (TextView) findViewById(R.id.txt_value4);
mTxtValue5 = (TextView) findViewById(R.id.txt_value5);
mTxtValue6 = (TextView) findViewById(R.id.txt_value6);
mTxtValue7 = (TextView) findViewById(R.id.txt_value7);
// 获取传感器管理对象
mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
}
@Override
protected void onResume() {
super.onResume();
// 为加速度传感器注册监听器
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_GAME);
// 为方向传感器注册监听器
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ORIENTATION), SensorManager.SENSOR_DELAY_GAME);
// 为陀螺仪传感器注册监听器
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_GAME);
// 为磁场传感器注册监听器
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_GAME);
// 为重力传感器注册监听器
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY), SensorManager.SENSOR_DELAY_GAME);
// 为线性加速度传感器注册监听器
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION), SensorManager.SENSOR_DELAY_GAME);
}
@Override
protected void onStop() {
super.onStop();
// 取消监听
mSensorManager.unregisterListener(this);
}
// 当传感器的值改变的时候回调该方法
@Override
public void onSensorChanged(SensorEvent event) {
float[] values = event.values;
// 获取传感器类型
int type = event.sensor.getType();
StringBuilder sb;
switch (type){
case Sensor.TYPE_ACCELEROMETER:
sb = new StringBuilder();
sb.append("加速度传感器返回数据：");
sb.append("\nX方向的加速度：");
sb.append(values[0]);
sb.append("\nY方向的加速度：");
sb.append(values[1]);
sb.append("\nZ方向的加速度：");
sb.append(values[2]);
mTxtValue1.setText(sb.toString());
break;
case Sensor.TYPE_ORIENTATION:
sb = new StringBuilder();
sb.append("\n方向传感器返回数据：");
sb.append("\n绕Z轴转过的角度：");
sb.append(values[0]);
sb.append("\n绕X轴转过的角度：");
sb.append(values[1]);
sb.append("\n绕Y轴转过的角度：");
sb.append(values[2]);
mTxtValue2.setText(sb.toString());
break;
case Sensor.TYPE_GYROSCOPE:
sb = new StringBuilder();
sb.append("\n陀螺仪传感器返回数据：");
sb.append("\n绕X轴旋转的角速度：");
sb.append(values[0]);
sb.append("\n绕Y轴旋转的角速度：");
sb.append(values[1]);
sb.append("\n绕Z轴旋转的角速度：");
sb.append(values[2]);
mTxtValue3.setText(sb.toString());
break;
case Sensor.TYPE_MAGNETIC_FIELD:
sb = new StringBuilder();
sb.append("\n磁场传感器返回数据：");
sb.append("\nX轴方向上的磁场强度：");
sb.append(values[0]);
sb.append("\nY轴方向上的磁场强度：");
sb.append(values[1]);
sb.append("\nZ轴方向上的磁场强度：");
sb.append(values[2]);
mTxtValue4.setText(sb.toString());
break;
case Sensor.TYPE_GRAVITY:
sb = new StringBuilder();
sb.append("\n重力传感器返回数据：");
sb.append("\nX轴方向上的重力：");
sb.append(values[0]);
sb.append("\nY轴方向上的重力：");
sb.append(values[1]);
sb.append("\nZ轴方向上的重力：");
sb.append(values[2]);
mTxtValue5.setText(sb.toString());
break;
case Sensor.TYPE_LINEAR_ACCELERATION:
sb = new StringBuilder();
sb.append("\n线性加速度传感器返回数据：");
sb.append("\nX轴方向上的线性加速度：");
sb.append(values[0]);
sb.append("\nY轴方向上的线性加速度：");
sb.append(values[1]);
sb.append("\nZ轴方向上的线性加速度：");
sb.append(values[2]);
mTxtValue6.setText(sb.toString());
break;
}
}
// 当传感器精度发生改变时回调该方法
@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
}
}
方向传感器
方向传感器用于感应手机的摆放位置，它给我们返回了三个角度，这三个角度可以确定手机的摆放状态。
- 第一个角度：表示手机顶部朝向与正北方的夹角。当手机绕着Z轴旋转时，该角度值发生改变。比如，当该角度为0度时，表明手机顶部朝向正北；该角度为90度时，表明手机顶部朝向正东；该角度为180度时，表明手机朝向正南；该角度为270度时，表明手机顶部朝向正西。
- 第二个角度：表示手机顶部或尾部翘起的高度。当手机绕着X轴倾斜时，该角度值发生变化，该角度的取值范围是-180~180度。假设手机屏幕朝上水平放在桌子上，如果桌子是完全水平的，该角度值应该是0度。假如从手机顶部开始抬起，直到将手机沿X轴旋转180度(屏幕向下水平放在桌子上)，在这个旋转的过程中，该角度值会从0度变化到-180度。也就是说，从手机顶部抬起时，该角度的值会逐渐减少，直到等于-180度；如果从手机底部开始抬起，直到将手机沿X轴旋转180度(屏幕向下水平放在桌子上)，该角度的值会从0度变化到180度，也就是说，从手机底部抬起时，该角度的值会逐渐增大，直到等于180度。
- 第三个角度：表示手机左侧或右侧翘起的角度。当手机绕着Y轴倾斜时，该角度值发生改变。该角度的取值范围是：-90~90度。假设将手机屏幕朝上水平放在桌面上，如果桌面是完全水平的，该角度应该为0度。如果将手机从左侧开始慢慢抬起，知道将手机沿着Y轴旋转90度(手机与桌面垂直)，在这个旋转的过程中，该角度值会从0度变化到-90度。也就是说，从手机左侧开始抬起时，该角度的值会逐渐的减少，知道等于-90度。如果从手机的右侧抬起，则刚好相反，会从0度变化，直到90度。
通过在应用程序中使用方向传感器，可以实现如：地图导航、水平仪、指南针等应用。
陀螺仪传感器
陀螺仪传感器用于感应手机的旋转速度。陀螺仪传感器给我们返回了当前设备的X、Y、Z三个坐标轴（坐标系统与加速度传感器一模一样）的旋转速度。旋转速度的单位是弧度/秒，旋转速度为：
正值代表逆时针旋转，负值代表顺时针旋转。关于返回的三个角速度说明如下：
- 第一个值：代表该设备绕X轴旋转的角速度。
- 第二个值：代表该设备绕Y轴旋转的角速度。
- 第三个值：代表该设备绕Z轴旋转的角速度。
磁场传感器
磁场感应器主要读取设备周围的磁场强度。即便是设备周围没有任何直接的磁场，设备也会始终处于地球的磁场中，除非你不在地球。。随着手机设备摆放状态的改变，周围磁场在手机的X、Y、Z方向上的影响也会发生改变。磁场传感器会返回三个数据，分别代表周围磁场分解到X、Y、Z三个方向的磁场分量，磁场数据的单位是微特斯拉。
重力传感器
重力传感器会返回一个三维向量，这个三维向量可显示重力的方向和强度。重力传感器的坐标系统和加速度传感器的坐标系统相同。
线性加速度传感器
线性加速度传感器返回一个三维向量显示设备在各个方向的加速度（不包含重力加速度）。线性加速度传感器的坐标系统和加速度传感器的坐标系统相同。
线性加速度传感器、重力传感器、加速度传感器，这三者输出值的关系如下：
加速度传感器 = 重力传感器 + 线性加速度传感器。
行人行位推算
接下来进入正题，使用方向传感器和线性加速度传感器进行行人行位推算。参考https://github.com/aureliencousin/PDR，原文需要使用谷歌地图，由于只想验证行人航位算法，所以这里做了改动，简化不必要的代码，重点几种行人航位推算算法。
源码比较简单，代码的主要结构如下图所示。
其中，
MainActivity类是主界面，用于显示信息；
Location类，用来记录当前所在的位置，特别注意这个类不是Android自带的类；
StepDetectionHandler类用于步态检测；
DeviceAttitudeHandler类用于获取当前的航向角（手机头的指向）；
StepPositioningHandler类主要用于根据上一步的位置、步态检测、航向角计算当前位置。
MainActivity
主界面即提供的代码MainActivity，没有使用地图或其它绘图控价，而只是显示一个坐标、当前行走的方向（走动时手机头的指向）、走动的步数。
在onCreate中需要初始化步态检测、航向角检测、位置计算三个类及相关的显示控件并监听步态检测的接口，初始位置设为（0,0）
在onResume()中，启动步态检测和航向角检测；
在onStop()中，停止步态检测和航向角检测；
当步态检测检测到走到了一步通过监听接口，回调到MainActivity时，将会调用航向角检测的结果获得最新的航向角作为当前的航向角并调用位置计算类获得当前位置。
package com.example.sensortest;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.support.v7.app.AppCompatActivity;
import android.os.Bundle;
import android.util.Log;
import android.widget.TextView;
import com.example.sensortest.pdr.DeviceAttitudeHandler;
import com.example.sensortest.pdr.StepDetectionHandler;
import com.example.sensortest.pdr.StepPositioningHandler;
import com.example.sensortest.bean.Location;
public class MainActivity extends AppCompatActivity {
private SensorManager mSensorManager;
private TextView mTxtValue1;
private TextView mTxtValue2;
private TextView mTxtValue3;
StepDetectionHandler sdh;
StepPositioningHandler sph;
DeviceAttitudeHandler dah;
boolean isWalking = false;
Location lKloc;
int stepCounter = 0;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
mTxtValue1 = (TextView) findViewById(R.id.txt_value1);
mTxtValue2 = (TextView) findViewById(R.id.txt_value2);
mTxtValue3 = (TextView) findViewById(R.id.txt_value3);
lKloc = new Location(0.0, 0.0);
SensorManager sm = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
sdh = new StepDetectionHandler(sm);
sdh.setStepListener(mStepDetectionListener);
dah = new DeviceAttitudeHandler(sm);
sph = new StepPositioningHandler();
sph.setmCurrentLocation(lKloc);
isWalking = true;
}
@Override
protected void onResume() {
super.onResume();
sdh.start();
dah.start();
}
@Override
protected void onStop() {
super.onStop();
sdh.stop();
dah.stop();
}
private StepDetectionHandler.StepDetectionListener mStepDetectionListener = new StepDetectionHandler.StepDetectionListener() {
public void newStep(float stepSize) {
stepCounter++;
Location newloc = sph.computeNextStep(stepSize, dah.orientationVals[0]);
Log.d("LATLNG", newloc.getxAxis() + " " + newloc.getyAxis()+ " " + dah.orientationVals[0]);
if (isWalking) {
mTxtValue1.setText("最新位置：" + String.valueOf(newloc.getxAxis())+","+String.valueOf(newloc.getyAxis()));
mTxtValue2.setText(String.valueOf(360-dah.orientationVals[0]-90));
mTxtValue3.setText("走过的步数：" + String.valueOf(stepCounter));
}
}
};
}
Location
这是一个实体类，只有x,y两个属性及相关方法。
package com.example.sensortest.bean;
public class Location {
/*x轴坐标*/
private Double xAxis;
/*y轴坐标*/
private Double yAxis;
public Location(Double xAxis, Double yAxis) {
super();
this.xAxis = xAxis;
this.yAxis = yAxis;
}
public Double getxAxis() {
return xAxis;
}
public void setxAxis(Double xAxis) {
this.xAxis = xAxis;
}
public Double getyAxis() {
return yAxis;
}
public void setyAxis(Double yAxis) {
this.yAxis = yAxis;
}
}
StepDetectionHandler
步态检测类，用来检测是否走了一步。在初始化部分确定监听的传感器类型为线性传感器。判断是否走了一步的标准是线性加速度传感器的沿y轴方向（手机头指向的方向）的加速度值变化是否大于1，如果大于1的话，将通过接口将检测结果返回MainActivity。
主要注意的是这里判断是否走了一步的标准并不唯一，如果聪明的你有更好的方法，不防也分享出来。另外，关于一步走了多长，也在这个类中设置，我这里设置的是0.8米。
package com.example.sensortest.pdr;
import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
public class StepDetectionHandler extends Activity implements
SensorEventListener {
SensorManager sm;
Sensor sensor;
private StepDetectionListener mStepDetectionListener;
int step = 0;
public StepDetectionHandler(SensorManager sm) {
super();
this.sm = sm;
sensor = sm.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
}
public void start() {
sm.registerListener(this, sensor, SensorManager.SENSOR_DELAY_NORMAL);
}
public void stop() {
sm.unregisterListener(this);
}
@Override
public void onAccuracyChanged(Sensor arg0, int arg1) {
// TODO Auto-generated method stub
}
@Override
public void onSensorChanged(SensorEvent e) {
float y;
if (e.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) {
y = e.values[1];
//
if (y > 1 && mStepDetectionListener != null) {
onNewStepDetected();
}
}
}
public void onNewStepDetected() {
float distanceStep = 0.8f;
step++;
mStepDetectionListener.newStep(distanceStep);
}
public void setStepListener(StepDetectionListener listener) {
mStepDetectionListener = listener;
}
public interface StepDetectionListener {
public void newStep(float stepSize);
}
}
DeviceAttitudeHandler
航向检测类，用来检测行人走动的方向。在初始化部分确定监听的传感器类型为线性传感器。
需要说明的是这里的航向角实际上是手机头的方向，在方向的范围为（0o,360o]，其中，正北方向为360o，顺时针方向增大。由于的大家的建的相对坐标系不尽相同，大家主要在StepPositioningHandler类中计算当前位置时，记得把角度进行调整。
package com.example.sensortest.pdr;
import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
public class DeviceAttitudeHandler extends Activity implements
SensorEventListener {
SensorManager sm;
Sensor sensor;
public float[] orientationVals = new float[3];
private final int sensorType = Sensor.TYPE_ORIENTATION;
public DeviceAttitudeHandler(SensorManager sm) {
super();
this.sm = sm;
sensor = sm.getDefaultSensor(sensorType);
}
public void start() {
sm.registerListener(this, sensor, SensorManager.SENSOR_DELAY_NORMAL);
}
public void stop() {
sm.unregisterListener(this);
}
@Override
public void onAccuracyChanged(Sensor arg0, int arg1) {
// TODO Auto-generated method stub
}
public void onSensorChanged(SensorEvent event) {
orientationVals[0] = (float) event.values[0];
orientationVals[1] = (float) event.values[1]; // axe de rotation
orientationVals[2] = (float) event.values[2];
}
}
StepPositioningHandler
用于计算当前的坐标位置类。需要注意的DeviceAttitudeHandler类传来的航向角是角度制的，但Math.sin()和Math.cos()的参数是弧度制的，代码中已经做了转换。但在这里还是需要提醒大家。再唠叨一句，相对坐标系建的不同，记得把角度调整一下。
package com.example.sensortest.pdr;
import com.example.sensortest.bean.Location;
public class StepPositioningHandler {
private Location mCurrentLocation;
private static final int eRadius = 6371000; //rayon de la terre en m
public Location getmCurrentLocation() {
return mCurrentLocation;
}
public void setmCurrentLocation(Location mCurrentLocation) {
this.mCurrentLocation = mCurrentLocation;
}
public Location computeNextStep(float stepSize,float bearing) {
bearing = (float) (bearing * Math.PI / 180);
Location newLoc = new Location(mCurrentLocation.getxAxis(), mCurrentLocation.getyAxis());
float angDistance = stepSize / eRadius;
double newX = mCurrentLocation.getxAxis() - stepSize*Math.sin(bearing);
double newY = mCurrentLocation.getyAxis() - stepSize*Math.cos(bearing);
newLoc.setxAxis(newX);
newLoc.setyAxis(newY);
mCurrentLocation = newLoc;
return newLoc;
}
}
展望
接下来，将会介绍使用的蓝牙进行室内定位的实现方法，最后，使用粒子滤波算法将结合行人航位推算算法和蓝牙定位的结果。期待下一次相见。