精华内容
下载资源
问答
  • 运动型多功能车辆高速公路弯道行驶横摆和侧翻稳定性综合控制研究,葛瑾,高峰,为增加高速公路弯道行驶车辆的主动安全性,同时提高车辆侧翻和横摆稳定性,本文利用主动转向和差动制动方法,设计了一种横摆和侧
  • Variable structure and variable coefficient proportional-integral-derivative control to prevent actuator saturation of yaw movement for a coaxial eight-rotor unmanned aerial vehicle
  • pid控制器代码matlab 无人驾驶游泳潜水器的俯仰和偏航角控制 无人驾驶水下航行器俯仰和偏航角控制的仿真 该项目是对无人驾驶潜水艇的俯仰和偏航角控制进行建模和仿真的结果。 该代码由Ahmed Wael在2018年Spring针对...
  • Arducopter Yaw角分析

    2018-09-05 02:30:11
    Arducopter Yaw 现梳理一遍Poshold模式下的yaw的情况: 首先从 Copter::fast_loop() –> update_flight_mode()–> Copter::ModePosHold::run() // get_pilot_desired_heading - transform ...

    Arducopter Yaw

    现梳理一遍Poshold模式下的yaw的情况:


    • 首先从 Copter::fast_loop() –> update_flight_mode()–> Copter::ModePosHold::run()
    // get_pilot_desired_heading - transform pilot's yaw input into a desired yaw rate returns desired yaw rate in centi-degrees per second
    // 偏航的控制,之间进行比例控制,然后把输出作为内环的控制量,由于偏航通道可以控制无人机顺时针旋转和逆时针旋转,
    // 所以这里怎么区分出来的呢?肯定是摇杆中立点是分界线,处理后,往左打是负值,往右是正,区分两个方向。
    // 最后得到偏航角速度目标控制量,偏航打杆控制速率,跟横滚,俯仰有所不同。
    float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
    {
        float yaw_request;
    
        // calculate yaw rate request
        // acro_y_expo 这个值为0-0.5 用来设置当打杆向一边时,设置旋转速度的快慢
        // acro_yaw_p 这个值为把遥控yaw输入转化为期望旋转速率,越大意味着旋转速率会更大
        if (g2.acro_y_expo <= 0) {
            yaw_request = stick_angle * g.acro_yaw_p;
        } else {
            // expo variables
            float y_in, y_in3, y_out;
    
            // range check expo
            if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
                g2.acro_y_expo = 1.0f;
            }
    
            // yaw expo
            y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
            y_in3 = y_in*y_in*y_in;
            y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
            yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
        }
        // convert pilot input to the desired yaw rate
        // 转化pilot 的输入为期望的yaw速率
        return yaw_request;
    }

    • 当无人机在地面ap.land_complete时attitude_control->set_yaw_target_to_current_heading();
      —>shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)100.0f);
    //改变地球坐标系下的目标yaw角度 ,最终算入四元数中
    //shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)*100.0f)
    void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
    {
        float yaw_shift = radians(yaw_shift_cd*0.01f);
        Quaternion _attitude_target_update_quat;
        _attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
        _attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
    }

    • 接下来就是姿态更新器attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
    
    // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
    void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
    {
        // Convert from centidegrees on public interface to radians
        // 转换为弧度且缩小100
        float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
        float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
        float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);
    
        // calculate the attitude target euler angles
        // 计算所需要的目标姿态欧拉角度
        _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
    
        // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
        // 在四轴上 get_roll_trim_rad() 为0
        euler_roll_angle += get_roll_trim_rad();
        // 机体坐标系下的前馈 _rate_bf_ff_enabled 开启或者关闭
        if (_rate_bf_ff_enabled) {
            // translate the roll pitch and yaw acceleration limits to the euler axis
            // 将横滚,俯仰和偏航加速度极限转换为欧拉轴  get_accel_yaw_max_radss()为yaw角的最大加速度设置为 27000 cdeg/s/s
            // 总的来说,就是为了限制角度变化的加速度
            Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
    
            // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
            // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
            // and an exponential decay specified by smoothing_gain at the end.
            //当启用加速限制和前馈时,使用Sqt控制器计算欧拉。
            //角速度将使欧拉角在有限减速下的输入角平稳停止。
            //最后由平滑增益指定的指数衰减。
    
            _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
            _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
    
            // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
            // the output rate towards the input rate.
            // 缓慢让输出速率朝输入速率变化
            _attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);
    
            // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
            // 将实际的姿态的欧拉角导数,转化到机体坐标系中速度矢量,作为前馈速度矢量
            euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
            // Limit the angular velocity
            ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
            // Convert body-frame angular velocity into euler angle derivative of desired attitude
            // 转化机体坐标系角度速度到期望姿态欧垃角倒数
            ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
        } else {
            // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
            _attitude_target_euler_angle.x = euler_roll_angle;
            _attitude_target_euler_angle.y = euler_pitch_angle;
            _attitude_target_euler_angle.z += euler_yaw_rate*_dt;
            // Compute quaternion target attitude
            _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
    
            // Set rate feedforward requests to zero
            _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
            _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
        }
    
        // Call quaternion attitude controller
        attitude_controller_run_quat();
    }
    
    // limits the acceleration and deceleration of a velocity request
    float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
    {
        // Acceleration is limited directly to smooth the beginning of the curve.
        if (is_positive(accel_max)) {
            float delta_ang_vel = accel_max * dt;
            return constrain_float(desired_ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
        } else {
            return desired_ang_vel;
        }
    }
    

    • 当无人机起飞了,takeoff
    // update attitude controller targets
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
    展开全文
  • 2.6 考虑yaw rate 和侧偏角的动力学模型 想将偏航速度和侧偏角作为状态变量的一个公式。 2.7 从body fixed 坐标系到全局坐标系 之前的动力学模型状态变量是相对于局部坐标系,为了轨迹,需转换到全局坐标系中...

    2.1 商业开发中的横向控制系统

    汽车行业开发了三种横向系统:
    车道偏离预警系统(LDWS):如果发现偏离则预警
    车道保持系统(LKS):保持在道上
    偏航稳定控制系统:防止车辆旋转和漂移。

    其中偏航稳定控制系统大致有三种:
    Differential Braking systems:利用汽车上的ABS制动系统,在左右车轮之间采用差动制动来控制偏航力矩的系统
    Steer-by-Wire systems:系统可以修改驾驶员的转向角输入,并为车轮添加一个修正的转向角
    Active Torque Distribution systems:系统利用主动微分和全轮驱动技术,独立控制分配给每个车轮的驱动力矩,从而提供主动控制牵引力矩和偏航力矩

    2.2 横向车辆运动学模型

    在这里插入图片描述
    假设:轮胎侧偏角为0。
    运动学方程:

    note:
    在这里插入图片描述
    在这里插入图片描述

    2.3 横向车辆动力学模型

    在这里插入图片描述
    在这里插入图片描述
    计算轮胎速度夹角:tan小角度线性假设
    在这里插入图片描述
    在这里插入图片描述

    2.4 质点相对于旋转坐标系的运动

    在这里插入图片描述
    在这里插入图片描述
    可以推出:
    在这里插入图片描述

    2.5 考虑相对于道路误差的动力学模型

    为了实现自动车道保持的转向控制系统,需要利用一个基于与道路位置、姿态偏差的动力学模型。
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    2.6 考虑yaw rate 和侧偏角的动力学模型

    想将偏航速度和侧偏角作为状态变量的一个公式。

    2.7 从body fixed 坐标系到全局坐标系

    之前的动力学模型状态变量是相对于局部坐标系,为了轨迹,需转换到全局坐标系中。
    在这里插入图片描述

    展开全文
  • <div><p>该提问来源于开源项目:betaflight/betaflight</p></div>
  • 无人驾驶13:PID控制器

    千次阅读 2020-02-29 21:43:40
    ------------------------------------ # # run - does a single control run robot = Robot() robot.set(0, 1, 0) def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0): x_trajectory = [] y_trajectory = [] ...

    基于环境反馈的控制方法,叫做反馈控制

    控制模块的目的是让无人车能够按照规划好的路径行驶,需要将环境当前的反馈和规划的参考量进行比较,得到当前偏离参考量的误差,并基于这个误差设计一定的算法来产生输出信号,使得误差不断变小,直到为0.

    PID控制器是目前应用最广泛的控制理论。

    PID控制器是指P(Proportion 比例)、I(Integral 积分),D(Derivative 微分) ,这三项如何使用误差来产生控制指令。

    1. P控制

    假设控制输出的是车辆的转角;

    偏离航迹误差(Cross Track Error, CTE):就是车辆到参考线的距离,用CTE作为误差度量,即转向角与CTE成比例;

    s t e r r i n g a n g l e = K p ∗ e ( t ) sterring angle = K_p *e(t) sterringangle=Kpe(t)

    e(t)是车辆在t时刻的CTE,CTE偏大时,转向角也大,反之偏转小一点;

    在P控制器中,系数 K p K_p Kp会直接影响实际控制效果,在合理范围内, K p K_p Kp越大,控制效果越好。

    伪代码实现:

    def run(robot, tau, n=100, speed=1.0):
        x_trajectory = []
        y_trajectory = []
        for i in range(n):
            cte = robot.y
            steer = -tau * cte
            robot.move(steer, speed)
            x_trajectory.append(robot.x)
            y_trajectory.append(robot.y)
        return x_trajectory, y_trajectory
    

    但是P控制器容易受零值影响,实际中表现为来回多次穿过参考线

    P_Control

    为了矫正这个问题,我们引入一个新的参数,CTE变化率

    2. PD控制

    CTE的变化率,描述无人车向着参考线移动的多块,如果无人车一直在参考线上运动,CTE变化率就是0;

    s t e e r i n g a n g l e = K p ∗ e ( t ) + K d d e ( t ) d t steering angle = K_p*e(t) + K_d\frac{de(t)}{dt} steeringangle=Kpe(t)+Kddtde(t)

    增大P系数会增大无人车向参考线运动的倾向,增大D系数则会增大无人车转向角快速变化的“阻尼”;

    对于P偏大,D偏小的系统,称为“欠阻尼系统(underdamped)”, 这种情况无人车将沿参考线震荡前行;

    而P偏小,D偏大的系统,称为“过阻尼(overdamped)”,这使得无人车需较长时间才能纠正误差;

    合适的P,D系数,可以使无人车快速回到参考线,同时很好维持在参考线运动;

    伪代码实现:

    def run(robot, tau_p, tau_d, n=100, speed=1.0):
        x_trajectory = []
        y_trajectory = []
        prev_cte = robot.y
        for i in range(n):
            cte = robot.y
            diff_cte = cte - prev_cte
            prev_cte = cte
            steer = -tau_p * cte - tau_d * diff_cte
            robot.move(steer, speed)
            x_trajectory.append(robot.x)
            y_trajectory.append(robot.y)
        return x_trajectory, y_trajectory
    

    pd.png

    系统偏差

    PD控制器可以保证正常的控制需求,但是容易受系统偏差影响;

    比如当车轮存在偏移角(非理想化),对人类驾驶来说,这问题不大,但是对于无人驾驶,会产生巨大的CTE。

    为解决熊偏差问题,引入一个控制量来消除这种误差。

    3 PID控制器

    引入一个积分项,加入到控制输出函数中

    s t e e r i n g a n g l e = K p ∗ e ( t ) + K d d e ( t ) d t + k i ∫ 0 t e ( t ) d t steering angle = K_p*e(t) + K_d\frac{de(t)}{dt} + k_i\int_0^te(t)dt steeringangle=Kpe(t)+Kddtde(t)+ki0te(t)dt

    K i K_i Ki积分项,本质上是车的实际路线和参考线的图形面积,加入积分项后,控制器会使车辆路线的积分尽可能小,从而避免稳态误差情况。

    伪代码实现:

    def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
        x_trajectory = []
        y_trajectory = []
        prev_cte = robot.y
        int_cte = 0
        for i in range(n):
            cte = robot.y
            diff_cte = cte - prev_cte
            prev_cte = cte
            int_cte += cte
            steer = -tau_p * cte - tau_d * diff_cte - tau_i * int_cte
            robot.move(steer, speed)
            x_trajectory.append(robot.x)
            y_trajectory.append(robot.y)
        return x_trajectory, y_trajectory
    

    积分向系数的大小也会影响整个控制系统的稳定性, K i K_i Ki过大,会使系统“震荡”运行, K i K_i Ki过小又会需要较长时间才能修正车辆稳态误差。

    pid

    PID控制器,就是由 K p , K i , K d K_p, K_i, K_d Kp,Ki,Kd三项共同决定的,其实现非常简单,但是这三个系数的选择缺很难。

    PID算法Python实现:

    # -----------
    # User Instructions
    #
    # Implement a P controller by running 100 iterations
    # of robot motion. The steering angle should be set
    # by the parameter tau so that:
    #
    # steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE
    #
    # where the integrated crosstrack error (int_CTE) is
    # the sum of all the previous crosstrack errors.
    # This term works to cancel out steering drift.
    #
    # Only modify code at the bottom! Look for the TODO.
    # ------------
    
    import random
    import numpy as np
    import matplotlib.pyplot as plt
    
    # ------------------------------------------------
    # 
    # this is the Robot class
    #
    
    class Robot(object):
        def __init__(self, length=20.0):
            """
            Creates robot and initializes location/orientation to 0, 0, 0.
            """
            self.x = 0.0
            self.y = 0.0
            self.orientation = 0.0
            self.length = length
            self.steering_noise = 0.0
            self.distance_noise = 0.0
            self.steering_drift = 0.0
    
        def set(self, x, y, orientation):
            """
            Sets a robot coordinate.
            """
            self.x = x
            self.y = y
            self.orientation = orientation % (2.0 * np.pi)
    
        def set_noise(self, steering_noise, distance_noise):
            """
            Sets the noise parameters.
            """
            # makes it possible to change the noise parameters
            # this is often useful in particle filters
            self.steering_noise = steering_noise
            self.distance_noise = distance_noise
    
        def set_steering_drift(self, drift):
            """
            Sets the systematical steering drift parameter
            """
            self.steering_drift = drift
    
        def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):
            """
            steering = front wheel steering angle, limited by max_steering_angle
            distance = total distance driven, most be non-negative
            """
            if steering > max_steering_angle:
                steering = max_steering_angle
            if steering < -max_steering_angle:
                steering = -max_steering_angle
            if distance < 0.0:
                distance = 0.0
    
            # apply noise
            steering2 = random.gauss(steering, self.steering_noise)
            distance2 = random.gauss(distance, self.distance_noise)
    
            # apply steering drift
            steering2 += self.steering_drift
    
            # Execute motion
            turn = np.tan(steering2) * distance2 / self.length
    
            if abs(turn) < tolerance:
                # approximate by straight line motion
                self.x += distance2 * np.cos(self.orientation)
                self.y += distance2 * np.sin(self.orientation)
                self.orientation = (self.orientation + turn) % (2.0 * np.pi)
            else:
                # approximate bicycle model for motion
                radius = distance2 / turn
                cx = self.x - (np.sin(self.orientation) * radius)
                cy = self.y + (np.cos(self.orientation) * radius)
                self.orientation = (self.orientation + turn) % (2.0 * np.pi)
                self.x = cx + (np.sin(self.orientation) * radius)
                self.y = cy - (np.cos(self.orientation) * radius)
    
        def __repr__(self):
            return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
    
    ############# ADD / MODIFY CODE BELOW ####################
    # ------------------------------------------------------------------------
    #
    # run - does a single control run
    
    robot = Robot()
    robot.set(0, 1, 0)
    
    
    def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
        x_trajectory = []
        y_trajectory = []
        prev_cte = robot.y
        int_cte = 0
        for i in range(n):
            cte = robot.y
            diff_cte = cte - prev_cte
            prev_cte = cte
            int_cte += cte
            steer = -tau_p * cte - tau_d * diff_cte - tau_i * int_cte
            robot.move(steer, speed)
            x_trajectory.append(robot.x)
            y_trajectory.append(robot.y)
        return x_trajectory, y_trajectory
    
    x_trajectory, y_trajectory = run(robot, 0.2, 3.0, 0.001)
    n = len(x_trajectory)
    
    plt.plot(x_trajectory, y_trajectory, 'b', label='PID controller')
    plt.plot(x_trajectory, np.zeros(n), 'r', label='reference')
    plt.savefig("PID_Contrl.png")
    

    在这里插入图片描述

    PID系数调整方法:

    1.twiddle算法:

    如上例PID算法实现,返回一个CTE的平均值来度量系统的误差程度,问题转化为,寻找使CTE均值最小化的PID系数。

    设立一组初始值, 比如系数初值p=[0, 0, 0]; 系数调整范围初值dp=[1,1,1]

    循环遍历迭代系数p,

    首先调大p[i]值,p[i] += dp[i]

    若效果改善,保留该值,将对应系数范围加大dp[i]*=1.1

    若无改善,调小p[i], p[i] -= 2*dp[i] ;

    若有效,保留该值,将对应系数范围加大dp[i]*=1.1

    若无效,p[i]保留原值,缩小dp[i]范围 dp[i]*=1.1;

    循环迭代,通过缩放得到的参数越来越精确,直到收敛(系数范围和sum(dp[i])足够小).

    这是一种局部爬山算法,但是它非常高效.

    伪代码实现:

    def twiddle(tol=0.2): 
        p = [0, 0, 0]
        dp = [1, 1, 1]
        robot = make_robot()
        x_trajectory, y_trajectory, best_err = run(robot, p)
    
        it = 0
        while sum(dp) > tol:
            print("Iteration {}, best error = {}".format(it, best_err))
            for i in range(len(p)):
                p[i] += dp[i]
                robot = make_robot()
                x_trajectory, y_trajectory, err = run(robot, p)
    
                if err < best_err:
                    best_err = err
                    dp[i] *= 1.1
                else:
                    p[i] -= 2 * dp[i]
                    robot = make_robot()
                    x_trajectory, y_trajectory, err = run(robot, p)
    
                    if err < best_err:
                        best_err = err
                        dp[i] *= 1.1
                    else:
                        p[i] += dp[i]
                        dp[i] *= 0.9
            it += 1
        return p
    

    twiddle

    2. 实践调试方法

    该方法是做优达学城项目过程中,mentor给的建议,实践中效果挺不错,哈哈

    具体参考:https://blog.csdn.net/luteresa/article/details/104582834

    Additional Resources on Control

    Model Predictive Control (MPC)

    Vision-Based High Speed Driving with a Deep Dynamic Observer by P. Drews, et. al.

    Abstract: In this paper we present a framework for combining deep learning-based road detection, particle filters, and Model Predictive Control (MPC) to drive aggressively using only a monocular camera, IMU, and wheel speed sensors. This framework uses deep convolutional neural networks combined with LSTMs to learn a local cost map representation of the track in front of the vehicle. A particle filter uses this dynamic observation model to localize in a schematic map, and MPC is used to drive aggressively using this particle filter based state estimate. We show extensive real world testing results, and demonstrate reliable operation of the vehicle at the friction limits on a complex dirt track. We reach speeds above 27 mph (12 m/s) on a dirt track with a 105 foot (32m) long straight using our 1:5 scale test vehicle. […]

    Reinforcement Learning-based

    Reinforcement Learning and Deep Learning based Lateral Control for Autonomous Driving by D. Li, et. al.

    Abstract: This paper investigates the vision-based autonomous driving with deep learning and reinforcement learning methods. Different from the end-to-end learning method, our method breaks the vision-based lateral control system down into a perception module and a control module. The perception module which is based on a multi-task learning neural network first takes a driver-view image as its input and predicts the track features. The control module which is based on reinforcement learning then makes a control decision based on these features. In order to improve the data efficiency, we propose visual TORCS (VTORCS), a deep reinforcement learning environment which is based on the open racing car simulator (TORCS). By means of the provided functions, one can train an agent with the input of an image or various physical sensor measurement, or evaluate the perception algorithm on this simulator. The trained reinforcement learning controller outperforms the linear quadratic regulator (LQR) controller and model predictive control (MPC) controller on different tracks. The experiments demonstrate that the perception module shows promising performance and the controller is capable of controlling the vehicle drive well along the track center with visual input.

    Behavioral Cloning

    The below paper shows one of the techniques Waymo has researched using imitation learning (aka behavioral cloning) to drive a car.

    ChauffeurNet: Learning to Drive by Imitating the Best and Synthesizing the Worst by M. Bansal, A. Krizhevsky and A. Ogale

    Abstract: Our goal is to train a policy for autonomous driving via imitation learning that is robust enough to drive a real vehicle. We find that standard behavior cloning is insufficient for handling complex driving scenarios, even when we leverage a perception system for preprocessing the input and a controller for executing the output on the car: 30 million examples are still not enough. We propose exposing the learner to synthesized data in the form of perturbations to the expert’s driving, which creates interesting situations such as collisions and/or going off the road. Rather than purely imitating all data, we augment the imitation loss with additional losses that penalize undesirable events and encourage progress – the perturbations then provide an important signal for these losses and lead to robustness of the learned model. We show that the ChauffeurNet model can handle complex situations in simulation, and present ablation experiments that emphasize the importance of each of our proposed changes and show that the model is responding to the appropriate causal factors. Finally, we demonstrate the model driving a car in the real world.

    展开全文
  • After that, desired longitudinal acceleration and desired yaw moment are obtained by a linear matrix inequality based robust constrained state feedback method. Finally, driver-in-the-loop tests on a ...
  • Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析源码解析这个函数做了什么部分细节euler_accel_limit()input_shaping_angle()姿态变化率与机体角速度之间的关系 本文将以input_euler_angle_roll_...


    本文将以input_euler_angle_roll_pitch_yaw()函数为例,讲解一下APM中的前馈及平滑控制函数,当然其余的类似input_euler_angle_roll_pitch_euler_rate_yaw()等等的函数也是类似的,当学习完这个函数之后,其他的上手应该也会很快的。

    源码解析

    函数名:input_euler_angle_roll_pitch_yaw()

    函数位置:ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl

    官方注释:Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing

    注:以下内容会尽可能详细注释,力求让大家都看懂,一些细节部分会着重放在后面讲解。其中以n系简称NED坐标系,b系简称机体坐标系,tb表示期望姿态,cb表示当前姿态。

    // 通过角速度前馈和平滑控制欧拉角,俯仰角和偏航角
    // 前馈控制器控制量计算在此函数中完成
    // 前三个输入参数:本次输入的期望姿态的roll、pitch、yaw角
    void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
    {
        // 角度制转换为弧度制(乘以0.01是因为在输入这个函数之前乘以了100)
        float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
        float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
        float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
    
        // _attitude_target_quat表示当前(未更新本次输入前)的期望姿态的四元数
    	// 可以理解为操作员通过遥控器摇杆输入的期望姿态是在实时变更的
    	// 这个总函数(input_euler_xxx)的输入是本次输入的期望,而_attitude_target_quat中保存的是还未收到本次输入进行更新前,当下的期望姿态
    	// 此处将其转换为欧拉角形式_attitude_target_euler_angle
        _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
    
        // 添加侧倾调整以补偿直升机尾部螺旋桨的推力(在多旋翼飞机上将返回零)
        euler_roll_angle += get_roll_trim_rad();
    
        if (_rate_bf_ff_enabled) {
        	// 如果开启了机体速率前馈,注意下方将期望角度转换为期望角速率的操作表示的是FF前馈控制器的作用
    		// FF前馈控制器根据期望姿态误差计算出期望姿态的期望角速率,其是作为前馈控制量叠加到P控制器输出的当前姿态的期望角速率上的
    
            // 将侧倾、俯仰和偏航的加速度限制转换到欧拉轴上
    		// euler_accel_limit()接收当前期望姿态欧拉角和roll、pitch、yaw上最大加速度
    		// 以此计算出最小加速度限制,使增加加速度时不会超过任何一个轴的最大加速度
            Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
    
            // 根据本次输入的期望姿态和未更新输入的当前期望姿态的姿态角度误差计算期望欧拉角速率校正值_attitude_target_euler_rate
    		// 以roll角为例
    		// wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x):计算本次输入期望roll角和当前期望姿态roll角之间的误差,并将其限制在[-pi pi]之间
    		// input_tc:姿态控制输入时间常数,数字越小,响应越尖锐;数字越大,响应越柔和,默认设定为0.15
    		// euler_accel.x经限制后的欧拉角加速度
    		// _attitude_target_euler_rate.x:当前期望姿态欧拉角速率
    		// _dt:采样周期
            _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
            _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
            _attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _attitude_target_euler_angle.z), _input_tc, euler_accel.z, _attitude_target_euler_rate.z, _dt);
            if (slew_yaw) {		// 如果开启了摆率限制,那么还要对期望欧拉角速率进行限制
                _attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
            }
    
            // 将期望姿态的欧拉角速率_attitude_target_euler_rate转换为前馈的期望机体角速度矢量_attitude_target_ang_vel
    		// 姿态变化率(n系)转换为期望机体角速度(cb系)
            euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
            // 角速度_attitude_target_ang_vel限幅
            ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
            // 然后将限幅后的期望机体角速度重新转换为所需姿态的欧拉角速率
            ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
            // 最后得到的期望角速率_attitude_target_ang_vel和期望欧拉角速率_attitude_target_euler_rate是前馈速率,即前馈控制量
            // 注意以上运算过程是在本次输入的期望姿态与当前期望姿态之间实现的,最后得到的期望角速率是针对于期望机体坐标系的
            // 因此该期望角速率在使用前需要先转换到当前姿态下
        } else {
            // 如果未启用前馈功能,则将目标欧拉角输入到目标中并将前馈速率归零
            
            // 将本次输入的期望roll和yaw保存进当下的_attitude_target_euler_angle
            _attitude_target_euler_angle.x = euler_roll_angle;
            _attitude_target_euler_angle.y = euler_pitch_angle;
            if (slew_yaw) {
                // 如果开启了摆率限制
    			// 计算出本次输入期望yaw角和当前期望yaw角之间的误差,并限幅
                float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _attitude_target_euler_angle.z), -get_slew_yaw_rads() * _dt, get_slew_yaw_rads() * _dt);
                // 根据误差更新yaw角期望姿态
                _attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
            } else {
            	// 如果没有开启摆率限制
    			// 直接将本次输入保存为当前期望姿态yaw角
                _attitude_target_euler_angle.z = euler_yaw_angle;
            }
            // 将期望姿态从欧拉角转换为四元数形式(n系下)
            _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
    
            // 将速率前馈设置为零,即没有前馈量
            _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
            _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
        }
    
        // 调用四元数姿态控制器(真正的P控制器在其中运行)
    	// 根据姿态角误差计算出期望角速率
        attitude_controller_run_quat();
    }
    
    

     

    这个函数做了什么

    关于什么是前馈,怎么叠加的,以及APM中的PID控制流程,详见我之前的博文:Ardupilot姿态控制器 PID控制流程

    这里还是先放一张APM官方的老图:

    在这里插入图片描述

    本函数中主要做了以下几件事:

    • 获取到本次输入的期望姿态euler_roll\pitch\yaw_angle(无论是来自遥控器还是地面站MAVLINK消息),以及未更新前(未处理本次输入前)的当下期望姿态_attitude_target_euler_angle(全局变量);
    • 根据是否开启前馈控制分别进行处理:
      • 如果开启了前馈控制:
        1. 根据本次输入的期望和当前期望计算出欧拉角误差,然后计算得出期望欧拉角速率_attitude_target_euler_rate;
        2. 期望欧拉角速率_attitude_target_euler_rate转换为b系下的期望机体角速度_attitude_target_ang_vel并且进行角速度限幅;
        3. 最后重新获取到前馈速率:_attitude_target_euler_rate和_attitude_target_ang_vel;
        4. _attitude_target_quat的更新将在attitude_controller_run_quat()中进行;
      • 如果没有开启前馈控制:
        1. 直接将本次输入的期望姿态保存到全局变量 _attitude_target_euler_angle,并获取四元数形式的期望姿态 _attitude_target_quat;
        2. 然后将前馈速率 _attitude_target_euler_rate和_attitude_target_ang_vel置零。
    • 最后运行四元数姿态控制器,真正的P控制器将在其内部运行。

    总结:input_euler_angle_roll_pitch_yaw()这个函数主要在其内部实现了前馈控制量计算及平滑的操作,最后调用了四元数姿态控制器(废话 =.=)。


    注意:如果开启了前馈,计算出来的前馈速率 _attitude_target_ang_vel是基于期望姿态 的,因此在使用前,需要先转换到当前姿态坐标系下。

    原因:因为input_euler_xxx()里面期望角速率是根据本次输入期望姿态当下期望姿态计算得到(基于tb系),而attitude_controller_run_quat()中的P控制器则是通过当下期望姿态_attitude_target_quat当前姿态的误差计算得到期望角速率(基于cb系)

     

    部分细节

    euler_accel_limit()

    // translates body frame acceleration limits to the euler axis
    Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const Vector3f &euler_accel)
    {
        float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
        float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
        float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
    
        Vector3f rot_accel;
        if (is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || is_negative(euler_accel.x) || is_negative(euler_accel.y) || is_negative(euler_accel.z)) {
            rot_accel.x = euler_accel.x;
            rot_accel.y = euler_accel.y;
            rot_accel.z = euler_accel.z;
        } else {
            rot_accel.x = euler_accel.x;
            rot_accel.y = MIN(euler_accel.y / cos_phi, euler_accel.z / sin_phi);
            rot_accel.z = MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / sin_phi), euler_accel.z / cos_phi);
        }
        return rot_accel;
    }
    
    

    原谅我截止到写博文的时间还没有推出这个原型公式到底是什么(然而我还是厚脸皮地放上来了,有大佬懂麻烦留一下言)。然而我可以说一下这个函数的目的到底是啥。

    参考资料:Copter: Is there any problem in function euler_accel_limit()?

    So the problem here is we need to calculate the acceleration limits that will not let us increase past any one individual axis acceleration limit. I think you are calculating the maximum acceleration limits possible, not the minimum that we need.
    So for example, if we are banked over by 45 degrees in roll. If we ask apply full yaw then the acceleration will be shared by both the body pitch and body yaw. However, if we apply full yaw and push the pitch stick forward then that euler yaw acceleration is applied only to the body frame yaw and is increased in magnitude because the aircraft is banked by 45 degrees. So we need to limit the yaw acceleration based on this limit rather than the vector addition of both body pitch and body yaw.

    翻译过来的大致意思就是:

    这个函数计算加速度极限,该极限让我们增加加速度时不会超过欧拉角上任何一个轴的加速度极限。因此该函数求的是各轴上的最小加速度限制。

    举例来说,当我们沿俯仰角倾斜了45°,也就是说机头向上仰起,如果要求完全偏航,此时加速度将会由俯仰和偏航共同承担,但是如果我们施加了完全偏航并且向前推动俯仰杆,欧拉偏航加速度仅应用于机体框架,使得在当前倾斜情况下欧拉角的旋转幅度大于机体的旋转幅度,因此我们需要这个函数来限制偏航加速度。

    input_shaping_angle()

    这个函数实际上内部调用了sqrt_controller(一个修改过的P控制器)来计算得到前馈控制量_attitude_target_euler_rate。其他的不再多做解释。

    关于sqrt_controller的解释,看这篇:详解APM的开方控制器sqrt_controller

    // 根据角度误差计算速度校正。 角速度具有加速和减速限制,包括使用_input_tc进行的基本加速度限制
    float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt)
    {
    // sqrt_controller作为调整后的P控制器,根据接收到的角度误差计算出期望角速率
    // Kp = 1/max(input_tc,0.01)
        float desired_ang_vel = sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
    
        // 直接限制加速度以平滑曲线的起点。
        return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
    }
    ...
    // 限制请求速度的加减速
    float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
    {
        // 直接限制加速度以平滑曲线的起点。
        if (is_positive(accel_max)) {
            float delta_ang_vel = accel_max * dt;
            return constrain_float(desired_ang_vel, target_ang_vel - delta_ang_vel, target_ang_vel + delta_ang_vel);
        } else {
            return desired_ang_vel;
        }
    }
    
    

    姿态变化率与机体角速度之间的关系

    主要是下面两个函数:

    // Convert a 321-intrinsic euler angle derivative to an angular velocity vector
    void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
    {
        float sin_theta = sinf(euler_rad.y);
        float cos_theta = cosf(euler_rad.y);
        float sin_phi = sinf(euler_rad.x);
        float cos_phi = cosf(euler_rad.x);
    
        ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z;
        ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z;
        ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
    }
    
    // Convert an angular velocity vector to a 321-intrinsic euler angle derivative
    // Returns false if the vehicle is pitched 90 degrees up or down
    bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads)
    {
        float sin_theta = sinf(euler_rad.y);
        float cos_theta = cosf(euler_rad.y);
        float sin_phi = sinf(euler_rad.x);
        float cos_phi = cosf(euler_rad.x);
    
        // When the vehicle pitches all the way up or all the way down, the euler angles become discontinuous. In this case, we just return false.
        if (is_zero(cos_theta)) {
            return false;
        }
    
        euler_rate_rads.x = ang_vel_rads.x + sin_phi * (sin_theta / cos_theta) * ang_vel_rads.y + cos_phi * (sin_theta / cos_theta) * ang_vel_rads.z;
        euler_rate_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z;
        euler_rate_rads.z = (sin_phi / cos_theta) * ang_vel_rads.y + (cos_phi / cos_theta) * ang_vel_rads.z;
        return true;
    }
    

    实际上一些基础理论我都汇总在了这篇博文里面:APM姿态旋转理论基础

    这边还是放一下数学原型,摘自全权老师的《多旋翼飞行器设计与控制》:

    在这里插入图片描述
     

    如有错误请及时告知

    展开全文
  • Z轴正方向为前进方向 pitch():俯仰,将物体绕X轴旋转(localRotationX) yaw():航向,将物体绕Y轴旋转(localRotationY) roll():横滚,将物体绕Z轴旋转(localRotationZ)
  • 基于模型预测控制(MPC)的车道保持控制实现方法

    万次阅读 多人点赞 2019-04-10 19:05:44
    % tighter control on lateral deviation over relative yaw angle ScaleFactor 的作用是在优化的目标函数中将控制量和输出量统一成一样的范围,方便调节输出权重和控制权重。   总结  本文介绍...
  • mavlink manual_control_send

    2020-06-16 15:16:43
    manual_control_send(self, target, x, y, z, r, buttons, force_mavlink1=False) method of pymavlink.dialects.v10.ardupilotmega.MAVLink instance This message provides an API for manually controlling the ...
  • depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.control
  • 大疆DJI_OSDK控制无人机飞行

    千次阅读 2020-06-06 21:29:49
    // Publish the control signal ctrlPosYawPub = nh.advertise("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 10); ctrlBrakePub = nh.advertise("dji_sdk/flight_control_setpoint_generic", 10); // ...
  • Autonomous Driving Steering Control

    千次阅读 2020-01-11 18:58:56
    ind: ai = PIDControl(target_speed, state.v) di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) state = update(state, ai, di) time = time + dt x.append(state.x) y.append(state.y) yaw....
  • GAN-Control 论文笔记简介1 摘要2 StyleGAN2 框架3 GAN-Control 差异点:4 GAN-Control 框架:5 GAN-Control 的 loss function:6 生成效果:7 总结 简介 论文名称:GAN-Control: Explicitly Controllable GANs ...
  • mc_att_control源码解析

    2018-05-19 15:57:19
    * the weight that we use to control the yaw. This gives precedence to roll & pitch correction. * The weight is 1 if there is no tilt error. */ /** * 计算偏航控制的权重,tilt angle error越大,yaw...
  • PX4中vtol_att_control 源码解析

    千次阅读 2018-04-17 17:20:52
    px4中vtol姿态控制源码分析/src/modules/vtol_att_control/文件夹中包含vtol_att_control_main、vtol_type、standard/tailsitter/tiltrotor等文件。下面是主要控制逻辑:事实上,PX4飞控系统支持所有的垂直起降机型...
  • 本博客主要是对《Robotics, Vision and Control》第三章课后习题答案进行总结,与大家交流学习。 2.For a lspb trajectory from 0 to 1 in 50 steps explore the effects of specifying the velocity for the ...
  • m trying to control the roll and pitch angles, yaw rate, and altitude and running into trouble. I am using the setpoint_raw/attitude command. I can control my throttle through it, but I don't seem...
  • __EXPORT int fw_att_control_main(int argc, char *argv[])”,在这个函数里就实现了,这个程序是否已近启动,如果没有启动就会注册函数来启动。 声明FixedwingAttitudeControl是主函数 int fw_att_control_...
  • pixhawk mc_pos_control.cpp源码解读

    万次阅读 2016-06-21 16:04:26
    这篇blog是顺着上一篇pixhawk 整体架构的认识写的,接下来看程序的话,打算把各个功能模块理解一遍,先从mc_pos_control.cpp看起。 先提一下pixhawk的整体逻辑: commander和navigator产生期望位置 position_...
  • "liveness_control" , "LOW" ) ; JSONObject res2 = client . detect ( image1 , "BASE64" , options ) ; System . out . println ( res2 . toString ( 2 ) ) ; } } 上面 方法二 中的相关类: ...
  • So, what we need to control here is the lateral position and the yaw angle. And we’re going to do so by adjusting the steering angle. The reference values for the lateral position and yaw angle are ...
  • 原文链接:https://pixhawk.org/dev/mixing Control Mixing ...This page discusses the general-purpose control mixing architecture in PX4. If you are looking for specific mixer setup instructions, y
  • 这篇论文是无人机路径规划和控制领域的经典文章,这里给出中文译文,翻译是逐渐进行的,不保证正确率,如有错误请指出。...我们开发了一种算法,允许通过一组x、y、z、yaw序列实时生成三维最优路径,...
  • control_position(dt)
  • Book: Robotics, Vision and Control, Second Edition Author: Peter Corke 公开课: The open online robotics education resource 个人主页:www.doctorsrn.cn 摘录 Robots are data-driven machines. They ac...
  • PX4——mc_att_control(姿态控制)

    千次阅读 2016-12-13 18:00:29
    0 means very slow control,controller will start to move yaw only when sees yaw position error, 1 meansvery fast responsive control, but with some overshot, controller will move yawimmediately, always...
  • pixhawk mc_pos_control.cpp思路整理

    千次阅读 热门讨论 2016-07-11 10:09:15
    削去旁支(细节请参考pixhawk mc_pos_control.cpp源码解读),位置控制主要流程如下 1.PID参数 param_get(_params_handles.xy_p, &v); _params.pos_p(0) = v; _params.pos_p(1) = v; param_get(_params_handles.z_p, ...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,951
精华内容 780
热门标签
关键字:

controlyaw