在GPS不可用的情况下，从IMU数据计算位置

如何解决在GPS不可用的情况下，从IMU数据计算位置

第一篇文章在这里，我正用双脚跳进python。

我的项目是尝试仅使用IMU传感器和速度表来计算水下机器人的位置。

我对编程很陌生，我敢肯定我会在所附的代码中获得很多反馈，但是我目前坚持的步骤是在以下之间创建一个反馈循环：

LATD（GPS A的状态=可用V =不可用），

LOND / IMU_LAT（十进制GPS位置）和

IMU_LON / UTC[2]（IMU位置以十进制表示）

该想法是，如果IMU_LAT为“ A”，则逻辑将平均IMU_LON / LATD和LOND / UTC[2]平均，但如果{{ 1}}为“ V”，它将仅根据记录的最后位置和IMU_north / IMU_east（基于航向和加速度值的偏移量）来计算位置。

import time,inspect
import IMU
import serial
import datetime
import os
import math
import logging

log = open(time.strftime("%Y%m%d-%H%M%S")+'_GSPData.csv','w')
#f.write("UTC TIME,NAVSTATUS,LAT,LON,HDG,SPD,X,Y,Z")

RAD_TO_DEG = 57.29578
M_PI = 3.14159265358979323846
G_GAIN = 0.070  # [deg/s/LSB]  If you change the dps for gyro,you need to update this value accordingly
AA =  0.40      # Complementary filter constant

magXmin =  0
magYmin =  0
magZmin =  0
magXmax =  0
magYmax =  0
magZmax =  0

gyroXangle = 0.0
gyroYangle = 0.0
gyroZangle = 0.0
CFangleX = 0.0
CFangleY = 0.0

IMU.detectIMU()     #Detect if BerryIMUv1 or BerryIMUv2 is connected.
IMU.initIMU()       #Initialise the accelerometer,gyroscope and compass

a = datetime.datetime.now()

ser = serial.Serial('/dev/serial0',9600)

def truncate(n,decimals=0):
    multiplier = 10 ** decimals
    return int(n * multiplier) / multiplier

log.write("UTC,NAVSTAT,xm/s,ym/s,zm/s")
log.write("\n")

try:
    
    while True:
    #Read the GPS,accelerometer,gyroscope and magnetometer values
        NMEA = ser.readline()
        NMEA_str_data = NMEA.decode('utf-8')
        NMEA_data_arr=NMEA_str_data.split()
        UTC = NMEA_str_data.split(',')
        GYRx = IMU.readGYRx()
        GYRy = IMU.readGYRy()
        GYRz = IMU.readGYRz()
        ACCx = IMU.readACCx()
        ACCy = IMU.readACCy()
        ACCz = IMU.readACCz()
#output the values of the accelerometer in m/s
        yG = ((ACCx * 0.244)/1000)*9.80665
        xG = ((ACCy * 0.244)/1000)*9.80665
        zG = ((ACCz * 0.244)/1000)*9.80665
        MAGx = IMU.readMAGx()
        MAGy = IMU.readMAGy()
        MAGz = IMU.readMAGz()
    
      
#Apply compass calibration
        MAGx -= (magXmin + magXmax) /2
        MAGy -= (magYmin + magYmax) /2
        MAGz -= (magZmin + magZmax) /2
    
##Calculate loop Period(LP). How long between Gyro Reads
        b = datetime.datetime.now() - a
        a = datetime.datetime.now()
        LP = b.microseconds/(1000000*1.0)
        outputString = "Loop Time %5.2f " % ( LP )

    
#Convert Gyro raw to degrees per second
        rate_gyr_x =  GYRx * G_GAIN
        rate_gyr_y =  GYRy * G_GAIN
        rate_gyr_z =  GYRz * G_GAIN


