ROS交流群
ROS Group
产品服务
Product Service
开源代码库
Github
官网
Official website
技术交流
Technological exchanges
激光雷达
LIDAR
ROS教程
ROS Tourials
深度学习
Deep Learning
机器视觉
Computer Vision

电机控制与缓动函数



  • 问题

    在机器人的控制中最最基本的就是电机的控制。以舵机为例,最简单的控制方法就是直接给想要控制的舵机发送目标角度。但是我们实际会发现这样会有很多问题。比如舵机的动作速度没办法控制。动作看起来非常的生硬,直来直去。这样的控制也会比较影响舵机的使用寿命。这样就会出现一个问题,如何能够流畅的控制电机呢?

    首先什么样的动作才叫做流畅呢?


    同样是左右来回运动的两个方块,第二块的运动方式就要比第一块流畅许多。第一块只是一恒定大小的速度在运动,在两个边缘处的速度是突变的。这样对电机的伤害就很大。第二块的运动包含了加速和减速的过程,整过过程总电机的力相对要更均匀些。

    如何选取缓动函数

    现在我们已经知道了应该在运动中加入适当的过渡变化。那么如何选取合适的过度函数呢?
    这里实际上可以考虑成一个常见的物理运动过程。比如一个有阻尼的弹簧振子的运动。

    0_1446100359847_Damped_spring.gif

    根据阻尼大小的不同可以分为欠阻尼,过阻尼和临界阻尼三种状态。这些状态都运动特点如下图所示

    0_1446100484976_Damping_types.PNG

    假如我们想要把一个运动的电机停下来。按照阻尼运动来进行控制就是一个比较好的方式。但是在实际开发过程中,我们肯定不能直接使用上图的这种运动方程,因为算起来会比较复杂。在计算机绘图程序中经常使用贝塞尔函数来绘制平滑的曲线。利用这个函数就可以近似的模拟出以上的几个运动方程。

    比如欠阻尼的情况
    0_1446100961675_QQ截图20151029144214.png
    又比如过阻尼的情况
    0_1446101025205_QQ截图20151029144335.png

    具体怎么实现呢

    这个具体的实现过程和一般程序的动画控制函数差不多。下面以java为例来分析具体应该如何实现。
    一半的动画函数都包含以下几个参数,初始值, 结束值,时间函数(这个就是用来控制变化曲线的),更新函数。下面是我的code

    public static ActionStatusController actionAnimation(final int start, final int end, final int duration, final AnimationTimeFunction timeFunction, final AnimationHandler controlFunction){
        final ThreadUtils mAnimationThread = new ThreadUtils(new OnThreadStartHandler(){
            @Override
            public void OnStart() {
                super.OnStart();
                int changes = end - start;
                int time = 0;
                while (time < duration){
                    controlFunction.onProgress((int)(start + changes * timeFunction.getInterpolation((float)time/duration)));
                    try{
                        Thread.sleep(40);
                        time += 40;
                    }catch (Exception e){
                        e.printStackTrace();
                        break; //action was canceled
                    }
                }
            }
        });
        ActionStatusController mController = new ActionStatusController(mAnimationThread);
        return mController;
    }
    

    每次根据时间算出变化量,然后再把变化量传入到更新函数里面去进行具体的操作。
    因为一个动作可能包含了一些列电机的很多行为,所以为了使用方便可以包装出一个动作控制类。

    package com.novelor.servicebot.servicebot.models;
    
    import android.util.Log;
    
    import com.novelor.servicebot.servicebot.callbacks.OnThreadStartHandler;
    import com.novelor.servicebot.servicebot.utils.ArrayUtils;
    import com.novelor.servicebot.servicebot.utils.ThreadUtils;
    
    import java.util.Collection;
    
    /**
     * Created by randoms on 2015/10/23.
     */
    public class ActionStatusController {
    
        private ThreadUtils mAction;
        private boolean startFlag = false;
    
        public boolean isFinished(){
            return !mAction.isAlive();
        }
    
        public ActionStatusController (ThreadUtils mAction){
            this.mAction = mAction;
        }
    
        public void start(){
    
            startFlag = true;
            mAction.start();
        }
    
        public void cancel(){
            this.mAction.interrupt();
        }
    
        public ActionStatusController then(ActionStatusController next){
            while (!isFinished() || !startFlag){
                try{
                    Thread.sleep(5);
                }catch (Exception e){e.printStackTrace();}
            }
            next.start();
        return next;
    }
    
    public ActionStatusController then(ActionStatusController[] nexts){
        Log.d("Bot", "here1");
        while (!isFinished() || !startFlag){
            try{
                Thread.sleep(5);
            }catch (Exception e){e.printStackTrace();}
        }
        Log.d("Bot", "here2");
        for(int i = 0;i<nexts.length;i++){
            nexts[i].start();
        }
        // wait all actions to finish
        for(int i=0;i<nexts.length;i++){
            while (!nexts[i].isFinished() || !nexts[i].startFlag){
                try{
                    Thread.sleep(5);
                }catch (Exception e){e.printStackTrace();}
            }
        }
        ActionStatusController res =  new ActionStatusController(new ThreadUtils(new OnThreadStartHandler(){
            @Override
            public void OnStart() {
                super.OnStart();
            }
        }));
        res.start();
        return res;
    }
    
    public static ActionStatusController Start(ActionStatusController[] actions){
            Log.d("Bot", "before start");
            for(int i = 0;i<actions.length;i++){
                actions[i].start();
            }
            Log.d("Bot", "started");
            for(int i=0;i<actions.length;i++){
                while (!actions[i].isFinished() || !actions[i].startFlag){
                    try{
                        Thread.sleep(5);
                    }catch (Exception e){e.printStackTrace();}
                }
            }
            ActionStatusController res =  new ActionStatusController(new ThreadUtils(new OnThreadStartHandler(){
                @Override
                public void OnStart() {
                    super.OnStart();
                }
            }));
            res.start();
            return res;
        }
    }
    

    这样在实际使用中就可以这样了

    ActionStatusController.Start(reset()).then(new ActionStatusController[]{
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 194, 500, 1),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 180, 500, 2),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 180, 500, 3),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 158, 500, 4),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 25, 500, 5),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 3, 500, BotApi.STEER_RIGHT_HAND)
                }).then(new ActionStatusController[]{
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 194, 500, 1),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 140, 500, 2),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 172, 500, 3),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 500, 3, BotApi.STEER_RIGHT_HAND),
                }).then(
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 39, 300, BotApi.STEER_RIGHT_HAND) // 抓
                ).then(
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 165, 300, 3)
                ).then(new ActionStatusController[]{
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 237, 500, 1),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 144, 500, 3),
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 0, 500, 5)
                }).then(
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 12, 300, BotApi.STEER_RIGHT_HAND)
                ).then(
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 172, 300, 2)
                ).then(
                        AnimateSteer(BotApi.STEER_ANGEL_CURRENT, 93, 300, 5)
                ).then(
                        reset()
                )
    
    /**
     *
     * @param start
     * @param end
     * @param duration
     * @param index index start from 1
     * @return
     */
    public static ActionStatusController AnimateSteer(int start, int end, int duration, final int index){
        if(start == BotApi.STEER_ANGEL_DEFAULT){
            start = BotApi.getDefaultAngles()[BotApi.jointMapping[index -1] - 1];
        }
        if(start == BotApi.STEER_ANGEL_CURRENT){
            start = BotApi.getCurrentAngles()[BotApi.jointMapping[index -1] - 1];
        }
        if(end == BotApi.STEER_ANGEL_DEFAULT){
            end = BotApi.getDefaultAngles()[BotApi.jointMapping[index -1] - 1];
        }
        if(end == BotApi.STEER_ANGEL_CURRENT){
            end = BotApi.getCurrentAngles()[BotApi.jointMapping[index -1] - 1];
        }
        return actionAnimation(start, end, duration, new Cubicbezier(0.445, 0.05, 0.55, 0.95), new AnimationHandler(){
            @Override
            public void onProgress(int value) {
                super.onProgress(value);
                BotApi.SetSteer(BotApi.jointMapping[index - 1], value);
            }
        });
    }
    

    其中Cubicbezier(0.445, 0.05, 0.55, 0.95)就是缓动函数

    package com.novelor.servicebot.servicebot.utils;
    
    import android.graphics.PointF;
    
    import com.novelor.servicebot.servicebot.models.AnimationTimeFunction;
    
    /**
     * Created by randoms on 2015/10/23.
     */
    public class Cubicbezier implements AnimationTimeFunction {
    
        protected PointF start;
        protected PointF end;
        protected PointF a = new PointF();
        protected PointF b = new PointF();
        protected PointF c = new PointF();
    
        public Cubicbezier(PointF start, PointF end) throws IllegalArgumentException {
            if (start.x < 0 || start.x > 1) {
                throw new IllegalArgumentException("startX value must be in the range [0, 1]");
            }
            if (end.x < 0 || end.x > 1) {
                throw new IllegalArgumentException("endX value must be in the range [0, 1]");
            }
            this.start = start;
            this.end = end;
        }
    
        public Cubicbezier(float startX, float startY, float endX, float endY) {
            this(new PointF(startX, startY), new PointF(endX, endY));
        }
    
        public Cubicbezier(double startX, double startY, double endX, double endY) {
            this((float) startX, (float) startY, (float) endX, (float) endY);
        }
    
        /**
         *
         * @param time between 0 and 1
         * @return between 0 and 1
         */
        @Override
        public float getInterpolation(float time) {
            return getBezierCoordinateY(getXForTime(time));
        }
    
        protected float getBezierCoordinateY(float time) {
            c.y = 3 * start.y;
            b.y = 3 * (end.y - start.y) - c.y;
            a.y = 1 - c.y - b.y;
            return time * (c.y + time * (b.y + time * a.y));
        }
    
        protected float getXForTime(float time) {
            float x = time;
            float z;
            for (int i = 1; i < 14; i++) {
                z = getBezierCoordinateX(x) - time;
            if (Math.abs(z) < 1e-3) {
                break;
            }
            x -= z / getXDerivate(x);
        }
        return x;
    }
    
    private float getXDerivate(float t) {
        return c.x + t * (2 * b.x + 3 * a.x * t);
    }
    
    private float getBezierCoordinateX(float time) {
        c.x = 3 * start.x;
        b.x = 3 * (end.x - start.x) - c.x;
        a.x = 1 - c.x - b.x;
        return time * (c.x + time * (b.x + time * a.x));
    }
    }
    

    暂搁于此,以后再优化补充。


Log in to reply