使用三边定位算法进行室内定位

来源：互联网发布：mpp数据库和oracle区别编辑：程序博客网时间：2024/04/27 00:24

使用超宽带进行室内定位时，一般是通过测量时间来计算标签到基站的距离，通过多个标签到基站的距离,以及基站的实际物理坐标,就可以计算出标签所在的三维坐标.

这里我假设已经使用超宽带信号（实际上采用开发板:DW1000FOLLOWER_F405 4套作为基站，DWM1000WEAR 2套作为标签）测得带有一定误差的多个距离,则根据这多个距离可以计算出标签的三维坐标.假设四个基站物理坐标为(x1,y1,z1),(x2,y2,z2),(x3,y3,z3)和(x4,y4,z4),未知的tag坐标为(x,y,z), tag到基站的测距为d1,d2,d3,d4,则有方程组

（x-x1）^2+(y-y1)^2+(z-z1)^2=d1^2

(x-x2)^2+(y-y2)^2+(z-z2)^2=d2^2

(x-x3)^2+(y-y3)^2+(z-z3)^2=d3^2

(x-x4)^2+(y-y4)^2+(z-z4)^2=d4^2

该方程组可以使用最小二乘法进行求解,具体的可以参考https://github.com/Meihai/IndoorPos,但是使用该方法会遇到一个问题,那就是测距存在较大误差时很难精确估算tag所在的三维坐标,总之，我试过,在测距偏差有200mm的时候,z轴上的偏差估计有十几米了,x轴和y轴倒不是很敏感.

所以，下面本博主主要采用牛顿迭代法求tag坐标.

假设目标函数

obj=0.5sum(fi(x,y,z)^2)

fi(x,y,z)=sqrt((x-xi)^2+(y-yi)^2+(z-zi)^2)-di

求目标函数的最小值对应的x,y,z就是我们所需要求的tag坐标。

使用牛顿迭代法求最小值过程：

1）求目标函数对x,y,z的一阶偏导数

2）求目标函数对x,y,z的二阶偏导数

3）使用迭代公式:

x=x-f'/f''

4)达到指定迭代次数或者指定误差则结束，输出tag坐标

具体实现代码如下：

import serial
import time
import thread
import numpy as np

#ser=serial.Serial("/dev/ttyUSB0",115200)
#ser=serial.Serial("COM3",115200)

#基站坐标基本信息
basestationDicts={"1":np.array([2630.00,2700.00,30.00]), #单位mm
                  "2":np.array([2600.00,3540.00,800.00]),
                  "3":np.array([800.00,4000.00,30.00]),
                  "4":np.array([480.00,3720.00,620.00])}
#保存标签坐标信息的数据文件路径
tagaxisFilePath="tagaxis.txt"

#串口多线程读取
class MSerialPort:
    message=''
    tagaxis=[] #列表数据
    def __init__(self,port,buad):
        self.port=serial.Serial(port,buad)
        if not self.port.isOpen():
            self.port.open()
    def port_open(self):
        if not self.port.isOpen():
            self.port.open()
    def port_close(self):
        self.port.close()
        np.savetxt(tagaxisFilePath,self.tagaxis,fmt="%.2f",delimiter=",")
    def send_data(self,data):
        number=self.write(data)
        return number
    ## 解析串口收到的字符串，并计算tag坐标
    def dealRecvStr(self,recvMessage):
        mapx={}
        strArray=recvMessage.split("\r\n")
        for i in range(len(strArray)):
            if(strArray[i].startswith("distance") and strArray[i].find("mm")>0 and strArray[i].find(":")>0):
               # print "*****************************************"
               # print strArray[i]
                locationName=strArray[i][(strArray[i].index("distance")+8):(strArray[i].index(":"))].strip()
                distance=strArray[i][(strArray[i].index(":")+1):(strArray[i].index("mm"))].strip()
                try:
                    mapx[locationName]=np.double(distance)
                except:
                    continue
        if(len(mapx)<4):
            print "can not calculate the tag axis as distance data count is less than 4"
            return
        baseStationInfo=np.zeros((len(mapx),4))
        index=0
        for key in mapx:
            if(basestationDicts.has_key(key)):
                baseStationInfo[index,0]=basestationDicts[key][0]
                baseStationInfo[index,1]=basestationDicts[key][1]
                baseStationInfo[index,2]=basestationDicts[key][2]
                baseStationInfo[index,3]=mapx[key]
            else:
                return
            index=index+1
        originalX=np.array([0.0,0.0,0.0])
        insNewton=MultivariateNewton(originalX,0.00001,100,baseStationInfo)
        x,y=insNewton.getNewtonMin()
        print "caculate axis:*****************************"
        print x,y
        print "calculate over****************************"
        self.tagaxis.append(x)

    def read_data(self):
        cnt=0
        while True:
            data=self.port.readline()
            self.message+=data
            cnt=cnt+1
            if(cnt>30):
                cnt=0
                print("----------------------------------------")
                print self.message
                recvStr=self.message
                self.message=''
                self.dealRecvStr(recvStr)