#Calculate the angles from the gyro.
        gyroXangle+=rate_gyr_x*LP
        gyroYangle+=rate_gyr_y*LP
        gyroZangle+=rate_gyr_z*LP


        #Convert Accelerometer values to degrees
        AccXangle =  (math.atan2(ACCy,ACCz)*RAD_TO_DEG)
        AccYangle =  (math.atan2(ACCz,ACCx)+M_PI)*RAD_TO_DEG

        #convert the values to -180 and +180
        if AccYangle > 90:
            AccYangle -= 270.0
        else:
            AccYangle += 90.0

        #Complementary filter used to combine the accelerometer and gyro values.
        CFangleX=AA*(CFangleX+rate_gyr_x*LP) +(1 - AA) * AccXangle
        CFangleY=AA*(CFangleY+rate_gyr_y*LP) +(1 - AA) * AccYangle

        #Calculate heading
        heading = 180 * math.atan2(MAGy,MAGx)/M_PI

        #Only have our heading between 0 and 360
        if heading < 0:
            heading += 360
                          
    ####################################################################
    ###################Tilt compensated heading#########################
    ####################################################################
        #Normalize accelerometer raw values.
        accXnorm = ACCx/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
        accYnorm = ACCy/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
        accZnorm = ACCz/math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz)
        Zms_norm = zG-9.80665
        Yms_norm = yG
        Xms_norm = xG
        #Calculate course
        Course = (180*math.atan2(Xms_norm,Yms_norm)/M_PI)
        
        #Only have our course between 0 and 360
        if Course < 0:
            Course +=360
        #Calculate pitch and roll
        pitch = math.asin(accXnorm)
        roll = -math.asin(accYnorm/math.cos(pitch))

        #Calculate the new tilt compensated values
        magXcomp = MAGx*math.cos(pitch)+MAGz*math.sin(pitch)

    #The compass and accelerometer are orientated differently on the LSM9DS0 and LSM9DS1 and the Z axis on the compass
    #is also reversed. This needs to be taken into consideration when performing the calculations
        if(IMU.LSM9DS0):
            magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)-MAGz*math.sin(roll)*math.cos(pitch)   #LSM9DS0
        else:
            magYcomp = MAGx*math.sin(roll)*math.sin(pitch)+MAGy*math.cos(roll)+MAGz*math.sin(roll)*math.cos(pitch)   #LSM9DS1

    #Calculate tilt compensated heading
        tiltCompensatedHeading = 180 * math.atan2(magYcomp,magXcomp)/M_PI

        if tiltCompensatedHeading < 0:
            tiltCompensatedHeading += 360
        #convert IMU readings to northings and eastings
        IMU_north= (math.cos(tiltCompensatedHeading))*(Yms_norm+Xms_norm)
        IMU_east= (math.sin(tiltCompensatedHeading))*(Yms_norm+Xms_norm)
 
         #convert IMU_north to D.D
        IMU_north_D= IMU_north/110723.41272
         #Convert IMU_east to d.d
        IMU_east_D= IMU_east/103616.02936
        
############################ END ##################################

       #"%am/s": no rounding   "%bm/s": unsupported "%cm/s": unsupported
        #"%dm/s": whole numbers "%em/s": scientific notation "%fm/s": six digits
        #"%gm/s": five digits
        
        if NMEA_str_data.startswith('$GNRMC'):
            
            if UTC[2] =="V":
                #print("GPS unavaliable","heading",round(tiltCompensatedHeading,2),",course",round(Course,xG,yG,zG,"IMU_LAT","IMU_LON")
                print("UTC",truncate(float(UTC[1]),0),IMU",LAT",LON",heading",truncate(IMU_north,4),truncate(IMU_east,4))
                #log the output GPS invalid
                log.write(UTC[1]+','+UTC[2]+','+""+','""+','+str(round(tiltCompensatedHeading,2))+','+UTC[7]+','+str(IMU_north)+','+str(IMU_east))
            else:   
                #convert UTC from DDMM.MMM to DD.DDDD
                if UTC[4] =="N":
                    LATD= (truncate(float(UTC[3]),-2)/100)+((float(UTC[3])-(truncate(float(UTC[3]),-2)))/60)
                else:
                    LATD= -(truncate(float(UTC[3]),-2)))/60)
                if UTC[6] =="E":
                    LOND= (truncate(float(UTC[5]),-2)/100)+((float(UTC[5])-(truncate(float(UTC[5]),-2)))/60)            
                else:
                    LOND= -(truncate(float(UTC[5]),-2)))/60)
                #calculate IMU_LAT
                IMU_LAT= LATD+IMU_north_D
                #Calculate IMU_LON
                IMU_LON= LOND+IMU_east_D    
                #write the output                 
                print("UTC",GPS",truncate(LATD,5),truncate(IMU_LAT,truncate(LOND,truncate(IMU_LON,UTC[8],speed",truncate(float(UTC[7]),2))
                #log the output GPS valid
                log.write(UTC[1]+','+str(LATD)+','+str(LOND)+','+str(IMU_east))
            log.write("\n")
            #slow program down a bit,makes the output more readable
            time.sleep(0.5)
    
        #print(" aX = %fG   aY =%fG   aZ =%fG  " % ( ACCx,ACCy,ACCz))
        #slow program down a bit,makes the output more readable
        #time.sleep(0.5)
except (KeyboardInterrupt,SystemExit): #when you press ctrl+c
    print ("Done.\nExiting.")
    log.close()

就像我说的我是新手一样，我相信你们的专业人士会告诉我它的确很草率，但是我很乐意接受任何建设性的批评。

谢谢特洛伊