自动跟随机器人教程（六）软件部分 树莓派 声源定位

接下来的部分比较有趣，是本项目提出的机器人和市场上大多数机器人不同的地方。它除了能够通过摄像头图像和超声波数据来做一些自动控制外，还使用了Respeaker的4通道麦克风阵列。使用这个模块的好处是，在摄像头所没有覆盖到的区域，可以通过对人声的测向来获取人员的大致位置。
声源测向的原理在好多地方有所应用，可以把这个模块看作一个简单的被动雷达。虽然目前的算法（互相关）比较简单，但是以后也可以扩展为高级的算法，所有的DSP操作都可以在树莓派上实现，而不是专用的DSP芯片，所以可塑性很强。
测向原理比较简单，基本上就是把4个麦克风中对角两个一组取出来。由于声波到达4个麦克风上的信号波形比较类似，只是幅度和时间上有一些变化。通过计算声波到达这些麦克风上的时间差，再把这个时间差与0度或者180度时所造成的时间差的最大值进行比较，经过一些三角运算就能测出发射源相对这2个麦克风的角度。再把发射源相对另一组2个麦克风的角度综合比较、换算一下，就能得到更为精确的角度。（理论上只有2个麦克风也能在0～180度范围内测向，同样就用这个respeaker 4mic模块就能实现，只是四通道时可以测0～360度的范围，并且精度更高。）
前面说了通过各麦克风上的几个信号之间的时间差可以测出声源位置，那么这个时间差时如何测出的呢？目前使用的是互相关算法，在通信领域无论是模拟信号还是数字信号都会经常用到，把两个信号经过换元和卷积以后就能得到互相关函数，互相关函数值最大时的坐标位置就是延迟大小了，这个计算一般用FFT变换到频域后用乘法代替，然后在做FFT逆变换得到。
上面这个算法可以说是本项目中理论难度最高的部分，可能业余爱好者比较难以理解，不过幸运的是Respeaker已经给出了4通道测向的例程，只需要把例子稍作修改，用测得的角度去控制最终的旋转方向和大小就行了。
另外本人还改掉了原例程中的几个小bug，比如互相关算法里有些时候归一化会有除以0的问题，还有把snowboy的语音识别去掉了，代之以声音强度识别（因为大多数时候语音识别不是很有效，还不如强度识别靠谱）。
树莓派主程序中调用测向算法的代码：class doa_thread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
def run(self):
src = Source(rate=16000, channels=4, frames_size=320)
ch1 = ChannelPicker(channels=4, pick=1)
doa = DOA(rate=16000)
src.link(ch1)
src.link(doa)
src.recursive_start()

self.running = True
while self.running:
try:
time.sleep(1)
position, amplitute = doa.get_direction()
if amplitute > 2000:
pixels.wakeup(position)
print amplitute,position
if position > 0 and position < 180:
pivot_right()
time.sleep(position/200)
stop()
elif position >= 180 and position < 360:
pivot_left()
position = 360 - position
time.sleep(position/200)
stop()
time.sleep(3)
else:
pixels.speak()
except:
print sys.exc_info()

src.recursive_stop()
测向算法的核心（互相关算法）："""
Estimate time delay using GCC-PHAT
"""

import numpy as np

def gcc_phat(sig, refsig, fs=1, max_tau=None, interp=1):
'''
This function computes the offset between the signal sig and the reference signal refsig
using the Generalized Cross Correlation - Phase Transform (GCC-PHAT)method.
'''

# make sure the length for the FFT is larger or equal than len(sig) + len(refsig)
n = sig.shape[0] + refsig.shape[0]

# Generalized Cross Correlation Phase Transform
SIG = np.fft.rfft(sig, n=n)
REFSIG = np.fft.rfft(refsig, n=n)
R = SIG * np.conj(REFSIG)

if all(np.abs(R)) == True:
cc = np.fft.irfft(R / np.abs(R), n=(interp * n))
else:
cc = np.fft.irfft(R, n =(interp * n))

max_shift = int(interp * n / 2)
if max_tau:
max_shift = np.minimum(int(interp * fs * max_tau), max_shift)

cc = np.concatenate((cc[-max_shift:], cc[:max_shift+1]))

# find max cross correlation index
shift = np.argmax(np.abs(cc)) - max_shift

tau = shift / float(interp * fs)

return tau, cc

def main():

refsig = np.linspace(1, 10, 10)

for i in range(0, 10):
sig = np.concatenate((np.linspace(0, 0, i), refsig, np.linspace(0, 0, 10 - i)))
offset, _ = gcc_phat(sig, refsig)
print(offset)

if __name__ == "__main__":
main()把互相关算法得到的延迟结果转化为角度数据并返回给我主程序的代码：# -*- coding: utf-8 -*-

"""
Time Difference of Arrival for ReSpeaker 4 Mic Array
"""

import numpy as np
import collections
from .gcc_phat import gcc_phat
from .element import Element

SOUND_SPEED = 340.0

MIC_DISTANCE_4 = 0.081
MAX_TDOA_4 = MIC_DISTANCE_4 / float(SOUND_SPEED)

class DOA(Element):
def __init__(self, rate=16000, chunks=10):
super(DOA, self).__init__()

self.queue = collections.deque(maxlen=chunks)
self.sample_rate = rate

self.pair = [[0, 2], [1, 3]]

def put(self, data):
self.queue.append(data)

super(DOA, self).put(data)

def get_direction(self):
tau = [0, 0]
theta = [0, 0]

buf = b''.join(self.queue)
buf
a439
= np.fromstring(buf, dtype='int16')
for i, v in enumerate(self.pair):
tau[i], _ = gcc_phat(buf[v[0]::4], buf[v[1]::4], fs=self.sample_rate, max_tau=MAX_TDOA_4, interp=1)
theta[i] = np.arcsin(tau[i] / MAX_TDOA_4) * 180 / np.pi

if np.abs(theta[0]) < np.abs(theta[1]):
if theta[1] > 0:
best_guess = (theta[0] + 360) % 360
else:
best_guess = (180 - theta[0])
else:
if theta[0] < 0:
best_guess = (theta[1] + 360) % 360
else:
best_guess = (180 - theta[1])

best_guess = (best_guess + 270) % 360

best_guess = (-best_guess + 120) % 360

return best_guess