class MultivariateNewton(object):
#    def __init__(self):
#        self.originalX=np.zeros(3,1)
#        self.e=0.0
#        self.maxCycle=100;
#        #基站坐标信息
#        self.baseStationInfo=np.array([2630.00,2700.00,30.00,1279.25],
#                                      [2630.00,3540.00,800.00,1700.5],
#                                      [800.00,4000.00,30.00,2298.5],
#                                      [480.00,3720.00,620.00,1949.00])

    def __init__(self,originalx,e,maxcycle,baseStationInfo):
        self.originalX=originalx
        self.e=e
        self.maxCycle=maxcycle
        self.baseStationInfo=baseStationInfo

    def getOriginal(self,x):
        objv=0.0
        for i in range(4):
            sqSum=np.dot(x-self.baseStationInfo[i,0:3],x-self.baseStationInfo[i,0:3])
            fi=np.sqrt(sqSum)-self.baseStationInfo[i,3]
            objv=objv+fi**2
        objv=objv*0.5;
        return objv

    def getHessian(self,x):
        #3行乘3列二阶导数矩阵
        jacobian=np.zeros((3,3))
        for i in range(4):
            xxi=x[0]-self.baseStationInfo[i,0]
            xxisq=xxi**2
            yyi=x[1]-self.baseStationInfo[i,1]
            yyisq=yyi**2
            zzi=x[2]-self.baseStationInfo[i,2]
            zzisq=zzi**2
            sumxyzSq=xxisq+yyisq+zzisq
            sumxyzSqCube=sumxyzSq**3
            #二阶导数第一行
            jacobian[0,0]+=1.0-(self.baseStationInfo[i,3]/np.sqrt(sumxyzSq))+(self.baseStationInfo[i,3]*xxisq/np.sqrt(sumxyzSqCube))
            jacobian[0,1]+=self.baseStationInfo[i,3]*xxi*yyi/np.sqrt(sumxyzSqCube)
            jacobian[0,2]+=self.baseStationInfo[i,3]*xxi*zzi/np.sqrt(sumxyzSqCube)

            #二阶导数第二行
            jacobian[1,0]+=self.baseStationInfo[i,3]*xxi*yyi/np.sqrt(sumxyzSqCube)
            jacobian[1,1]+=1.0-(self.baseStationInfo[i,3]/np.sqrt(sumxyzSq))+(self.baseStationInfo[i,3]*yyisq/np.sqrt(sumxyzSqCube))
            jacobian[1,2]+=self.baseStationInfo[i,3]*yyi*zzi/np.sqrt(sumxyzSqCube)

            #二阶导数第三行
            jacobian[2,0]+=self.baseStationInfo[i,3]*xxi*zzi/np.sqrt(sumxyzSqCube)
            jacobian[2,1]+=self.baseStationInfo[i,3]*yyi*zzi/np.sqrt(sumxyzSqCube)
            jacobian[2,2]+=1.0-(self.baseStationInfo[i,3]/np.sqrt(sumxyzSq))+(self.baseStationInfo[i,3]*zzisq/np.sqrt(sumxyzSqCube))
        return jacobian

    def getOneDerivative(self,x):
        oneDerivative=np.zeros((3,1))
        for i in range(4):
            xxi=x[0]-self.baseStationInfo[i,0]
            xxiSq=xxi**2
            yyi=x[1]-self.baseStationInfo[i,1]
            yyiSq=yyi**2
            zzi=x[2]-self.baseStationInfo[i,2]
            zziSq=zzi**2
            sumxyzSq=xxiSq+yyiSq+zziSq
            fic=(np.sqrt(sumxyzSq)-self.baseStationInfo[i,3])/np.sqrt(sumxyzSq)
            oneDerivative[0,0]+=fic*xxi
            oneDerivative[1,0]+=fic*yyi
            oneDerivative[2,0]+=fic*zzi
        return oneDerivative

    def getNewtonMin(self):
        x=self.originalX
        y=0.0
        k=1
        #梯度下降更新公式
        while k<self.maxCycle:
            y=self.getOriginal(x)
            one=self.getOneDerivative(x)
            while(np.abs(one[0,0])<self.e and np.abs(one[1,0])<self.e and np.abs(one[2,0])<self.e):
                break
            two=self.getHessian(x)
            twom=np.mat(two)
            twoInv=twom.I
            twoInv=np.array(twoInv)
           # print twoInv
          # print one.T
            delta=np.dot(twoInv,one)
            x[0]-=delta[0]
            x[1]-=delta[1]
            x[2]-=delta[2]
            k=k+1
        return x,y




#def main():
#    while True:
#        count=ser.inWaiting()
#        if count!=0:
#            recv=ser.read(count)
#            print recv
#            print("---------------------------------------")
#        ser.flushInput()
#        time.sleep(0.1)

if __name__=='__main__':
    #测试1
#    try:
#        main()
#    except KeyboardInterrupt:
#        ser.close()
    #测试2
#    originalX=np.array([0.0,0.0,0.0])
#    baseStationInfo=np.array([[2630.00,2700.0,30.0,1279.25],
#                             [2630.0,3540.0,800.0,1700.5],
#                             [800.0,4000.0,30.0,2298.5],
#                             [480.0,3720.0,620.0,1949.5]]
#                             )
#    insNewton=MultivariateNewton(originalX,0.00001,100,baseStationInfo)
#    x,y=insNewton.getNewtonMin()
#    print x,y
    #测试3
    mSerial=MSerialPort('COM3',115200)
    thread.start_new_thread(mSerial.read_data,())
    try:
        while True:
            time.sleep(0.1)
       # print mSerial.message
       # print 'next line'
    except KeyboardInterrupt:
        mSerial.port_close()

阅读全文

0 0