@aibee/crc-bmap 0.8.44 → 0.8.46

package/lib/src/plugins/car-inertial-position/car-inertial-position.js

@@ -1,69 +1,298 @@
 // 车辆惯性导航
+/**
+ * 定位结果有三种：视觉定位结果、beacon定位结果、pdr定位结果
+ * 以 pdr 定位为主要流程，以视觉结果和beacon结果为校准，其中主视觉定位，beacon是在长期无视觉情况下才会采用的
+ * 以视觉定位结果计算角度、速度，通过卡尔曼滤波平滑速度和角度的变化
+ * 什么时候通过视觉、蓝牙校准pdr的结果???
+ */ import { _ as _extends } from "@swc/helpers/_/_extends";
+import "core-js/modules/es.array.find-last-index.js";
+import "core-js/modules/es.array.find-last.js";
+import "core-js/modules/es.array.push.js";
 import { EventDispatcher } from "three";
-import { Compass } from "./compass";
-import { calculateLineDirection, predictFuturePosition, predictFutureSpeed, transformSpeed } from "./utils";
+import { calculateLineDirection, predictFuturePosition, transformSpeed } from "./utils";
 import { getLength, Timer } from "../../utils";
+import { KalmanFilter } from "./kalman-filter";
+import { isNil } from "lodash";
 export class CarInertialPosition extends EventDispatcher {
-    startCompass() {
-        this.compass.start();
+    setPathAngle(angle) {
+        this.pathAngle = angle;
+        this.dispatchEvent({
+            type: "change-compass",
+            value: this.pathAngle
+        });
+        if (isNil(this.angle)) {
+            this.angle = angle;
+        }
+    }
+    changeSpeed() {
+        const serverHistory = this.history.filter((item)=>[
+                "vision"
+            ].includes(item.type));
+        if (serverHistory.length < 2) {
+            this.speedFilter.filter(0);
+            this.speed = 0;
+            return;
+        }
+        ;
+        // 计算速度：位置变化 / 时间差
+        // const speeds = serverHistory.slice(1).map((current, index) => {
+        //   const previous = serverHistory[index];
+        //   const timeDiff = current.time - previous.time;
+        //   const distanceDiff = getLength(current.position, previous.position);
+        //   // 防止除零
+        //   return timeDiff > 0 ? (distanceDiff / timeDiff) : 0;
+        // });
+        // // 计算平均速度
+        // const speed = speeds.reduce((a, b) => a + b, 0) / speeds.length;
+        const first = serverHistory[0];
+        const last = serverHistory.slice(-1)[0];
+        const distance = getLength(last.position, first.position);
+        const time = last.time - first.time;
+        const speed = time > 0 ? distance / time : 0;
+        // 使用卡尔曼滤波器平滑速度
+        const smoothedSpeed = this.speedFilter.filter(speed);
+        this.speed = smoothedSpeed;
+    }
+    changeAngle(histories) {
+        if (histories === void 0) histories = this.visionHistory;
+        if (histories.length < 2) {
+            this.angle = null;
+        }
+        ;
+        const angle = calculateLineDirection(histories.map((item)=>item.position));
+        if (angle !== null) {
+            this.setAngle(angle);
+        }
     }
-    setPosition(position, time, duration) {
-        this.history.push({
+    setAngle(angle, dispatch) {
+        if (dispatch === void 0) dispatch = true;
+        this.angle = angle;
+        if (dispatch) {
+            this.dispatchEvent({
+                type: "change-position-compass",
+                value: angle
+            });
+        }
+    }
+    /**
+   * 添加视觉结果
+   * @param position 
+   * @param time 
+   * @param duration 
+   * @returns 
+   */ setPosition(position, time, duration) {
+        const item = {
             position,
             time,
             clientTime: Date.now() - duration,
             type: "vision"
-        });
-        const MAX_DISTANCE = 5; // 保留最近20米的点
-        for(let i = 0; i < this.history.length; i++){
-            const item = this.history[i];
-            const distance = getLength(item.position, position);
-            if (distance > MAX_DISTANCE) {
-                this.history.shift();
-                i--;
+        };
+        this._setVisionHistoryForAngle(item);
+        if (isNil(this.angle) || !this.speed) {
+            this.addHistory(item);
+            return;
+        }
+        const lastIndex = this.history.findLastIndex((item)=>item.type === "vision");
+        const last = this.history[lastIndex];
+        if (last) {
+            if (last.time > time) {
+                // 这一桢的时间比本地最后一桢的时间还早，是个已经走过的点，废弃
+                return;
+            }
+            // 如果这个在pdr的点前面，说明pdr滞后了，采用视觉点
+            const angle = calculateLineDirection([
+                this.history.slice(-1)[0].position,
+                position
+            ]);
+            if (angle && Math.abs(angle - this.angle) < 60) {
+                this.addHistory(item);
+                return;
+            }
+            // 判断视觉点连续三次超出最后的点5米，就采用视觉点
+            // 预估这个视觉点的正确位置
+            const predictPos = predictFuturePosition(position, this.speed, 360 - this.pathAngle, duration);
+            const distance = getLength(predictPos, this.history.slice(-1)[0].position);
+            // 超过10米就把位置拉过来
+            if (distance > 10) {
+                this.visionExcessesCount++;
             } else {
-                break;
+                this.visionExcessesCount = 0;
+            }
+            if (this.visionExcessesCount > 3) {
+                console.warn("连续三次视觉和pdr差距大于5米，使用视觉校准");
+                // 采用视觉结果
+                this.addHistory(item);
+                this.visionExcessesCount = 0;
             }
         }
-        const posInfo = this.getPosition("vision");
-        this.dispatchEvent({
-            type: "change-pos",
-            value: posInfo
-        });
-        this.startPositionTimer();
+    }
+    resetPdrPosition() {
+        const lastVision = this.history.findLast((item)=>item.type === "vision");
+        if (lastVision && this.speed) {
+            // 预估这个视觉点的正确位置
+            const predictPos = predictFuturePosition(lastVision.position, this.speed, 360 - this.pathAngle, Date.now() - lastVision.clientTime);
+            if (predictPos) {
+                this.history.push({
+                    position: predictPos,
+                    time: Date.now(),
+                    clientTime: Date.now(),
+                    type: "pdr"
+                });
+            }
+        }
+    }
+    /**
+   * 获取 最后一个视觉坐标在加了pdr惯性的坐标
+   */ getLastVisionPdrPos() {
+        const lastIndex = this.history.findLastIndex((item)=>item.type === "vision");
+        if (lastIndex !== -1) {
+            var _this_history_;
+            return (_this_history_ = this.history[lastIndex + 1]) != null ? _this_history_ : this.history[lastIndex];
+        }
+        return null;
+    }
+    _setVisionHistoryForAngle(history) {
+        if (!this.visionHistory.length) {
+            this.visionHistory.push(history);
+            return;
+        }
+        const angle = calculateLineDirection([
+            this.visionHistory.slice(-1)[0].position,
+            history.position
+        ]);
+        if (angle !== null) {
+            // console.error("角度差", this.angle, angle, Math.abs(this.angle! - angle))
+            if (this.angle && Math.abs(this.angle - angle) > 60) {
+                this.angleExcessesCount++;
+            } else {
+                this.angleExcessesCount = 0;
+                this.visionHistory.push(history);
+            }
+        }
+        // 连续三次角度大于90度就修改角度
+        if (this.angleExcessesCount > 3) {
+            console.warn("连续三次角度比较歪，重新矫正");
+            // this.angle = angle!;
+            this.visionHistory = [
+                history
+            ];
+            this.angleExcessesCount = 0;
+        } else {
+            // 保留两米的点
+            let distance = getLength(this.visionHistory[0].position, this.visionHistory.slice(-1)[0].position);
+            while(distance > 5 && this.visionHistory.length > 15){
+                this.visionHistory.shift();
+                distance = getLength(this.visionHistory[0].position, this.visionHistory.slice(-1)[0].position);
+            }
+        }
+        // 更新角度
+        this.changeAngle();
+    }
+    addHistory(data) {
+        this.history.push(data);
+        // 处理history超出
+        let visionHistory = this.history.filter((item)=>item.type === 'vision');
+        // 最后一个视觉点的时间离当前时间的差值
+        const lastVisionTimeDelta = visionHistory.length ? Date.now() - visionHistory.slice(-1)[0].clientTime : 0;
+        let maxDistance = getLength(this.history[0].position, this.history.slice(-1)[0].position);
+        // 如果有5s内的视觉点，保留3m内的点，如果没有5s内的视觉点，保留10米的蓝牙点
+        const MAX_DISTANCE = lastVisionTimeDelta > 5000 ? 10 : 3;
+        // 最少保留5个点 但是如果已经5s没有视觉点了就全部抛弃，采用蓝牙点
+        const MIN_POSITION_HISTORY = lastVisionTimeDelta > 5000 ? 0 : 5;
+        if (lastVisionTimeDelta > 5000) {
+            console.warn("视觉结果超出5s不可用，全部清空");
+        }
+        while(visionHistory.length > MIN_POSITION_HISTORY && maxDistance > MAX_DISTANCE){
+            this.history.shift();
+            visionHistory = this.history.filter((item)=>item.type === 'vision');
+            maxDistance = getLength(this.history[0].position, this.history.slice(-1)[0].position);
+        }
+        // 如果视觉都失效了用蓝牙更新速度和角度
+        if (MIN_POSITION_HISTORY === 0) {
+            // this.changeAngle(this.history.filter(item => item.type === "beacon"))
+            this.changeSpeed();
+        } else if (data.type === "vision") {
+            // 如果是视觉点位就更新速度
+            this.changeSpeed();
+        }
+        this.changePosition(data.type);
     }
     setBeaconPosition(position, time, duration) {
-        this.history.push({
+        const clientTime = Date.now() - duration;
+        const item = {
             position,
             time,
-            clientTime: Date.now() - duration,
+            clientTime,
             type: "beacon"
-        });
-        const posInfo = this.getPosition("beacon");
-        this.dispatchEvent({
-            type: "change-pos",
-            value: posInfo
-        });
-        this.startPositionTimer();
+        };
+        const lastServerIndex = this.history.findLastIndex((item)=>item.type !== "pdr");
+        const lastServerItem = this.history[lastServerIndex];
+        if (!lastServerItem) {
+            // 之前没有定位结果采用蓝牙结果
+            this.addHistory(item);
+            return;
+        }
+        if (isNil(this.pathAngle) || !this.speed) {
+            // 定位结果还不足以支持pdr 采用后端的结果
+            this.addHistory(item);
+            return;
+        }
+        // 收到了一个已经过去了的点，弃用
+        if (lastServerItem && lastServerItem.time > time) {
+            return;
+        }
+        // 预估这个视觉点的正确位置
+        const predictPos = predictFuturePosition(position, this.speed, 360 - this.pathAngle, duration);
+        const distance = getLength(predictPos, this.history.slice(-1)[0].position);
+        if (distance > 5) {
+            this.beaconExcessesCount++;
+        } else {
+            this.beaconExcessesCount = 0;
+        }
+        if (this.beaconExcessesCount > 3) {
+            console.warn("连续三次beacon和pdr差距大于5米，使用beacon校准");
+            // 采用beacon结果
+            this.addHistory(item);
+            this.beaconExcessesCount = 0;
+        }
     }
     startPositionTimer() {
         if (this.positionTimer) {
             this.timer.clearInterval(this.positionTimer);
         }
-        if (this.autoCleanTimer) {
-            this.timer.clearTimeout(this.autoCleanTimer);
-        }
         this.positionTimer = this.timer.setInterval(()=>{
-            const posInfo = this.getPosition("pdr");
-            this.dispatchEvent({
-                type: "change-pos",
-                value: posInfo
-            });
+            this.changePosition();
         }, 20);
-        this.autoCleanTimer = this.timer.setTimeout(()=>{
-            this.positionTimer && this.timer.clearInterval(this.positionTimer);
-            this.positionTimer = null;
-        }, 1000);
+    }
+    changePosition(from) {
+        if (from === void 0) from = 'pdr';
+        const posInfo = this.getPosition(from);
+        if (posInfo.success) {
+            this.history.push({
+                position: posInfo.pos,
+                time: Date.now(),
+                clientTime: Date.now(),
+                type: "pdr"
+            });
+            if (this.speed) {
+                // 再往前补2米的车头的距离
+                // const headPos = predictFuturePosition(
+                //   posInfo.pos,
+                //   this.speed,
+                //   360 - this.pathAngle!,
+                //   2 / this.speed
+                // )
+                this.dispatchEvent({
+                    type: "change-pos",
+                    value: _extends({}, posInfo)
+                });
+            } else {
+                this.dispatchEvent({
+                    type: "change-pos",
+                    value: posInfo
+                });
+            }
+        }
     }
     getPosition(from) {
         if (this.history.length === 0) {
@@ -82,45 +311,34 @@ export class CarInertialPosition extends EventDispatcher {
             return {
                 success: true,
                 pos: lastHistory.position,
-                compass: null,
-                speed: 0
+                compass: this.pathAngle,
+                speed: this.speed
             };
         }
-        // 计算角度只用视觉的结果
-        const visionHistory = this.history.filter((item)=>item.type === "vision");
-        // 推导到当前时间距离最后一个定位点的时间
-        const angle = calculateLineDirection(visionHistory.map((history)=>history.position));
-        if (angle !== null) {
-            this.compassAngle = angle;
-            this.dispatchEvent({
-                type: "change-compass",
-                value: angle
-            });
-        // this.compass.setAbsoluteCompass(angle, lastHistory.time);
+        if (isNil(this.pathAngle) || !this.speed) {
+            return {
+                success: true,
+                pos: lastHistory.position,
+                compass: this.pathAngle,
+                speed: this.speed
+            };
         }
         const lastTime = lastHistory.clientTime;
         const deltaTime = Date.now() - lastTime;
-        // 预估后的速度
-        const speed = predictFutureSpeed(this.history, deltaTime);
-        // console.log("speed", from, speed, transformSpeed(speed), deltaTime, lastTime)
         // 根据角度和速度时间计算预估后的位置
-        const pos = this.compassAngle ? predictFuturePosition(lastHistory.position, speed, 360 - this.compassAngle, deltaTime) : lastHistory.position;
+        const pos = this.pathAngle ? predictFuturePosition(lastHistory.position, this.speed, 360 - this.pathAngle, deltaTime) : lastHistory.position;
         return {
             success: true,
             pos,
-            compass: this.compassAngle,
-            speed: transformSpeed(speed)
+            compass: this.pathAngle,
+            speed: transformSpeed(this.speed)
         };
     }
     dispose() {
-        this.compass.stop();
         this.timer.dispose();
     }
     constructor(){
-        super(), this.history = [], this.compass = new Compass(), this.compassAngle = 0, this.timer = new Timer(), this.positionTimer = null, this.autoCleanTimer = null;
-    // this.compass.addEventListener("compass", ({ value }) => { 
-    //   this.compassAngle = value;
-    //   this.dispatchEvent({ type: "change-compass", value })
-    // })
+        super(), this.history = [], this.visionHistory = [], this.speed = 0, this.angle = null, this.timer = new Timer(), this.positionTimer = null, this.speedFilter = new KalmanFilter(), this.visionExcessesCount = 0, this.beaconExcessesCount = 0, this.angleExcessesCount = 0, this.pathAngle = 0;
+        this.startPositionTimer();
     }
 }

package/lib/src/plugins/car-inertial-position/compass.d.ts

@@ -1,4 +1,5 @@
 import { EventDispatcher } from "three";
+import { KalmanFilter } from "./kalman-filter";
 interface SensorEventMap {
     start: {};
     stop: {};
@@ -15,12 +16,10 @@ interface CompassData {
  */
 export declare class Compass extends EventDispatcher<SensorEventMap> {
     compassData: CompassData[];
-    absoluteCompass: null | {
-        time: number;
-        compass: number;
-    };
-    delta: number;
+    delta: number | null;
     deltas: number[];
+    kalmanFilter: KalmanFilter;
+    deltaExcessesCount: number;
     constructor();
     start(): void;
     deviceOrientationAbsHandler: (e: DeviceOrientationEvent) => void;

package/lib/src/plugins/car-inertial-position/compass.js

@@ -1,8 +1,8 @@
-import { _ as _async_to_generator } from "@swc/helpers/_/_async_to_generator";
+import "core-js/modules/es.array.push.js";
 import { isNil } from "lodash";
 import { isIphone } from "../../utils";
 import { EventDispatcher } from "three";
-import { removeOutliers } from "./utils";
+import { KalmanFilter } from "./kalman-filter";
 /**
  * 监听compass 在获取到视觉定位的角度之后，计算视觉和compass的误差，保存一下这个误差，在后续定位中不断的调整这个误差
  */ export class Compass extends EventDispatcher {
@@ -17,16 +17,12 @@ import { removeOutliers } from "./utils";
    * @param compass
    * @param time
    */ setAbsoluteCompass(compass, time) {
-        if (!this.delta) {
+        if (isNil(this.delta)) {
             this.emitCompass(compass);
         }
         if (!this.compassData.length) {
             return;
         }
-        this.absoluteCompass = {
-            compass: compass,
-            time
-        };
         let cutTimeCompassIndex = this.compassData.findIndex((item)=>item.timestamp >= time);
         if (cutTimeCompassIndex === -1) {
             cutTimeCompassIndex = this.compassData.length - 1;
@@ -42,18 +38,32 @@ import { removeOutliers } from "./utils";
             curCompass = nextCompass.timestamp - time > prevCompass.timestamp - time ? prevCompass : nextCompass;
         }
         const delta = compass - curCompass.res;
-        this.deltas.push(delta);
-        const deltas = removeOutliers(this.deltas);
-        if (deltas.length) {
-            this.delta = deltas.reduce((sum, cur)=>sum + cur, 0) / deltas.length;
+        const filteredDelta = this.kalmanFilter.filter(delta);
+        this.deltas.push(filteredDelta);
+        if (isNil(this.delta)) {
+            this.delta = filteredDelta;
+        } else {
+            console.error(Math.abs(delta - this.delta), Math.abs(filteredDelta - this.delta), compass, curCompass.res);
+            if (Math.abs(filteredDelta - this.delta) > 10) {
+                this.deltaExcessesCount++;
+            } else {
+                this.deltaExcessesCount = 0;
+                this.delta = filteredDelta;
+            }
+            if (this.deltaExcessesCount > 10) {
+                console.warn("连续10次角度变化比较大", this.delta, filteredDelta);
+                this.delta = filteredDelta;
+                this.deltaExcessesCount = 0;
+            }
         }
     }
     emitCompass(compass) {
+        var _this_delta;
         // 把 compass 限制在 0 ~ 360 
-        console.log("compass delta", this.delta, compass);
+        const delta = (_this_delta = this.delta) != null ? _this_delta : 0;
         this.dispatchEvent({
             type: "compass",
-            value: (compass + this.delta + 360) % 360
+            value: (compass + delta + 360) % 360
         });
     }
     /**
@@ -67,32 +77,27 @@ import { removeOutliers } from "./utils";
             });
         }
     }
-    checkSensor() {
-        var _this = this;
-        return _async_to_generator(function*() {
-            const deviceOrientation = yield _this.checkDeviceOrientation();
-            return {
-                deviceOrientation
-            };
-        })();
+    async checkSensor() {
+        const deviceOrientation = await this.checkDeviceOrientation();
+        return {
+            deviceOrientation
+        };
     }
-    checkDeviceOrientation() {
-        return _async_to_generator(function*() {
-            var _window_DeviceOrientationEvent;
-            if (!isIphone) {
-                return true;
-            }
-            if (typeof window.DeviceOrientationEvent !== "undefined" && typeof ((_window_DeviceOrientationEvent = window.DeviceOrientationEvent) == null ? void 0 : _window_DeviceOrientationEvent.requestPermission) === "function") {
-                try {
-                    var _window_DeviceOrientationEvent1;
-                    const permission = yield (_window_DeviceOrientationEvent1 = window.DeviceOrientationEvent) == null ? void 0 : _window_DeviceOrientationEvent1.requestPermission();
-                    return permission === "granted";
-                } catch (e) {
-                    return false;
-                }
+    async checkDeviceOrientation() {
+        var _window_DeviceOrientationEvent;
+        if (!isIphone) {
+            return true;
+        }
+        if (typeof window.DeviceOrientationEvent !== "undefined" && typeof ((_window_DeviceOrientationEvent = window.DeviceOrientationEvent) == null ? void 0 : _window_DeviceOrientationEvent.requestPermission) === "function") {
+            try {
+                var _window_DeviceOrientationEvent1;
+                const permission = await ((_window_DeviceOrientationEvent1 = window.DeviceOrientationEvent) == null ? void 0 : _window_DeviceOrientationEvent1.requestPermission());
+                return permission === "granted";
+            } catch (e) {
+                return false;
             }
-            return false;
-        })();
+        }
+        return false;
     }
     stop() {
         if (isIphone) {
@@ -108,7 +113,7 @@ import { removeOutliers } from "./utils";
         });
     }
     constructor(){
-        super(), this.compassData = [], this.absoluteCompass = null, this.delta = 0, this.deltas = [], this.deviceOrientationAbsHandler = (e)=>{
+        super(), this.compassData = [], this.delta = null, this.deltas = [], this.kalmanFilter = new KalmanFilter(), this.deltaExcessesCount = 0, this.deviceOrientationAbsHandler = (e)=>{
             const currTime = Date.now();
             const { alpha, beta, gamma } = e;
             if (isNil(alpha) || isNil(beta) || isNil(gamma)) {

package/lib/src/plugins/car-inertial-position/kalman-filter.d.ts

@@ -0,0 +1,14 @@
+interface KalmanFilterOptions {
+    measurementNoise: number;
+    processNoise: number;
+    errorEstimate: number;
+}
+export declare class KalmanFilter {
+    estimate: number;
+    errorEstimate: number;
+    processNoise: number;
+    measurementNoise: number;
+    constructor(options?: Partial<KalmanFilterOptions>);
+    filter(measurement: number): number;
+}
+export {};

package/lib/src/plugins/car-inertial-position/kalman-filter.js

@@ -0,0 +1,30 @@
+// 卡尔曼滤波器实现
+export class KalmanFilter {
+    // 核心滤波方法
+    filter(measurement) {
+        // 1. 预测阶段
+        const prediction = this.estimate;
+        const predictionError = this.errorEstimate + this.processNoise;
+        // 2. 计算卡尔曼增益
+        const kalmanGain = predictionError / (predictionError + this.measurementNoise);
+        // 3. 更新估计
+        this.estimate = prediction + kalmanGain * (measurement - prediction);
+        // 4. 更新误差
+        this.errorEstimate = (1 - kalmanGain) * predictionError;
+        return this.estimate;
+    }
+    constructor(options = {}){
+        // 当前状态估计
+        this.estimate = 0;
+        // 估计误差
+        this.errorEstimate = 1;
+        // 过程噪声（系统内在变化）
+        this.processNoise = 0.1;
+        // 测量噪声（传感器不确定性）
+        this.measurementNoise = 1.0;
+        this.measurementNoise = options.measurementNoise || 1.0;
+        // 过程噪声（系统内在变化）
+        this.processNoise = options.processNoise || 0.1;
+        this.errorEstimate = options.errorEstimate || 1;
+    }
+}

package/lib/src/plugins/car-inertial-position/utils.d.ts

@@ -17,7 +17,7 @@ export declare function calculateInstantaneousSpeed(positions: CarPosition[]): n
  * @param timeIntervalMs
  * @returns
  */
-export declare function predictFutureSpeed(positions: CarPosition[], timeIntervalMs: number): number;
+export declare function predictFutureSpeed(positions: CarPosition[], timeIntervalMs: number): number | null;
 /**
  * 转换速度从 m/ms 转成  km/h
  * @param speed

package/lib/src/plugins/car-inertial-position/utils.js

@@ -1,3 +1,6 @@
+import "core-js/modules/web.dom-collections.iterator.js";
+import "core-js/modules/es.array.push.js";
+import "core-js/modules/es.array.sort.js";
 import { Vector2 } from "three";
 import { getLength } from "../../utils";
 // 计算最小二乘法的斜率 (m) 和截距 (b)
@@ -113,11 +116,11 @@ function getLineEndpoints(m, b, points) {
     // const n = speeds.length;
     const first = positions[0];
     const last = positions.slice(-1)[0];
-    const distance = positions.slice(1).reduce((sum, cur, index)=>{
-        const l = getLength(cur.position, positions[index].position);
-        return sum + l;
-    }, 0);
+    const distance = getLength(last.position, first.position);
     const time = last.time - first.time;
+    if (time < 100 || distance < 3) {
+        return null;
+    }
     return distance / time;
 // if (n === 0) {
 //   return 0; // 如果没有速度数据，返回0
@@ -165,7 +168,7 @@ function getLineEndpoints(m, b, points) {
     const vY = Math.sin(angleInRadians);
     const dir = new Vector2(vX, vY);
     // 一般车头到四轮中间的位置是2米左右
-    vec.add(dir.normalize().multiplyScalar(distance + 2));
+    vec.add(dir.normalize().multiplyScalar(distance));
     return [
         vec.x,
         vec.y